git.sur5r.net Git - openocd/blob - src/target/arm7_9_common.c

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