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

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