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