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",
 738                   Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
 739
 740         if (!(jtag_reset_config & RESET_HAS_SRST))
 741         {
 742                 LOG_ERROR("Can't assert SRST");
 743                 return ERROR_FAIL;
 744         }
 745
 746         if (target->reset_halt)
 747         {
 748                 /*
 749                  * Some targets do not support communication while SRST is asserted. We need to
 750                  * set up the reset vector catch here.
 751                  *
 752                  * If TRST is asserted, then these settings will be reset anyway, so setting them
 753                  * here is harmless.
 754                  */
 755                 if (arm7_9->has_vector_catch)
 756                 {
 757                         /* program vector catch register to catch reset vector */
 758                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH], 0x1);
 759                 }
 760                 else
 761                 {
 762                         /* program watchpoint unit to match on reset vector address */
 763                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], 0x0);
 764                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0x3);
 765                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
 766                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 767                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 768                 }
 769         }
 770
 771         /* here we should issue a srst only, but we may have to assert trst as well */
 772         if (jtag_reset_config & RESET_SRST_PULLS_TRST)
 773         {
 774                 jtag_add_reset(1, 1);
 775         } else
 776         {
 777                 jtag_add_reset(0, 1);
 778         }
 779
 780
 781         target->state = TARGET_RESET;
 782         jtag_add_sleep(50000);
 783
 784         armv4_5_invalidate_core_regs(target);
 785
 786     if ((target->reset_halt)&&((jtag_reset_config & RESET_SRST_PULLS_TRST)==0))
 787         {
 788                 /* debug entry was already prepared in arm7_9_assert_reset() */
 789                 target->debug_reason = DBG_REASON_DBGRQ;
 790         }
 791
 792         return ERROR_OK;
 793
 794 }
 795
 796 int arm7_9_deassert_reset(target_t *target)
 797 {
 798         int retval=ERROR_OK;
 799         LOG_DEBUG("target->state: %s",
 800                   Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
 801
 802
 803         /* deassert reset lines */
 804         jtag_add_reset(0, 0);
 805
 806         if (target->reset_halt&&(jtag_reset_config & RESET_SRST_PULLS_TRST)!=0)
 807         {
 808                 LOG_WARNING("srst pulls trst - can not reset into halted mode. Issuing halt after reset.");
 809                 /* set up embedded ice registers again */
 810                 if ((retval=target->type->examine(target))!=ERROR_OK)
 811                         return retval;
 812
 813                 if ((retval=target_poll(target))!=ERROR_OK)
 814                 {
 815                         return retval;
 816                 }
 817
 818                 if ((retval=target_halt(target))!=ERROR_OK)
 819                 {
 820                         return retval;
 821                 }
 822
 823         }
 824         return retval;
 825 }
 826
 827 int arm7_9_clear_halt(target_t *target)
 828 {
 829         armv4_5_common_t *armv4_5 = target->arch_info;
 830         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 831         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 832
 833         /* we used DBGRQ only if we didn't come out of reset */
 834         if (!arm7_9->debug_entry_from_reset && arm7_9->use_dbgrq)
 835         {
 836                 /* program EmbeddedICE Debug Control Register to deassert DBGRQ
 837                  */
 838                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
 839                 embeddedice_store_reg(dbg_ctrl);
 840         }
 841         else
 842         {
 843                 if (arm7_9->debug_entry_from_reset && arm7_9->has_vector_catch)
 844                 {
 845                         /* if we came out of reset, and vector catch is supported, we used
 846                          * vector catch to enter debug state
 847                          * restore the register in that case
 848                          */
 849                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH]);
 850                 }
 851                 else
 852                 {
 853                         /* restore registers if watchpoint unit 0 was in use
 854                          */
 855                         if (arm7_9->wp0_used)
 856                         {
 857                                 if (arm7_9->debug_entry_from_reset)
 858                                 {
 859                                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE]);
 860                                 }
 861                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
 862                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
 863                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
 864                         }
 865                         /* control value always has to be restored, as it was either disabled,
 866                          * or enabled with possibly different bits
 867                          */
 868                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
 869                 }
 870         }
 871
 872         return ERROR_OK;
 873 }
 874
 875 int arm7_9_soft_reset_halt(struct target_s *target)
 876 {
 877         armv4_5_common_t *armv4_5 = target->arch_info;
 878         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 879         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 880         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 881         int i;
 882         int retval;
 883
 884         if ((retval=target_halt(target))!=ERROR_OK)
 885                 return retval;
 886
 887         long long then=timeval_ms();
 888         int timeout;
 889         while (!(timeout=((timeval_ms()-then)>1000)))
 890         {
 891                 if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) != 0)
 892                         break;
 893                 embeddedice_read_reg(dbg_stat);
 894                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 895                         return retval;
 896                 if (debug_level>=3)
 897                 {
 898                         alive_sleep(100);
 899                 } else
 900                 {
 901                         keep_alive();
 902                 }
 903         }
 904         if (timeout)
 905         {
 906                 LOG_ERROR("Failed to halt CPU after 1 sec");
 907                 return ERROR_TARGET_TIMEOUT;
 908         }
 909         target->state = TARGET_HALTED;
 910
 911         /* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
 912          * ensure that DBGRQ is cleared
 913          */
 914         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
 915         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
 916         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
 917         embeddedice_store_reg(dbg_ctrl);
 918
 919         arm7_9_clear_halt(target);
 920
 921         /* if the target is in Thumb state, change to ARM state */
 922         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
 923         {
 924                 u32 r0_thumb, pc_thumb;
 925                 LOG_DEBUG("target entered debug from Thumb state, changing to ARM");
 926                 /* Entered debug from Thumb mode */
 927                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
 928                 arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
 929         }
 930
 931         /* all register content is now invalid */
 932         armv4_5_invalidate_core_regs(target);
 933
 934         /* SVC, ARM state, IRQ and FIQ disabled */
 935         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
 936         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
 937         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
 938
 939         /* start fetching from 0x0 */
 940         buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
 941         armv4_5->core_cache->reg_list[15].dirty = 1;
 942         armv4_5->core_cache->reg_list[15].valid = 1;
 943
 944         armv4_5->core_mode = ARMV4_5_MODE_SVC;
 945         armv4_5->core_state = ARMV4_5_STATE_ARM;
 946
 947         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
 948                 return ERROR_FAIL;
 949
 950         /* reset registers */
 951         for (i = 0; i <= 14; i++)
 952         {
 953                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, 0xffffffff);
 954                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
 955                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
 956         }
 957
 958         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 959
 960         return ERROR_OK;
 961 }
 962
 963 int arm7_9_halt(target_t *target)
 964 {
 965         if (target->state==TARGET_RESET)
 966         {
 967                 LOG_ERROR("BUG: arm7/9 does not support halt during reset. This is handled in arm7_9_assert_reset()");
 968                 return ERROR_OK;
 969         }
 970
 971         armv4_5_common_t *armv4_5 = target->arch_info;
 972         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 973         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 974
 975         LOG_DEBUG("target->state: %s",
 976                   Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
 977
 978         if (target->state == TARGET_HALTED)
 979         {
 980                 LOG_DEBUG("target was already halted");
 981                 return ERROR_OK;
 982         }
 983
 984         if (target->state == TARGET_UNKNOWN)
 985         {
 986                 LOG_WARNING("target was in unknown state when halt was requested");
 987         }
 988
 989         if (arm7_9->use_dbgrq)
 990         {
 991                 /* program EmbeddedICE Debug Control Register to assert DBGRQ
 992                  */
 993                 if (arm7_9->set_special_dbgrq) {
 994                         arm7_9->set_special_dbgrq(target);
 995                 } else {
 996                         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 1);
 997                         embeddedice_store_reg(dbg_ctrl);
 998                 }
 999         }
1000         else
1001         {
1002                 /* program watchpoint unit to match on any address
1003                  */
1004                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1005                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1006                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
1007                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1008         }
1009
1010         target->debug_reason = DBG_REASON_DBGRQ;
1011
1012         return ERROR_OK;
1013 }
1014
1015 int arm7_9_debug_entry(target_t *target)
1016 {
1017         int i;
1018         u32 context[16];
1019         u32* context_p[16];
1020         u32 r0_thumb, pc_thumb;
1021         u32 cpsr;
1022         int retval;
1023         /* get pointers to arch-specific information */
1024         armv4_5_common_t *armv4_5 = target->arch_info;
1025         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1026         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
1027         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1028
1029 #ifdef _DEBUG_ARM7_9_
1030         LOG_DEBUG("-");
1031 #endif
1032
1033         if (arm7_9->pre_debug_entry)
1034                 arm7_9->pre_debug_entry(target);
1035
1036         /* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
1037          * ensure that DBGRQ is cleared
1038          */
1039         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
1040         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
1041         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
1042         embeddedice_store_reg(dbg_ctrl);
1043
1044         arm7_9_clear_halt(target);
1045
1046         if ((retval = jtag_execute_queue()) != ERROR_OK)
1047         {
1048                 return retval;
1049         }
1050
1051         if ((retval = arm7_9->examine_debug_reason(target)) != ERROR_OK)
1052                 return retval;
1053
1054
1055         if (target->state != TARGET_HALTED)
1056         {
1057                 LOG_WARNING("target not halted");
1058                 return ERROR_TARGET_NOT_HALTED;
1059         }
1060
1061         /* if the target is in Thumb state, change to ARM state */
1062         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
1063         {
1064                 LOG_DEBUG("target entered debug from Thumb state");
1065                 /* Entered debug from Thumb mode */
1066                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
1067                 arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
1068                 LOG_DEBUG("r0_thumb: 0x%8.8x, pc_thumb: 0x%8.8x", r0_thumb, pc_thumb);
1069         }
1070         else
1071         {
1072                 LOG_DEBUG("target entered debug from ARM state");
1073                 /* Entered debug from ARM mode */
1074                 armv4_5->core_state = ARMV4_5_STATE_ARM;
1075         }
1076
1077         for (i = 0; i < 16; i++)
1078                 context_p[i] = &context[i];
1079         /* save core registers (r0 - r15 of current core mode) */
1080         arm7_9->read_core_regs(target, 0xffff, context_p);
1081
1082         arm7_9->read_xpsr(target, &cpsr, 0);
1083
1084         if ((retval = jtag_execute_queue()) != ERROR_OK)
1085                 return retval;
1086
1087         /* if the core has been executing in Thumb state, set the T bit */
1088         if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1089                 cpsr |= 0x20;
1090
1091         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, cpsr);
1092         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
1093         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1094
1095         armv4_5->core_mode = cpsr & 0x1f;
1096
1097         if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
1098         {
1099                 target->state = TARGET_UNKNOWN;
1100                 LOG_ERROR("cpsr contains invalid mode value - communication failure");
1101                 return ERROR_TARGET_FAILURE;
1102         }
1103
1104         LOG_DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);
1105
1106         if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1107         {
1108                 LOG_DEBUG("thumb state, applying fixups");
1109                 context[0] = r0_thumb;
1110                 context[15] = pc_thumb;
1111         } else if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1112         {
1113                 /* adjust value stored by STM */
1114                 context[15] -= 3 * 4;
1115         }
1116
1117         if ((target->debug_reason == DBG_REASON_BREAKPOINT)
1118                         || (target->debug_reason == DBG_REASON_SINGLESTEP)
1119                         || (target->debug_reason == DBG_REASON_WATCHPOINT)
1120                         || (target->debug_reason == DBG_REASON_WPTANDBKPT)
1121                         || ((target->debug_reason == DBG_REASON_DBGRQ) && (arm7_9->use_dbgrq == 0)))
1122                 context[15] -= 3 * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
1123         else if (target->debug_reason == DBG_REASON_DBGRQ)
1124                 context[15] -= arm7_9->dbgreq_adjust_pc * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
1125         else
1126         {
1127                 LOG_ERROR("unknown debug reason: %i", target->debug_reason);
1128         }
1129
1130         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1131                 return ERROR_FAIL;
1132
1133         for (i=0; i<=15; i++)
1134         {
1135                 LOG_DEBUG("r%i: 0x%8.8x", i, context[i]);
1136                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, context[i]);
1137                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;
1138                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
1139         }
1140
1141         LOG_DEBUG("entered debug state at PC 0x%x", context[15]);
1142
1143         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1144                 return ERROR_FAIL;
1145
1146         /* exceptions other than USR & SYS have a saved program status register */
1147         if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))
1148         {
1149                 u32 spsr;
1150                 arm7_9->read_xpsr(target, &spsr, 1);
1151                 jtag_execute_queue();
1152                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, spsr);
1153                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;
1154                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;
1155         }
1156
1157         /* r0 and r15 (pc) have to be restored later */
1158         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;
1159         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;
1160
1161         if ((retval = jtag_execute_queue()) != ERROR_OK)
1162                 return retval;
1163
1164         if (arm7_9->post_debug_entry)
1165                 arm7_9->post_debug_entry(target);
1166
1167         return ERROR_OK;
1168 }
1169
1170 int arm7_9_full_context(target_t *target)
1171 {
1172         int i;
1173         int retval;
1174         armv4_5_common_t *armv4_5 = target->arch_info;
1175         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1176
1177         LOG_DEBUG("-");
1178
1179         if (target->state != TARGET_HALTED)
1180         {
1181                 LOG_WARNING("target not halted");
1182                 return ERROR_TARGET_NOT_HALTED;
1183         }
1184
1185         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1186                 return ERROR_FAIL;
1187
1188         /* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
1189          * SYS shares registers with User, so we don't touch SYS
1190          */
1191         for(i = 0; i < 6; i++)
1192         {
1193                 u32 mask = 0;
1194                 u32* reg_p[16];
1195                 int j;
1196                 int valid = 1;
1197
1198                 /* check if there are invalid registers in the current mode
1199                  */
1200                 for (j = 0; j <= 16; j++)
1201                 {
1202                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1203                                 valid = 0;
1204                 }
1205
1206                 if (!valid)
1207                 {
1208                         u32 tmp_cpsr;
1209
1210                         /* change processor mode (and mask T bit) */
1211                         tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1212                         tmp_cpsr |= armv4_5_number_to_mode(i);
1213                         tmp_cpsr &= ~0x20;
1214                         arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1215
1216                         for (j = 0; j < 15; j++)
1217                         {
1218                                 if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1219                                 {
1220                                         reg_p[j] = (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value;
1221                                         mask |= 1 << j;
1222                                         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;
1223                                         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
1224                                 }
1225                         }
1226
1227                         /* if only the PSR is invalid, mask is all zeroes */
1228                         if (mask)
1229                                 arm7_9->read_core_regs(target, mask, reg_p);
1230
1231                         /* check if the PSR has to be read */
1232                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid == 0)
1233                         {
1234                                 arm7_9->read_xpsr(target, (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).value, 1);
1235                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;
1236                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
1237                         }
1238                 }
1239         }
1240
1241         /* restore processor mode (mask T bit) */
1242         arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1243
1244         if ((retval = jtag_execute_queue()) != ERROR_OK)
1245         {
1246                 return retval;
1247         }
1248         return ERROR_OK;
1249 }
1250
1251 int arm7_9_restore_context(target_t *target)
1252 {
1253         armv4_5_common_t *armv4_5 = target->arch_info;
1254         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1255         reg_t *reg;
1256         armv4_5_core_reg_t *reg_arch_info;
1257         enum armv4_5_mode current_mode = armv4_5->core_mode;
1258         int i, j;
1259         int dirty;
1260         int mode_change;
1261
1262         LOG_DEBUG("-");
1263
1264         if (target->state != TARGET_HALTED)
1265         {
1266                 LOG_WARNING("target not halted");
1267                 return ERROR_TARGET_NOT_HALTED;
1268         }
1269
1270         if (arm7_9->pre_restore_context)
1271                 arm7_9->pre_restore_context(target);
1272
1273         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1274                 return ERROR_FAIL;
1275
1276         /* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
1277          * SYS shares registers with User, so we don't touch SYS
1278          */
1279         for (i = 0; i < 6; i++)
1280         {
1281                 LOG_DEBUG("examining %s mode", armv4_5_mode_strings[i]);
1282                 dirty = 0;
1283                 mode_change = 0;
1284                 /* check if there are dirty registers in the current mode
1285                 */
1286                 for (j = 0; j <= 16; j++)
1287                 {
1288                         reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
1289                         reg_arch_info = reg->arch_info;
1290                         if (reg->dirty == 1)
1291                         {
1292                                 if (reg->valid == 1)
1293                                 {
1294                                         dirty = 1;
1295                                         LOG_DEBUG("examining dirty reg: %s", reg->name);
1296                                         if ((reg_arch_info->mode != ARMV4_5_MODE_ANY)
1297                                                 && (reg_arch_info->mode != current_mode)
1298                                                 && !((reg_arch_info->mode == ARMV4_5_MODE_USR) && (armv4_5->core_mode == ARMV4_5_MODE_SYS))
1299                                                 && !((reg_arch_info->mode == ARMV4_5_MODE_SYS) && (armv4_5->core_mode == ARMV4_5_MODE_USR)))
1300                                         {
1301                                                 mode_change = 1;
1302                                                 LOG_DEBUG("require mode change");
1303                                         }
1304                                 }
1305                                 else
1306                                 {
1307                                         LOG_ERROR("BUG: dirty register '%s', but no valid data", reg->name);
1308                                 }
1309                         }
1310                 }
1311
1312                 if (dirty)
1313                 {
1314                         u32 mask = 0x0;
1315                         int num_regs = 0;
1316                         u32 regs[16];
1317
1318                         if (mode_change)
1319                         {
1320                                 u32 tmp_cpsr;
1321
1322                                 /* change processor mode (mask T bit) */
1323                                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1324                                 tmp_cpsr |= armv4_5_number_to_mode(i);
1325                                 tmp_cpsr &= ~0x20;
1326                                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1327                                 current_mode = armv4_5_number_to_mode(i);
1328                         }
1329
1330                         for (j = 0; j <= 14; j++)
1331                         {
1332                                 reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
1333                                 reg_arch_info = reg->arch_info;
1334
1335
1336                                 if (reg->dirty == 1)
1337                                 {
1338                                         regs[j] = buf_get_u32(reg->value, 0, 32);
1339                                         mask |= 1 << j;
1340                                         num_regs++;
1341                                         reg->dirty = 0;
1342                                         reg->valid = 1;
1343                                         LOG_DEBUG("writing register %i of mode %s with value 0x%8.8x", j, armv4_5_mode_strings[i], regs[j]);
1344                                 }
1345                         }
1346
1347                         if (mask)
1348                         {
1349                                 arm7_9->write_core_regs(target, mask, regs);
1350                         }
1351
1352                         reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16);
1353                         reg_arch_info = reg->arch_info;
1354                         if ((reg->dirty) && (reg_arch_info->mode != ARMV4_5_MODE_ANY))
1355                         {
1356                                 LOG_DEBUG("writing SPSR of mode %i with value 0x%8.8x", i, buf_get_u32(reg->value, 0, 32));
1357                                 arm7_9->write_xpsr(target, buf_get_u32(reg->value, 0, 32), 1);
1358                         }
1359                 }
1360         }
1361
1362         if ((armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 0) && (armv4_5->core_mode != current_mode))
1363         {
1364                 /* restore processor mode (mask T bit) */
1365                 u32 tmp_cpsr;
1366
1367                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1368                 tmp_cpsr |= armv4_5_number_to_mode(i);
1369                 tmp_cpsr &= ~0x20;
1370                 LOG_DEBUG("writing lower 8 bit of cpsr with value 0x%2.2x", tmp_cpsr);
1371                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1372         }
1373         else if (armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 1)
1374         {
1375                 /* CPSR has been changed, full restore necessary (mask T bit) */
1376                 LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1377                 arm7_9->write_xpsr(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32) & ~0x20, 0);
1378                 armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
1379                 armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1380         }
1381
1382         /* restore PC */
1383         LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1384         arm7_9->write_pc(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1385         armv4_5->core_cache->reg_list[15].dirty = 0;
1386
1387         if (arm7_9->post_restore_context)
1388                 arm7_9->post_restore_context(target);
1389
1390         return ERROR_OK;
1391 }
1392
1393 int arm7_9_restart_core(struct target_s *target)
1394 {
1395         armv4_5_common_t *armv4_5 = target->arch_info;
1396         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1397         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
1398
1399         /* set RESTART instruction */
1400         jtag_add_end_state(TAP_RTI);
1401         if (arm7_9->need_bypass_before_restart) {
1402                 arm7_9->need_bypass_before_restart = 0;
1403                 arm_jtag_set_instr(jtag_info, 0xf, NULL);
1404         }
1405         arm_jtag_set_instr(jtag_info, 0x4, NULL);
1406
1407         jtag_add_runtest(1, TAP_RTI);
1408         return jtag_execute_queue();
1409 }
1410
1411 void arm7_9_enable_watchpoints(struct target_s *target)
1412 {
1413         watchpoint_t *watchpoint = target->watchpoints;
1414
1415         while (watchpoint)
1416         {
1417                 if (watchpoint->set == 0)
1418                         arm7_9_set_watchpoint(target, watchpoint);
1419                 watchpoint = watchpoint->next;
1420         }
1421 }
1422
1423 void arm7_9_enable_breakpoints(struct target_s *target)
1424 {
1425         breakpoint_t *breakpoint = target->breakpoints;
1426
1427         /* set any pending breakpoints */
1428         while (breakpoint)
1429         {
1430                 arm7_9_set_breakpoint(target, breakpoint);
1431                 breakpoint = breakpoint->next;
1432         }
1433 }
1434
1435
1436 int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
1437 {
1438         armv4_5_common_t *armv4_5 = target->arch_info;
1439         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1440         breakpoint_t *breakpoint = target->breakpoints;
1441         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1442         int err;
1443
1444         LOG_DEBUG("-");
1445
1446         if (target->state != TARGET_HALTED)
1447         {
1448                 LOG_WARNING("target not halted");
1449                 return ERROR_TARGET_NOT_HALTED;
1450         }
1451
1452         if (!debug_execution)
1453         {
1454                 target_free_all_working_areas(target);
1455         }
1456
1457         /* current = 1: continue on current pc, otherwise continue at <address> */
1458         if (!current)
1459                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1460
1461         /* the front-end may request us not to handle breakpoints */
1462         if (handle_breakpoints)
1463         {
1464                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1465                 {
1466                         LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
1467                         arm7_9_unset_breakpoint(target, breakpoint);
1468
1469                         LOG_DEBUG("enable single-step");
1470                         arm7_9->enable_single_step(target);
1471
1472                         target->debug_reason = DBG_REASON_SINGLESTEP;
1473
1474                         arm7_9_restore_context(target);
1475
1476                         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1477                                 arm7_9->branch_resume(target);
1478                         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1479                         {
1480                                 arm7_9->branch_resume_thumb(target);
1481                         }
1482                         else
1483                         {
1484                                 LOG_ERROR("unhandled core state");
1485                                 return ERROR_FAIL;
1486                         }
1487
1488                         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1489                         embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1490                         err = arm7_9_execute_sys_speed(target);
1491
1492                         LOG_DEBUG("disable single-step");
1493                         arm7_9->disable_single_step(target);
1494
1495                         if (err != ERROR_OK)
1496                         {
1497                                 arm7_9_set_breakpoint(target, breakpoint);
1498                                 target->state = TARGET_UNKNOWN;
1499                                 return err;
1500                         }
1501
1502                         arm7_9_debug_entry(target);
1503                         LOG_DEBUG("new PC after step: 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1504
1505                         LOG_DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
1506                         arm7_9_set_breakpoint(target, breakpoint);
1507                 }
1508         }
1509
1510         /* enable any pending breakpoints and watchpoints */
1511         arm7_9_enable_breakpoints(target);
1512         arm7_9_enable_watchpoints(target);
1513
1514         arm7_9_restore_context(target);
1515
1516         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1517         {
1518                 arm7_9->branch_resume(target);
1519         }
1520         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1521         {
1522                 arm7_9->branch_resume_thumb(target);
1523         }
1524         else
1525         {
1526                 LOG_ERROR("unhandled core state");
1527                 return ERROR_FAIL;
1528         }
1529
1530         /* deassert DBGACK and INTDIS */
1531         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1532         /* INTDIS only when we really resume, not during debug execution */
1533         if (!debug_execution)
1534                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 0);
1535         embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1536
1537         arm7_9_restart_core(target);
1538
1539         target->debug_reason = DBG_REASON_NOTHALTED;
1540
1541         if (!debug_execution)
1542         {
1543                 /* registers are now invalid */
1544                 armv4_5_invalidate_core_regs(target);
1545                 target->state = TARGET_RUNNING;
1546                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1547         }
1548         else
1549         {
1550                 target->state = TARGET_DEBUG_RUNNING;
1551                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
1552         }
1553
1554         LOG_DEBUG("target resumed");
1555
1556         return ERROR_OK;
1557 }
1558
1559 void arm7_9_enable_eice_step(target_t *target)
1560 {
1561         armv4_5_common_t *armv4_5 = target->arch_info;
1562         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1563
1564         /* setup an inverse breakpoint on the current PC
1565         * - comparator 1 matches the current address
1566         * - rangeout from comparator 1 is connected to comparator 0 rangein
1567         * - comparator 0 matches any address, as long as rangein is low */
1568         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1569         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1570         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
1571         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~(EICE_W_CTRL_RANGE|EICE_W_CTRL_nOPC) & 0xff);
1572         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));
1573         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0);
1574         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffff);
1575         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
1576         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1577 }
1578
1579 void arm7_9_disable_eice_step(target_t *target)
1580 {
1581         armv4_5_common_t *armv4_5 = target->arch_info;
1582         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1583
1584         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
1585         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
1586         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
1587         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
1588         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE]);
1589         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK]);
1590         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK]);
1591         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK]);
1592         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE]);
1593 }
1594
1595 int arm7_9_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
1596 {
1597         armv4_5_common_t *armv4_5 = target->arch_info;
1598         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1599         breakpoint_t *breakpoint = NULL;
1600         int err;
1601
1602         if (target->state != TARGET_HALTED)
1603         {
1604                 LOG_WARNING("target not halted");
1605                 return ERROR_TARGET_NOT_HALTED;
1606         }
1607
1608         /* current = 1: continue on current pc, otherwise continue at <address> */
1609         if (!current)
1610                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1611
1612         /* the front-end may request us not to handle breakpoints */
1613         if (handle_breakpoints)
1614                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1615                         arm7_9_unset_breakpoint(target, breakpoint);
1616
1617         target->debug_reason = DBG_REASON_SINGLESTEP;
1618
1619         arm7_9_restore_context(target);
1620
1621         arm7_9->enable_single_step(target);
1622
1623         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1624         {
1625                 arm7_9->branch_resume(target);
1626         }
1627         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1628         {
1629                 arm7_9->branch_resume_thumb(target);
1630         }
1631         else
1632         {
1633                 LOG_ERROR("unhandled core state");
1634                 return ERROR_FAIL;
1635         }
1636
1637         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1638
1639         err = arm7_9_execute_sys_speed(target);
1640         arm7_9->disable_single_step(target);
1641
1642         /* registers are now invalid */
1643         armv4_5_invalidate_core_regs(target);
1644
1645         if (err != ERROR_OK)
1646         {
1647                 target->state = TARGET_UNKNOWN;
1648         } else {
1649                 arm7_9_debug_entry(target);
1650                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1651                 LOG_DEBUG("target stepped");
1652         }
1653
1654         if (breakpoint)
1655                 arm7_9_set_breakpoint(target, breakpoint);
1656
1657         return err;
1658
1659 }
1660
1661 int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
1662 {
1663         u32* reg_p[16];
1664         u32 value;
1665         int retval;
1666         armv4_5_common_t *armv4_5 = target->arch_info;
1667         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1668
1669         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1670                 return ERROR_FAIL;
1671
1672         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;
1673
1674         if ((num < 0) || (num > 16))
1675                 return ERROR_INVALID_ARGUMENTS;
1676
1677         if ((mode != ARMV4_5_MODE_ANY)
1678                         && (mode != armv4_5->core_mode)
1679                         && (reg_mode != ARMV4_5_MODE_ANY))
1680         {
1681                 u32 tmp_cpsr;
1682
1683                 /* change processor mode (mask T bit) */
1684                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1685                 tmp_cpsr |= mode;
1686                 tmp_cpsr &= ~0x20;
1687                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1688         }
1689
1690         if ((num >= 0) && (num <= 15))
1691         {
1692                 /* read a normal core register */
1693                 reg_p[num] = &value;
1694
1695                 arm7_9->read_core_regs(target, 1 << num, reg_p);
1696         }
1697         else
1698         {
1699                 /* read a program status register
1700                  * if the register mode is MODE_ANY, we read the cpsr, otherwise a spsr
1701                  */
1702                 armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
1703                 int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1704
1705                 arm7_9->read_xpsr(target, &value, spsr);
1706         }
1707
1708         if ((retval = jtag_execute_queue()) != ERROR_OK)
1709         {
1710                 return retval;
1711         }
1712
1713         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
1714         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1715         buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value, 0, 32, value);
1716
1717         if ((mode != ARMV4_5_MODE_ANY)
1718                         && (mode != armv4_5->core_mode)
1719                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1720                 /* restore processor mode (mask T bit) */
1721                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1722         }
1723
1724         return ERROR_OK;
1725
1726 }
1727
1728 int arm7_9_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)
1729 {
1730         u32 reg[16];
1731         armv4_5_common_t *armv4_5 = target->arch_info;
1732         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1733
1734         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1735                 return ERROR_FAIL;
1736
1737         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;
1738
1739         if ((num < 0) || (num > 16))
1740                 return ERROR_INVALID_ARGUMENTS;
1741
1742         if ((mode != ARMV4_5_MODE_ANY)
1743                         && (mode != armv4_5->core_mode)
1744                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1745                 u32 tmp_cpsr;
1746
1747                 /* change processor mode (mask T bit) */
1748                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1749                 tmp_cpsr |= mode;
1750                 tmp_cpsr &= ~0x20;
1751                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1752         }
1753
1754         if ((num >= 0) && (num <= 15))
1755         {
1756                 /* write a normal core register */
1757                 reg[num] = value;
1758
1759                 arm7_9->write_core_regs(target, 1 << num, reg);
1760         }
1761         else
1762         {
1763                 /* write a program status register
1764                 * if the register mode is MODE_ANY, we write the cpsr, otherwise a spsr
1765                 */
1766                 armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
1767                 int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1768
1769                 /* if we're writing the CPSR, mask the T bit */
1770                 if (!spsr)
1771                         value &= ~0x20;
1772
1773                 arm7_9->write_xpsr(target, value, spsr);
1774         }
1775
1776         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
1777         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1778
1779         if ((mode != ARMV4_5_MODE_ANY)
1780                         && (mode != armv4_5->core_mode)
1781                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1782                 /* restore processor mode (mask T bit) */
1783                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1784         }
1785
1786         return jtag_execute_queue();
1787 }
1788
1789 int arm7_9_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1790 {
1791         armv4_5_common_t *armv4_5 = target->arch_info;
1792         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1793
1794         u32 reg[16];
1795         int num_accesses = 0;
1796         int thisrun_accesses;
1797         int i;
1798         u32 cpsr;
1799         int retval;
1800         int last_reg = 0;
1801
1802         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1803
1804         if (target->state != TARGET_HALTED)
1805         {
1806                 LOG_WARNING("target not halted");
1807                 return ERROR_TARGET_NOT_HALTED;
1808         }
1809
1810         /* sanitize arguments */
1811         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1812                 return ERROR_INVALID_ARGUMENTS;
1813
1814         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1815                 return ERROR_TARGET_UNALIGNED_ACCESS;
1816
1817         /* load the base register with the address of the first word */
1818         reg[0] = address;
1819         arm7_9->write_core_regs(target, 0x1, reg);
1820
1821         switch (size)
1822         {
1823                 case 4:
1824                         while (num_accesses < count)
1825                         {
1826                                 u32 reg_list;
1827                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1828                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1829
1830                                 if (last_reg <= thisrun_accesses)
1831                                         last_reg = thisrun_accesses;
1832
1833                                 arm7_9->load_word_regs(target, reg_list);
1834
1835                                 /* fast memory reads are only safe when the target is running
1836                                  * from a sufficiently high clock (32 kHz is usually too slow)
1837                                  */
1838                                 if (arm7_9->fast_memory_access)
1839                                         arm7_9_execute_fast_sys_speed(target);
1840                                 else
1841                                         arm7_9_execute_sys_speed(target);
1842
1843                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 4);
1844
1845                                 /* advance buffer, count number of accesses */
1846                                 buffer += thisrun_accesses * 4;
1847                                 num_accesses += thisrun_accesses;
1848                         }
1849                         break;
1850                 case 2:
1851                         while (num_accesses < count)
1852                         {
1853                                 u32 reg_list;
1854                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1855                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1856
1857                                 for (i = 1; i <= thisrun_accesses; i++)
1858                                 {
1859                                         if (i > last_reg)
1860                                                 last_reg = i;
1861                                         arm7_9->load_hword_reg(target, i);
1862                                         /* fast memory reads are only safe when the target is running
1863                                          * from a sufficiently high clock (32 kHz is usually too slow)
1864                                          */
1865                                         if (arm7_9->fast_memory_access)
1866                                                 arm7_9_execute_fast_sys_speed(target);
1867                                         else
1868                                                 arm7_9_execute_sys_speed(target);
1869                                 }
1870
1871                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 2);
1872
1873                                 /* advance buffer, count number of accesses */
1874                                 buffer += thisrun_accesses * 2;
1875                                 num_accesses += thisrun_accesses;
1876                         }
1877                         break;
1878                 case 1:
1879                         while (num_accesses < count)
1880                         {
1881                                 u32 reg_list;
1882                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1883                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1884
1885                                 for (i = 1; i <= thisrun_accesses; i++)
1886                                 {
1887                                         if (i > last_reg)
1888                                                 last_reg = i;
1889                                         arm7_9->load_byte_reg(target, i);
1890                                         /* fast memory reads are only safe when the target is running
1891                                          * from a sufficiently high clock (32 kHz is usually too slow)
1892                                          */
1893                                         if (arm7_9->fast_memory_access)
1894                                                 arm7_9_execute_fast_sys_speed(target);
1895                                         else
1896                                                 arm7_9_execute_sys_speed(target);
1897                                 }
1898
1899                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 1);
1900
1901                                 /* advance buffer, count number of accesses */
1902                                 buffer += thisrun_accesses * 1;
1903                                 num_accesses += thisrun_accesses;
1904                         }
1905                         break;
1906                 default:
1907                         LOG_ERROR("BUG: we shouldn't get here");
1908                         exit(-1);
1909                         break;
1910         }
1911
1912         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1913                 return ERROR_FAIL;
1914
1915         for (i=0; i<=last_reg; i++)
1916                 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;
1917
1918         arm7_9->read_xpsr(target, &cpsr, 0);
1919         if ((retval = jtag_execute_queue()) != ERROR_OK)
1920         {
1921                 LOG_ERROR("JTAG error while reading cpsr");
1922                 return ERROR_TARGET_DATA_ABORT;
1923         }
1924
1925         if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
1926         {
1927                 LOG_WARNING("memory read caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
1928
1929                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1930
1931                 return ERROR_TARGET_DATA_ABORT;
1932         }
1933
1934         return ERROR_OK;
1935 }
1936
1937 int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1938 {
1939         armv4_5_common_t *armv4_5 = target->arch_info;
1940         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1941         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1942
1943         u32 reg[16];
1944         int num_accesses = 0;
1945         int thisrun_accesses;
1946         int i;
1947         u32 cpsr;
1948         int retval;
1949         int last_reg = 0;
1950
1951 #ifdef _DEBUG_ARM7_9_
1952         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1953 #endif
1954
1955         if (target->state != TARGET_HALTED)
1956         {
1957                 LOG_WARNING("target not halted");
1958                 return ERROR_TARGET_NOT_HALTED;
1959         }
1960
1961         /* sanitize arguments */
1962         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1963                 return ERROR_INVALID_ARGUMENTS;
1964
1965         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1966                 return ERROR_TARGET_UNALIGNED_ACCESS;
1967
1968         /* load the base register with the address of the first word */
1969         reg[0] = address;
1970         arm7_9->write_core_regs(target, 0x1, reg);
1971
1972         /* Clear DBGACK, to make sure memory fetches work as expected */
1973         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1974         embeddedice_store_reg(dbg_ctrl);
1975
1976         switch (size)
1977         {
1978                 case 4:
1979                         while (num_accesses < count)
1980                         {
1981                                 u32 reg_list;
1982                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1983                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1984
1985                                 for (i = 1; i <= thisrun_accesses; i++)
1986                                 {
1987                                         if (i > last_reg)
1988                                                 last_reg = i;
1989                                         reg[i] = target_buffer_get_u32(target, buffer);
1990                                         buffer += 4;
1991                                 }
1992
1993                                 arm7_9->write_core_regs(target, reg_list, reg);
1994
1995                                 arm7_9->store_word_regs(target, reg_list);
1996
1997                                 /* fast memory writes are only safe when the target is running
1998                                  * from a sufficiently high clock (32 kHz is usually too slow)
1999                                  */
2000                                 if (arm7_9->fast_memory_access)
2001                                         arm7_9_execute_fast_sys_speed(target);
2002                                 else
2003                                         arm7_9_execute_sys_speed(target);
2004
2005                                 num_accesses += thisrun_accesses;
2006                         }
2007                         break;
2008                 case 2:
2009                         while (num_accesses < count)
2010                         {
2011                                 u32 reg_list;
2012                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2013                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2014
2015                                 for (i = 1; i <= thisrun_accesses; i++)
2016                                 {
2017                                         if (i > last_reg)
2018                                                 last_reg = i;
2019                                         reg[i] = target_buffer_get_u16(target, buffer) & 0xffff;
2020                                         buffer += 2;
2021                                 }
2022
2023                                 arm7_9->write_core_regs(target, reg_list, reg);
2024
2025                                 for (i = 1; i <= thisrun_accesses; i++)
2026                                 {
2027                                         arm7_9->store_hword_reg(target, i);
2028
2029                                         /* fast memory writes are only safe when the target is running
2030                                          * from a sufficiently high clock (32 kHz is usually too slow)
2031                                          */
2032                                         if (arm7_9->fast_memory_access)
2033                                                 arm7_9_execute_fast_sys_speed(target);
2034                                         else
2035                                                 arm7_9_execute_sys_speed(target);
2036                                 }
2037
2038                                 num_accesses += thisrun_accesses;
2039                         }
2040                         break;
2041                 case 1:
2042                         while (num_accesses < count)
2043                         {
2044                                 u32 reg_list;
2045                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2046                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2047
2048                                 for (i = 1; i <= thisrun_accesses; i++)
2049                                 {
2050                                         if (i > last_reg)
2051                                                 last_reg = i;
2052                                         reg[i] = *buffer++ & 0xff;
2053                                 }
2054
2055                                 arm7_9->write_core_regs(target, reg_list, reg);
2056
2057                                 for (i = 1; i <= thisrun_accesses; i++)
2058                                 {
2059                                         arm7_9->store_byte_reg(target, i);
2060                                         /* fast memory writes are only safe when the target is running
2061                                          * from a sufficiently high clock (32 kHz is usually too slow)
2062                                          */
2063                                         if (arm7_9->fast_memory_access)
2064                                                 arm7_9_execute_fast_sys_speed(target);
2065                                         else
2066                                                 arm7_9_execute_sys_speed(target);
2067                                 }
2068
2069                                 num_accesses += thisrun_accesses;
2070                         }
2071                         break;
2072                 default:
2073                         LOG_ERROR("BUG: we shouldn't get here");
2074                         exit(-1);
2075                         break;
2076         }
2077
2078         /* Re-Set DBGACK */
2079         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
2080         embeddedice_store_reg(dbg_ctrl);
2081
2082         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
2083                 return ERROR_FAIL;
2084
2085         for (i=0; i<=last_reg; i++)
2086                 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;
2087
2088         arm7_9->read_xpsr(target, &cpsr, 0);
2089         if ((retval = jtag_execute_queue()) != ERROR_OK)
2090         {
2091                 LOG_ERROR("JTAG error while reading cpsr");
2092                 return ERROR_TARGET_DATA_ABORT;
2093         }
2094
2095         if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
2096         {
2097                 LOG_WARNING("memory write caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
2098
2099                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
2100
2101                 return ERROR_TARGET_DATA_ABORT;
2102         }
2103
2104         return ERROR_OK;
2105 }
2106
2107 static const u32 dcc_code[] =
2108 {
2109         /* MRC      TST         BNE         MRC         STR         B */
2110         0xee101e10, 0xe3110001, 0x0afffffc, 0xee111e10, 0xe4801004, 0xeafffff9
2111 };
2112
2113 int arm7_9_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)
2114 {
2115         armv4_5_common_t *armv4_5 = target->arch_info;
2116         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
2117         enum armv4_5_state core_state = armv4_5->core_state;
2118         u32 r0 = buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32);
2119         u32 r1 = buf_get_u32(armv4_5->core_cache->reg_list[1].value, 0, 32);
2120         u32 pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
2121         int i;
2122
2123         if (!arm7_9->dcc_downloads)
2124                 return target->type->write_memory(target, address, 4, count, buffer);
2125
2126         /* regrab previously allocated working_area, or allocate a new one */
2127         if (!arm7_9->dcc_working_area)
2128         {
2129                 u8 dcc_code_buf[6 * 4];
2130
2131                 /* make sure we have a working area */
2132                 if (target_alloc_working_area(target, 24, &arm7_9->dcc_working_area) != ERROR_OK)
2133                 {
2134                         LOG_INFO("no working area available, falling back to memory writes");
2135                         return target->type->write_memory(target, address, 4, count, buffer);
2136                 }
2137
2138                 /* copy target instructions to target endianness */
2139                 for (i = 0; i < 6; i++)
2140                 {
2141                         target_buffer_set_u32(target, dcc_code_buf + i*4, dcc_code[i]);
2142                 }
2143
2144                 /* write DCC code to working area */
2145                 target->type->write_memory(target, arm7_9->dcc_working_area->address, 4, 6, dcc_code_buf);
2146         }
2147
2148         buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, address);
2149         armv4_5->core_cache->reg_list[0].valid = 1;
2150         armv4_5->core_cache->reg_list[0].dirty = 1;
2151         armv4_5->core_state = ARMV4_5_STATE_ARM;
2152
2153         arm7_9_resume(target, 0, arm7_9->dcc_working_area->address, 1, 1);
2154
2155         int little=target->endianness==TARGET_LITTLE_ENDIAN;
2156         if (count>2)
2157         {
2158                 /* Handle first & last using standard embeddedice_write_reg and the middle ones w/the
2159                    core function repeated.
2160                  */
2161                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2162                 buffer+=4;
2163
2164                 embeddedice_reg_t *ice_reg = arm7_9->eice_cache->reg_list[EICE_COMMS_DATA].arch_info;
2165                 u8 reg_addr = ice_reg->addr & 0x1f;
2166                 int chain_pos = ice_reg->jtag_info->chain_pos;
2167
2168                 embeddedice_write_dcc(chain_pos, reg_addr, buffer, little, count-2);
2169                 buffer += (count-2)*4;
2170
2171                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2172         } else
2173         {
2174                 for (i = 0; i < count; i++)
2175                 {
2176                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2177                         buffer += 4;
2178                 }
2179         }
2180
2181         target_halt(target);
2182
2183         long long then=timeval_ms();
2184         int timeout;
2185         while (!(timeout=((timeval_ms()-then)>100)))
2186         {
2187                 target_poll(target);
2188                 if (target->state == TARGET_HALTED)
2189                         break;
2190                 if (debug_level>=3)
2191                 {
2192                         alive_sleep(100);
2193                 } else
2194                 {
2195                         keep_alive();
2196                 }
2197         }
2198         if (timeout)
2199         {
2200                 LOG_ERROR("bulk write timed out, target not halted");
2201                 return ERROR_TARGET_TIMEOUT;
2202         }
2203
2204         /* restore target state */
2205         buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, r0);
2206         armv4_5->core_cache->reg_list[0].valid = 1;
2207         armv4_5->core_cache->reg_list[0].dirty = 1;
2208         buf_set_u32(armv4_5->core_cache->reg_list[1].value, 0, 32, r1);
2209         armv4_5->core_cache->reg_list[1].valid = 1;
2210         armv4_5->core_cache->reg_list[1].dirty = 1;
2211         buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, pc);
2212         armv4_5->core_cache->reg_list[15].valid = 1;
2213         armv4_5->core_cache->reg_list[15].dirty = 1;
2214         armv4_5->core_state = core_state;
2215
2216         return ERROR_OK;
2217 }
2218
2219 int arm7_9_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)
2220 {
2221         working_area_t *crc_algorithm;
2222         armv4_5_algorithm_t armv4_5_info;
2223         reg_param_t reg_params[2];
2224         int retval;
2225
2226         u32 arm7_9_crc_code[] = {
2227                 0xE1A02000,                             /* mov          r2, r0 */
2228                 0xE3E00000,                             /* mov          r0, #0xffffffff */
2229                 0xE1A03001,                             /* mov          r3, r1 */
2230                 0xE3A04000,                             /* mov          r4, #0 */
2231                 0xEA00000B,                             /* b            ncomp */
2232                                                                 /* nbyte: */
2233                 0xE7D21004,                             /* ldrb r1, [r2, r4] */
2234                 0xE59F7030,                             /* ldr          r7, CRC32XOR */
2235                 0xE0200C01,                             /* eor          r0, r0, r1, asl 24 */
2236                 0xE3A05000,                             /* mov          r5, #0 */
2237                                                                 /* loop: */
2238                 0xE3500000,                             /* cmp          r0, #0 */
2239                 0xE1A06080,                             /* mov          r6, r0, asl #1 */
2240                 0xE2855001,                             /* add          r5, r5, #1 */
2241                 0xE1A00006,                             /* mov          r0, r6 */
2242                 0xB0260007,                             /* eorlt        r0, r6, r7 */
2243                 0xE3550008,                             /* cmp          r5, #8 */
2244                 0x1AFFFFF8,                             /* bne          loop */
2245                 0xE2844001,                             /* add          r4, r4, #1 */
2246                                                                 /* ncomp: */
2247                 0xE1540003,                             /* cmp          r4, r3 */
2248                 0x1AFFFFF1,                             /* bne          nbyte */
2249                                                                 /* end: */
2250                 0xEAFFFFFE,                             /* b            end */
2251                 0x04C11DB7                              /* CRC32XOR:    .word 0x04C11DB7 */
2252         };
2253
2254         int i;
2255
2256         if (target_alloc_working_area(target, sizeof(arm7_9_crc_code), &crc_algorithm) != ERROR_OK)
2257         {
2258                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2259         }
2260
2261         /* convert flash writing code into a buffer in target endianness */
2262         for (i = 0; i < (sizeof(arm7_9_crc_code)/sizeof(u32)); i++)
2263                 target_write_u32(target, crc_algorithm->address + i*sizeof(u32), arm7_9_crc_code[i]);
2264
2265         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2266         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2267         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2268
2269         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
2270         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
2271
2272         buf_set_u32(reg_params[0].value, 0, 32, address);
2273         buf_set_u32(reg_params[1].value, 0, 32, count);
2274
2275         if ((retval = target->type->run_algorithm(target, 0, NULL, 2, reg_params,
2276                 crc_algorithm->address, crc_algorithm->address + (sizeof(arm7_9_crc_code) - 8), 20000, &armv4_5_info)) != ERROR_OK)
2277         {
2278                 LOG_ERROR("error executing arm7_9 crc algorithm");
2279                 destroy_reg_param(&reg_params[0]);
2280                 destroy_reg_param(&reg_params[1]);
2281                 target_free_working_area(target, crc_algorithm);
2282                 return retval;
2283         }
2284
2285         *checksum = buf_get_u32(reg_params[0].value, 0, 32);
2286
2287         destroy_reg_param(&reg_params[0]);
2288         destroy_reg_param(&reg_params[1]);
2289
2290         target_free_working_area(target, crc_algorithm);
2291
2292         return ERROR_OK;
2293 }
2294
2295 int arm7_9_blank_check_memory(struct target_s *target, u32 address, u32 count, u32* blank)
2296 {
2297         working_area_t *erase_check_algorithm;
2298         reg_param_t reg_params[3];
2299         armv4_5_algorithm_t armv4_5_info;
2300         int retval;
2301         int i;
2302
2303         u32 erase_check_code[] =
2304         {
2305                                                 /* loop: */
2306                 0xe4d03001,             /* ldrb r3, [r0], #1    */
2307                 0xe0022003,             /* and r2, r2, r3               */
2308                 0xe2511001,     /* subs r1, r1, #1              */
2309                 0x1afffffb,             /* bne loop                             */
2310                                                 /* end: */
2311                 0xeafffffe              /* b end                                */
2312         };
2313
2314         /* make sure we have a working area */
2315         if (target_alloc_working_area(target, sizeof(erase_check_code), &erase_check_algorithm) != ERROR_OK)
2316         {
2317                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2318         }
2319
2320         /* convert flash writing code into a buffer in target endianness */
2321         for (i = 0; i < (sizeof(erase_check_code)/sizeof(u32)); i++)
2322                 target_write_u32(target, erase_check_algorithm->address + i*sizeof(u32), erase_check_code[i]);
2323
2324         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2325         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2326         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2327
2328         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
2329         buf_set_u32(reg_params[0].value, 0, 32, address);
2330
2331         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
2332         buf_set_u32(reg_params[1].value, 0, 32, count);
2333
2334         init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
2335         buf_set_u32(reg_params[2].value, 0, 32, 0xff);
2336
2337         if ((retval = target->type->run_algorithm(target, 0, NULL, 3, reg_params,
2338                         erase_check_algorithm->address, erase_check_algorithm->address + (sizeof(erase_check_code) - 4), 10000, &armv4_5_info)) != ERROR_OK)
2339         {
2340                 destroy_reg_param(&reg_params[0]);
2341                 destroy_reg_param(&reg_params[1]);
2342                 destroy_reg_param(&reg_params[2]);
2343                 target_free_working_area(target, erase_check_algorithm);
2344                 return 0;
2345         }
2346
2347         *blank = buf_get_u32(reg_params[2].value, 0, 32);
2348
2349         destroy_reg_param(&reg_params[0]);
2350         destroy_reg_param(&reg_params[1]);
2351         destroy_reg_param(&reg_params[2]);
2352
2353         target_free_working_area(target, erase_check_algorithm);
2354
2355         return ERROR_OK;
2356 }
2357
2358 int arm7_9_register_commands(struct command_context_s *cmd_ctx)
2359 {
2360         command_t *arm7_9_cmd;
2361
2362         arm7_9_cmd = register_command(cmd_ctx, NULL, "arm7_9", NULL, COMMAND_ANY, "arm7/9 specific commands");
2363
2364         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>");
2365         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>");
2366
2367         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>");
2368
2369         register_command(cmd_ctx, arm7_9_cmd, "dbgrq", handle_arm7_9_dbgrq_command,
2370                 COMMAND_ANY, "use EmbeddedICE dbgrq instead of breakpoint for target halt requests <enable|disable>");
2371         register_command(cmd_ctx, arm7_9_cmd, "fast_writes", handle_arm7_9_fast_memory_access_command,
2372                  COMMAND_ANY, "(deprecated, see: arm7_9 fast_memory_access)");
2373         register_command(cmd_ctx, arm7_9_cmd, "fast_memory_access", handle_arm7_9_fast_memory_access_command,
2374                  COMMAND_ANY, "use fast memory accesses instead of slower but potentially unsafe slow accesses <enable|disable>");
2375         register_command(cmd_ctx, arm7_9_cmd, "dcc_downloads", handle_arm7_9_dcc_downloads_command,
2376                 COMMAND_ANY, "use DCC downloads for larger memory writes <enable|disable>");
2377
2378         armv4_5_register_commands(cmd_ctx);
2379
2380         etm_register_commands(cmd_ctx);
2381
2382         return ERROR_OK;
2383 }
2384
2385 int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2386 {
2387         u32 value;
2388         int spsr;
2389         int retval;
2390         target_t *target = get_current_target(cmd_ctx);
2391         armv4_5_common_t *armv4_5;
2392         arm7_9_common_t *arm7_9;
2393
2394         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2395         {
2396                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2397                 return ERROR_OK;
2398         }
2399
2400         if (target->state != TARGET_HALTED)
2401         {
2402                 command_print(cmd_ctx, "can't write registers while running");
2403                 return ERROR_OK;
2404         }
2405
2406         if (argc < 2)
2407         {
2408                 command_print(cmd_ctx, "usage: write_xpsr <value> <not cpsr|spsr>");
2409                 return ERROR_OK;
2410         }
2411
2412         value = strtoul(args[0], NULL, 0);
2413         spsr = strtol(args[1], NULL, 0);
2414
2415         /* if we're writing the CPSR, mask the T bit */
2416         if (!spsr)
2417                 value &= ~0x20;
2418
2419         arm7_9->write_xpsr(target, value, spsr);
2420         if ((retval = jtag_execute_queue()) != ERROR_OK)
2421         {
2422                 LOG_ERROR("JTAG error while writing to xpsr");
2423                 return retval;
2424         }
2425
2426         return ERROR_OK;
2427 }
2428
2429 int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2430 {
2431         u32 value;
2432         int rotate;
2433         int spsr;
2434         int retval;
2435         target_t *target = get_current_target(cmd_ctx);
2436         armv4_5_common_t *armv4_5;
2437         arm7_9_common_t *arm7_9;
2438
2439         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2440         {
2441                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2442                 return ERROR_OK;
2443         }
2444
2445         if (target->state != TARGET_HALTED)
2446         {
2447                 command_print(cmd_ctx, "can't write registers while running");
2448                 return ERROR_OK;
2449         }
2450
2451         if (argc < 3)
2452         {
2453                 command_print(cmd_ctx, "usage: write_xpsr_im8 <im8> <rotate> <not cpsr|spsr>");
2454                 return ERROR_OK;
2455         }
2456
2457         value = strtoul(args[0], NULL, 0);
2458         rotate = strtol(args[1], NULL, 0);
2459         spsr = strtol(args[2], NULL, 0);
2460
2461         arm7_9->write_xpsr_im8(target, value, rotate, spsr);
2462         if ((retval = jtag_execute_queue()) != ERROR_OK)
2463         {
2464                 LOG_ERROR("JTAG error while writing 8-bit immediate to xpsr");
2465                 return retval;
2466         }
2467
2468         return ERROR_OK;
2469 }
2470
2471 int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2472 {
2473         u32 value;
2474         u32 mode;
2475         int num;
2476         target_t *target = get_current_target(cmd_ctx);
2477         armv4_5_common_t *armv4_5;
2478         arm7_9_common_t *arm7_9;
2479
2480         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2481         {
2482                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2483                 return ERROR_OK;
2484         }
2485
2486         if (target->state != TARGET_HALTED)
2487         {
2488                 command_print(cmd_ctx, "can't write registers while running");
2489                 return ERROR_OK;
2490         }
2491
2492         if (argc < 3)
2493         {
2494                 command_print(cmd_ctx, "usage: write_core_reg <num> <mode> <value>");
2495                 return ERROR_OK;
2496         }
2497
2498         num = strtol(args[0], NULL, 0);
2499         mode = strtoul(args[1], NULL, 0);
2500         value = strtoul(args[2], NULL, 0);
2501
2502         arm7_9_write_core_reg(target, num, mode, value);
2503
2504         return ERROR_OK;
2505 }
2506
2507
2508 int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2509 {
2510         target_t *target = get_current_target(cmd_ctx);
2511         armv4_5_common_t *armv4_5;
2512         arm7_9_common_t *arm7_9;
2513
2514         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2515         {
2516                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2517                 return ERROR_OK;
2518         }
2519
2520         if (argc > 0)
2521         {
2522                 if (strcmp("enable", args[0]) == 0)
2523                 {
2524                         arm7_9->use_dbgrq = 1;
2525                 }
2526                 else if (strcmp("disable", args[0]) == 0)
2527                 {
2528                         arm7_9->use_dbgrq = 0;
2529                 }
2530                 else
2531                 {
2532                         command_print(cmd_ctx, "usage: arm7_9 dbgrq <enable|disable>");
2533                 }
2534         }
2535
2536         command_print(cmd_ctx, "use of EmbeddedICE dbgrq instead of breakpoint for target halt %s", (arm7_9->use_dbgrq) ? "enabled" : "disabled");
2537
2538         return ERROR_OK;
2539 }
2540
2541 int handle_arm7_9_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2542 {
2543         target_t *target = get_current_target(cmd_ctx);
2544         armv4_5_common_t *armv4_5;
2545         arm7_9_common_t *arm7_9;
2546
2547         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2548         {
2549                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2550                 return ERROR_OK;
2551         }
2552
2553         if (argc > 0)
2554         {
2555                 if (strcmp("enable", args[0]) == 0)
2556                 {
2557                         arm7_9->fast_memory_access = 1;
2558                 }
2559                 else if (strcmp("disable", args[0]) == 0)
2560                 {
2561                         arm7_9->fast_memory_access = 0;
2562                 }
2563                 else
2564                 {
2565                         command_print(cmd_ctx, "usage: arm7_9 fast_memory_access <enable|disable>");
2566                 }
2567         }
2568
2569         command_print(cmd_ctx, "fast memory access is %s", (arm7_9->fast_memory_access) ? "enabled" : "disabled");
2570
2571         return ERROR_OK;
2572 }
2573
2574 int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2575 {
2576         target_t *target = get_current_target(cmd_ctx);
2577         armv4_5_common_t *armv4_5;
2578         arm7_9_common_t *arm7_9;
2579
2580         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2581         {
2582                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2583                 return ERROR_OK;
2584         }
2585
2586         if (argc > 0)
2587         {
2588                 if (strcmp("enable", args[0]) == 0)
2589                 {
2590                         arm7_9->dcc_downloads = 1;
2591                 }
2592                 else if (strcmp("disable", args[0]) == 0)
2593                 {
2594                         arm7_9->dcc_downloads = 0;
2595                 }
2596                 else
2597                 {
2598                         command_print(cmd_ctx, "usage: arm7_9 dcc_downloads <enable|disable>");
2599                 }
2600         }
2601
2602         command_print(cmd_ctx, "dcc downloads are %s", (arm7_9->dcc_downloads) ? "enabled" : "disabled");
2603
2604         return ERROR_OK;
2605 }
2606
2607 int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9)
2608 {
2609         armv4_5_common_t *armv4_5 = &arm7_9->armv4_5_common;
2610
2611         arm7_9->common_magic = ARM7_9_COMMON_MAGIC;
2612
2613         arm_jtag_setup_connection(&arm7_9->jtag_info);
2614         arm7_9->wp_available = 0; /* this is set up in arm7_9_clear_watchpoints() */
2615         arm7_9->wp_available_max = 2;
2616         arm7_9->sw_breakpoints_added = 0;
2617         arm7_9->breakpoint_count = 0;
2618         arm7_9->wp0_used = 0;
2619         arm7_9->wp1_used = 0;
2620         arm7_9->wp1_used_default = 0;
2621         arm7_9->use_dbgrq = 0;
2622
2623         arm7_9->etm_ctx = NULL;
2624         arm7_9->has_single_step = 0;
2625         arm7_9->has_monitor_mode = 0;
2626         arm7_9->has_vector_catch = 0;
2627
2628         arm7_9->debug_entry_from_reset = 0;
2629
2630         arm7_9->dcc_working_area = NULL;
2631
2632         arm7_9->fast_memory_access = fast_and_dangerous;
2633         arm7_9->dcc_downloads = fast_and_dangerous;
2634
2635         arm7_9->need_bypass_before_restart = 0;
2636
2637         armv4_5->arch_info = arm7_9;
2638         armv4_5->read_core_reg = arm7_9_read_core_reg;
2639         armv4_5->write_core_reg = arm7_9_write_core_reg;
2640         armv4_5->full_context = arm7_9_full_context;
2641
2642         armv4_5_init_arch_info(target, armv4_5);
2643
2644         target_register_timer_callback(arm7_9_handle_target_request, 1, 1, target);
2645
2646         return ERROR_OK;
2647 }