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

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