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

   1 /***************************************************************************
   2  *   Copyright (C) 2008 digenius technology GmbH.                          *
   3  *   Michael Bruck                                                         *
   4  *                                                                         *
   5  *   Copyright (C) 2008,2009 Oyvind Harboe oyvind.harboe@zylin.com         *
   6  *                                                                         *
   7  *   Copyright (C) 2008 Georg Acher <acher@in.tum.de>                      *
   8  *                                                                         *
   9  *   This program is free software; you can redistribute it and/or modify  *
  10  *   it under the terms of the GNU General Public License as published by  *
  11  *   the Free Software Foundation; either version 2 of the License, or     *
  12  *   (at your option) any later version.                                   *
  13  *                                                                         *
  14  *   This program is distributed in the hope that it will be useful,       *
  15  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  16  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  17  *   GNU General Public License for more details.                          *
  18  *                                                                         *
  19  *   You should have received a copy of the GNU General Public License     *
  20  *   along with this program; if not, write to the                         *
  21  *   Free Software Foundation, Inc.,                                       *
  22  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  23  ***************************************************************************/
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include "config.h"
  27 #endif
  28
  29 #include "etm.h"
  30 #include "arm11.h"
  31 #include "breakpoints.h"
  32 #include "arm11_dbgtap.h"
  33 #include "arm_simulator.h"
  34 #include "time_support.h"
  35 #include "target_type.h"
  36 #include "algorithm.h"
  37 #include "register.h"
  38
  39
  40 #if 0
  41 #define _DEBUG_INSTRUCTION_EXECUTION_
  42 #endif
  43
  44 #if 0
  45 #define FNC_INFO        LOG_DEBUG("-")
  46 #else
  47 #define FNC_INFO
  48 #endif
  49
  50 #if 1
  51 #define FNC_INFO_NOTIMPLEMENTED do { LOG_DEBUG("NOT IMPLEMENTED"); /*exit(-1);*/ } while (0)
  52 #else
  53 #define FNC_INFO_NOTIMPLEMENTED
  54 #endif
  55
  56 static bool arm11_config_memwrite_burst = true;
  57 static bool arm11_config_memwrite_error_fatal = true;
  58 static uint32_t arm11_vcr = 0;
  59 static bool arm11_config_step_irq_enable = false;
  60 static bool arm11_config_hardware_step = false;
  61
  62 static int arm11_regs_arch_type = -1;
  63
  64 enum arm11_regtype
  65 {
  66         ARM11_REGISTER_CORE,
  67         ARM11_REGISTER_CPSR,
  68
  69         ARM11_REGISTER_FX,
  70         ARM11_REGISTER_FPS,
  71
  72         ARM11_REGISTER_FIQ,
  73         ARM11_REGISTER_SVC,
  74         ARM11_REGISTER_ABT,
  75         ARM11_REGISTER_IRQ,
  76         ARM11_REGISTER_UND,
  77         ARM11_REGISTER_MON,
  78
  79         ARM11_REGISTER_SPSR_FIQ,
  80         ARM11_REGISTER_SPSR_SVC,
  81         ARM11_REGISTER_SPSR_ABT,
  82         ARM11_REGISTER_SPSR_IRQ,
  83         ARM11_REGISTER_SPSR_UND,
  84         ARM11_REGISTER_SPSR_MON,
  85
  86         /* debug regs */
  87         ARM11_REGISTER_DSCR,
  88         ARM11_REGISTER_WDTR,
  89         ARM11_REGISTER_RDTR,
  90 };
  91
  92
  93 struct arm11_reg_defs
  94 {
  95         char *                                  name;
  96         uint32_t                                                num;
  97         int                                             gdb_num;
  98         enum arm11_regtype              type;
  99 };
 100
 101 /* update arm11_regcache_ids when changing this */
 102 static const struct arm11_reg_defs arm11_reg_defs[] =
 103 {
 104         {"r0",  0,      0,      ARM11_REGISTER_CORE},
 105         {"r1",  1,      1,      ARM11_REGISTER_CORE},
 106         {"r2",  2,      2,      ARM11_REGISTER_CORE},
 107         {"r3",  3,      3,      ARM11_REGISTER_CORE},
 108         {"r4",  4,      4,      ARM11_REGISTER_CORE},
 109         {"r5",  5,      5,      ARM11_REGISTER_CORE},
 110         {"r6",  6,      6,      ARM11_REGISTER_CORE},
 111         {"r7",  7,      7,      ARM11_REGISTER_CORE},
 112         {"r8",  8,      8,      ARM11_REGISTER_CORE},
 113         {"r9",  9,      9,      ARM11_REGISTER_CORE},
 114         {"r10", 10,     10,     ARM11_REGISTER_CORE},
 115         {"r11", 11,     11,     ARM11_REGISTER_CORE},
 116         {"r12", 12,     12,     ARM11_REGISTER_CORE},
 117         {"sp",  13,     13,     ARM11_REGISTER_CORE},
 118         {"lr",  14,     14,     ARM11_REGISTER_CORE},
 119         {"pc",  15,     15,     ARM11_REGISTER_CORE},
 120
 121 #if ARM11_REGCACHE_FREGS
 122         {"f0",  0,      16,     ARM11_REGISTER_FX},
 123         {"f1",  1,      17,     ARM11_REGISTER_FX},
 124         {"f2",  2,      18,     ARM11_REGISTER_FX},
 125         {"f3",  3,      19,     ARM11_REGISTER_FX},
 126         {"f4",  4,      20,     ARM11_REGISTER_FX},
 127         {"f5",  5,      21,     ARM11_REGISTER_FX},
 128         {"f6",  6,      22,     ARM11_REGISTER_FX},
 129         {"f7",  7,      23,     ARM11_REGISTER_FX},
 130         {"fps", 0,      24,     ARM11_REGISTER_FPS},
 131 #endif
 132
 133         {"cpsr",        0,      25,     ARM11_REGISTER_CPSR},
 134
 135 #if ARM11_REGCACHE_MODEREGS
 136         {"r8_fiq",      8,      -1,     ARM11_REGISTER_FIQ},
 137         {"r9_fiq",      9,      -1,     ARM11_REGISTER_FIQ},
 138         {"r10_fiq",     10,     -1,     ARM11_REGISTER_FIQ},
 139         {"r11_fiq",     11,     -1,     ARM11_REGISTER_FIQ},
 140         {"r12_fiq",     12,     -1,     ARM11_REGISTER_FIQ},
 141         {"r13_fiq",     13,     -1,     ARM11_REGISTER_FIQ},
 142         {"r14_fiq",     14,     -1,     ARM11_REGISTER_FIQ},
 143         {"spsr_fiq", 0, -1,     ARM11_REGISTER_SPSR_FIQ},
 144
 145         {"r13_svc",     13,     -1,     ARM11_REGISTER_SVC},
 146         {"r14_svc",     14,     -1,     ARM11_REGISTER_SVC},
 147         {"spsr_svc", 0, -1,     ARM11_REGISTER_SPSR_SVC},
 148
 149         {"r13_abt",     13,     -1,     ARM11_REGISTER_ABT},
 150         {"r14_abt",     14,     -1,     ARM11_REGISTER_ABT},
 151         {"spsr_abt", 0, -1,     ARM11_REGISTER_SPSR_ABT},
 152
 153         {"r13_irq",     13,     -1,     ARM11_REGISTER_IRQ},
 154         {"r14_irq",     14,     -1,     ARM11_REGISTER_IRQ},
 155         {"spsr_irq", 0, -1,     ARM11_REGISTER_SPSR_IRQ},
 156
 157         {"r13_und",     13,     -1,     ARM11_REGISTER_UND},
 158         {"r14_und",     14,     -1,     ARM11_REGISTER_UND},
 159         {"spsr_und", 0, -1,     ARM11_REGISTER_SPSR_UND},
 160
 161         /* ARM1176 only */
 162         {"r13_mon",     13,     -1,     ARM11_REGISTER_MON},
 163         {"r14_mon",     14,     -1,     ARM11_REGISTER_MON},
 164         {"spsr_mon", 0, -1,     ARM11_REGISTER_SPSR_MON},
 165 #endif
 166
 167         /* Debug Registers */
 168         {"dscr",        0,      -1,     ARM11_REGISTER_DSCR},
 169         {"wdtr",        0,      -1,     ARM11_REGISTER_WDTR},
 170         {"rdtr",        0,      -1,     ARM11_REGISTER_RDTR},
 171 };
 172
 173 enum arm11_regcache_ids
 174 {
 175         ARM11_RC_R0,
 176         ARM11_RC_RX                     = ARM11_RC_R0,
 177
 178         ARM11_RC_R1,
 179         ARM11_RC_R2,
 180         ARM11_RC_R3,
 181         ARM11_RC_R4,
 182         ARM11_RC_R5,
 183         ARM11_RC_R6,
 184         ARM11_RC_R7,
 185         ARM11_RC_R8,
 186         ARM11_RC_R9,
 187         ARM11_RC_R10,
 188         ARM11_RC_R11,
 189         ARM11_RC_R12,
 190         ARM11_RC_R13,
 191         ARM11_RC_SP                     = ARM11_RC_R13,
 192         ARM11_RC_R14,
 193         ARM11_RC_LR                     = ARM11_RC_R14,
 194         ARM11_RC_R15,
 195         ARM11_RC_PC                     = ARM11_RC_R15,
 196
 197 #if ARM11_REGCACHE_FREGS
 198         ARM11_RC_F0,
 199         ARM11_RC_FX                     = ARM11_RC_F0,
 200         ARM11_RC_F1,
 201         ARM11_RC_F2,
 202         ARM11_RC_F3,
 203         ARM11_RC_F4,
 204         ARM11_RC_F5,
 205         ARM11_RC_F6,
 206         ARM11_RC_F7,
 207         ARM11_RC_FPS,
 208 #endif
 209
 210         ARM11_RC_CPSR,
 211
 212 #if ARM11_REGCACHE_MODEREGS
 213         ARM11_RC_R8_FIQ,
 214         ARM11_RC_R9_FIQ,
 215         ARM11_RC_R10_FIQ,
 216         ARM11_RC_R11_FIQ,
 217         ARM11_RC_R12_FIQ,
 218         ARM11_RC_R13_FIQ,
 219         ARM11_RC_R14_FIQ,
 220         ARM11_RC_SPSR_FIQ,
 221
 222         ARM11_RC_R13_SVC,
 223         ARM11_RC_R14_SVC,
 224         ARM11_RC_SPSR_SVC,
 225
 226         ARM11_RC_R13_ABT,
 227         ARM11_RC_R14_ABT,
 228         ARM11_RC_SPSR_ABT,
 229
 230         ARM11_RC_R13_IRQ,
 231         ARM11_RC_R14_IRQ,
 232         ARM11_RC_SPSR_IRQ,
 233
 234         ARM11_RC_R13_UND,
 235         ARM11_RC_R14_UND,
 236         ARM11_RC_SPSR_UND,
 237
 238         ARM11_RC_R13_MON,
 239         ARM11_RC_R14_MON,
 240         ARM11_RC_SPSR_MON,
 241 #endif
 242
 243         ARM11_RC_DSCR,
 244         ARM11_RC_WDTR,
 245         ARM11_RC_RDTR,
 246
 247         ARM11_RC_MAX,
 248 };
 249
 250 #define ARM11_GDB_REGISTER_COUNT        26
 251
 252 /* FIXME these are *identical* to the ARMv4_5 dummies ...  except
 253  * for their names, and being static vs global, and having different
 254  * addresses.  Ditto ARMv7a and ARMv7m dummies.
 255  */
 256
 257 static uint8_t arm11_gdb_dummy_fp_value[12];
 258
 259 static struct reg arm11_gdb_dummy_fp_reg =
 260 {
 261         .name = "GDB dummy floating-point register",
 262         .value = arm11_gdb_dummy_fp_value,
 263         .dirty = 0,
 264         .valid = 1,
 265         .size = 96,
 266         .arch_info = NULL,
 267         .arch_type = 0,
 268 };
 269
 270 static uint8_t arm11_gdb_dummy_fps_value[4];
 271
 272 static struct reg arm11_gdb_dummy_fps_reg =
 273 {
 274         .name = "GDB dummy floating-point status register",
 275         .value = arm11_gdb_dummy_fps_value,
 276         .dirty = 0,
 277         .valid = 1,
 278         .size = 32,
 279         .arch_info = NULL,
 280         .arch_type = 0,
 281 };
 282
 283
 284 static int arm11_on_enter_debug_state(struct arm11_common *arm11);
 285 static int arm11_step(struct target *target, int current,
 286                 uint32_t address, int handle_breakpoints);
 287 /* helpers */
 288 static int arm11_build_reg_cache(struct target *target);
 289 static int arm11_set_reg(struct reg *reg, uint8_t *buf);
 290 static int arm11_get_reg(struct reg *reg);
 291
 292 static void arm11_record_register_history(struct arm11_common * arm11);
 293 static void arm11_dump_reg_changes(struct arm11_common * arm11);
 294
 295
 296 /** Check and if necessary take control of the system
 297  *
 298  * \param arm11         Target state variable.
 299  * \param dscr          If the current DSCR content is
 300  *                                      available a pointer to a word holding the
 301  *                                      DSCR can be passed. Otherwise use NULL.
 302  */
 303 static int arm11_check_init(struct arm11_common *arm11, uint32_t *dscr)
 304 {
 305         FNC_INFO;
 306
 307         uint32_t                        dscr_local_tmp_copy;
 308
 309         if (!dscr)
 310         {
 311                 dscr = &dscr_local_tmp_copy;
 312
 313                 CHECK_RETVAL(arm11_read_DSCR(arm11, dscr));
 314         }
 315
 316         if (!(*dscr & ARM11_DSCR_MODE_SELECT))
 317         {
 318                 LOG_DEBUG("Bringing target into debug mode");
 319
 320                 *dscr |= ARM11_DSCR_MODE_SELECT;                /* Halt debug-mode */
 321                 arm11_write_DSCR(arm11, *dscr);
 322
 323                 /* add further reset initialization here */
 324
 325                 arm11->simulate_reset_on_next_halt = true;
 326
 327                 if (*dscr & ARM11_DSCR_CORE_HALTED)
 328                 {
 329                         /** \todo TODO: this needs further scrutiny because
 330                           * arm11_on_enter_debug_state() never gets properly called.
 331                           * As a result we don't read the actual register states from
 332                           * the target.
 333                           */
 334
 335                         arm11->target->state    = TARGET_HALTED;
 336                         arm11->target->debug_reason     = arm11_get_DSCR_debug_reason(*dscr);
 337                 }
 338                 else
 339                 {
 340                         arm11->target->state    = TARGET_RUNNING;
 341                         arm11->target->debug_reason     = DBG_REASON_NOTHALTED;
 342                 }
 343
 344                 arm11_sc7_clear_vbw(arm11);
 345         }
 346
 347         return ERROR_OK;
 348 }
 349
 350
 351
 352 #define R(x) \
 353         (arm11->reg_values[ARM11_RC_##x])
 354
 355 /** Save processor state.
 356   *
 357   * This is called when the HALT instruction has succeeded
 358   * or on other occasions that stop the processor.
 359   *
 360   */
 361 static int arm11_on_enter_debug_state(struct arm11_common *arm11)
 362 {
 363         int retval;
 364         FNC_INFO;
 365
 366         for (size_t i = 0; i < ARRAY_SIZE(arm11->reg_values); i++)
 367         {
 368                 arm11->reg_list[i].valid        = 1;
 369                 arm11->reg_list[i].dirty        = 0;
 370         }
 371
 372         /* Save DSCR */
 373         CHECK_RETVAL(arm11_read_DSCR(arm11, &R(DSCR)));
 374
 375         /* Save wDTR */
 376
 377         if (R(DSCR) & ARM11_DSCR_WDTR_FULL)
 378         {
 379                 arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
 380
 381                 arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
 382
 383                 struct scan_field       chain5_fields[3];
 384
 385                 arm11_setup_field(arm11, 32, NULL, &R(WDTR),    chain5_fields + 0);
 386                 arm11_setup_field(arm11,  1, NULL, NULL,                chain5_fields + 1);
 387                 arm11_setup_field(arm11,  1, NULL, NULL,                chain5_fields + 2);
 388
 389                 arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, TAP_DRPAUSE);
 390         }
 391         else
 392         {
 393                 arm11->reg_list[ARM11_RC_WDTR].valid    = 0;
 394         }
 395
 396
 397         /* DSCR: set ARM11_DSCR_EXECUTE_ARM_INSTRUCTION_ENABLE */
 398         /* ARM1176 spec says this is needed only for wDTR/rDTR's "ITR mode", but not to issue ITRs
 399            ARM1136 seems to require this to issue ITR's as well */
 400
 401         uint32_t new_dscr = R(DSCR) | ARM11_DSCR_EXECUTE_ARM_INSTRUCTION_ENABLE;
 402
 403         /* this executes JTAG queue: */
 404
 405         arm11_write_DSCR(arm11, new_dscr);
 406
 407
 408         /* From the spec:
 409            Before executing any instruction in debug state you have to drain the write buffer.
 410            This ensures that no imprecise Data Aborts can return at a later point:*/
 411
 412         /** \todo TODO: Test drain write buffer. */
 413
 414 #if 0
 415         while (1)
 416         {
 417                 /* MRC p14,0,R0,c5,c10,0 */
 418                 //      arm11_run_instr_no_data1(arm11, /*0xee150e1a*/0xe320f000);
 419
 420                 /* mcr     15, 0, r0, cr7, cr10, {4} */
 421                 arm11_run_instr_no_data1(arm11, 0xee070f9a);
 422
 423                 uint32_t dscr = arm11_read_DSCR(arm11);
 424
 425                 LOG_DEBUG("DRAIN, DSCR %08x", dscr);
 426
 427                 if (dscr & ARM11_DSCR_STICKY_IMPRECISE_DATA_ABORT)
 428                 {
 429                         arm11_run_instr_no_data1(arm11, 0xe320f000);
 430
 431                         dscr = arm11_read_DSCR(arm11);
 432
 433                         LOG_DEBUG("DRAIN, DSCR %08x (DONE)", dscr);
 434
 435                         break;
 436                 }
 437         }
 438 #endif
 439
 440         retval = arm11_run_instr_data_prepare(arm11);
 441         if (retval != ERROR_OK)
 442                 return retval;
 443
 444         /* save r0 - r14 */
 445
 446         /** \todo TODO: handle other mode registers */
 447
 448         for (size_t i = 0; i < 15; i++)
 449         {
 450                 /* MCR p14,0,R?,c0,c5,0 */
 451                 retval = arm11_run_instr_data_from_core(arm11, 0xEE000E15 | (i << 12), &R(RX + i), 1);
 452                 if (retval != ERROR_OK)
 453                         return retval;
 454         }
 455
 456         /* save rDTR */
 457
 458         /* check rDTRfull in DSCR */
 459
 460         if (R(DSCR) & ARM11_DSCR_RDTR_FULL)
 461         {
 462                 /* MRC p14,0,R0,c0,c5,0 (move rDTR -> r0 (-> wDTR -> local var)) */
 463                 retval = arm11_run_instr_data_from_core_via_r0(arm11, 0xEE100E15, &R(RDTR));
 464                 if (retval != ERROR_OK)
 465                         return retval;
 466         }
 467         else
 468         {
 469                 arm11->reg_list[ARM11_RC_RDTR].valid    = 0;
 470         }
 471
 472         /* save CPSR */
 473
 474         /* MRS r0,CPSR (move CPSR -> r0 (-> wDTR -> local var)) */
 475         retval = arm11_run_instr_data_from_core_via_r0(arm11, 0xE10F0000, &R(CPSR));
 476         if (retval != ERROR_OK)
 477                 return retval;
 478
 479         /* save PC */
 480
 481         /* MOV R0,PC (move PC -> r0 (-> wDTR -> local var)) */
 482         retval = arm11_run_instr_data_from_core_via_r0(arm11, 0xE1A0000F, &R(PC));
 483         if (retval != ERROR_OK)
 484                 return retval;
 485
 486         /* adjust PC depending on ARM state */
 487
 488         if (R(CPSR) & ARM11_CPSR_J)     /* Java state */
 489         {
 490                 arm11->reg_values[ARM11_RC_PC] -= 0;
 491         }
 492         else if (R(CPSR) & ARM11_CPSR_T)        /* Thumb state */
 493         {
 494                 arm11->reg_values[ARM11_RC_PC] -= 4;
 495         }
 496         else                                    /* ARM state */
 497         {
 498                 arm11->reg_values[ARM11_RC_PC] -= 8;
 499         }
 500
 501         if (arm11->simulate_reset_on_next_halt)
 502         {
 503                 arm11->simulate_reset_on_next_halt = false;
 504
 505                 LOG_DEBUG("Reset c1 Control Register");
 506
 507                 /* Write 0 (reset value) to Control register 0 to disable MMU/Cache etc. */
 508
 509                 /* MCR p15,0,R0,c1,c0,0 */
 510                 retval = arm11_run_instr_data_to_core_via_r0(arm11, 0xee010f10, 0);
 511                 if (retval != ERROR_OK)
 512                         return retval;
 513
 514         }
 515
 516         retval = arm11_run_instr_data_finish(arm11);
 517         if (retval != ERROR_OK)
 518                 return retval;
 519
 520         arm11_dump_reg_changes(arm11);
 521
 522         return ERROR_OK;
 523 }
 524
 525 void arm11_dump_reg_changes(struct arm11_common * arm11)
 526 {
 527
 528         if (!(debug_level >= LOG_LVL_DEBUG))
 529         {
 530                 return;
 531         }
 532
 533         for (size_t i = 0; i < ARM11_REGCACHE_COUNT; i++)
 534         {
 535                 if (!arm11->reg_list[i].valid)
 536                 {
 537                         if (arm11->reg_history[i].valid)
 538                                 LOG_DEBUG("%8s INVALID   (%08" PRIx32 ")", arm11_reg_defs[i].name, arm11->reg_history[i].value);
 539                 }
 540                 else
 541                 {
 542                         if (arm11->reg_history[i].valid)
 543                         {
 544                                 if (arm11->reg_history[i].value != arm11->reg_values[i])
 545                                         LOG_DEBUG("%8s %08" PRIx32 " (%08" PRIx32 ")", arm11_reg_defs[i].name, arm11->reg_values[i], arm11->reg_history[i].value);
 546                         }
 547                         else
 548                         {
 549                                 LOG_DEBUG("%8s %08" PRIx32 " (INVALID)", arm11_reg_defs[i].name, arm11->reg_values[i]);
 550                         }
 551                 }
 552         }
 553 }
 554
 555 /** Restore processor state
 556   *
 557   * This is called in preparation for the RESTART function.
 558   *
 559   */
 560 static int arm11_leave_debug_state(struct arm11_common *arm11)
 561 {
 562         FNC_INFO;
 563         int retval;
 564
 565         retval = arm11_run_instr_data_prepare(arm11);
 566         if (retval != ERROR_OK)
 567                 return retval;
 568
 569         /** \todo TODO: handle other mode registers */
 570
 571         /* restore R1 - R14 */
 572
 573         for (size_t i = 1; i < 15; i++)
 574         {
 575                 if (!arm11->reg_list[ARM11_RC_RX + i].dirty)
 576                         continue;
 577
 578                 /* MRC p14,0,r?,c0,c5,0 */
 579                 arm11_run_instr_data_to_core1(arm11, 0xee100e15 | (i << 12), R(RX + i));
 580
 581                 //      LOG_DEBUG("RESTORE R" ZU " %08x", i, R(RX + i));
 582         }
 583
 584         retval = arm11_run_instr_data_finish(arm11);
 585         if (retval != ERROR_OK)
 586                 return retval;
 587
 588         /* spec says clear wDTR and rDTR; we assume they are clear as
 589            otherwise our programming would be sloppy */
 590         {
 591                 uint32_t DSCR;
 592
 593                 CHECK_RETVAL(arm11_read_DSCR(arm11, &DSCR));
 594
 595                 if (DSCR & (ARM11_DSCR_RDTR_FULL | ARM11_DSCR_WDTR_FULL))
 596                 {
 597                         /*
 598                         The wDTR/rDTR two registers that are used to send/receive data to/from
 599                         the core in tandem with corresponding instruction codes that are
 600                         written into the core. The RDTR FULL/WDTR FULL flag indicates that the
 601                         registers hold data that was written by one side (CPU or JTAG) and not
 602                         read out by the other side.
 603                         */
 604                         LOG_ERROR("wDTR/rDTR inconsistent (DSCR %08" PRIx32 ")", DSCR);
 605                         return ERROR_FAIL;
 606                 }
 607         }
 608
 609         retval = arm11_run_instr_data_prepare(arm11);
 610         if (retval != ERROR_OK)
 611                 return retval;
 612
 613         /* restore original wDTR */
 614
 615         if ((R(DSCR) & ARM11_DSCR_WDTR_FULL) || arm11->reg_list[ARM11_RC_WDTR].dirty)
 616         {
 617                 /* MCR p14,0,R0,c0,c5,0 */
 618                 retval = arm11_run_instr_data_to_core_via_r0(arm11, 0xee000e15, R(WDTR));
 619                 if (retval != ERROR_OK)
 620                         return retval;
 621         }
 622
 623         /* restore CPSR */
 624
 625         /* MSR CPSR,R0*/
 626         retval = arm11_run_instr_data_to_core_via_r0(arm11, 0xe129f000, R(CPSR));
 627         if (retval != ERROR_OK)
 628                 return retval;
 629
 630
 631         /* restore PC */
 632
 633         /* MOV PC,R0 */
 634         retval = arm11_run_instr_data_to_core_via_r0(arm11, 0xe1a0f000, R(PC));
 635         if (retval != ERROR_OK)
 636                 return retval;
 637
 638
 639         /* restore R0 */
 640
 641         /* MRC p14,0,r0,c0,c5,0 */
 642         arm11_run_instr_data_to_core1(arm11, 0xee100e15, R(R0));
 643
 644         retval = arm11_run_instr_data_finish(arm11);
 645         if (retval != ERROR_OK)
 646                 return retval;
 647
 648         /* restore DSCR */
 649
 650         arm11_write_DSCR(arm11, R(DSCR));
 651
 652         /* restore rDTR */
 653
 654         if (R(DSCR) & ARM11_DSCR_RDTR_FULL || arm11->reg_list[ARM11_RC_RDTR].dirty)
 655         {
 656                 arm11_add_debug_SCAN_N(arm11, 0x05, ARM11_TAP_DEFAULT);
 657
 658                 arm11_add_IR(arm11, ARM11_EXTEST, ARM11_TAP_DEFAULT);
 659
 660                 struct scan_field       chain5_fields[3];
 661
 662                 uint8_t                 Ready           = 0;    /* ignored */
 663                 uint8_t                 Valid           = 0;    /* ignored */
 664
 665                 arm11_setup_field(arm11, 32, &R(RDTR),  NULL, chain5_fields + 0);
 666                 arm11_setup_field(arm11,  1, &Ready,    NULL, chain5_fields + 1);
 667                 arm11_setup_field(arm11,  1, &Valid,    NULL, chain5_fields + 2);
 668
 669                 arm11_add_dr_scan_vc(ARRAY_SIZE(chain5_fields), chain5_fields, TAP_DRPAUSE);
 670         }
 671
 672         arm11_record_register_history(arm11);
 673
 674         return ERROR_OK;
 675 }
 676
 677 static void arm11_record_register_history(struct arm11_common *arm11)
 678 {
 679         for (size_t i = 0; i < ARM11_REGCACHE_COUNT; i++)
 680         {
 681                 arm11->reg_history[i].value     = arm11->reg_values[i];
 682                 arm11->reg_history[i].valid     = arm11->reg_list[i].valid;
 683
 684                 arm11->reg_list[i].valid        = 0;
 685                 arm11->reg_list[i].dirty        = 0;
 686         }
 687 }
 688
 689
 690 /* poll current target status */
 691 static int arm11_poll(struct target *target)
 692 {
 693         FNC_INFO;
 694         int retval;
 695         struct arm11_common *arm11 = target_to_arm11(target);
 696         uint32_t        dscr;
 697
 698         CHECK_RETVAL(arm11_read_DSCR(arm11, &dscr));
 699
 700         LOG_DEBUG("DSCR %08" PRIx32 "", dscr);
 701
 702         CHECK_RETVAL(arm11_check_init(arm11, &dscr));
 703
 704         if (dscr & ARM11_DSCR_CORE_HALTED)
 705         {
 706                 if (target->state != TARGET_HALTED)
 707                 {
 708                         enum target_state old_state = target->state;
 709
 710                         LOG_DEBUG("enter TARGET_HALTED");
 711                         target->state                   = TARGET_HALTED;
 712                         target->debug_reason    = arm11_get_DSCR_debug_reason(dscr);
 713                         retval = arm11_on_enter_debug_state(arm11);
 714                         if (retval != ERROR_OK)
 715                                 return retval;
 716
 717                         target_call_event_callbacks(target,
 718                                 old_state == TARGET_DEBUG_RUNNING ? TARGET_EVENT_DEBUG_HALTED : TARGET_EVENT_HALTED);
 719                 }
 720         }
 721         else
 722         {
 723                 if (target->state != TARGET_RUNNING && target->state != TARGET_DEBUG_RUNNING)
 724                 {
 725                         LOG_DEBUG("enter TARGET_RUNNING");
 726                         target->state                   = TARGET_RUNNING;
 727                         target->debug_reason    = DBG_REASON_NOTHALTED;
 728                 }
 729         }
 730
 731         return ERROR_OK;
 732 }
 733 /* architecture specific status reply */
 734 static int arm11_arch_state(struct target *target)
 735 {
 736         struct arm11_common *arm11 = target_to_arm11(target);
 737
 738         LOG_USER("target halted due to %s\ncpsr: 0x%8.8" PRIx32 " pc: 0x%8.8" PRIx32 "",
 739                          Jim_Nvp_value2name_simple(nvp_target_debug_reason, target->debug_reason)->name,
 740                          R(CPSR),
 741                          R(PC));
 742
 743         return ERROR_OK;
 744 }
 745
 746 /* target request support */
 747 static int arm11_target_request_data(struct target *target,
 748                 uint32_t size, uint8_t *buffer)
 749 {
 750         FNC_INFO_NOTIMPLEMENTED;
 751
 752         return ERROR_OK;
 753 }
 754
 755 /* target execution control */
 756 static int arm11_halt(struct target *target)
 757 {
 758         FNC_INFO;
 759         struct arm11_common *arm11 = target_to_arm11(target);
 760
 761         LOG_DEBUG("target->state: %s",
 762                 target_state_name(target));
 763
 764         if (target->state == TARGET_UNKNOWN)
 765         {
 766                 arm11->simulate_reset_on_next_halt = true;
 767         }
 768
 769         if (target->state == TARGET_HALTED)
 770         {
 771                 LOG_DEBUG("target was already halted");
 772                 return ERROR_OK;
 773         }
 774
 775         arm11_add_IR(arm11, ARM11_HALT, TAP_IDLE);
 776
 777         CHECK_RETVAL(jtag_execute_queue());
 778
 779         uint32_t dscr;
 780
 781         int i = 0;
 782         while (1)
 783         {
 784                 CHECK_RETVAL(arm11_read_DSCR(arm11, &dscr));
 785
 786                 if (dscr & ARM11_DSCR_CORE_HALTED)
 787                         break;
 788
 789
 790                 long long then = 0;
 791                 if (i == 1000)
 792                 {
 793                         then = timeval_ms();
 794                 }
 795                 if (i >= 1000)
 796                 {
 797                         if ((timeval_ms()-then) > 1000)
 798                         {
 799                                 LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 800                                 return ERROR_FAIL;
 801                         }
 802                 }
 803                 i++;
 804         }
 805
 806         arm11_on_enter_debug_state(arm11);
 807
 808         enum target_state old_state     = target->state;
 809
 810         target->state           = TARGET_HALTED;
 811         target->debug_reason    = arm11_get_DSCR_debug_reason(dscr);
 812
 813         CHECK_RETVAL(
 814                 target_call_event_callbacks(target,
 815                         old_state == TARGET_DEBUG_RUNNING ? TARGET_EVENT_DEBUG_HALTED : TARGET_EVENT_HALTED));
 816
 817         return ERROR_OK;
 818 }
 819
 820 static int arm11_resume(struct target *target, int current,
 821                 uint32_t address, int handle_breakpoints, int debug_execution)
 822 {
 823         FNC_INFO;
 824
 825         //        LOG_DEBUG("current %d  address %08x  handle_breakpoints %d  debug_execution %d",
 826         //      current, address, handle_breakpoints, debug_execution);
 827
 828         struct arm11_common *arm11 = target_to_arm11(target);
 829
 830         LOG_DEBUG("target->state: %s",
 831                 target_state_name(target));
 832
 833
 834         if (target->state != TARGET_HALTED)
 835         {
 836                 LOG_ERROR("Target not halted");
 837                 return ERROR_TARGET_NOT_HALTED;
 838         }
 839
 840         if (!current)
 841                 R(PC) = address;
 842
 843         LOG_DEBUG("RESUME PC %08" PRIx32 "%s", R(PC), !current ? "!" : "");
 844
 845         /* clear breakpoints/watchpoints and VCR*/
 846         arm11_sc7_clear_vbw(arm11);
 847
 848         /* Set up breakpoints */
 849         if (!debug_execution)
 850         {
 851                 /* check if one matches PC and step over it if necessary */
 852
 853                 struct breakpoint *     bp;
 854
 855                 for (bp = target->breakpoints; bp; bp = bp->next)
 856                 {
 857                         if (bp->address == R(PC))
 858                         {
 859                                 LOG_DEBUG("must step over %08" PRIx32 "", bp->address);
 860                                 arm11_step(target, 1, 0, 0);
 861                                 break;
 862                         }
 863                 }
 864
 865                 /* set all breakpoints */
 866
 867                 size_t          brp_num = 0;
 868
 869                 for (bp = target->breakpoints; bp; bp = bp->next)
 870                 {
 871                         struct arm11_sc7_action brp[2];
 872
 873                         brp[0].write    = 1;
 874                         brp[0].address  = ARM11_SC7_BVR0 + brp_num;
 875                         brp[0].value    = bp->address;
 876                         brp[1].write    = 1;
 877                         brp[1].address  = ARM11_SC7_BCR0 + brp_num;
 878                         brp[1].value    = 0x1 | (3 << 1) | (0x0F << 5) | (0 << 14) | (0 << 16) | (0 << 20) | (0 << 21);
 879
 880                         arm11_sc7_run(arm11, brp, ARRAY_SIZE(brp));
 881
 882                         LOG_DEBUG("Add BP " ZU " at %08" PRIx32 "", brp_num, bp->address);
 883
 884                         brp_num++;
 885                 }
 886
 887                 arm11_sc7_set_vcr(arm11, arm11_vcr);
 888         }
 889
 890         arm11_leave_debug_state(arm11);
 891
 892         arm11_add_IR(arm11, ARM11_RESTART, TAP_IDLE);
 893
 894         CHECK_RETVAL(jtag_execute_queue());
 895
 896         int i = 0;
 897         while (1)
 898         {
 899                 uint32_t dscr;
 900
 901                 CHECK_RETVAL(arm11_read_DSCR(arm11, &dscr));
 902
 903                 LOG_DEBUG("DSCR %08" PRIx32 "", dscr);
 904
 905                 if (dscr & ARM11_DSCR_CORE_RESTARTED)
 906                         break;
 907
 908
 909                 long long then = 0;
 910                 if (i == 1000)
 911                 {
 912                         then = timeval_ms();
 913                 }
 914                 if (i >= 1000)
 915                 {
 916                         if ((timeval_ms()-then) > 1000)
 917                         {
 918                                 LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
 919                                 return ERROR_FAIL;
 920                         }
 921                 }
 922                 i++;
 923         }
 924
 925         if (!debug_execution)
 926         {
 927                 target->state                   = TARGET_RUNNING;
 928                 target->debug_reason    = DBG_REASON_NOTHALTED;
 929
 930                 CHECK_RETVAL(target_call_event_callbacks(target, TARGET_EVENT_RESUMED));
 931         }
 932         else
 933         {
 934                 target->state                   = TARGET_DEBUG_RUNNING;
 935                 target->debug_reason    = DBG_REASON_NOTHALTED;
 936
 937                 CHECK_RETVAL(target_call_event_callbacks(target, TARGET_EVENT_RESUMED));
 938         }
 939
 940         return ERROR_OK;
 941 }
 942
 943
 944 static int armv4_5_to_arm11(int reg)
 945 {
 946         if (reg < 16)
 947                 return reg;
 948         switch (reg)
 949         {
 950         case ARMV4_5_CPSR:
 951                 return ARM11_RC_CPSR;
 952         case 16:
 953                 /* FIX!!! handle thumb better! */
 954                 return ARM11_RC_CPSR;
 955         default:
 956                 LOG_ERROR("BUG: register translation from armv4_5 to arm11 not supported %d", reg);
 957                 exit(-1);
 958         }
 959 }
 960
 961
 962 static uint32_t arm11_sim_get_reg(struct arm_sim_interface *sim, int reg)
 963 {
 964         struct arm11_common * arm11 = (struct arm11_common *)sim->user_data;
 965
 966         reg=armv4_5_to_arm11(reg);
 967
 968         return buf_get_u32(arm11->reg_list[reg].value, 0, 32);
 969 }
 970
 971 static void arm11_sim_set_reg(struct arm_sim_interface *sim,
 972                 int reg, uint32_t value)
 973 {
 974         struct arm11_common * arm11 = (struct arm11_common *)sim->user_data;
 975
 976         reg=armv4_5_to_arm11(reg);
 977
 978         buf_set_u32(arm11->reg_list[reg].value, 0, 32, value);
 979 }
 980
 981 static uint32_t arm11_sim_get_cpsr(struct arm_sim_interface *sim,
 982                 int pos, int bits)
 983 {
 984         struct arm11_common * arm11 = (struct arm11_common *)sim->user_data;
 985
 986         return buf_get_u32(arm11->reg_list[ARM11_RC_CPSR].value, pos, bits);
 987 }
 988
 989 static enum armv4_5_state arm11_sim_get_state(struct arm_sim_interface *sim)
 990 {
 991 //      struct arm11_common * arm11 = (struct arm11_common *)sim->user_data;
 992
 993         /* FIX!!!! we should implement thumb for arm11 */
 994         return ARMV4_5_STATE_ARM;
 995 }
 996
 997 static void arm11_sim_set_state(struct arm_sim_interface *sim,
 998                 enum armv4_5_state mode)
 999 {
1000 //      struct arm11_common * arm11 = (struct arm11_common *)sim->user_data;
1001
1002         /* FIX!!!! we should implement thumb for arm11 */
1003         LOG_ERROR("Not implemetned!");
1004 }
1005
1006
1007 static enum armv4_5_mode arm11_sim_get_mode(struct arm_sim_interface *sim)
1008 {
1009         //struct arm11_common * arm11 = (struct arm11_common *)sim->user_data;
1010
1011         /* FIX!!!! we should implement something that returns the current mode here!!! */
1012         return ARMV4_5_MODE_USR;
1013 }
1014
1015 static int arm11_simulate_step(struct target *target, uint32_t *dry_run_pc)
1016 {
1017         struct arm_sim_interface sim;
1018
1019         sim.user_data=target->arch_info;
1020         sim.get_reg=&arm11_sim_get_reg;
1021         sim.set_reg=&arm11_sim_set_reg;
1022         sim.get_reg_mode=&arm11_sim_get_reg;
1023         sim.set_reg_mode=&arm11_sim_set_reg;
1024         sim.get_cpsr=&arm11_sim_get_cpsr;
1025         sim.get_mode=&arm11_sim_get_mode;
1026         sim.get_state=&arm11_sim_get_state;
1027         sim.set_state=&arm11_sim_set_state;
1028
1029         return arm_simulate_step_core(target, dry_run_pc, &sim);
1030
1031 }
1032
1033 static int arm11_step(struct target *target, int current,
1034                 uint32_t address, int handle_breakpoints)
1035 {
1036         FNC_INFO;
1037
1038         LOG_DEBUG("target->state: %s",
1039                 target_state_name(target));
1040
1041         if (target->state != TARGET_HALTED)
1042         {
1043                 LOG_WARNING("target was not halted");
1044                 return ERROR_TARGET_NOT_HALTED;
1045         }
1046
1047         struct arm11_common *arm11 = target_to_arm11(target);
1048
1049         if (!current)
1050                 R(PC) = address;
1051
1052         LOG_DEBUG("STEP PC %08" PRIx32 "%s", R(PC), !current ? "!" : "");
1053
1054
1055         /** \todo TODO: Thumb not supported here */
1056
1057         uint32_t        next_instruction;
1058
1059         CHECK_RETVAL(arm11_read_memory_word(arm11, R(PC), &next_instruction));
1060
1061         /* skip over BKPT */
1062         if ((next_instruction & 0xFFF00070) == 0xe1200070)
1063         {
1064                 R(PC) += 4;
1065                 arm11->reg_list[ARM11_RC_PC].valid = 1;
1066                 arm11->reg_list[ARM11_RC_PC].dirty = 0;
1067                 LOG_DEBUG("Skipping BKPT");
1068         }
1069         /* skip over Wait for interrupt / Standby */
1070         /* mcr  15, 0, r?, cr7, cr0, {4} */
1071         else if ((next_instruction & 0xFFFF0FFF) == 0xee070f90)
1072         {
1073                 R(PC) += 4;
1074                 arm11->reg_list[ARM11_RC_PC].valid = 1;
1075                 arm11->reg_list[ARM11_RC_PC].dirty = 0;
1076                 LOG_DEBUG("Skipping WFI");
1077         }
1078         /* ignore B to self */
1079         else if ((next_instruction & 0xFEFFFFFF) == 0xeafffffe)
1080         {
1081                 LOG_DEBUG("Not stepping jump to self");
1082         }
1083         else
1084         {
1085                 /** \todo TODO: check if break-/watchpoints make any sense at all in combination
1086                 * with this. */
1087
1088                 /** \todo TODO: check if disabling IRQs might be a good idea here. Alternatively
1089                 * the VCR might be something worth looking into. */
1090
1091
1092                 /* Set up breakpoint for stepping */
1093
1094                 struct arm11_sc7_action brp[2];
1095
1096                 brp[0].write    = 1;
1097                 brp[0].address  = ARM11_SC7_BVR0;
1098                 brp[1].write    = 1;
1099                 brp[1].address  = ARM11_SC7_BCR0;
1100
1101                 if (arm11_config_hardware_step)
1102                 {
1103                         /* hardware single stepping be used if possible or is it better to
1104                          * always use the same code path? Hardware single stepping is not supported
1105                          * on all hardware
1106                          */
1107                          brp[0].value   = R(PC);
1108                          brp[1].value   = 0x1 | (3 << 1) | (0x0F << 5) | (0 << 14) | (0 << 16) | (0 << 20) | (2 << 21);
1109                 } else
1110                 {
1111                         /* sets a breakpoint on the next PC(calculated by simulation),
1112                          */
1113                         uint32_t next_pc;
1114                         int retval;
1115                         retval = arm11_simulate_step(target, &next_pc);
1116                         if (retval != ERROR_OK)
1117                                 return retval;
1118
1119                         brp[0].value    = next_pc;
1120                         brp[1].value    = 0x1 | (3 << 1) | (0x0F << 5) | (0 << 14) | (0 << 16) | (0 << 20) | (0 << 21);
1121                 }
1122
1123                 CHECK_RETVAL(arm11_sc7_run(arm11, brp, ARRAY_SIZE(brp)));
1124
1125                 /* resume */
1126
1127
1128                 if (arm11_config_step_irq_enable)
1129                         R(DSCR) &= ~ARM11_DSCR_INTERRUPTS_DISABLE;              /* should be redundant */
1130                 else
1131                         R(DSCR) |= ARM11_DSCR_INTERRUPTS_DISABLE;
1132
1133
1134                 CHECK_RETVAL(arm11_leave_debug_state(arm11));
1135
1136                 arm11_add_IR(arm11, ARM11_RESTART, TAP_IDLE);
1137
1138                 CHECK_RETVAL(jtag_execute_queue());
1139
1140                 /* wait for halt */
1141                 int i = 0;
1142                 while (1)
1143                 {
1144                         uint32_t dscr;
1145
1146                         CHECK_RETVAL(arm11_read_DSCR(arm11, &dscr));
1147
1148                         LOG_DEBUG("DSCR %08" PRIx32 "e", dscr);
1149
1150                         if ((dscr & (ARM11_DSCR_CORE_RESTARTED | ARM11_DSCR_CORE_HALTED)) ==
1151                                 (ARM11_DSCR_CORE_RESTARTED | ARM11_DSCR_CORE_HALTED))
1152                                 break;
1153
1154                         long long then = 0;
1155                         if (i == 1000)
1156                         {
1157                                 then = timeval_ms();
1158                         }
1159                         if (i >= 1000)
1160                         {
1161                                 if ((timeval_ms()-then) > 1000)
1162                                 {
1163                                         LOG_WARNING("Timeout (1000ms) waiting for instructions to complete");
1164                                         return ERROR_FAIL;
1165                                 }
1166                         }
1167                         i++;
1168                 }
1169
1170                 /* clear breakpoint */
1171                 arm11_sc7_clear_vbw(arm11);
1172
1173                 /* save state */
1174                 CHECK_RETVAL(arm11_on_enter_debug_state(arm11));
1175
1176             /* restore default state */
1177                 R(DSCR) &= ~ARM11_DSCR_INTERRUPTS_DISABLE;
1178
1179         }
1180
1181         //        target->state         = TARGET_HALTED;
1182         target->debug_reason    = DBG_REASON_SINGLESTEP;
1183
1184         CHECK_RETVAL(target_call_event_callbacks(target, TARGET_EVENT_HALTED));
1185
1186         return ERROR_OK;
1187 }
1188
1189 static int arm11_assert_reset(struct target *target)
1190 {
1191         FNC_INFO;
1192         int retval;
1193         struct arm11_common *arm11 = target_to_arm11(target);
1194
1195         retval = arm11_check_init(arm11, NULL);
1196         if (retval != ERROR_OK)
1197                 return retval;
1198
1199         target->state = TARGET_UNKNOWN;
1200
1201         /* we would very much like to reset into the halted, state,
1202          * but resetting and halting is second best... */
1203         if (target->reset_halt)
1204         {
1205                 CHECK_RETVAL(target_halt(target));
1206         }
1207
1208
1209         /* srst is funny. We can not do *anything* else while it's asserted
1210          * and it has unkonwn side effects. Make sure no other code runs
1211          * meanwhile.
1212          *
1213          * Code below assumes srst:
1214          *
1215          * - Causes power-on-reset (but of what parts of the system?). Bug
1216          * in arm11?
1217          *
1218          * - Messes us TAP state without asserting trst.
1219          *
1220          * - There is another bug in the arm11 core. When you generate an access to
1221          * external logic (for example ddr controller via AHB bus) and that block
1222          * is not configured (perhaps it is still held in reset), that transaction
1223          * will never complete. This will hang arm11 core but it will also hang
1224          * JTAG controller. Nothing, short of srst assertion will bring it out of
1225          * this.
1226          *
1227          * Mysteries:
1228          *
1229          * - What should the PC be after an srst reset when starting in the halted
1230          * state?
1231          */
1232
1233         jtag_add_reset(0, 1);
1234         jtag_add_reset(0, 0);
1235
1236         /* How long do we have to wait? */
1237         jtag_add_sleep(5000);
1238
1239         /* un-mess up TAP state */
1240         jtag_add_tlr();
1241
1242         retval = jtag_execute_queue();
1243         if (retval != ERROR_OK)
1244         {
1245                 return retval;
1246         }
1247
1248         return ERROR_OK;
1249 }
1250
1251 static int arm11_deassert_reset(struct target *target)
1252 {
1253         return ERROR_OK;
1254 }
1255
1256 static int arm11_soft_reset_halt(struct target *target)
1257 {
1258         FNC_INFO_NOTIMPLEMENTED;
1259
1260         return ERROR_OK;
1261 }
1262
1263 /* target register access for gdb */
1264 static int arm11_get_gdb_reg_list(struct target *target,
1265                 struct reg **reg_list[], int *reg_list_size)
1266 {
1267         FNC_INFO;
1268         struct arm11_common *arm11 = target_to_arm11(target);
1269
1270         *reg_list_size  = ARM11_GDB_REGISTER_COUNT;
1271         *reg_list               = malloc(sizeof(struct reg*) * ARM11_GDB_REGISTER_COUNT);
1272
1273         for (size_t i = 16; i < 24; i++)
1274         {
1275                 (*reg_list)[i] = &arm11_gdb_dummy_fp_reg;
1276         }
1277
1278         (*reg_list)[24] = &arm11_gdb_dummy_fps_reg;
1279
1280         for (size_t i = 0; i < ARM11_REGCACHE_COUNT; i++)
1281         {
1282                 if (arm11_reg_defs[i].gdb_num == -1)
1283                         continue;
1284
1285                 (*reg_list)[arm11_reg_defs[i].gdb_num] = arm11->reg_list + i;
1286         }
1287
1288         return ERROR_OK;
1289 }
1290
1291 /* target memory access
1292  * size: 1 = byte (8bit), 2 = half-word (16bit), 4 = word (32bit)
1293  * count: number of items of <size>
1294  *
1295  * arm11_config_memrw_no_increment - in the future we may want to be able
1296  * to read/write a range of data to a "port". a "port" is an action on
1297  * read memory address for some peripheral.
1298  */
1299 static int arm11_read_memory_inner(struct target *target,
1300                 uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer,
1301                 bool arm11_config_memrw_no_increment)
1302 {
1303         /** \todo TODO: check if buffer cast to uint32_t* and uint16_t* might cause alignment problems */
1304         int retval;
1305
1306         FNC_INFO;
1307
1308         if (target->state != TARGET_HALTED)
1309         {
1310                 LOG_WARNING("target was not halted");
1311                 return ERROR_TARGET_NOT_HALTED;
1312         }
1313
1314         LOG_DEBUG("ADDR %08" PRIx32 "  SIZE %08" PRIx32 "  COUNT %08" PRIx32 "", address, size, count);
1315
1316         struct arm11_common *arm11 = target_to_arm11(target);
1317
1318         retval = arm11_run_instr_data_prepare(arm11);
1319         if (retval != ERROR_OK)
1320                 return retval;
1321
1322         /* MRC p14,0,r0,c0,c5,0 */
1323         retval = arm11_run_instr_data_to_core1(arm11, 0xee100e15, address);
1324         if (retval != ERROR_OK)
1325                 return retval;
1326
1327         switch (size)
1328         {
1329         case 1:
1330                 /** \todo TODO: check if dirty is the right choice to force a rewrite on arm11_resume() */
1331                 arm11->reg_list[ARM11_RC_R1].dirty = 1;
1332
1333                 for (size_t i = 0; i < count; i++)
1334                 {
1335                         /* ldrb    r1, [r0], #1 */
1336                         /* ldrb    r1, [r0] */
1337                         arm11_run_instr_no_data1(arm11,
1338                                         !arm11_config_memrw_no_increment ? 0xe4d01001 : 0xe5d01000);
1339
1340                         uint32_t res;
1341                         /* MCR p14,0,R1,c0,c5,0 */
1342                         arm11_run_instr_data_from_core(arm11, 0xEE001E15, &res, 1);
1343
1344                         *buffer++ = res;
1345                 }
1346
1347                 break;
1348
1349         case 2:
1350                 {
1351                         arm11->reg_list[ARM11_RC_R1].dirty = 1;
1352
1353                         for (size_t i = 0; i < count; i++)
1354                         {
1355                                 /* ldrh    r1, [r0], #2 */
1356                                 arm11_run_instr_no_data1(arm11,
1357                                         !arm11_config_memrw_no_increment ? 0xe0d010b2 : 0xe1d010b0);
1358
1359                                 uint32_t res;
1360
1361                                 /* MCR p14,0,R1,c0,c5,0 */
1362                                 arm11_run_instr_data_from_core(arm11, 0xEE001E15, &res, 1);
1363
1364                                 uint16_t svalue = res;
1365                                 memcpy(buffer + i * sizeof(uint16_t), &svalue, sizeof(uint16_t));
1366                         }
1367
1368                         break;
1369                 }
1370
1371         case 4:
1372                 {
1373                 uint32_t instr = !arm11_config_memrw_no_increment ? 0xecb05e01 : 0xed905e00;
1374                 /** \todo TODO: buffer cast to uint32_t* causes alignment warnings */
1375                 uint32_t *words = (uint32_t *)buffer;
1376
1377                 /* LDC p14,c5,[R0],#4 */
1378                 /* LDC p14,c5,[R0] */
1379                 arm11_run_instr_data_from_core(arm11, instr, words, count);
1380                 break;
1381                 }
1382         }
1383
1384         return arm11_run_instr_data_finish(arm11);
1385 }
1386
1387 static int arm11_read_memory(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
1388 {
1389         return arm11_read_memory_inner(target, address, size, count, buffer, false);
1390 }
1391
1392 /*
1393 * arm11_config_memrw_no_increment - in the future we may want to be able
1394 * to read/write a range of data to a "port". a "port" is an action on
1395 * read memory address for some peripheral.
1396 */
1397 static int arm11_write_memory_inner(struct target *target,
1398                 uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer,
1399                 bool arm11_config_memrw_no_increment)
1400 {
1401         int retval;
1402         FNC_INFO;
1403
1404         if (target->state != TARGET_HALTED)
1405         {
1406                 LOG_WARNING("target was not halted");
1407                 return ERROR_TARGET_NOT_HALTED;
1408         }
1409
1410         LOG_DEBUG("ADDR %08" PRIx32 "  SIZE %08" PRIx32 "  COUNT %08" PRIx32 "", address, size, count);
1411
1412         struct arm11_common *arm11 = target_to_arm11(target);
1413
1414         retval = arm11_run_instr_data_prepare(arm11);
1415         if (retval != ERROR_OK)
1416                 return retval;
1417
1418         /* MRC p14,0,r0,c0,c5,0 */
1419         retval = arm11_run_instr_data_to_core1(arm11, 0xee100e15, address);
1420         if (retval != ERROR_OK)
1421                 return retval;
1422
1423         /* burst writes are not used for single words as those may well be
1424          * reset init script writes.
1425          *
1426          * The other advantage is that as burst writes are default, we'll
1427          * now exercise both burst and non-burst code paths with the
1428          * default settings, increasing code coverage.
1429          */
1430         bool burst = arm11_config_memwrite_burst && (count > 1);
1431
1432         switch (size)
1433         {
1434         case 1:
1435                 {
1436                         arm11->reg_list[ARM11_RC_R1].dirty = 1;
1437
1438                         for (size_t i = 0; i < count; i++)
1439                         {
1440                                 /* MRC p14,0,r1,c0,c5,0 */
1441                                 retval = arm11_run_instr_data_to_core1(arm11, 0xee101e15, *buffer++);
1442                                 if (retval != ERROR_OK)
1443                                         return retval;
1444
1445                                 /* strb    r1, [r0], #1 */
1446                                 /* strb    r1, [r0] */
1447                                 retval = arm11_run_instr_no_data1(arm11,
1448                                         !arm11_config_memrw_no_increment ? 0xe4c01001 : 0xe5c01000);
1449                                 if (retval != ERROR_OK)
1450                                         return retval;
1451                         }
1452
1453                         break;
1454                 }
1455
1456         case 2:
1457                 {
1458                         arm11->reg_list[ARM11_RC_R1].dirty = 1;
1459
1460                         for (size_t i = 0; i < count; i++)
1461                         {
1462                                 uint16_t value;
1463                                 memcpy(&value, buffer + i * sizeof(uint16_t), sizeof(uint16_t));
1464
1465                                 /* MRC p14,0,r1,c0,c5,0 */
1466                                 retval = arm11_run_instr_data_to_core1(arm11, 0xee101e15, value);
1467                                 if (retval != ERROR_OK)
1468                                         return retval;
1469
1470                                 /* strh    r1, [r0], #2 */
1471                                 /* strh    r1, [r0] */
1472                                 retval = arm11_run_instr_no_data1(arm11,
1473                                         !arm11_config_memrw_no_increment ? 0xe0c010b2 : 0xe1c010b0);
1474                                 if (retval != ERROR_OK)
1475                                         return retval;
1476                         }
1477
1478                         break;
1479                 }
1480
1481         case 4: {
1482                 uint32_t instr = !arm11_config_memrw_no_increment ? 0xeca05e01 : 0xed805e00;
1483
1484                 /** \todo TODO: buffer cast to uint32_t* causes alignment warnings */
1485                 uint32_t *words = (uint32_t*)buffer;
1486
1487                 if (!burst)
1488                 {
1489                         /* STC p14,c5,[R0],#4 */
1490                         /* STC p14,c5,[R0]*/
1491                         retval = arm11_run_instr_data_to_core(arm11, instr, words, count);
1492                         if (retval != ERROR_OK)
1493                                 return retval;
1494                 }
1495                 else
1496                 {
1497                         /* STC p14,c5,[R0],#4 */
1498                         /* STC p14,c5,[R0]*/
1499                         retval = arm11_run_instr_data_to_core_noack(arm11, instr, words, count);
1500                         if (retval != ERROR_OK)
1501                                 return retval;
1502                 }
1503
1504                 break;
1505         }
1506         }
1507
1508         /* r0 verification */
1509         if (!arm11_config_memrw_no_increment)
1510         {
1511                 uint32_t r0;
1512
1513                 /* MCR p14,0,R0,c0,c5,0 */
1514                 retval = arm11_run_instr_data_from_core(arm11, 0xEE000E15, &r0, 1);
1515                 if (retval != ERROR_OK)
1516                         return retval;
1517
1518                 if (address + size * count != r0)
1519                 {
1520                         LOG_ERROR("Data transfer failed. Expected end "
1521                                         "address 0x%08x, got 0x%08x",
1522                                         (unsigned) (address + size * count),
1523                                         (unsigned) r0);
1524
1525                         if (burst)
1526                                 LOG_ERROR("use 'arm11 memwrite burst disable' to disable fast burst mode");
1527
1528                         if (arm11_config_memwrite_error_fatal)
1529                                 return ERROR_FAIL;
1530                 }
1531         }
1532
1533         return arm11_run_instr_data_finish(arm11);
1534 }
1535
1536 static int arm11_write_memory(struct target *target,
1537                 uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
1538 {
1539         return arm11_write_memory_inner(target, address, size, count, buffer, false);
1540 }
1541
1542 /* write target memory in multiples of 4 byte, optimized for writing large quantities of data */
1543 static int arm11_bulk_write_memory(struct target *target,
1544                 uint32_t address, uint32_t count, uint8_t *buffer)
1545 {
1546         FNC_INFO;
1547
1548         if (target->state != TARGET_HALTED)
1549         {
1550                 LOG_WARNING("target was not halted");
1551                 return ERROR_TARGET_NOT_HALTED;
1552         }
1553
1554         return arm11_write_memory(target, address, 4, count, buffer);
1555 }
1556
1557 /* target break-/watchpoint control
1558 * rw: 0 = write, 1 = read, 2 = access
1559 */
1560 static int arm11_add_breakpoint(struct target *target,
1561                 struct breakpoint *breakpoint)
1562 {
1563         FNC_INFO;
1564         struct arm11_common *arm11 = target_to_arm11(target);
1565
1566 #if 0
1567         if (breakpoint->type == BKPT_SOFT)
1568         {
1569                 LOG_INFO("sw breakpoint requested, but software breakpoints not enabled");
1570                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1571         }
1572 #endif
1573
1574         if (!arm11->free_brps)
1575         {
1576                 LOG_DEBUG("no breakpoint unit available for hardware breakpoint");
1577                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1578         }
1579
1580         if (breakpoint->length != 4)
1581         {
1582                 LOG_DEBUG("only breakpoints of four bytes length supported");
1583                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1584         }
1585
1586         arm11->free_brps--;
1587
1588         return ERROR_OK;
1589 }
1590
1591 static int arm11_remove_breakpoint(struct target *target,
1592                 struct breakpoint *breakpoint)
1593 {
1594         FNC_INFO;
1595         struct arm11_common *arm11 = target_to_arm11(target);
1596
1597         arm11->free_brps++;
1598
1599         return ERROR_OK;
1600 }
1601
1602 static int arm11_add_watchpoint(struct target *target,
1603                 struct watchpoint *watchpoint)
1604 {
1605         FNC_INFO_NOTIMPLEMENTED;
1606
1607         return ERROR_OK;
1608 }
1609
1610 static int arm11_remove_watchpoint(struct target *target,
1611                 struct watchpoint *watchpoint)
1612 {
1613         FNC_INFO_NOTIMPLEMENTED;
1614
1615         return ERROR_OK;
1616 }
1617
1618 // HACKHACKHACK - FIXME mode/state
1619 /* target algorithm support */
1620 static int arm11_run_algorithm(struct target *target,
1621                 int num_mem_params, struct mem_param *mem_params,
1622                 int num_reg_params, struct reg_param *reg_params,
1623                 uint32_t entry_point, uint32_t exit_point,
1624                 int timeout_ms, void *arch_info)
1625 {
1626         struct arm11_common *arm11 = target_to_arm11(target);
1627 //      enum armv4_5_state core_state = arm11->core_state;
1628 //      enum armv4_5_mode core_mode = arm11->core_mode;
1629         uint32_t context[16];
1630         uint32_t cpsr;
1631         int exit_breakpoint_size = 0;
1632         int retval = ERROR_OK;
1633                 LOG_DEBUG("Running algorithm");
1634
1635
1636         if (target->state != TARGET_HALTED)
1637         {
1638                 LOG_WARNING("target not halted");
1639                 return ERROR_TARGET_NOT_HALTED;
1640         }
1641
1642         // FIXME
1643 //      if (armv4_5_mode_to_number(arm11->core_mode)==-1)
1644 //              return ERROR_FAIL;
1645
1646         // Save regs
1647         for (unsigned i = 0; i < 16; i++)
1648         {
1649                 context[i] = buf_get_u32((uint8_t*)(&arm11->reg_values[i]),0,32);
1650                 LOG_DEBUG("Save %u: 0x%" PRIx32 "", i, context[i]);
1651         }
1652
1653         cpsr = buf_get_u32((uint8_t*)(arm11->reg_values + ARM11_RC_CPSR),0,32);
1654         LOG_DEBUG("Save CPSR: 0x%" PRIx32 "", cpsr);
1655
1656         for (int i = 0; i < num_mem_params; i++)
1657         {
1658                 target_write_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
1659         }
1660
1661         // Set register parameters
1662         for (int i = 0; i < num_reg_params; i++)
1663         {
1664                 struct reg *reg = register_get_by_name(arm11->core_cache, reg_params[i].reg_name, 0);
1665                 if (!reg)
1666                 {
1667                         LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
1668                         exit(-1);
1669                 }
1670
1671                 if (reg->size != reg_params[i].size)
1672                 {
1673                         LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
1674                         exit(-1);
1675                 }
1676                 arm11_set_reg(reg,reg_params[i].value);
1677 //              printf("%i: Set %s =%08x\n", i, reg_params[i].reg_name,val);
1678         }
1679
1680         exit_breakpoint_size = 4;
1681
1682 /*      arm11->core_state = arm11_algorithm_info->core_state;
1683         if (arm11->core_state == ARMV4_5_STATE_ARM)
1684                                 exit_breakpoint_size = 4;
1685         else if (arm11->core_state == ARMV4_5_STATE_THUMB)
1686                 exit_breakpoint_size = 2;
1687         else
1688         {
1689                 LOG_ERROR("BUG: can't execute algorithms when not in ARM or Thumb state");
1690                 exit(-1);
1691         }
1692 */
1693
1694
1695 /* arm11 at this point only supports ARM not THUMB mode
1696    however if this test needs to be reactivated the current state can be read back
1697    from CPSR */
1698 #if 0
1699         if (arm11_algorithm_info->core_mode != ARMV4_5_MODE_ANY)
1700         {
1701                 LOG_DEBUG("setting core_mode: 0x%2.2x", arm11_algorithm_info->core_mode);
1702                 buf_set_u32(arm11->reg_list[ARM11_RC_CPSR].value, 0, 5, arm11_algorithm_info->core_mode);
1703                 arm11->reg_list[ARM11_RC_CPSR].dirty = 1;
1704                 arm11->reg_list[ARM11_RC_CPSR].valid = 1;
1705         }
1706 #endif
1707
1708         if ((retval = breakpoint_add(target, exit_point, exit_breakpoint_size, BKPT_HARD)) != ERROR_OK)
1709         {
1710                 LOG_ERROR("can't add breakpoint to finish algorithm execution");
1711                 retval = ERROR_TARGET_FAILURE;
1712                 goto restore;
1713         }
1714
1715         // no debug, otherwise breakpoint is not set
1716         CHECK_RETVAL(target_resume(target, 0, entry_point, 1, 0));
1717
1718         CHECK_RETVAL(target_wait_state(target, TARGET_HALTED, timeout_ms));
1719
1720         if (target->state != TARGET_HALTED)
1721         {
1722                 CHECK_RETVAL(target_halt(target));
1723
1724                 CHECK_RETVAL(target_wait_state(target, TARGET_HALTED, 500));
1725
1726                 retval = ERROR_TARGET_TIMEOUT;
1727
1728                 goto del_breakpoint;
1729         }
1730
1731         if (buf_get_u32(arm11->reg_list[15].value, 0, 32) != exit_point)
1732         {
1733                 LOG_WARNING("target reentered debug state, but not at the desired exit point: 0x%4.4" PRIx32 "",
1734                         buf_get_u32(arm11->reg_list[15].value, 0, 32));
1735                 retval = ERROR_TARGET_TIMEOUT;
1736                 goto del_breakpoint;
1737         }
1738
1739         for (int i = 0; i < num_mem_params; i++)
1740         {
1741                 if (mem_params[i].direction != PARAM_OUT)
1742                         target_read_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
1743         }
1744
1745         for (int i = 0; i < num_reg_params; i++)
1746         {
1747                 if (reg_params[i].direction != PARAM_OUT)
1748                 {
1749                         struct reg *reg = register_get_by_name(arm11->core_cache, reg_params[i].reg_name, 0);
1750                         if (!reg)
1751                         {
1752                                 LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
1753                                 exit(-1);
1754                         }
1755
1756                         if (reg->size != reg_params[i].size)
1757                         {
1758                                 LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
1759                                 exit(-1);
1760                         }
1761
1762                         buf_set_u32(reg_params[i].value, 0, 32, buf_get_u32(reg->value, 0, 32));
1763                 }
1764         }
1765
1766 del_breakpoint:
1767         breakpoint_remove(target, exit_point);
1768
1769 restore:
1770         // Restore context
1771         for (size_t i = 0; i < 16; i++)
1772         {
1773                 LOG_DEBUG("restoring register %s with value 0x%8.8" PRIx32 "",
1774                          arm11->reg_list[i].name, context[i]);
1775                 arm11_set_reg(&arm11->reg_list[i], (uint8_t*)&context[i]);
1776         }
1777         LOG_DEBUG("restoring CPSR with value 0x%8.8" PRIx32 "", cpsr);
1778         arm11_set_reg(&arm11->reg_list[ARM11_RC_CPSR], (uint8_t*)&cpsr);
1779
1780 //      arm11->core_state = core_state;
1781 //      arm11->core_mode = core_mode;
1782
1783         return retval;
1784 }
1785
1786 static int arm11_target_create(struct target *target, Jim_Interp *interp)
1787 {
1788         FNC_INFO;
1789
1790         NEW(struct arm11_common, arm11, 1);
1791
1792         arm11->target = target;
1793
1794         if (target->tap == NULL)
1795                 return ERROR_FAIL;
1796
1797         if (target->tap->ir_length != 5)
1798         {
1799                 LOG_ERROR("'target arm11' expects IR LENGTH = 5");
1800                 return ERROR_COMMAND_SYNTAX_ERROR;
1801         }
1802
1803         armv4_5_init_arch_info(target, &arm11->arm);
1804
1805         arm11->jtag_info.tap = target->tap;
1806         arm11->jtag_info.scann_size = 5;
1807         arm11->jtag_info.scann_instr = ARM11_SCAN_N;
1808         /* cur_scan_chain == 0 */
1809         arm11->jtag_info.intest_instr = ARM11_INTEST;
1810
1811         return ERROR_OK;
1812 }
1813
1814 static int arm11_init_target(struct command_context *cmd_ctx,
1815                 struct target *target)
1816 {
1817         /* Initialize anything we can set up without talking to the target */
1818         return arm11_build_reg_cache(target);
1819 }
1820
1821 /* talk to the target and set things up */
1822 static int arm11_examine(struct target *target)
1823 {
1824         int retval;
1825
1826         FNC_INFO;
1827         struct arm11_common *arm11 = target_to_arm11(target);
1828
1829         /* check IDCODE */
1830
1831         arm11_add_IR(arm11, ARM11_IDCODE, ARM11_TAP_DEFAULT);
1832
1833         struct scan_field               idcode_field;
1834
1835         arm11_setup_field(arm11, 32, NULL, &arm11->device_id, &idcode_field);
1836
1837         arm11_add_dr_scan_vc(1, &idcode_field, TAP_DRPAUSE);
1838
1839         /* check DIDR */
1840
1841         arm11_add_debug_SCAN_N(arm11, 0x00, ARM11_TAP_DEFAULT);
1842
1843         arm11_add_IR(arm11, ARM11_INTEST, ARM11_TAP_DEFAULT);
1844
1845         struct scan_field               chain0_fields[2];
1846
1847         arm11_setup_field(arm11, 32, NULL,      &arm11->didr,           chain0_fields + 0);
1848         arm11_setup_field(arm11,  8, NULL,      &arm11->implementor,    chain0_fields + 1);
1849
1850         arm11_add_dr_scan_vc(ARRAY_SIZE(chain0_fields), chain0_fields, TAP_IDLE);
1851
1852         CHECK_RETVAL(jtag_execute_queue());
1853
1854         switch (arm11->device_id & 0x0FFFF000)
1855         {
1856         case 0x07B36000:        LOG_INFO("found ARM1136"); break;
1857         case 0x07B56000:        LOG_INFO("found ARM1156"); break;
1858         case 0x07B76000:        LOG_INFO("found ARM1176"); break;
1859         default:
1860                 LOG_ERROR("'target arm11' expects IDCODE 0x*7B*7****");
1861                 return ERROR_FAIL;
1862         }
1863
1864         arm11->debug_version = (arm11->didr >> 16) & 0x0F;
1865
1866         if (arm11->debug_version != ARM11_DEBUG_V6 &&
1867                 arm11->debug_version != ARM11_DEBUG_V61)
1868         {
1869                 LOG_ERROR("Only ARMv6 v6 and v6.1 architectures supported.");
1870                 return ERROR_FAIL;
1871         }
1872
1873         arm11->brp      = ((arm11->didr >> 24) & 0x0F) + 1;
1874         arm11->wrp      = ((arm11->didr >> 28) & 0x0F) + 1;
1875
1876         /** \todo TODO: reserve one brp slot if we allow breakpoints during step */
1877         arm11->free_brps = arm11->brp;
1878         arm11->free_wrps = arm11->wrp;
1879
1880         LOG_DEBUG("IDCODE %08" PRIx32 " IMPLEMENTOR %02x DIDR %08" PRIx32 "",
1881                 arm11->device_id,
1882                 (int)(arm11->implementor),
1883                 arm11->didr);
1884
1885         /* as a side-effect this reads DSCR and thus
1886          * clears the ARM11_DSCR_STICKY_PRECISE_DATA_ABORT / Sticky Precise Data Abort Flag
1887          * as suggested by the spec.
1888          */
1889
1890         retval = arm11_check_init(arm11, NULL);
1891         if (retval != ERROR_OK)
1892                 return retval;
1893
1894         /* ETM on ARM11 still uses original scanchain 6 access mode */
1895         if (arm11->arm.etm && !target_was_examined(target)) {
1896                 *register_get_last_cache_p(&target->reg_cache) =
1897                         etm_build_reg_cache(target, &arm11->jtag_info,
1898                                         arm11->arm.etm);
1899                 retval = etm_setup(target);
1900         }
1901
1902         target_set_examined(target);
1903
1904         return ERROR_OK;
1905 }
1906
1907
1908 /** Load a register that is marked !valid in the register cache */
1909 static int arm11_get_reg(struct reg *reg)
1910 {
1911         FNC_INFO;
1912
1913         struct target * target = ((struct arm11_reg_state *)reg->arch_info)->target;
1914
1915         if (target->state != TARGET_HALTED)
1916         {
1917                 LOG_WARNING("target was not halted");
1918                 return ERROR_TARGET_NOT_HALTED;
1919         }
1920
1921         /** \todo TODO: Check this. We assume that all registers are fetched at debug entry. */
1922
1923 #if 0
1924         struct arm11_common *arm11 = target_to_arm11(target);
1925         const struct arm11_reg_defs *arm11_reg_info = arm11_reg_defs + ((struct arm11_reg_state *)reg->arch_info)->def_index;
1926 #endif
1927
1928         return ERROR_OK;
1929 }
1930
1931 /** Change a value in the register cache */
1932 static int arm11_set_reg(struct reg *reg, uint8_t *buf)
1933 {
1934         FNC_INFO;
1935
1936         struct target *target = ((struct arm11_reg_state *)reg->arch_info)->target;
1937         struct arm11_common *arm11 = target_to_arm11(target);
1938 //      const struct arm11_reg_defs *arm11_reg_info = arm11_reg_defs + ((struct arm11_reg_state *)reg->arch_info)->def_index;
1939
1940         arm11->reg_values[((struct arm11_reg_state *)reg->arch_info)->def_index] = buf_get_u32(buf, 0, 32);
1941         reg->valid      = 1;
1942         reg->dirty      = 1;
1943
1944         return ERROR_OK;
1945 }
1946
1947 static int arm11_build_reg_cache(struct target *target)
1948 {
1949         struct arm11_common *arm11 = target_to_arm11(target);
1950
1951         NEW(struct reg_cache,           cache,                          1);
1952         NEW(struct reg,                         reg_list,                       ARM11_REGCACHE_COUNT);
1953         NEW(struct arm11_reg_state,     arm11_reg_states,       ARM11_REGCACHE_COUNT);
1954
1955         if (arm11_regs_arch_type == -1)
1956                 arm11_regs_arch_type = register_reg_arch_type(arm11_get_reg, arm11_set_reg);
1957
1958         register_init_dummy(&arm11_gdb_dummy_fp_reg);
1959         register_init_dummy(&arm11_gdb_dummy_fps_reg);
1960
1961         arm11->reg_list = reg_list;
1962
1963         /* Build the process context cache */
1964         cache->name             = "arm11 registers";
1965         cache->next             = NULL;
1966         cache->reg_list = reg_list;
1967         cache->num_regs = ARM11_REGCACHE_COUNT;
1968
1969         struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
1970         (*cache_p) = cache;
1971
1972         arm11->core_cache = cache;
1973 //        armv7m->process_context = cache;
1974
1975         size_t i;
1976
1977         /* Not very elegant assertion */
1978         if (ARM11_REGCACHE_COUNT != ARRAY_SIZE(arm11->reg_values) ||
1979                 ARM11_REGCACHE_COUNT != ARRAY_SIZE(arm11_reg_defs) ||
1980                 ARM11_REGCACHE_COUNT != ARM11_RC_MAX)
1981         {
1982                 LOG_ERROR("BUG: arm11->reg_values inconsistent (%d " ZU " " ZU " %d)", ARM11_REGCACHE_COUNT, ARRAY_SIZE(arm11->reg_values), ARRAY_SIZE(arm11_reg_defs), ARM11_RC_MAX);
1983                 exit(-1);
1984         }
1985
1986         for (i = 0; i < ARM11_REGCACHE_COUNT; i++)
1987         {
1988                 struct reg *                                            r       = reg_list                      + i;
1989                 const struct arm11_reg_defs *   rd      = arm11_reg_defs        + i;
1990                 struct arm11_reg_state *                        rs      = arm11_reg_states      + i;
1991
1992                 r->name                         = rd->name;
1993                 r->size                         = 32;
1994                 r->value                        = (uint8_t *)(arm11->reg_values + i);
1995                 r->dirty                        = 0;
1996                 r->valid                        = 0;
1997                 r->arch_type            = arm11_regs_arch_type;
1998                 r->arch_info            = rs;
1999
2000                 rs->def_index           = i;
2001                 rs->target                      = target;
2002         }
2003
2004         return ERROR_OK;
2005 }
2006
2007 static COMMAND_HELPER(arm11_handle_bool, bool *var, char *name)
2008 {
2009         if (argc == 0)
2010         {
2011                 LOG_INFO("%s is %s.", name, *var ? "enabled" : "disabled");
2012                 return ERROR_OK;
2013         }
2014
2015         if (argc != 1)
2016                 return ERROR_COMMAND_SYNTAX_ERROR;
2017
2018         switch (args[0][0])
2019         {
2020         case '0':       /* 0 */
2021         case 'f':       /* false */
2022         case 'F':
2023         case 'd':       /* disable */
2024         case 'D':
2025                 *var = false;
2026                 break;
2027
2028         case '1':       /* 1 */
2029         case 't':       /* true */
2030         case 'T':
2031         case 'e':       /* enable */
2032         case 'E':
2033                 *var = true;
2034                 break;
2035         }
2036
2037         LOG_INFO("%s %s.", *var ? "Enabled" : "Disabled", name);
2038
2039         return ERROR_OK;
2040 }
2041
2042 #define BOOL_WRAPPER(name, print_name)  \
2043 COMMAND_HANDLER(arm11_handle_bool_##name) \
2044 { \
2045         return CALL_COMMAND_HANDLER(arm11_handle_bool, \
2046                         &arm11_config_##name, print_name); \
2047 }
2048
2049 BOOL_WRAPPER(memwrite_burst,                    "memory write burst mode")
2050 BOOL_WRAPPER(memwrite_error_fatal,              "fatal error mode for memory writes")
2051 BOOL_WRAPPER(step_irq_enable,                   "IRQs while stepping")
2052 BOOL_WRAPPER(hardware_step,                     "hardware single step")
2053
2054 COMMAND_HANDLER(arm11_handle_vcr)
2055 {
2056         switch (argc) {
2057         case 0:
2058                 break;
2059         case 1:
2060                 COMMAND_PARSE_NUMBER(u32, args[0], arm11_vcr);
2061                 break;
2062         default:
2063                 return ERROR_COMMAND_SYNTAX_ERROR;
2064         }
2065
2066         LOG_INFO("VCR 0x%08" PRIx32 "", arm11_vcr);
2067         return ERROR_OK;
2068 }
2069
2070 static const uint32_t arm11_coproc_instruction_limits[] =
2071 {
2072         15,                             /* coprocessor */
2073         7,                              /* opcode 1 */
2074         15,                             /* CRn */
2075         15,                             /* CRm */
2076         7,                              /* opcode 2 */
2077         0xFFFFFFFF,             /* value */
2078 };
2079
2080 static int arm11_mrc_inner(struct target *target, int cpnum,
2081                 uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
2082                 uint32_t *value, bool read)
2083 {
2084         int retval;
2085         struct arm11_common *arm11 = target_to_arm11(target);
2086
2087         if (target->state != TARGET_HALTED)
2088         {
2089                 LOG_ERROR("Target not halted");
2090                 return ERROR_FAIL;
2091         }
2092
2093         uint32_t instr = 0xEE000010     |
2094                 (cpnum <<  8) |
2095                 (op1 << 21) |
2096                 (CRn << 16) |
2097                 (CRm <<  0) |
2098                 (op2 <<  5);
2099
2100         if (read)
2101                 instr |= 0x00100000;
2102
2103         retval = arm11_run_instr_data_prepare(arm11);
2104         if (retval != ERROR_OK)
2105                 return retval;
2106
2107         if (read)
2108         {
2109                 retval = arm11_run_instr_data_from_core_via_r0(arm11, instr, value);
2110                 if (retval != ERROR_OK)
2111                         return retval;
2112         }
2113         else
2114         {
2115                 retval = arm11_run_instr_data_to_core_via_r0(arm11, instr, *value);
2116                 if (retval != ERROR_OK)
2117                         return retval;
2118         }
2119
2120         return arm11_run_instr_data_finish(arm11);
2121 }
2122
2123 static int arm11_mrc(struct target *target, int cpnum,
2124                 uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm, uint32_t *value)
2125 {
2126         return arm11_mrc_inner(target, cpnum, op1, op2, CRn, CRm, value, true);
2127 }
2128
2129 static int arm11_mcr(struct target *target, int cpnum,
2130                 uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm, uint32_t value)
2131 {
2132         return arm11_mrc_inner(target, cpnum, op1, op2, CRn, CRm, &value, false);
2133 }
2134
2135 static int arm11_register_commands(struct command_context *cmd_ctx)
2136 {
2137         FNC_INFO;
2138
2139         struct command *top_cmd, *mw_cmd;
2140
2141         top_cmd = register_command(cmd_ctx, NULL, "arm11",
2142                         NULL, COMMAND_ANY, NULL);
2143
2144         /* "hardware_step" is only here to check if the default
2145          * simulate + breakpoint implementation is broken.
2146          * TEMPORARY! NOT DOCUMENTED!
2147          */
2148         register_command(cmd_ctx, top_cmd, "hardware_step",
2149                         arm11_handle_bool_hardware_step, COMMAND_ANY,
2150                         "DEBUG ONLY - Hardware single stepping"
2151                                 " (default: disabled)");
2152
2153         mw_cmd = register_command(cmd_ctx, top_cmd, "memwrite",
2154                         NULL, COMMAND_ANY, NULL);
2155         register_command(cmd_ctx, mw_cmd, "burst",
2156                         arm11_handle_bool_memwrite_burst, COMMAND_ANY,
2157                         "Enable/Disable non-standard but fast burst mode"
2158                                 " (default: enabled)");
2159         register_command(cmd_ctx, mw_cmd, "error_fatal",
2160                         arm11_handle_bool_memwrite_error_fatal, COMMAND_ANY,
2161                         "Terminate program if transfer error was found"
2162                                 " (default: enabled)");
2163
2164         register_command(cmd_ctx, top_cmd, "step_irq_enable",
2165                         arm11_handle_bool_step_irq_enable, COMMAND_ANY,
2166                         "Enable interrupts while stepping"
2167                                 " (default: disabled)");
2168         register_command(cmd_ctx, top_cmd, "vcr",
2169                         arm11_handle_vcr, COMMAND_ANY,
2170                         "Control (Interrupt) Vector Catch Register");
2171
2172         return etm_register_commands(cmd_ctx);
2173 }
2174
2175 /** Holds methods for ARM11xx targets. */
2176 struct target_type arm11_target = {
2177         .name =                 "arm11",
2178
2179         .poll =                 arm11_poll,
2180         .arch_state =           arm11_arch_state,
2181
2182         .target_request_data =  arm11_target_request_data,
2183
2184         .halt =                 arm11_halt,
2185         .resume =               arm11_resume,
2186         .step =                 arm11_step,
2187
2188         .assert_reset =         arm11_assert_reset,
2189         .deassert_reset =       arm11_deassert_reset,
2190         .soft_reset_halt =      arm11_soft_reset_halt,
2191
2192         .get_gdb_reg_list =     arm11_get_gdb_reg_list,
2193
2194         .read_memory =          arm11_read_memory,
2195         .write_memory =         arm11_write_memory,
2196
2197         .bulk_write_memory =    arm11_bulk_write_memory,
2198
2199         .checksum_memory =      arm_checksum_memory,
2200         .blank_check_memory =   arm_blank_check_memory,
2201
2202         .add_breakpoint =       arm11_add_breakpoint,
2203         .remove_breakpoint =    arm11_remove_breakpoint,
2204         .add_watchpoint =       arm11_add_watchpoint,
2205         .remove_watchpoint =    arm11_remove_watchpoint,
2206
2207         .run_algorithm =        arm11_run_algorithm,
2208
2209         .register_commands =    arm11_register_commands,
2210         .target_create =        arm11_target_create,
2211         .init_target =          arm11_init_target,
2212         .examine =              arm11_examine,
2213
2214         .mrc =                  arm11_mrc,
2215         .mcr =                  arm11_mcr,
2216 };