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