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

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