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