git.gag.com Git - fw/openocd/blob - src/target/arm7_9_common.c

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