src/target/arm7_9_common.c

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