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