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

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