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