git.sur5r.net Git - openocd/blob - src/target/arm7_9_common.c

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
   6  *   oyvind.harboe@zylin.com                                               *
   7  *                                                                         *
   8  *   Copyright (C) 2008 by Spencer Oliver                                  *
   9  *   spen@spen-soft.co.uk                                                  *
  10  *                                                                         *
  11  *   Copyright (C) 2008 by Hongtao Zheng                                   *
  12  *   hontor@126.com                                                        *
  13  *                                                                         *
  14  *   This program is free software; you can redistribute it and/or modify  *
  15  *   it under the terms of the GNU General Public License as published by  *
  16  *   the Free Software Foundation; either version 2 of the License, or     *
  17  *   (at your option) any later version.                                   *
  18  *                                                                         *
  19  *   This program is distributed in the hope that it will be useful,       *
  20  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  21  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  22  *   GNU General Public License for more details.                          *
  23  *                                                                         *
  24  *   You should have received a copy of the GNU General Public License     *
  25  *   along with this program; if not, write to the                         *
  26  *   Free Software Foundation, Inc.,                                       *
  27  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  28  ***************************************************************************/
  29 #ifdef HAVE_CONFIG_H
  30 #include "config.h"
  31 #endif
  32
  33 #include "embeddedice.h"
  34 #include "target_request.h"
  35 #include "arm7_9_common.h"
  36 #include "time_support.h"
  37 #include "arm_simulator.h"
  38
  39
  40 int arm7_9_debug_entry(target_t *target);
  41 int arm7_9_enable_sw_bkpts(struct target_s *target);
  42
  43 /* command handler forward declarations */
  44 int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  45 int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  46 int handle_arm7_9_read_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  47 int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  48 int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  49 int handle_arm7_9_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  50 int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  51 int handle_arm7_9_etm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  52
  53 static int arm7_9_clear_watchpoints(arm7_9_common_t *arm7_9)
  54 {
  55         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
  56         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
  57         arm7_9->sw_breakpoints_added = 0;
  58         arm7_9->wp0_used = 0;
  59         arm7_9->wp1_used = arm7_9->wp1_used_default;
  60         arm7_9->wp_available = arm7_9->wp_available_max;
  61
  62         return jtag_execute_queue();
  63 }
  64
  65 static void arm7_9_assign_wp(arm7_9_common_t *arm7_9, breakpoint_t *breakpoint)
  66 {
  67         if (!arm7_9->wp0_used)
  68         {
  69                 arm7_9->wp0_used = 1;
  70                 breakpoint->set = 1;
  71                 arm7_9->wp_available--;
  72         }
  73         else if (!arm7_9->wp1_used)
  74         {
  75                 arm7_9->wp1_used = 1;
  76                 breakpoint->set = 2;
  77                 arm7_9->wp_available--;
  78         }
  79         else
  80         {
  81                 LOG_ERROR("BUG: no hardware comparator available");
  82         }
  83 }
  84
  85 /* set up embedded ice registers */
  86 static int arm7_9_set_software_breakpoints(arm7_9_common_t *arm7_9)
  87 {
  88         if (arm7_9->sw_breakpoints_added)
  89         {
  90                 return ERROR_OK;
  91         }
  92         if (arm7_9->wp_available < 1)
  93         {
  94                 LOG_WARNING("can't enable sw breakpoints with no watchpoint unit available");
  95                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
  96         }
  97         arm7_9->wp_available--;
  98
  99         /* pick a breakpoint unit */
 100         if (!arm7_9->wp0_used)
 101         {
 102                 arm7_9->sw_breakpoints_added=1;
 103                 arm7_9->wp0_used = 3;
 104         } else if (!arm7_9->wp1_used)
 105         {
 106                 arm7_9->sw_breakpoints_added=2;
 107                 arm7_9->wp1_used = 3;
 108         }
 109         else
 110         {
 111                 LOG_ERROR("BUG: both watchpoints used, but wp_available >= 1");
 112                 return ERROR_FAIL;
 113         }
 114
 115         if (arm7_9->sw_breakpoints_added==1)
 116         {
 117                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE], arm7_9->arm_bkpt);
 118                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0x0);
 119                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffffu);
 120                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 121                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 122         }
 123         else if (arm7_9->sw_breakpoints_added==2)
 124         {
 125                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE], arm7_9->arm_bkpt);
 126                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0x0);
 127                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0xffffffffu);
 128                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 129                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 130         }
 131         else
 132         {
 133                 LOG_ERROR("BUG: both watchpoints used, but wp_available >= 1");
 134                 return ERROR_FAIL;
 135         }
 136
 137         return jtag_execute_queue();
 138 }
 139
 140 /* set things up after a reset / on startup */
 141 int arm7_9_setup(target_t *target)
 142 {
 143         armv4_5_common_t *armv4_5 = target->arch_info;
 144         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 145
 146         return arm7_9_clear_watchpoints(arm7_9);
 147 }
 148
 149 int arm7_9_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p)
 150 {
 151         armv4_5_common_t *armv4_5 = target->arch_info;
 152         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 153
 154         if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
 155         {
 156                 return -1;
 157         }
 158
 159         if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
 160         {
 161                 return -1;
 162         }
 163
 164         *armv4_5_p = armv4_5;
 165         *arm7_9_p = arm7_9;
 166
 167         return ERROR_OK;
 168 }
 169
 170 /* we set up the breakpoint even if it is already set. Some action, e.g. reset
 171  * might have erased the values in embedded ice
 172  */
 173 int arm7_9_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 174 {
 175         armv4_5_common_t *armv4_5 = target->arch_info;
 176         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 177         int retval=ERROR_OK;
 178
 179         if (target->state != TARGET_HALTED)
 180         {
 181                 LOG_WARNING("target not halted");
 182                 return ERROR_TARGET_NOT_HALTED;
 183         }
 184
 185         if (breakpoint->type == BKPT_HARD)
 186         {
 187                 /* either an ARM (4 byte) or Thumb (2 byte) breakpoint */
 188                 u32 mask = (breakpoint->length == 4) ? 0x3u : 0x1u;
 189
 190                 /* reassign a hw breakpoint */
 191                 if (breakpoint->set==0)
 192                 {
 193                         arm7_9_assign_wp(arm7_9, breakpoint);
 194                 }
 195
 196                 if (breakpoint->set==1)
 197                 {
 198                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], breakpoint->address);
 199                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
 200                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffffu);
 201                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 202                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 203                 }
 204                 else if (breakpoint->set==2)
 205                 {
 206                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], breakpoint->address);
 207                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
 208                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffffu);
 209                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 210                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 211                 }
 212                 else
 213                 {
 214                         LOG_ERROR("BUG: no hardware comparator available");
 215                         return ERROR_OK;
 216                 }
 217
 218                 retval=jtag_execute_queue();
 219         }
 220         else if (breakpoint->type == BKPT_SOFT)
 221         {
 222                 if ((retval=arm7_9_set_software_breakpoints(arm7_9))!=ERROR_OK)
 223                         return retval;
 224
 225                 /* did we already set this breakpoint? */
 226                 if (breakpoint->set)
 227                         return ERROR_OK;
 228
 229                 if (breakpoint->length == 4)
 230                 {
 231                         u32 verify = 0xffffffff;
 232                         /* keep the original instruction in target endianness */
 233                         if ((retval = target->type->read_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
 234                         {
 235                                 return retval;
 236                         }
 237                         /* write the breakpoint instruction in target endianness (arm7_9->arm_bkpt is host endian) */
 238                         if ((retval = target_write_u32(target, breakpoint->address, arm7_9->arm_bkpt)) != ERROR_OK)
 239                         {
 240                                 return retval;
 241                         }
 242
 243                         if ((retval = target_read_u32(target, breakpoint->address, &verify)) != ERROR_OK)
 244                         {
 245                                 return retval;
 246                         }
 247                         if (verify != arm7_9->arm_bkpt)
 248                         {
 249                                 LOG_ERROR("Unable to set 32 bit software breakpoint at address %08x - check that memory is read/writable", breakpoint->address);
 250                                 return ERROR_OK;
 251                         }
 252                 }
 253                 else
 254                 {
 255                         u16 verify = 0xffff;
 256                         /* keep the original instruction in target endianness */
 257                         if ((retval = target->type->read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr)) != ERROR_OK)
 258                         {
 259                                 return retval;
 260                         }
 261                         /* write the breakpoint instruction in target endianness (arm7_9->thumb_bkpt is host endian) */
 262                         if ((retval = target_write_u16(target, breakpoint->address, arm7_9->thumb_bkpt)) != ERROR_OK)
 263                         {
 264                                 return retval;
 265                         }
 266
 267                         if ((retval = target_read_u16(target, breakpoint->address, &verify)) != ERROR_OK)
 268                         {
 269                                 return retval;
 270                         }
 271                         if (verify != arm7_9->thumb_bkpt)
 272                         {
 273                                 LOG_ERROR("Unable to set thumb software breakpoint at address %08x - check that memory is read/writable", breakpoint->address);
 274                                 return ERROR_OK;
 275                         }
 276                 }
 277                 breakpoint->set = 1;
 278         }
 279
 280         return retval;
 281 }
 282
 283 int arm7_9_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 284 {
 285         int retval = ERROR_OK;
 286
 287         armv4_5_common_t *armv4_5 = target->arch_info;
 288         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 289
 290         if (!breakpoint->set)
 291         {
 292                 LOG_WARNING("breakpoint not set");
 293                 return ERROR_OK;
 294         }
 295
 296         if (breakpoint->type == BKPT_HARD)
 297         {
 298                 if (breakpoint->set == 1)
 299                 {
 300                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
 301                         arm7_9->wp0_used = 0;
 302                         arm7_9->wp_available++;
 303                 }
 304                 else if (breakpoint->set == 2)
 305                 {
 306                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
 307                         arm7_9->wp1_used = 0;
 308                         arm7_9->wp_available++;
 309                 }
 310                 retval = jtag_execute_queue();
 311                 breakpoint->set = 0;
 312         }
 313         else
 314         {
 315                 /* restore original instruction (kept in target endianness) */
 316                 if (breakpoint->length == 4)
 317                 {
 318                         u32 current_instr;
 319                         /* check that user program as not modified breakpoint instruction */
 320                         if ((retval = target->type->read_memory(target, breakpoint->address, 4, 1, (u8*)&current_instr)) != ERROR_OK)
 321                         {
 322                                 return retval;
 323                         }
 324                         if (current_instr==arm7_9->arm_bkpt)
 325                                 if ((retval = target->type->write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
 326                                 {
 327                                         return retval;
 328                                 }
 329                 }
 330                 else
 331                 {
 332                         u16 current_instr;
 333                         /* check that user program as not modified breakpoint instruction */
 334                         if ((retval = target->type->read_memory(target, breakpoint->address, 2, 1, (u8*)&current_instr)) != ERROR_OK)
 335                         {
 336                                 return retval;
 337                         }
 338                         if (current_instr==arm7_9->thumb_bkpt)
 339                                 if ((retval = target->type->write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr)) != ERROR_OK)
 340                                 {
 341                                         return retval;
 342                                 }
 343                 }
 344                 breakpoint->set = 0;
 345         }
 346
 347         return retval;
 348 }
 349
 350 int arm7_9_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 351 {
 352         armv4_5_common_t *armv4_5 = target->arch_info;
 353         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 354
 355         if (target->state != TARGET_HALTED)
 356         {
 357                 LOG_WARNING("target not halted");
 358                 return ERROR_TARGET_NOT_HALTED;
 359         }
 360
 361         if (arm7_9->breakpoint_count==0)
 362         {
 363                 /* make sure we don't have any dangling breakpoints. This is vital upon
 364                  * GDB connect/disconnect
 365                  */
 366                 arm7_9_clear_watchpoints(arm7_9);
 367         }
 368
 369         if ((breakpoint->type == BKPT_HARD) && (arm7_9->wp_available < 1))
 370         {
 371                 LOG_INFO("no watchpoint unit available for hardware breakpoint");
 372                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 373         }
 374
 375         if ((breakpoint->length != 2) && (breakpoint->length != 4))
 376         {
 377                 LOG_INFO("only breakpoints of two (Thumb) or four (ARM) bytes length supported");
 378                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 379         }
 380
 381         if (breakpoint->type == BKPT_HARD)
 382         {
 383                 arm7_9_assign_wp(arm7_9, breakpoint);
 384         }
 385
 386         arm7_9->breakpoint_count++;
 387
 388         return arm7_9_set_breakpoint(target, breakpoint);
 389 }
 390
 391 int arm7_9_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 392 {
 393         int retval = ERROR_OK;
 394         armv4_5_common_t *armv4_5 = target->arch_info;
 395         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 396
 397         if((retval = arm7_9_unset_breakpoint(target, breakpoint)) != ERROR_OK)
 398         {
 399                 return retval;
 400         }
 401
 402         if (breakpoint->type == BKPT_HARD)
 403                 arm7_9->wp_available++;
 404
 405         arm7_9->breakpoint_count--;
 406         if (arm7_9->breakpoint_count==0)
 407         {
 408                 /* make sure we don't have any dangling breakpoints */
 409                 if((retval = arm7_9_clear_watchpoints(arm7_9)) != ERROR_OK)
 410                 {
 411                         return retval;
 412                 }
 413         }
 414
 415         return ERROR_OK;
 416 }
 417
 418 int arm7_9_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 419 {
 420         int retval = ERROR_OK;
 421         armv4_5_common_t *armv4_5 = target->arch_info;
 422         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 423         int rw_mask = 1;
 424         u32 mask;
 425
 426         mask = watchpoint->length - 1;
 427
 428         if (target->state != TARGET_HALTED)
 429         {
 430                 LOG_WARNING("target not halted");
 431                 return ERROR_TARGET_NOT_HALTED;
 432         }
 433
 434         if (watchpoint->rw == WPT_ACCESS)
 435                 rw_mask = 0;
 436         else
 437                 rw_mask = 1;
 438
 439         if (!arm7_9->wp0_used)
 440         {
 441                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], watchpoint->address);
 442                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
 443                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], watchpoint->mask);
 444                 if( watchpoint->mask != 0xffffffffu )
 445                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE], watchpoint->value);
 446                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xff & ~EICE_W_CTRL_nOPC & ~rw_mask);
 447                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE | EICE_W_CTRL_nOPC | (watchpoint->rw & 1));
 448
 449                 if((retval = jtag_execute_queue()) != ERROR_OK)
 450                 {
 451                         return retval;
 452                 }
 453                 watchpoint->set = 1;
 454                 arm7_9->wp0_used = 2;
 455         }
 456         else if (!arm7_9->wp1_used)
 457         {
 458                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], watchpoint->address);
 459                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
 460                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], watchpoint->mask);
 461                 if( watchpoint->mask != 0xffffffffu )
 462                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE], watchpoint->value);
 463                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], 0xff & ~EICE_W_CTRL_nOPC & ~rw_mask);
 464                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE | EICE_W_CTRL_nOPC | (watchpoint->rw & 1));
 465
 466                 if((retval = jtag_execute_queue()) != ERROR_OK)
 467                 {
 468                         return retval;
 469                 }
 470                 watchpoint->set = 2;
 471                 arm7_9->wp1_used = 2;
 472         }
 473         else
 474         {
 475                 LOG_ERROR("BUG: no hardware comparator available");
 476                 return ERROR_OK;
 477         }
 478
 479         return ERROR_OK;
 480 }
 481
 482 int arm7_9_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 483 {
 484         int retval = ERROR_OK;
 485         armv4_5_common_t *armv4_5 = target->arch_info;
 486         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 487
 488         if (target->state != TARGET_HALTED)
 489         {
 490                 LOG_WARNING("target not halted");
 491                 return ERROR_TARGET_NOT_HALTED;
 492         }
 493
 494         if (!watchpoint->set)
 495         {
 496                 LOG_WARNING("breakpoint not set");
 497                 return ERROR_OK;
 498         }
 499
 500         if (watchpoint->set == 1)
 501         {
 502                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
 503                 if((retval = jtag_execute_queue()) != ERROR_OK)
 504                 {
 505                         return retval;
 506                 }
 507                 arm7_9->wp0_used = 0;
 508         }
 509         else if (watchpoint->set == 2)
 510         {
 511                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
 512                 if((retval = jtag_execute_queue()) != ERROR_OK)
 513                 {
 514                         return retval;
 515                 }
 516                 arm7_9->wp1_used = 0;
 517         }
 518         watchpoint->set = 0;
 519
 520         return ERROR_OK;
 521 }
 522
 523 int arm7_9_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 524 {
 525         armv4_5_common_t *armv4_5 = target->arch_info;
 526         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 527
 528         if (target->state != TARGET_HALTED)
 529         {
 530                 LOG_WARNING("target not halted");
 531                 return ERROR_TARGET_NOT_HALTED;
 532         }
 533
 534         if (arm7_9->wp_available < 1)
 535         {
 536                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 537         }
 538
 539         if ((watchpoint->length != 1) && (watchpoint->length != 2) && (watchpoint->length != 4))
 540         {
 541                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 542         }
 543
 544         arm7_9->wp_available--;
 545
 546         return ERROR_OK;
 547 }
 548
 549 int arm7_9_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 550 {
 551         int retval = ERROR_OK;
 552         armv4_5_common_t *armv4_5 = target->arch_info;
 553         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 554
 555         if (watchpoint->set)
 556         {
 557                 if((retval = arm7_9_unset_watchpoint(target, watchpoint)) != ERROR_OK)
 558                 {
 559                         return retval;
 560                 }
 561         }
 562
 563         arm7_9->wp_available++;
 564
 565         return ERROR_OK;
 566 }
 567
 568 int arm7_9_execute_sys_speed(struct target_s *target)
 569 {
 570         int retval;
 571
 572         armv4_5_common_t *armv4_5 = target->arch_info;
 573         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 574         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 575         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 576
 577         /* set RESTART instruction */
 578         jtag_add_end_state(TAP_IDLE);
 579         if (arm7_9->need_bypass_before_restart) {
 580                 arm7_9->need_bypass_before_restart = 0;
 581                 arm_jtag_set_instr(jtag_info, 0xf, NULL);
 582         }
 583         arm_jtag_set_instr(jtag_info, 0x4, NULL);
 584
 585         long long then=timeval_ms();
 586         int timeout;
 587         while (!(timeout=((timeval_ms()-then)>1000)))
 588         {
 589                 /* read debug status register */
 590                 embeddedice_read_reg(dbg_stat);
 591                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 592                         return retval;
 593                 if ((buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
 594                                    && (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_SYSCOMP, 1)))
 595                         break;
 596                 if (debug_level>=3)
 597                 {
 598                         alive_sleep(100);
 599                 } else
 600                 {
 601                         keep_alive();
 602                 }
 603         }
 604         if (timeout)
 605         {
 606                 LOG_ERROR("timeout waiting for SYSCOMP & DBGACK, last DBG_STATUS: %x", buf_get_u32(dbg_stat->value, 0, dbg_stat->size));
 607                 return ERROR_TARGET_TIMEOUT;
 608         }
 609
 610         return ERROR_OK;
 611 }
 612
 613 int arm7_9_execute_fast_sys_speed(struct target_s *target)
 614 {
 615         static int set=0;
 616         static u8 check_value[4], check_mask[4];
 617
 618         armv4_5_common_t *armv4_5 = target->arch_info;
 619         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 620         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 621         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 622
 623         /* set RESTART instruction */
 624         jtag_add_end_state(TAP_IDLE);
 625         if (arm7_9->need_bypass_before_restart) {
 626                 arm7_9->need_bypass_before_restart = 0;
 627                 arm_jtag_set_instr(jtag_info, 0xf, NULL);
 628         }
 629         arm_jtag_set_instr(jtag_info, 0x4, NULL);
 630
 631         if (!set)
 632         {
 633                 /* check for DBGACK and SYSCOMP set (others don't care) */
 634
 635                 /* NB! These are constants that must be available until after next jtag_execute() and
 636                  * we evaluate the values upon first execution in lieu of setting up these constants
 637                  * during early setup.
 638                  * */
 639                 buf_set_u32(check_value, 0, 32, 0x9);
 640                 buf_set_u32(check_mask, 0, 32, 0x9);
 641                 set=1;
 642         }
 643
 644         /* read debug status register */
 645         embeddedice_read_reg_w_check(dbg_stat, check_value, check_value);
 646
 647         return ERROR_OK;
 648 }
 649
 650 int arm7_9_target_request_data(target_t *target, u32 size, u8 *buffer)
 651 {
 652         armv4_5_common_t *armv4_5 = target->arch_info;
 653         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 654         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 655         u32 *data;
 656         int retval = ERROR_OK;
 657         u32 i;
 658
 659         data = malloc(size * (sizeof(u32)));
 660
 661         retval = embeddedice_receive(jtag_info, data, size);
 662
 663         for (i = 0; i < size; i++)
 664         {
 665                 h_u32_to_le(buffer + (i * 4), data[i]);
 666         }
 667
 668         free(data);
 669
 670         return retval;
 671 }
 672
 673 int arm7_9_handle_target_request(void *priv)
 674 {
 675         int retval = ERROR_OK;
 676         target_t *target = priv;
 677         if (!target->type->examined)
 678                 return ERROR_OK;
 679         armv4_5_common_t *armv4_5 = target->arch_info;
 680         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 681         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 682         reg_t *dcc_control = &arm7_9->eice_cache->reg_list[EICE_COMMS_CTRL];
 683
 684         if (!target->dbg_msg_enabled)
 685                 return ERROR_OK;
 686
 687         if (target->state == TARGET_RUNNING)
 688         {
 689                 /* read DCC control register */
 690                 embeddedice_read_reg(dcc_control);
 691                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 692                 {
 693                         return retval;
 694                 }
 695
 696                 /* check W bit */
 697                 if (buf_get_u32(dcc_control->value, 1, 1) == 1)
 698                 {
 699                         u32 request;
 700
 701                         if ((retval = embeddedice_receive(jtag_info, &request, 1)) != ERROR_OK)
 702                         {
 703                                 return retval;
 704                         }
 705                         if ((retval = target_request(target, request)) != ERROR_OK)
 706                         {
 707                                 return retval;
 708                         }
 709                 }
 710         }
 711
 712         return ERROR_OK;
 713 }
 714
 715 int arm7_9_poll(target_t *target)
 716 {
 717         int retval;
 718         armv4_5_common_t *armv4_5 = target->arch_info;
 719         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 720         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 721
 722         /* read debug status register */
 723         embeddedice_read_reg(dbg_stat);
 724         if ((retval = jtag_execute_queue()) != ERROR_OK)
 725         {
 726                 return retval;
 727         }
 728
 729         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
 730         {
 731 /*              LOG_DEBUG("DBGACK set, dbg_state->value: 0x%x", buf_get_u32(dbg_stat->value, 0, 32));*/
 732                 if (target->state == TARGET_UNKNOWN)
 733                 {
 734                         target->state = TARGET_RUNNING;
 735                         LOG_WARNING("DBGACK set while target was in unknown state. Reset or initialize target.");
 736                 }
 737                 if ((target->state == TARGET_RUNNING) || (target->state == TARGET_RESET))
 738                 {
 739                         int check_pc=0;
 740                         if (target->state == TARGET_RESET)
 741                         {
 742                                 if (target->reset_halt)
 743                                 {
 744                                         if ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0)
 745                                         {
 746                                                 check_pc = 1;
 747                                         }
 748                                 }
 749                         }
 750
 751                         target->state = TARGET_HALTED;
 752
 753                         if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
 754                                 return retval;
 755
 756                         if (check_pc)
 757                         {
 758                                 reg_t *reg = register_get_by_name(target->reg_cache, "pc", 1);
 759                                 u32 t=*((u32 *)reg->value);
 760                                 if (t!=0)
 761                                 {
 762                                         LOG_ERROR("PC was not 0. Does this target need srst_pulls_trst?");
 763                                 }
 764                         }
 765
 766                         if ((retval = target_call_event_callbacks(target, TARGET_EVENT_HALTED)) != ERROR_OK)
 767                         {
 768                                 return retval;
 769                         }
 770                 }
 771                 if (target->state == TARGET_DEBUG_RUNNING)
 772                 {
 773                         target->state = TARGET_HALTED;
 774                         if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
 775                                 return retval;
 776
 777                         if ((retval = target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED)) != ERROR_OK)
 778                         {
 779                                 return retval;
 780                         }
 781                 }
 782                 if (target->state != TARGET_HALTED)
 783                 {
 784                         LOG_WARNING("DBGACK set, but the target did not end up in the halted stated %d", target->state);
 785                 }
 786         }
 787         else
 788         {
 789                 if (target->state != TARGET_DEBUG_RUNNING)
 790                         target->state = TARGET_RUNNING;
 791         }
 792
 793         return ERROR_OK;
 794 }
 795
 796 /*
 797   Some -S targets (ARM966E-S in the STR912 isn't affected, ARM926EJ-S
 798   in the LPC3180 and AT91SAM9260 is affected) completely stop the JTAG clock
 799   while the core is held in reset(SRST). It isn't possible to program the halt
 800   condition once reset was asserted, hence a hook that allows the target to set
 801   up its reset-halt condition prior to asserting reset.
 802 */
 803
 804 int arm7_9_assert_reset(target_t *target)
 805 {
 806         armv4_5_common_t *armv4_5 = target->arch_info;
 807         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 808         LOG_DEBUG("target->state: %s",
 809                   Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
 810
 811         if (!(jtag_reset_config & RESET_HAS_SRST))
 812         {
 813                 LOG_ERROR("Can't assert SRST");
 814                 return ERROR_FAIL;
 815         }
 816
 817         if (target->reset_halt)
 818         {
 819                 /*
 820                  * Some targets do not support communication while SRST is asserted. We need to
 821                  * set up the reset vector catch here.
 822                  *
 823                  * If TRST is asserted, then these settings will be reset anyway, so setting them
 824                  * here is harmless.
 825                  */
 826                 if (arm7_9->has_vector_catch)
 827                 {
 828                         /* program vector catch register to catch reset vector */
 829                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH], 0x1);
 830                 }
 831                 else
 832                 {
 833                         /* program watchpoint unit to match on reset vector address */
 834                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], 0x0);
 835                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0x3);
 836                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
 837                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 838                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 839                 }
 840         }
 841
 842         /* here we should issue a srst only, but we may have to assert trst as well */
 843         if (jtag_reset_config & RESET_SRST_PULLS_TRST)
 844         {
 845                 jtag_add_reset(1, 1);
 846         } else
 847         {
 848                 jtag_add_reset(0, 1);
 849         }
 850
 851         target->state = TARGET_RESET;
 852         jtag_add_sleep(50000);
 853
 854         armv4_5_invalidate_core_regs(target);
 855
 856         if ((target->reset_halt)&&((jtag_reset_config & RESET_SRST_PULLS_TRST)==0))
 857         {
 858                 /* debug entry was already prepared in arm7_9_assert_reset() */
 859                 target->debug_reason = DBG_REASON_DBGRQ;
 860         }
 861
 862         return ERROR_OK;
 863 }
 864
 865 int arm7_9_deassert_reset(target_t *target)
 866 {
 867         int retval=ERROR_OK;
 868         LOG_DEBUG("target->state: %s",
 869                 Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
 870
 871         /* deassert reset lines */
 872         jtag_add_reset(0, 0);
 873
 874         if (target->reset_halt&&(jtag_reset_config & RESET_SRST_PULLS_TRST)!=0)
 875         {
 876                 LOG_WARNING("srst pulls trst - can not reset into halted mode. Issuing halt after reset.");
 877                 /* set up embedded ice registers again */
 878                 if ((retval=target->type->examine(target))!=ERROR_OK)
 879                         return retval;
 880
 881                 if ((retval=target_poll(target))!=ERROR_OK)
 882                 {
 883                         return retval;
 884                 }
 885
 886                 if ((retval=target_halt(target))!=ERROR_OK)
 887                 {
 888                         return retval;
 889                 }
 890
 891         }
 892         return retval;
 893 }
 894
 895 int arm7_9_clear_halt(target_t *target)
 896 {
 897         armv4_5_common_t *armv4_5 = target->arch_info;
 898         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 899         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 900
 901         /* we used DBGRQ only if we didn't come out of reset */
 902         if (!arm7_9->debug_entry_from_reset && arm7_9->use_dbgrq)
 903         {
 904                 /* program EmbeddedICE Debug Control Register to deassert DBGRQ
 905                  */
 906                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
 907                 embeddedice_store_reg(dbg_ctrl);
 908         }
 909         else
 910         {
 911                 if (arm7_9->debug_entry_from_reset && arm7_9->has_vector_catch)
 912                 {
 913                         /* if we came out of reset, and vector catch is supported, we used
 914                          * vector catch to enter debug state
 915                          * restore the register in that case
 916                          */
 917                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH]);
 918                 }
 919                 else
 920                 {
 921                         /* restore registers if watchpoint unit 0 was in use
 922                          */
 923                         if (arm7_9->wp0_used)
 924                         {
 925                                 if (arm7_9->debug_entry_from_reset)
 926                                 {
 927                                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE]);
 928                                 }
 929                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
 930                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
 931                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
 932                         }
 933                         /* control value always has to be restored, as it was either disabled,
 934                          * or enabled with possibly different bits
 935                          */
 936                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
 937                 }
 938         }
 939
 940         return ERROR_OK;
 941 }
 942
 943 int arm7_9_soft_reset_halt(struct target_s *target)
 944 {
 945         armv4_5_common_t *armv4_5 = target->arch_info;
 946         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 947         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 948         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 949         int i;
 950         int retval;
 951
 952         if ((retval=target_halt(target))!=ERROR_OK)
 953                 return retval;
 954
 955         long long then=timeval_ms();
 956         int timeout;
 957         while (!(timeout=((timeval_ms()-then)>1000)))
 958         {
 959                 if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) != 0)
 960                         break;
 961                 embeddedice_read_reg(dbg_stat);
 962                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 963                         return retval;
 964                 if (debug_level>=3)
 965                 {
 966                         alive_sleep(100);
 967                 } else
 968                 {
 969                         keep_alive();
 970                 }
 971         }
 972         if (timeout)
 973         {
 974                 LOG_ERROR("Failed to halt CPU after 1 sec");
 975                 return ERROR_TARGET_TIMEOUT;
 976         }
 977         target->state = TARGET_HALTED;
 978
 979         /* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
 980          * ensure that DBGRQ is cleared
 981          */
 982         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
 983         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
 984         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
 985         embeddedice_store_reg(dbg_ctrl);
 986
 987         if ((retval = arm7_9_clear_halt(target)) != ERROR_OK)
 988         {
 989                 return retval;
 990         }
 991
 992         /* if the target is in Thumb state, change to ARM state */
 993         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
 994         {
 995                 u32 r0_thumb, pc_thumb;
 996                 LOG_DEBUG("target entered debug from Thumb state, changing to ARM");
 997                 /* Entered debug from Thumb mode */
 998                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
 999                 arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
1000         }
1001
1002         /* all register content is now invalid */
1003         if ((retval = armv4_5_invalidate_core_regs(target)) != ERROR_OK)
1004         {
1005                 return retval;
1006         }
1007
1008         /* SVC, ARM state, IRQ and FIQ disabled */
1009         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
1010         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
1011         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1012
1013         /* start fetching from 0x0 */
1014         buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
1015         armv4_5->core_cache->reg_list[15].dirty = 1;
1016         armv4_5->core_cache->reg_list[15].valid = 1;
1017
1018         armv4_5->core_mode = ARMV4_5_MODE_SVC;
1019         armv4_5->core_state = ARMV4_5_STATE_ARM;
1020
1021         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1022                 return ERROR_FAIL;
1023
1024         /* reset registers */
1025         for (i = 0; i <= 14; i++)
1026         {
1027                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, 0xffffffff);
1028                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
1029                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
1030         }
1031
1032         if ((retval = target_call_event_callbacks(target, TARGET_EVENT_HALTED)) != ERROR_OK)
1033         {
1034                 return retval;
1035         }
1036
1037         return ERROR_OK;
1038 }
1039
1040 int arm7_9_halt(target_t *target)
1041 {
1042         if (target->state==TARGET_RESET)
1043         {
1044                 LOG_ERROR("BUG: arm7/9 does not support halt during reset. This is handled in arm7_9_assert_reset()");
1045                 return ERROR_OK;
1046         }
1047
1048         armv4_5_common_t *armv4_5 = target->arch_info;
1049         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1050         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1051
1052         LOG_DEBUG("target->state: %s",
1053                   Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
1054
1055         if (target->state == TARGET_HALTED)
1056         {
1057                 LOG_DEBUG("target was already halted");
1058                 return ERROR_OK;
1059         }
1060
1061         if (target->state == TARGET_UNKNOWN)
1062         {
1063                 LOG_WARNING("target was in unknown state when halt was requested");
1064         }
1065
1066         if (arm7_9->use_dbgrq)
1067         {
1068                 /* program EmbeddedICE Debug Control Register to assert DBGRQ
1069                  */
1070                 if (arm7_9->set_special_dbgrq) {
1071                         arm7_9->set_special_dbgrq(target);
1072                 } else {
1073                         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 1);
1074                         embeddedice_store_reg(dbg_ctrl);
1075                 }
1076         }
1077         else
1078         {
1079                 /* program watchpoint unit to match on any address
1080                  */
1081                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1082                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1083                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
1084                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1085         }
1086
1087         target->debug_reason = DBG_REASON_DBGRQ;
1088
1089         return ERROR_OK;
1090 }
1091
1092 int arm7_9_debug_entry(target_t *target)
1093 {
1094         int i;
1095         u32 context[16];
1096         u32* context_p[16];
1097         u32 r0_thumb, pc_thumb;
1098         u32 cpsr;
1099         int retval;
1100         /* get pointers to arch-specific information */
1101         armv4_5_common_t *armv4_5 = target->arch_info;
1102         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1103         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
1104         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1105
1106 #ifdef _DEBUG_ARM7_9_
1107         LOG_DEBUG("-");
1108 #endif
1109
1110         if (arm7_9->pre_debug_entry)
1111                 arm7_9->pre_debug_entry(target);
1112
1113         /* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
1114          * ensure that DBGRQ is cleared
1115          */
1116         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
1117         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
1118         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
1119         embeddedice_store_reg(dbg_ctrl);
1120
1121         if ((retval = arm7_9_clear_halt(target)) != ERROR_OK)
1122         {
1123                 return retval;
1124         }
1125
1126         if ((retval = jtag_execute_queue()) != ERROR_OK)
1127         {
1128                 return retval;
1129         }
1130
1131         if ((retval = arm7_9->examine_debug_reason(target)) != ERROR_OK)
1132                 return retval;
1133
1134
1135         if (target->state != TARGET_HALTED)
1136         {
1137                 LOG_WARNING("target not halted");
1138                 return ERROR_TARGET_NOT_HALTED;
1139         }
1140
1141         /* if the target is in Thumb state, change to ARM state */
1142         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
1143         {
1144                 LOG_DEBUG("target entered debug from Thumb state");
1145                 /* Entered debug from Thumb mode */
1146                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
1147                 arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
1148                 LOG_DEBUG("r0_thumb: 0x%8.8x, pc_thumb: 0x%8.8x", r0_thumb, pc_thumb);
1149         }
1150         else
1151         {
1152                 LOG_DEBUG("target entered debug from ARM state");
1153                 /* Entered debug from ARM mode */
1154                 armv4_5->core_state = ARMV4_5_STATE_ARM;
1155         }
1156
1157         for (i = 0; i < 16; i++)
1158                 context_p[i] = &context[i];
1159         /* save core registers (r0 - r15 of current core mode) */
1160         arm7_9->read_core_regs(target, 0xffff, context_p);
1161
1162         arm7_9->read_xpsr(target, &cpsr, 0);
1163
1164         if ((retval = jtag_execute_queue()) != ERROR_OK)
1165                 return retval;
1166
1167         /* if the core has been executing in Thumb state, set the T bit */
1168         if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1169                 cpsr |= 0x20;
1170
1171         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, cpsr);
1172         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
1173         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1174
1175         armv4_5->core_mode = cpsr & 0x1f;
1176
1177         if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
1178         {
1179                 target->state = TARGET_UNKNOWN;
1180                 LOG_ERROR("cpsr contains invalid mode value - communication failure");
1181                 return ERROR_TARGET_FAILURE;
1182         }
1183
1184         LOG_DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);
1185
1186         if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1187         {
1188                 LOG_DEBUG("thumb state, applying fixups");
1189                 context[0] = r0_thumb;
1190                 context[15] = pc_thumb;
1191         } else if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1192         {
1193                 /* adjust value stored by STM */
1194                 context[15] -= 3 * 4;
1195         }
1196
1197         if ((target->debug_reason == DBG_REASON_BREAKPOINT)
1198                         || (target->debug_reason == DBG_REASON_SINGLESTEP)
1199                         || (target->debug_reason == DBG_REASON_WATCHPOINT)
1200                         || (target->debug_reason == DBG_REASON_WPTANDBKPT)
1201                         || ((target->debug_reason == DBG_REASON_DBGRQ) && (arm7_9->use_dbgrq == 0)))
1202                 context[15] -= 3 * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
1203         else if (target->debug_reason == DBG_REASON_DBGRQ)
1204                 context[15] -= arm7_9->dbgreq_adjust_pc * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
1205         else
1206         {
1207                 LOG_ERROR("unknown debug reason: %i", target->debug_reason);
1208         }
1209
1210         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1211                 return ERROR_FAIL;
1212
1213         for (i=0; i<=15; i++)
1214         {
1215                 LOG_DEBUG("r%i: 0x%8.8x", i, context[i]);
1216                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, context[i]);
1217                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;
1218                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
1219         }
1220
1221         LOG_DEBUG("entered debug state at PC 0x%x", context[15]);
1222
1223         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1224                 return ERROR_FAIL;
1225
1226         /* exceptions other than USR & SYS have a saved program status register */
1227         if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))
1228         {
1229                 u32 spsr;
1230                 arm7_9->read_xpsr(target, &spsr, 1);
1231                 if ((retval = jtag_execute_queue()) != ERROR_OK)
1232                 {
1233                         return retval;
1234                 }
1235                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, spsr);
1236                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;
1237                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;
1238         }
1239
1240         /* r0 and r15 (pc) have to be restored later */
1241         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).valid;
1242         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).valid;
1243
1244         if ((retval = jtag_execute_queue()) != ERROR_OK)
1245                 return retval;
1246
1247         if (arm7_9->post_debug_entry)
1248                 arm7_9->post_debug_entry(target);
1249
1250         return ERROR_OK;
1251 }
1252
1253 int arm7_9_full_context(target_t *target)
1254 {
1255         int i;
1256         int retval;
1257         armv4_5_common_t *armv4_5 = target->arch_info;
1258         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1259
1260         LOG_DEBUG("-");
1261
1262         if (target->state != TARGET_HALTED)
1263         {
1264                 LOG_WARNING("target not halted");
1265                 return ERROR_TARGET_NOT_HALTED;
1266         }
1267
1268         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1269                 return ERROR_FAIL;
1270
1271         /* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
1272          * SYS shares registers with User, so we don't touch SYS
1273          */
1274         for (i = 0; i < 6; i++)
1275         {
1276                 u32 mask = 0;
1277                 u32* reg_p[16];
1278                 int j;
1279                 int valid = 1;
1280
1281                 /* check if there are invalid registers in the current mode
1282                  */
1283                 for (j = 0; j <= 16; j++)
1284                 {
1285                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1286                                 valid = 0;
1287                 }
1288
1289                 if (!valid)
1290                 {
1291                         u32 tmp_cpsr;
1292
1293                         /* change processor mode (and mask T bit) */
1294                         tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1295                         tmp_cpsr |= armv4_5_number_to_mode(i);
1296                         tmp_cpsr &= ~0x20;
1297                         arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1298
1299                         for (j = 0; j < 15; j++)
1300                         {
1301                                 if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1302                                 {
1303                                         reg_p[j] = (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value;
1304                                         mask |= 1 << j;
1305                                         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;
1306                                         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
1307                                 }
1308                         }
1309
1310                         /* if only the PSR is invalid, mask is all zeroes */
1311                         if (mask)
1312                                 arm7_9->read_core_regs(target, mask, reg_p);
1313
1314                         /* check if the PSR has to be read */
1315                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid == 0)
1316                         {
1317                                 arm7_9->read_xpsr(target, (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).value, 1);
1318                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;
1319                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
1320                         }
1321                 }
1322         }
1323
1324         /* restore processor mode (mask T bit) */
1325         arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1326
1327         if ((retval = jtag_execute_queue()) != ERROR_OK)
1328         {
1329                 return retval;
1330         }
1331         return ERROR_OK;
1332 }
1333
1334 int arm7_9_restore_context(target_t *target)
1335 {
1336         armv4_5_common_t *armv4_5 = target->arch_info;
1337         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1338         reg_t *reg;
1339         armv4_5_core_reg_t *reg_arch_info;
1340         enum armv4_5_mode current_mode = armv4_5->core_mode;
1341         int i, j;
1342         int dirty;
1343         int mode_change;
1344
1345         LOG_DEBUG("-");
1346
1347         if (target->state != TARGET_HALTED)
1348         {
1349                 LOG_WARNING("target not halted");
1350                 return ERROR_TARGET_NOT_HALTED;
1351         }
1352
1353         if (arm7_9->pre_restore_context)
1354                 arm7_9->pre_restore_context(target);
1355
1356         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1357                 return ERROR_FAIL;
1358
1359         /* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
1360          * SYS shares registers with User, so we don't touch SYS
1361          */
1362         for (i = 0; i < 6; i++)
1363         {
1364                 LOG_DEBUG("examining %s mode", armv4_5_mode_strings[i]);
1365                 dirty = 0;
1366                 mode_change = 0;
1367                 /* check if there are dirty registers in the current mode
1368                 */
1369                 for (j = 0; j <= 16; j++)
1370                 {
1371                         reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
1372                         reg_arch_info = reg->arch_info;
1373                         if (reg->dirty == 1)
1374                         {
1375                                 if (reg->valid == 1)
1376                                 {
1377                                         dirty = 1;
1378                                         LOG_DEBUG("examining dirty reg: %s", reg->name);
1379                                         if ((reg_arch_info->mode != ARMV4_5_MODE_ANY)
1380                                                 && (reg_arch_info->mode != current_mode)
1381                                                 && !((reg_arch_info->mode == ARMV4_5_MODE_USR) && (armv4_5->core_mode == ARMV4_5_MODE_SYS))
1382                                                 && !((reg_arch_info->mode == ARMV4_5_MODE_SYS) && (armv4_5->core_mode == ARMV4_5_MODE_USR)))
1383                                         {
1384                                                 mode_change = 1;
1385                                                 LOG_DEBUG("require mode change");
1386                                         }
1387                                 }
1388                                 else
1389                                 {
1390                                         LOG_ERROR("BUG: dirty register '%s', but no valid data", reg->name);
1391                                 }
1392                         }
1393                 }
1394
1395                 if (dirty)
1396                 {
1397                         u32 mask = 0x0;
1398                         int num_regs = 0;
1399                         u32 regs[16];
1400
1401                         if (mode_change)
1402                         {
1403                                 u32 tmp_cpsr;
1404
1405                                 /* change processor mode (mask T bit) */
1406                                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1407                                 tmp_cpsr |= armv4_5_number_to_mode(i);
1408                                 tmp_cpsr &= ~0x20;
1409                                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1410                                 current_mode = armv4_5_number_to_mode(i);
1411                         }
1412
1413                         for (j = 0; j <= 14; j++)
1414                         {
1415                                 reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
1416                                 reg_arch_info = reg->arch_info;
1417
1418
1419                                 if (reg->dirty == 1)
1420                                 {
1421                                         regs[j] = buf_get_u32(reg->value, 0, 32);
1422                                         mask |= 1 << j;
1423                                         num_regs++;
1424                                         reg->dirty = 0;
1425                                         reg->valid = 1;
1426                                         LOG_DEBUG("writing register %i of mode %s with value 0x%8.8x", j, armv4_5_mode_strings[i], regs[j]);
1427                                 }
1428                         }
1429
1430                         if (mask)
1431                         {
1432                                 arm7_9->write_core_regs(target, mask, regs);
1433                         }
1434
1435                         reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16);
1436                         reg_arch_info = reg->arch_info;
1437                         if ((reg->dirty) && (reg_arch_info->mode != ARMV4_5_MODE_ANY))
1438                         {
1439                                 LOG_DEBUG("writing SPSR of mode %i with value 0x%8.8x", i, buf_get_u32(reg->value, 0, 32));
1440                                 arm7_9->write_xpsr(target, buf_get_u32(reg->value, 0, 32), 1);
1441                         }
1442                 }
1443         }
1444
1445         if ((armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 0) && (armv4_5->core_mode != current_mode))
1446         {
1447                 /* restore processor mode (mask T bit) */
1448                 u32 tmp_cpsr;
1449
1450                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1451                 tmp_cpsr |= armv4_5_number_to_mode(i);
1452                 tmp_cpsr &= ~0x20;
1453                 LOG_DEBUG("writing lower 8 bit of cpsr with value 0x%2.2x", tmp_cpsr);
1454                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1455         }
1456         else if (armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 1)
1457         {
1458                 /* CPSR has been changed, full restore necessary (mask T bit) */
1459                 LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1460                 arm7_9->write_xpsr(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32) & ~0x20, 0);
1461                 armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
1462                 armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1463         }
1464
1465         /* restore PC */
1466         LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1467         arm7_9->write_pc(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1468         armv4_5->core_cache->reg_list[15].dirty = 0;
1469
1470         if (arm7_9->post_restore_context)
1471                 arm7_9->post_restore_context(target);
1472
1473         return ERROR_OK;
1474 }
1475
1476 int arm7_9_restart_core(struct target_s *target)
1477 {
1478         armv4_5_common_t *armv4_5 = target->arch_info;
1479         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1480         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
1481
1482         /* set RESTART instruction */
1483         jtag_add_end_state(TAP_IDLE);
1484         if (arm7_9->need_bypass_before_restart) {
1485                 arm7_9->need_bypass_before_restart = 0;
1486                 arm_jtag_set_instr(jtag_info, 0xf, NULL);
1487         }
1488         arm_jtag_set_instr(jtag_info, 0x4, NULL);
1489
1490         jtag_add_runtest(1, TAP_IDLE);
1491         return jtag_execute_queue();
1492 }
1493
1494 void arm7_9_enable_watchpoints(struct target_s *target)
1495 {
1496         watchpoint_t *watchpoint = target->watchpoints;
1497
1498         while (watchpoint)
1499         {
1500                 if (watchpoint->set == 0)
1501                         arm7_9_set_watchpoint(target, watchpoint);
1502                 watchpoint = watchpoint->next;
1503         }
1504 }
1505
1506 void arm7_9_enable_breakpoints(struct target_s *target)
1507 {
1508         breakpoint_t *breakpoint = target->breakpoints;
1509
1510         /* set any pending breakpoints */
1511         while (breakpoint)
1512         {
1513                 arm7_9_set_breakpoint(target, breakpoint);
1514                 breakpoint = breakpoint->next;
1515         }
1516 }
1517
1518 int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
1519 {
1520         armv4_5_common_t *armv4_5 = target->arch_info;
1521         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1522         breakpoint_t *breakpoint = target->breakpoints;
1523         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1524         int err, retval = ERROR_OK;
1525
1526         LOG_DEBUG("-");
1527
1528         if (target->state != TARGET_HALTED)
1529         {
1530                 LOG_WARNING("target not halted");
1531                 return ERROR_TARGET_NOT_HALTED;
1532         }
1533
1534         if (!debug_execution)
1535         {
1536                 target_free_all_working_areas(target);
1537         }
1538
1539         /* current = 1: continue on current pc, otherwise continue at <address> */
1540         if (!current)
1541                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1542
1543         u32 current_pc;
1544         current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1545
1546         /* the front-end may request us not to handle breakpoints */
1547         if (handle_breakpoints)
1548         {
1549                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1550                 {
1551                         LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
1552                         if ((retval = arm7_9_unset_breakpoint(target, breakpoint)) != ERROR_OK)
1553                         {
1554                                 return retval;
1555                         }
1556
1557                         /* calculate PC of next instruction */
1558                         u32 next_pc;
1559                         if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)
1560                         {
1561                                 u32 current_opcode;
1562                                 target_read_u32(target, current_pc, &current_opcode);
1563                                 LOG_ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);
1564                                 return retval;
1565                         }
1566
1567                         LOG_DEBUG("enable single-step");
1568                         arm7_9->enable_single_step(target, next_pc);
1569
1570                         target->debug_reason = DBG_REASON_SINGLESTEP;
1571
1572                         if ((retval = arm7_9_restore_context(target)) != ERROR_OK)
1573                         {
1574                                 return retval;
1575                         }
1576
1577                         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1578                                 arm7_9->branch_resume(target);
1579                         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1580                         {
1581                                 arm7_9->branch_resume_thumb(target);
1582                         }
1583                         else
1584                         {
1585                                 LOG_ERROR("unhandled core state");
1586                                 return ERROR_FAIL;
1587                         }
1588
1589                         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1590                         embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1591                         err = arm7_9_execute_sys_speed(target);
1592
1593                         LOG_DEBUG("disable single-step");
1594                         arm7_9->disable_single_step(target);
1595
1596                         if (err != ERROR_OK)
1597                         {
1598                                 if ((retval = arm7_9_set_breakpoint(target, breakpoint)) != ERROR_OK)
1599                                 {
1600                                         return retval;
1601                                 }
1602                                 target->state = TARGET_UNKNOWN;
1603                                 return err;
1604                         }
1605
1606                         arm7_9_debug_entry(target);
1607                         LOG_DEBUG("new PC after step: 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1608
1609                         LOG_DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
1610                         if ((retval = arm7_9_set_breakpoint(target, breakpoint)) != ERROR_OK)
1611                         {
1612                                 return retval;
1613                         }
1614                 }
1615         }
1616
1617         /* enable any pending breakpoints and watchpoints */
1618         arm7_9_enable_breakpoints(target);
1619         arm7_9_enable_watchpoints(target);
1620
1621         if ((retval = arm7_9_restore_context(target)) != ERROR_OK)
1622         {
1623                 return retval;
1624         }
1625
1626         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1627         {
1628                 arm7_9->branch_resume(target);
1629         }
1630         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1631         {
1632                 arm7_9->branch_resume_thumb(target);
1633         }
1634         else
1635         {
1636                 LOG_ERROR("unhandled core state");
1637                 return ERROR_FAIL;
1638         }
1639
1640         /* deassert DBGACK and INTDIS */
1641         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1642         /* INTDIS only when we really resume, not during debug execution */
1643         if (!debug_execution)
1644                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 0);
1645         embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1646
1647         if ((retval = arm7_9_restart_core(target)) != ERROR_OK)
1648         {
1649                 return retval;
1650         }
1651
1652         target->debug_reason = DBG_REASON_NOTHALTED;
1653
1654         if (!debug_execution)
1655         {
1656                 /* registers are now invalid */
1657                 armv4_5_invalidate_core_regs(target);
1658                 target->state = TARGET_RUNNING;
1659                 if ((retval = target_call_event_callbacks(target, TARGET_EVENT_RESUMED)) != ERROR_OK)
1660                 {
1661                         return retval;
1662                 }
1663         }
1664         else
1665         {
1666                 target->state = TARGET_DEBUG_RUNNING;
1667                 if ((retval = target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED)) != ERROR_OK)
1668                 {
1669                         return retval;
1670                 }
1671         }
1672
1673         LOG_DEBUG("target resumed");
1674
1675         return ERROR_OK;
1676 }
1677
1678 void arm7_9_enable_eice_step(target_t *target, u32 next_pc)
1679 {
1680         armv4_5_common_t *armv4_5 = target->arch_info;
1681         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1682
1683         u32 current_pc;
1684         current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1685
1686         if(next_pc != current_pc)
1687         {
1688                 /* setup an inverse breakpoint on the current PC
1689                 * - comparator 1 matches the current address
1690                 * - rangeout from comparator 1 is connected to comparator 0 rangein
1691                 * - comparator 0 matches any address, as long as rangein is low */
1692                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1693                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1694                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
1695                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~(EICE_W_CTRL_RANGE|EICE_W_CTRL_nOPC) & 0xff);
1696                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], current_pc);
1697                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0);
1698                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffff);
1699                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
1700                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1701         }
1702         else
1703         {
1704                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1705                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1706                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
1707                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xff);
1708                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], next_pc);
1709                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0);
1710                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffff);
1711                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
1712                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1713         }
1714 }
1715
1716 void arm7_9_disable_eice_step(target_t *target)
1717 {
1718         armv4_5_common_t *armv4_5 = target->arch_info;
1719         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1720
1721         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
1722         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
1723         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
1724         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
1725         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE]);
1726         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK]);
1727         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK]);
1728         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK]);
1729         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE]);
1730 }
1731
1732 int arm7_9_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
1733 {
1734         armv4_5_common_t *armv4_5 = target->arch_info;
1735         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1736         breakpoint_t *breakpoint = NULL;
1737         int err, retval;
1738
1739         if (target->state != TARGET_HALTED)
1740         {
1741                 LOG_WARNING("target not halted");
1742                 return ERROR_TARGET_NOT_HALTED;
1743         }
1744
1745         /* current = 1: continue on current pc, otherwise continue at <address> */
1746         if (!current)
1747                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1748
1749         u32 current_pc;
1750         current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1751
1752         /* the front-end may request us not to handle breakpoints */
1753         if (handle_breakpoints)
1754                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1755                         if ((retval = arm7_9_unset_breakpoint(target, breakpoint)) != ERROR_OK)
1756                         {
1757                                 return retval;
1758                         }
1759
1760         target->debug_reason = DBG_REASON_SINGLESTEP;
1761
1762         /* calculate PC of next instruction */
1763         u32 next_pc;
1764         if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)
1765         {
1766                 u32 current_opcode;
1767                 target_read_u32(target, current_pc, &current_opcode);
1768                 LOG_ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);
1769                 return retval;
1770         }
1771
1772         if ((retval = arm7_9_restore_context(target)) != ERROR_OK)
1773         {
1774                 return retval;
1775         }
1776
1777         arm7_9->enable_single_step(target, next_pc);
1778
1779         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1780         {
1781                 arm7_9->branch_resume(target);
1782         }
1783         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1784         {
1785                 arm7_9->branch_resume_thumb(target);
1786         }
1787         else
1788         {
1789                 LOG_ERROR("unhandled core state");
1790                 return ERROR_FAIL;
1791         }
1792
1793         if ((retval = target_call_event_callbacks(target, TARGET_EVENT_RESUMED)) != ERROR_OK)
1794         {
1795                 return retval;
1796         }
1797
1798         err = arm7_9_execute_sys_speed(target);
1799         arm7_9->disable_single_step(target);
1800
1801         /* registers are now invalid */
1802         armv4_5_invalidate_core_regs(target);
1803
1804         if (err != ERROR_OK)
1805         {
1806                 target->state = TARGET_UNKNOWN;
1807         } else {
1808                 arm7_9_debug_entry(target);
1809                 if ((retval = target_call_event_callbacks(target, TARGET_EVENT_HALTED)) != ERROR_OK)
1810                 {
1811                         return retval;
1812                 }
1813                 LOG_DEBUG("target stepped");
1814         }
1815
1816         if (breakpoint)
1817                 if ((retval = arm7_9_set_breakpoint(target, breakpoint)) != ERROR_OK)
1818                 {
1819                         return retval;
1820                 }
1821
1822         return err;
1823 }
1824
1825 int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
1826 {
1827         u32* reg_p[16];
1828         u32 value;
1829         int retval;
1830         armv4_5_common_t *armv4_5 = target->arch_info;
1831         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1832
1833         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1834                 return ERROR_FAIL;
1835
1836         enum armv4_5_mode reg_mode = ((armv4_5_core_reg_t*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info)->mode;
1837
1838         if ((num < 0) || (num > 16))
1839                 return ERROR_INVALID_ARGUMENTS;
1840
1841         if ((mode != ARMV4_5_MODE_ANY)
1842                         && (mode != armv4_5->core_mode)
1843                         && (reg_mode != ARMV4_5_MODE_ANY))
1844         {
1845                 u32 tmp_cpsr;
1846
1847                 /* change processor mode (mask T bit) */
1848                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1849                 tmp_cpsr |= mode;
1850                 tmp_cpsr &= ~0x20;
1851                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1852         }
1853
1854         if ((num >= 0) && (num <= 15))
1855         {
1856                 /* read a normal core register */
1857                 reg_p[num] = &value;
1858
1859                 arm7_9->read_core_regs(target, 1 << num, reg_p);
1860         }
1861         else
1862         {
1863                 /* read a program status register
1864                  * if the register mode is MODE_ANY, we read the cpsr, otherwise a spsr
1865                  */
1866                 armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
1867                 int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1868
1869                 arm7_9->read_xpsr(target, &value, spsr);
1870         }
1871
1872         if ((retval = jtag_execute_queue()) != ERROR_OK)
1873         {
1874                 return retval;
1875         }
1876
1877         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
1878         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1879         buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value, 0, 32, value);
1880
1881         if ((mode != ARMV4_5_MODE_ANY)
1882                         && (mode != armv4_5->core_mode)
1883                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1884                 /* restore processor mode (mask T bit) */
1885                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1886         }
1887
1888         return ERROR_OK;
1889 }
1890
1891 int arm7_9_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)
1892 {
1893         u32 reg[16];
1894         armv4_5_common_t *armv4_5 = target->arch_info;
1895         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1896
1897         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1898                 return ERROR_FAIL;
1899
1900         enum armv4_5_mode reg_mode = ((armv4_5_core_reg_t*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info)->mode;
1901
1902         if ((num < 0) || (num > 16))
1903                 return ERROR_INVALID_ARGUMENTS;
1904
1905         if ((mode != ARMV4_5_MODE_ANY)
1906                         && (mode != armv4_5->core_mode)
1907                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1908                 u32 tmp_cpsr;
1909
1910                 /* change processor mode (mask T bit) */
1911                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1912                 tmp_cpsr |= mode;
1913                 tmp_cpsr &= ~0x20;
1914                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1915         }
1916
1917         if ((num >= 0) && (num <= 15))
1918         {
1919                 /* write a normal core register */
1920                 reg[num] = value;
1921
1922                 arm7_9->write_core_regs(target, 1 << num, reg);
1923         }
1924         else
1925         {
1926                 /* write a program status register
1927                 * if the register mode is MODE_ANY, we write the cpsr, otherwise a spsr
1928                 */
1929                 armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
1930                 int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1931
1932                 /* if we're writing the CPSR, mask the T bit */
1933                 if (!spsr)
1934                         value &= ~0x20;
1935
1936                 arm7_9->write_xpsr(target, value, spsr);
1937         }
1938
1939         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
1940         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1941
1942         if ((mode != ARMV4_5_MODE_ANY)
1943                         && (mode != armv4_5->core_mode)
1944                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1945                 /* restore processor mode (mask T bit) */
1946                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1947         }
1948
1949         return jtag_execute_queue();
1950 }
1951
1952 int arm7_9_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1953 {
1954         armv4_5_common_t *armv4_5 = target->arch_info;
1955         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1956
1957         u32 reg[16];
1958         u32 num_accesses = 0;
1959         int thisrun_accesses;
1960         int i;
1961         u32 cpsr;
1962         int retval;
1963         int last_reg = 0;
1964
1965         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1966
1967         if (target->state != TARGET_HALTED)
1968         {
1969                 LOG_WARNING("target not halted");
1970                 return ERROR_TARGET_NOT_HALTED;
1971         }
1972
1973         /* sanitize arguments */
1974         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1975                 return ERROR_INVALID_ARGUMENTS;
1976
1977         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1978                 return ERROR_TARGET_UNALIGNED_ACCESS;
1979
1980         /* load the base register with the address of the first word */
1981         reg[0] = address;
1982         arm7_9->write_core_regs(target, 0x1, reg);
1983
1984         int j=0;
1985
1986         switch (size)
1987         {
1988                 case 4:
1989                         while (num_accesses < count)
1990                         {
1991                                 u32 reg_list;
1992                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1993                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1994
1995                                 if (last_reg <= thisrun_accesses)
1996                                         last_reg = thisrun_accesses;
1997
1998                                 arm7_9->load_word_regs(target, reg_list);
1999
2000                                 /* fast memory reads are only safe when the target is running
2001                                  * from a sufficiently high clock (32 kHz is usually too slow)
2002                                  */
2003                                 if (arm7_9->fast_memory_access)
2004                                         retval = arm7_9_execute_fast_sys_speed(target);
2005                                 else
2006                                         retval = arm7_9_execute_sys_speed(target);
2007                                 if (retval != ERROR_OK)
2008                                         return retval;
2009
2010                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 4);
2011
2012                                 /* advance buffer, count number of accesses */
2013                                 buffer += thisrun_accesses * 4;
2014                                 num_accesses += thisrun_accesses;
2015
2016                                 if ((j++%1024)==0)
2017                                 {
2018                                         keep_alive();
2019                                 }
2020                         }
2021                         break;
2022                 case 2:
2023                         while (num_accesses < count)
2024                         {
2025                                 u32 reg_list;
2026                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2027                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2028
2029                                 for (i = 1; i <= thisrun_accesses; i++)
2030                                 {
2031                                         if (i > last_reg)
2032                                                 last_reg = i;
2033                                         arm7_9->load_hword_reg(target, i);
2034                                         /* fast memory reads are only safe when the target is running
2035                                          * from a sufficiently high clock (32 kHz is usually too slow)
2036                                          */
2037                                         if (arm7_9->fast_memory_access)
2038                                                 retval = arm7_9_execute_fast_sys_speed(target);
2039                                         else
2040                                                 retval = arm7_9_execute_sys_speed(target);
2041                                         if(retval != ERROR_OK)
2042                                         {
2043                                                 return retval;
2044                                         }
2045
2046                                 }
2047
2048                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 2);
2049
2050                                 /* advance buffer, count number of accesses */
2051                                 buffer += thisrun_accesses * 2;
2052                                 num_accesses += thisrun_accesses;
2053
2054                                 if ((j++%1024)==0)
2055                                 {
2056                                         keep_alive();
2057                                 }
2058                         }
2059                         break;
2060                 case 1:
2061                         while (num_accesses < count)
2062                         {
2063                                 u32 reg_list;
2064                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2065                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2066
2067                                 for (i = 1; i <= thisrun_accesses; i++)
2068                                 {
2069                                         if (i > last_reg)
2070                                                 last_reg = i;
2071                                         arm7_9->load_byte_reg(target, i);
2072                                         /* fast memory reads are only safe when the target is running
2073                                          * from a sufficiently high clock (32 kHz is usually too slow)
2074                                          */
2075                                         if (arm7_9->fast_memory_access)
2076                                                 retval = arm7_9_execute_fast_sys_speed(target);
2077                                         else
2078                                                 retval = arm7_9_execute_sys_speed(target);
2079                                         if(retval != ERROR_OK)
2080                                         {
2081                                                 return retval;
2082                                         }
2083                                 }
2084
2085                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 1);
2086
2087                                 /* advance buffer, count number of accesses */
2088                                 buffer += thisrun_accesses * 1;
2089                                 num_accesses += thisrun_accesses;
2090
2091                                 if ((j++%1024)==0)
2092                                 {
2093                                         keep_alive();
2094                                 }
2095                         }
2096                         break;
2097                 default:
2098                         LOG_ERROR("BUG: we shouldn't get here");
2099                         exit(-1);
2100                         break;
2101         }
2102
2103         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
2104                 return ERROR_FAIL;
2105
2106         for (i=0; i<=last_reg; i++)
2107                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid;
2108
2109         arm7_9->read_xpsr(target, &cpsr, 0);
2110         if ((retval = jtag_execute_queue()) != ERROR_OK)
2111         {
2112                 LOG_ERROR("JTAG error while reading cpsr");
2113                 return ERROR_TARGET_DATA_ABORT;
2114         }
2115
2116         if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
2117         {
2118                 LOG_WARNING("memory read caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
2119
2120                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
2121
2122                 return ERROR_TARGET_DATA_ABORT;
2123         }
2124
2125         return ERROR_OK;
2126 }
2127
2128 int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
2129 {
2130         armv4_5_common_t *armv4_5 = target->arch_info;
2131         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
2132         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
2133
2134         u32 reg[16];
2135         u32 num_accesses = 0;
2136         int thisrun_accesses;
2137         int i;
2138         u32 cpsr;
2139         int retval;
2140         int last_reg = 0;
2141
2142 #ifdef _DEBUG_ARM7_9_
2143         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
2144 #endif
2145
2146         if (target->state != TARGET_HALTED)
2147         {
2148                 LOG_WARNING("target not halted");
2149                 return ERROR_TARGET_NOT_HALTED;
2150         }
2151
2152         /* sanitize arguments */
2153         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
2154                 return ERROR_INVALID_ARGUMENTS;
2155
2156         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
2157                 return ERROR_TARGET_UNALIGNED_ACCESS;
2158
2159         /* load the base register with the address of the first word */
2160         reg[0] = address;
2161         arm7_9->write_core_regs(target, 0x1, reg);
2162
2163         /* Clear DBGACK, to make sure memory fetches work as expected */
2164         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
2165         embeddedice_store_reg(dbg_ctrl);
2166
2167         switch (size)
2168         {
2169                 case 4:
2170                         while (num_accesses < count)
2171                         {
2172                                 u32 reg_list;
2173                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2174                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2175
2176                                 for (i = 1; i <= thisrun_accesses; i++)
2177                                 {
2178                                         if (i > last_reg)
2179                                                 last_reg = i;
2180                                         reg[i] = target_buffer_get_u32(target, buffer);
2181                                         buffer += 4;
2182                                 }
2183
2184                                 arm7_9->write_core_regs(target, reg_list, reg);
2185
2186                                 arm7_9->store_word_regs(target, reg_list);
2187
2188                                 /* fast memory writes are only safe when the target is running
2189                                  * from a sufficiently high clock (32 kHz is usually too slow)
2190                                  */
2191                                 if (arm7_9->fast_memory_access)
2192                                         retval = arm7_9_execute_fast_sys_speed(target);
2193                                 else
2194                                         retval = arm7_9_execute_sys_speed(target);
2195                                 if(retval != ERROR_OK)
2196                                 {
2197                                         return retval;
2198                                 }
2199
2200                                 num_accesses += thisrun_accesses;
2201                         }
2202                         break;
2203                 case 2:
2204                         while (num_accesses < count)
2205                         {
2206                                 u32 reg_list;
2207                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2208                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2209
2210                                 for (i = 1; i <= thisrun_accesses; i++)
2211                                 {
2212                                         if (i > last_reg)
2213                                                 last_reg = i;
2214                                         reg[i] = target_buffer_get_u16(target, buffer) & 0xffff;
2215                                         buffer += 2;
2216                                 }
2217
2218                                 arm7_9->write_core_regs(target, reg_list, reg);
2219
2220                                 for (i = 1; i <= thisrun_accesses; i++)
2221                                 {
2222                                         arm7_9->store_hword_reg(target, i);
2223
2224                                         /* fast memory writes are only safe when the target is running
2225                                          * from a sufficiently high clock (32 kHz is usually too slow)
2226                                          */
2227                                         if (arm7_9->fast_memory_access)
2228                                                 retval = arm7_9_execute_fast_sys_speed(target);
2229                                         else
2230                                                 retval = arm7_9_execute_sys_speed(target);
2231                                         if(retval != ERROR_OK)
2232                                         {
2233                                                 return retval;
2234                                         }
2235                                 }
2236
2237                                 num_accesses += thisrun_accesses;
2238                         }
2239                         break;
2240                 case 1:
2241                         while (num_accesses < count)
2242                         {
2243                                 u32 reg_list;
2244                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2245                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2246
2247                                 for (i = 1; i <= thisrun_accesses; i++)
2248                                 {
2249                                         if (i > last_reg)
2250                                                 last_reg = i;
2251                                         reg[i] = *buffer++ & 0xff;
2252                                 }
2253
2254                                 arm7_9->write_core_regs(target, reg_list, reg);
2255
2256                                 for (i = 1; i <= thisrun_accesses; i++)
2257                                 {
2258                                         arm7_9->store_byte_reg(target, i);
2259                                         /* fast memory writes are only safe when the target is running
2260                                          * from a sufficiently high clock (32 kHz is usually too slow)
2261                                          */
2262                                         if (arm7_9->fast_memory_access)
2263                                                 retval = arm7_9_execute_fast_sys_speed(target);
2264                                         else
2265                                                 retval = arm7_9_execute_sys_speed(target);
2266                                         if(retval != ERROR_OK)
2267                                         {
2268                                                 return retval;
2269                                         }
2270
2271                                 }
2272
2273                                 num_accesses += thisrun_accesses;
2274                         }
2275                         break;
2276                 default:
2277                         LOG_ERROR("BUG: we shouldn't get here");
2278                         exit(-1);
2279                         break;
2280         }
2281
2282         /* Re-Set DBGACK */
2283         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
2284         embeddedice_store_reg(dbg_ctrl);
2285
2286         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
2287                 return ERROR_FAIL;
2288
2289         for (i=0; i<=last_reg; i++)
2290                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid;
2291
2292         arm7_9->read_xpsr(target, &cpsr, 0);
2293         if ((retval = jtag_execute_queue()) != ERROR_OK)
2294         {
2295                 LOG_ERROR("JTAG error while reading cpsr");
2296                 return ERROR_TARGET_DATA_ABORT;
2297         }
2298
2299         if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
2300         {
2301                 LOG_WARNING("memory write caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
2302
2303                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
2304
2305                 return ERROR_TARGET_DATA_ABORT;
2306         }
2307
2308         return ERROR_OK;
2309 }
2310
2311 static int dcc_count;
2312 static u8 *dcc_buffer;
2313
2314 static int arm7_9_dcc_completion(struct target_s *target, u32 exit_point, int timeout_ms, void *arch_info)
2315 {
2316         int retval = ERROR_OK;
2317         armv4_5_common_t *armv4_5 = target->arch_info;
2318         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
2319
2320         if ((retval=target_wait_state(target, TARGET_DEBUG_RUNNING, 500))!=ERROR_OK)
2321                 return retval;
2322
2323         int little=target->endianness==TARGET_LITTLE_ENDIAN;
2324         int count=dcc_count;
2325         u8 *buffer=dcc_buffer;
2326         if (count>2)
2327         {
2328                 /* Handle first & last using standard embeddedice_write_reg and the middle ones w/the
2329                  * core function repeated. */
2330                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2331                 buffer+=4;
2332
2333                 embeddedice_reg_t *ice_reg = arm7_9->eice_cache->reg_list[EICE_COMMS_DATA].arch_info;
2334                 u8 reg_addr = ice_reg->addr & 0x1f;
2335                 jtag_tap_t *tap;
2336                 tap = ice_reg->jtag_info->tap;
2337
2338                 embeddedice_write_dcc(tap, reg_addr, buffer, little, count-2);
2339                 buffer += (count-2)*4;
2340
2341                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2342         } else
2343         {
2344                 int i;
2345                 for (i = 0; i < count; i++)
2346                 {
2347                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2348                         buffer += 4;
2349                 }
2350         }
2351
2352         if((retval = target_halt(target))!= ERROR_OK)
2353         {
2354                 return retval;
2355         }
2356         return target_wait_state(target, TARGET_HALTED, 500);
2357 }
2358
2359 static const u32 dcc_code[] =
2360 {
2361         /* MRC      TST         BNE         MRC         STR         B */
2362         0xee101e10, 0xe3110001, 0x0afffffc, 0xee111e10, 0xe4801004, 0xeafffff9
2363 };
2364
2365 int armv4_5_run_algorithm_inner(struct target_s *target, int num_mem_params, mem_param_t *mem_params, int num_reg_params, reg_param_t *reg_params, u32 entry_point, u32 exit_point, int timeout_ms, void *arch_info, int (*run_it)(struct target_s *target, u32 exit_point, int timeout_ms, void *arch_info));
2366
2367 int arm7_9_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)
2368 {
2369         int retval;
2370         armv4_5_common_t *armv4_5 = target->arch_info;
2371         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
2372         int i;
2373
2374         if (!arm7_9->dcc_downloads)
2375                 return target->type->write_memory(target, address, 4, count, buffer);
2376
2377         /* regrab previously allocated working_area, or allocate a new one */
2378         if (!arm7_9->dcc_working_area)
2379         {
2380                 u8 dcc_code_buf[6 * 4];
2381
2382                 /* make sure we have a working area */
2383                 if (target_alloc_working_area(target, 24, &arm7_9->dcc_working_area) != ERROR_OK)
2384                 {
2385                         LOG_INFO("no working area available, falling back to memory writes");
2386                         return target->type->write_memory(target, address, 4, count, buffer);
2387                 }
2388
2389                 /* copy target instructions to target endianness */
2390                 for (i = 0; i < 6; i++)
2391                 {
2392                         target_buffer_set_u32(target, dcc_code_buf + i*4, dcc_code[i]);
2393                 }
2394
2395                 /* write DCC code to working area */
2396                 if ((retval = target->type->write_memory(target, arm7_9->dcc_working_area->address, 4, 6, dcc_code_buf)) != ERROR_OK)
2397                 {
2398                         return retval;
2399                 }
2400         }
2401
2402         armv4_5_algorithm_t armv4_5_info;
2403         reg_param_t reg_params[1];
2404
2405         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2406         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2407         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2408
2409         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
2410
2411         buf_set_u32(reg_params[0].value, 0, 32, address);
2412
2413         dcc_count=count;
2414         dcc_buffer=buffer;
2415         retval = armv4_5_run_algorithm_inner(target, 0, NULL, 1, reg_params,
2416                         arm7_9->dcc_working_area->address, arm7_9->dcc_working_area->address+6*4, 20*1000, &armv4_5_info, arm7_9_dcc_completion);
2417
2418         if (retval==ERROR_OK)
2419         {
2420                 u32 endaddress=buf_get_u32(reg_params[0].value, 0, 32);
2421                 if (endaddress!=(address+count*4))
2422                 {
2423                         LOG_ERROR("DCC write failed, expected end address 0x%08x got 0x%0x", (address+count*4), endaddress);
2424                         retval=ERROR_FAIL;
2425                 }
2426         }
2427
2428         destroy_reg_param(&reg_params[0]);
2429
2430         return retval;
2431 }
2432
2433 int arm7_9_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)
2434 {
2435         working_area_t *crc_algorithm;
2436         armv4_5_algorithm_t armv4_5_info;
2437         reg_param_t reg_params[2];
2438         int retval;
2439
2440         u32 arm7_9_crc_code[] = {
2441                 0xE1A02000,                             /* mov          r2, r0 */
2442                 0xE3E00000,                             /* mov          r0, #0xffffffff */
2443                 0xE1A03001,                             /* mov          r3, r1 */
2444                 0xE3A04000,                             /* mov          r4, #0 */
2445                 0xEA00000B,                             /* b            ncomp */
2446                                                                 /* nbyte: */
2447                 0xE7D21004,                             /* ldrb r1, [r2, r4] */
2448                 0xE59F7030,                             /* ldr          r7, CRC32XOR */
2449                 0xE0200C01,                             /* eor          r0, r0, r1, asl 24 */
2450                 0xE3A05000,                             /* mov          r5, #0 */
2451                                                                 /* loop: */
2452                 0xE3500000,                             /* cmp          r0, #0 */
2453                 0xE1A06080,                             /* mov          r6, r0, asl #1 */
2454                 0xE2855001,                             /* add          r5, r5, #1 */
2455                 0xE1A00006,                             /* mov          r0, r6 */
2456                 0xB0260007,                             /* eorlt        r0, r6, r7 */
2457                 0xE3550008,                             /* cmp          r5, #8 */
2458                 0x1AFFFFF8,                             /* bne          loop */
2459                 0xE2844001,                             /* add          r4, r4, #1 */
2460                                                                 /* ncomp: */
2461                 0xE1540003,                             /* cmp          r4, r3 */
2462                 0x1AFFFFF1,                             /* bne          nbyte */
2463                                                                 /* end: */
2464                 0xEAFFFFFE,                             /* b            end */
2465                 0x04C11DB7                              /* CRC32XOR:    .word 0x04C11DB7 */
2466         };
2467
2468         u32 i;
2469
2470         if (target_alloc_working_area(target, sizeof(arm7_9_crc_code), &crc_algorithm) != ERROR_OK)
2471         {
2472                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2473         }
2474
2475         /* convert flash writing code into a buffer in target endianness */
2476         for (i = 0; i < (sizeof(arm7_9_crc_code)/sizeof(u32)); i++)
2477         {
2478                 if ((retval=target_write_u32(target, crc_algorithm->address + i*sizeof(u32), arm7_9_crc_code[i]))!=ERROR_OK)
2479                 {
2480                         return retval;
2481                 }
2482         }
2483
2484         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2485         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2486         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2487
2488         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
2489         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
2490
2491         buf_set_u32(reg_params[0].value, 0, 32, address);
2492         buf_set_u32(reg_params[1].value, 0, 32, count);
2493
2494         if ((retval = target->type->run_algorithm(target, 0, NULL, 2, reg_params,
2495                 crc_algorithm->address, crc_algorithm->address + (sizeof(arm7_9_crc_code) - 8), 20000, &armv4_5_info)) != ERROR_OK)
2496         {
2497                 LOG_ERROR("error executing arm7_9 crc algorithm");
2498                 destroy_reg_param(&reg_params[0]);
2499                 destroy_reg_param(&reg_params[1]);
2500                 target_free_working_area(target, crc_algorithm);
2501                 return retval;
2502         }
2503
2504         *checksum = buf_get_u32(reg_params[0].value, 0, 32);
2505
2506         destroy_reg_param(&reg_params[0]);
2507         destroy_reg_param(&reg_params[1]);
2508
2509         target_free_working_area(target, crc_algorithm);
2510
2511         return ERROR_OK;
2512 }
2513
2514 int arm7_9_blank_check_memory(struct target_s *target, u32 address, u32 count, u32* blank)
2515 {
2516         working_area_t *erase_check_algorithm;
2517         reg_param_t reg_params[3];
2518         armv4_5_algorithm_t armv4_5_info;
2519         int retval;
2520         u32 i;
2521
2522         u32 erase_check_code[] =
2523         {
2524                                                 /* loop: */
2525                 0xe4d03001,             /* ldrb r3, [r0], #1    */
2526                 0xe0022003,             /* and r2, r2, r3               */
2527                 0xe2511001,     /* subs r1, r1, #1              */
2528                 0x1afffffb,             /* bne loop                             */
2529                                                 /* end: */
2530                 0xeafffffe              /* b end                                */
2531         };
2532
2533         /* make sure we have a working area */
2534         if (target_alloc_working_area(target, sizeof(erase_check_code), &erase_check_algorithm) != ERROR_OK)
2535         {
2536                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2537         }
2538
2539         /* convert flash writing code into a buffer in target endianness */
2540         for (i = 0; i < (sizeof(erase_check_code)/sizeof(u32)); i++)
2541                 if ((retval = target_write_u32(target, erase_check_algorithm->address + i*sizeof(u32), erase_check_code[i])) != ERROR_OK)
2542                 {
2543                         return retval;
2544                 }
2545
2546         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2547         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2548         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2549
2550         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
2551         buf_set_u32(reg_params[0].value, 0, 32, address);
2552
2553         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
2554         buf_set_u32(reg_params[1].value, 0, 32, count);
2555
2556         init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
2557         buf_set_u32(reg_params[2].value, 0, 32, 0xff);
2558
2559         if ((retval = target->type->run_algorithm(target, 0, NULL, 3, reg_params,
2560                         erase_check_algorithm->address, erase_check_algorithm->address + (sizeof(erase_check_code) - 4), 10000, &armv4_5_info)) != ERROR_OK)
2561         {
2562                 destroy_reg_param(&reg_params[0]);
2563                 destroy_reg_param(&reg_params[1]);
2564                 destroy_reg_param(&reg_params[2]);
2565                 target_free_working_area(target, erase_check_algorithm);
2566                 return 0;
2567         }
2568
2569         *blank = buf_get_u32(reg_params[2].value, 0, 32);
2570
2571         destroy_reg_param(&reg_params[0]);
2572         destroy_reg_param(&reg_params[1]);
2573         destroy_reg_param(&reg_params[2]);
2574
2575         target_free_working_area(target, erase_check_algorithm);
2576
2577         return ERROR_OK;
2578 }
2579
2580 int arm7_9_register_commands(struct command_context_s *cmd_ctx)
2581 {
2582         command_t *arm7_9_cmd;
2583
2584         arm7_9_cmd = register_command(cmd_ctx, NULL, "arm7_9", NULL, COMMAND_ANY, "arm7/9 specific commands");
2585
2586         register_command(cmd_ctx, arm7_9_cmd, "write_xpsr", handle_arm7_9_write_xpsr_command, COMMAND_EXEC, "write program status register <value> <not cpsr|spsr>");
2587         register_command(cmd_ctx, arm7_9_cmd, "write_xpsr_im8", handle_arm7_9_write_xpsr_im8_command, COMMAND_EXEC, "write program status register <8bit immediate> <rotate> <not cpsr|spsr>");
2588
2589         register_command(cmd_ctx, arm7_9_cmd, "write_core_reg", handle_arm7_9_write_core_reg_command, COMMAND_EXEC, "write core register <num> <mode> <value>");
2590
2591         register_command(cmd_ctx, arm7_9_cmd, "dbgrq", handle_arm7_9_dbgrq_command,
2592                 COMMAND_ANY, "use EmbeddedICE dbgrq instead of breakpoint for target halt requests <enable|disable>");
2593         register_command(cmd_ctx, arm7_9_cmd, "fast_memory_access", handle_arm7_9_fast_memory_access_command,
2594                  COMMAND_ANY, "use fast memory accesses instead of slower but potentially unsafe slow accesses <enable|disable>");
2595         register_command(cmd_ctx, arm7_9_cmd, "dcc_downloads", handle_arm7_9_dcc_downloads_command,
2596                 COMMAND_ANY, "use DCC downloads for larger memory writes <enable|disable>");
2597
2598         armv4_5_register_commands(cmd_ctx);
2599
2600         etm_register_commands(cmd_ctx);
2601
2602         return ERROR_OK;
2603 }
2604
2605 int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2606 {
2607         u32 value;
2608         int spsr;
2609         int retval;
2610         target_t *target = get_current_target(cmd_ctx);
2611         armv4_5_common_t *armv4_5;
2612         arm7_9_common_t *arm7_9;
2613
2614         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2615         {
2616                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2617                 return ERROR_OK;
2618         }
2619
2620         if (target->state != TARGET_HALTED)
2621         {
2622                 command_print(cmd_ctx, "can't write registers while running");
2623                 return ERROR_OK;
2624         }
2625
2626         if (argc < 2)
2627         {
2628                 command_print(cmd_ctx, "usage: write_xpsr <value> <not cpsr|spsr>");
2629                 return ERROR_OK;
2630         }
2631
2632         value = strtoul(args[0], NULL, 0);
2633         spsr = strtol(args[1], NULL, 0);
2634
2635         /* if we're writing the CPSR, mask the T bit */
2636         if (!spsr)
2637                 value &= ~0x20;
2638
2639         arm7_9->write_xpsr(target, value, spsr);
2640         if ((retval = jtag_execute_queue()) != ERROR_OK)
2641         {
2642                 LOG_ERROR("JTAG error while writing to xpsr");
2643                 return retval;
2644         }
2645
2646         return ERROR_OK;
2647 }
2648
2649 int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2650 {
2651         u32 value;
2652         int rotate;
2653         int spsr;
2654         int retval;
2655         target_t *target = get_current_target(cmd_ctx);
2656         armv4_5_common_t *armv4_5;
2657         arm7_9_common_t *arm7_9;
2658
2659         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2660         {
2661                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2662                 return ERROR_OK;
2663         }
2664
2665         if (target->state != TARGET_HALTED)
2666         {
2667                 command_print(cmd_ctx, "can't write registers while running");
2668                 return ERROR_OK;
2669         }
2670
2671         if (argc < 3)
2672         {
2673                 command_print(cmd_ctx, "usage: write_xpsr_im8 <im8> <rotate> <not cpsr|spsr>");
2674                 return ERROR_OK;
2675         }
2676
2677         value = strtoul(args[0], NULL, 0);
2678         rotate = strtol(args[1], NULL, 0);
2679         spsr = strtol(args[2], NULL, 0);
2680
2681         arm7_9->write_xpsr_im8(target, value, rotate, spsr);
2682         if ((retval = jtag_execute_queue()) != ERROR_OK)
2683         {
2684                 LOG_ERROR("JTAG error while writing 8-bit immediate to xpsr");
2685                 return retval;
2686         }
2687
2688         return ERROR_OK;
2689 }
2690
2691 int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2692 {
2693         u32 value;
2694         u32 mode;
2695         int num;
2696         target_t *target = get_current_target(cmd_ctx);
2697         armv4_5_common_t *armv4_5;
2698         arm7_9_common_t *arm7_9;
2699
2700         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2701         {
2702                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2703                 return ERROR_OK;
2704         }
2705
2706         if (target->state != TARGET_HALTED)
2707         {
2708                 command_print(cmd_ctx, "can't write registers while running");
2709                 return ERROR_OK;
2710         }
2711
2712         if (argc < 3)
2713         {
2714                 command_print(cmd_ctx, "usage: write_core_reg <num> <mode> <value>");
2715                 return ERROR_OK;
2716         }
2717
2718         num = strtol(args[0], NULL, 0);
2719         mode = strtoul(args[1], NULL, 0);
2720         value = strtoul(args[2], NULL, 0);
2721
2722         return arm7_9_write_core_reg(target, num, mode, value);
2723 }
2724
2725 int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2726 {
2727         target_t *target = get_current_target(cmd_ctx);
2728         armv4_5_common_t *armv4_5;
2729         arm7_9_common_t *arm7_9;
2730
2731         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2732         {
2733                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2734                 return ERROR_OK;
2735         }
2736
2737         if (argc > 0)
2738         {
2739                 if (strcmp("enable", args[0]) == 0)
2740                 {
2741                         arm7_9->use_dbgrq = 1;
2742                 }
2743                 else if (strcmp("disable", args[0]) == 0)
2744                 {
2745                         arm7_9->use_dbgrq = 0;
2746                 }
2747                 else
2748                 {
2749                         command_print(cmd_ctx, "usage: arm7_9 dbgrq <enable|disable>");
2750                 }
2751         }
2752
2753         command_print(cmd_ctx, "use of EmbeddedICE dbgrq instead of breakpoint for target halt %s", (arm7_9->use_dbgrq) ? "enabled" : "disabled");
2754
2755         return ERROR_OK;
2756 }
2757
2758 int handle_arm7_9_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2759 {
2760         target_t *target = get_current_target(cmd_ctx);
2761         armv4_5_common_t *armv4_5;
2762         arm7_9_common_t *arm7_9;
2763
2764         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2765         {
2766                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2767                 return ERROR_OK;
2768         }
2769
2770         if (argc > 0)
2771         {
2772                 if (strcmp("enable", args[0]) == 0)
2773                 {
2774                         arm7_9->fast_memory_access = 1;
2775                 }
2776                 else if (strcmp("disable", args[0]) == 0)
2777                 {
2778                         arm7_9->fast_memory_access = 0;
2779                 }
2780                 else
2781                 {
2782                         command_print(cmd_ctx, "usage: arm7_9 fast_memory_access <enable|disable>");
2783                 }
2784         }
2785
2786         command_print(cmd_ctx, "fast memory access is %s", (arm7_9->fast_memory_access) ? "enabled" : "disabled");
2787
2788         return ERROR_OK;
2789 }
2790
2791 int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2792 {
2793         target_t *target = get_current_target(cmd_ctx);
2794         armv4_5_common_t *armv4_5;
2795         arm7_9_common_t *arm7_9;
2796
2797         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2798         {
2799                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2800                 return ERROR_OK;
2801         }
2802
2803         if (argc > 0)
2804         {
2805                 if (strcmp("enable", args[0]) == 0)
2806                 {
2807                         arm7_9->dcc_downloads = 1;
2808                 }
2809                 else if (strcmp("disable", args[0]) == 0)
2810                 {
2811                         arm7_9->dcc_downloads = 0;
2812                 }
2813                 else
2814                 {
2815                         command_print(cmd_ctx, "usage: arm7_9 dcc_downloads <enable|disable>");
2816                 }
2817         }
2818
2819         command_print(cmd_ctx, "dcc downloads are %s", (arm7_9->dcc_downloads) ? "enabled" : "disabled");
2820
2821         return ERROR_OK;
2822 }
2823
2824 int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9)
2825 {
2826         int retval = ERROR_OK;
2827         armv4_5_common_t *armv4_5 = &arm7_9->armv4_5_common;
2828
2829         arm7_9->common_magic = ARM7_9_COMMON_MAGIC;
2830
2831         if((retval = arm_jtag_setup_connection(&arm7_9->jtag_info)) != ERROR_OK)
2832         {
2833                 return retval;
2834         }
2835
2836         arm7_9->wp_available = 0; /* this is set up in arm7_9_clear_watchpoints() */
2837         arm7_9->wp_available_max = 2;
2838         arm7_9->sw_breakpoints_added = 0;
2839         arm7_9->breakpoint_count = 0;
2840         arm7_9->wp0_used = 0;
2841         arm7_9->wp1_used = 0;
2842         arm7_9->wp1_used_default = 0;
2843         arm7_9->use_dbgrq = 0;
2844
2845         arm7_9->etm_ctx = NULL;
2846         arm7_9->has_single_step = 0;
2847         arm7_9->has_monitor_mode = 0;
2848         arm7_9->has_vector_catch = 0;
2849
2850         arm7_9->debug_entry_from_reset = 0;
2851
2852         arm7_9->dcc_working_area = NULL;
2853
2854         arm7_9->fast_memory_access = fast_and_dangerous;
2855         arm7_9->dcc_downloads = fast_and_dangerous;
2856
2857         arm7_9->need_bypass_before_restart = 0;
2858
2859         armv4_5->arch_info = arm7_9;
2860         armv4_5->read_core_reg = arm7_9_read_core_reg;
2861         armv4_5->write_core_reg = arm7_9_write_core_reg;
2862         armv4_5->full_context = arm7_9_full_context;
2863
2864         if((retval = armv4_5_init_arch_info(target, armv4_5)) != ERROR_OK)
2865         {
2866                 return retval;
2867         }
2868
2869         if((retval = target_register_timer_callback(arm7_9_handle_target_request, 1, 1, target)) != ERROR_OK)
2870         {
2871                 return retval;
2872         }
2873
2874         return ERROR_OK;
2875 }