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