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

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   Copyright (C) 2006 by Magnus Lundin                                   *
   6  *   lundin@mlu.mine.nu                                                    *
   7  *                                                                         *
   8  *   Copyright (C) 2008 by Spencer Oliver                                  *
   9  *   spen@spen-soft.co.uk                                                  *
  10  *                                                                         *
  11  *   This program is free software; you can redistribute it and/or modify  *
  12  *   it under the terms of the GNU General Public License as published by  *
  13  *   the Free Software Foundation; either version 2 of the License, or     *
  14  *   (at your option) any later version.                                   *
  15  *                                                                         *
  16  *   This program is distributed in the hope that it will be useful,       *
  17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  19  *   GNU General Public License for more details.                          *
  20  *                                                                         *
  21  *   You should have received a copy of the GNU General Public License     *
  22  *   along with this program; if not, write to the                         *
  23  *   Free Software Foundation, Inc.,                                       *
  24  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  25  *                                                                         *
  26  *                                                                         *
  27  *   Cortex-M3(tm) TRM, ARM DDI 0337E (r1p1) and 0337G (r2p0)              *
  28  *                                                                         *
  29  ***************************************************************************/
  30 #ifdef HAVE_CONFIG_H
  31 #include "config.h"
  32 #endif
  33
  34 #include "cortex_m3.h"
  35 #include "target_request.h"
  36 #include "target_type.h"
  37 #include "arm_disassembler.h"
  38
  39
  40 #define ARRAY_SIZE(x)   ((int)(sizeof(x)/sizeof((x)[0])))
  41
  42
  43 /* forward declarations */
  44 static int cortex_m3_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
  45 static int cortex_m3_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
  46 static void cortex_m3_enable_watchpoints(struct target_s *target);
  47 static int cortex_m3_store_core_reg_u32(target_t *target,
  48                 enum armv7m_regtype type, uint32_t num, uint32_t value);
  49
  50 #ifdef ARMV7_GDB_HACKS
  51 extern uint8_t armv7m_gdb_dummy_cpsr_value[];
  52 extern reg_t armv7m_gdb_dummy_cpsr_reg;
  53 #endif
  54
  55 static int cortexm3_dap_read_coreregister_u32(swjdp_common_t *swjdp,
  56                 uint32_t *value, int regnum)
  57 {
  58         int retval;
  59         uint32_t dcrdr;
  60
  61         /* because the DCB_DCRDR is used for the emulated dcc channel
  62          * we have to save/restore the DCB_DCRDR when used */
  63
  64         mem_ap_read_u32(swjdp, DCB_DCRDR, &dcrdr);
  65
  66         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
  67
  68         /* mem_ap_write_u32(swjdp, DCB_DCRSR, regnum); */
  69         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
  70         dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (DCB_DCRSR & 0xC), regnum);
  71
  72         /* mem_ap_read_u32(swjdp, DCB_DCRDR, value); */
  73         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
  74         dap_ap_read_reg_u32(swjdp, AP_REG_BD0 | (DCB_DCRDR & 0xC), value);
  75
  76         retval = swjdp_transaction_endcheck(swjdp);
  77
  78         /* restore DCB_DCRDR - this needs to be in a seperate
  79          * transaction otherwise the emulated DCC channel breaks */
  80         if (retval == ERROR_OK)
  81                 retval = mem_ap_write_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
  82
  83         return retval;
  84 }
  85
  86 static int cortexm3_dap_write_coreregister_u32(swjdp_common_t *swjdp,
  87                 uint32_t value, int regnum)
  88 {
  89         int retval;
  90         uint32_t dcrdr;
  91
  92         /* because the DCB_DCRDR is used for the emulated dcc channel
  93          * we have to save/restore the DCB_DCRDR when used */
  94
  95         mem_ap_read_u32(swjdp, DCB_DCRDR, &dcrdr);
  96
  97         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
  98
  99         /* mem_ap_write_u32(swjdp, DCB_DCRDR, core_regs[i]); */
 100         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
 101         dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (DCB_DCRDR & 0xC), value);
 102
 103         /* mem_ap_write_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR); */
 104         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
 105         dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR);
 106
 107         retval = swjdp_transaction_endcheck(swjdp);
 108
 109         /* restore DCB_DCRDR - this needs to be in a seperate
 110          * transaction otherwise the emulated DCC channel breaks */
 111         if (retval == ERROR_OK)
 112                 retval = mem_ap_write_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
 113
 114         return retval;
 115 }
 116
 117 static int cortex_m3_write_debug_halt_mask(target_t *target,
 118                 uint32_t mask_on, uint32_t mask_off)
 119 {
 120         /* get pointers to arch-specific information */
 121         armv7m_common_t *armv7m = target->arch_info;
 122         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 123         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 124
 125         /* mask off status bits */
 126         cortex_m3->dcb_dhcsr &= ~((0xFFFF << 16) | mask_off);
 127         /* create new register mask */
 128         cortex_m3->dcb_dhcsr |= DBGKEY | C_DEBUGEN | mask_on;
 129
 130         return mem_ap_write_atomic_u32(swjdp, DCB_DHCSR, cortex_m3->dcb_dhcsr);
 131 }
 132
 133 static int cortex_m3_clear_halt(target_t *target)
 134 {
 135         /* get pointers to arch-specific information */
 136         armv7m_common_t *armv7m = target->arch_info;
 137         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 138         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 139
 140         /* clear step if any */
 141         cortex_m3_write_debug_halt_mask(target, C_HALT, C_STEP);
 142
 143         /* Read Debug Fault Status Register */
 144         mem_ap_read_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
 145         /* Clear Debug Fault Status */
 146         mem_ap_write_atomic_u32(swjdp, NVIC_DFSR, cortex_m3->nvic_dfsr);
 147         LOG_DEBUG(" NVIC_DFSR 0x%" PRIx32 "", cortex_m3->nvic_dfsr);
 148
 149         return ERROR_OK;
 150 }
 151
 152 static int cortex_m3_single_step_core(target_t *target)
 153 {
 154         /* get pointers to arch-specific information */
 155         armv7m_common_t *armv7m = target->arch_info;
 156         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 157         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 158         uint32_t dhcsr_save;
 159
 160         /* backup dhcsr reg */
 161         dhcsr_save = cortex_m3->dcb_dhcsr;
 162
 163         /* mask interrupts if not done already */
 164         if (!(cortex_m3->dcb_dhcsr & C_MASKINTS))
 165                 mem_ap_write_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_MASKINTS | C_HALT | C_DEBUGEN);
 166         mem_ap_write_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_MASKINTS | C_STEP | C_DEBUGEN);
 167         LOG_DEBUG(" ");
 168
 169         /* restore dhcsr reg */
 170         cortex_m3->dcb_dhcsr = dhcsr_save;
 171         cortex_m3_clear_halt(target);
 172
 173         return ERROR_OK;
 174 }
 175
 176 static int cortex_m3_endreset_event(target_t *target)
 177 {
 178         int i;
 179         uint32_t dcb_demcr;
 180
 181         /* get pointers to arch-specific information */
 182         armv7m_common_t *armv7m = target->arch_info;
 183         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 184         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 185         cortex_m3_fp_comparator_t *fp_list = cortex_m3->fp_comparator_list;
 186         cortex_m3_dwt_comparator_t *dwt_list = cortex_m3->dwt_comparator_list;
 187
 188         mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &dcb_demcr);
 189         LOG_DEBUG("DCB_DEMCR = 0x%8.8" PRIx32 "",dcb_demcr);
 190
 191         /* this regsiter is used for emulated dcc channel */
 192         mem_ap_write_u32(swjdp, DCB_DCRDR, 0);
 193
 194         /* Enable debug requests */
 195         mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 196         if (!(cortex_m3->dcb_dhcsr & C_DEBUGEN))
 197                 mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN);
 198
 199         /* clear any interrupt masking */
 200         cortex_m3_write_debug_halt_mask(target, 0, C_MASKINTS);
 201
 202         /* Enable trace and dwt */
 203         mem_ap_write_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR);
 204         /* Monitor bus faults */
 205         mem_ap_write_u32(swjdp, NVIC_SHCSR, SHCSR_BUSFAULTENA);
 206
 207         /* Enable FPB */
 208         target_write_u32(target, FP_CTRL, 3);
 209         cortex_m3->fpb_enabled = 1;
 210
 211         /* Restore FPB registers */
 212         for (i = 0; i < cortex_m3->fp_num_code + cortex_m3->fp_num_lit; i++)
 213         {
 214                 target_write_u32(target, fp_list[i].fpcr_address, fp_list[i].fpcr_value);
 215         }
 216
 217         /* Restore DWT registers */
 218         for (i = 0; i < cortex_m3->dwt_num_comp; i++)
 219         {
 220                 target_write_u32(target, dwt_list[i].dwt_comparator_address, dwt_list[i].comp);
 221                 target_write_u32(target, dwt_list[i].dwt_comparator_address | 0x4, dwt_list[i].mask);
 222                 target_write_u32(target, dwt_list[i].dwt_comparator_address | 0x8, dwt_list[i].function);
 223         }
 224         swjdp_transaction_endcheck(swjdp);
 225
 226         armv7m_invalidate_core_regs(target);
 227
 228         /* make sure we have latest dhcsr flags */
 229         mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 230
 231         return ERROR_OK;
 232 }
 233
 234 static int cortex_m3_examine_debug_reason(target_t *target)
 235 {
 236         /* get pointers to arch-specific information */
 237         armv7m_common_t *armv7m = target->arch_info;
 238         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 239
 240         /* THIS IS NOT GOOD, TODO - better logic for detection of debug state reason */
 241         /* only check the debug reason if we don't know it already */
 242
 243         if ((target->debug_reason != DBG_REASON_DBGRQ)
 244                 && (target->debug_reason != DBG_REASON_SINGLESTEP))
 245         {
 246                 if (cortex_m3->nvic_dfsr & DFSR_BKPT)
 247                 {
 248                         target->debug_reason = DBG_REASON_BREAKPOINT;
 249                         if (cortex_m3->nvic_dfsr & DFSR_DWTTRAP)
 250                                 target->debug_reason = DBG_REASON_WPTANDBKPT;
 251                 }
 252                 else if (cortex_m3->nvic_dfsr & DFSR_DWTTRAP)
 253                         target->debug_reason = DBG_REASON_WATCHPOINT;
 254                 else if (cortex_m3->nvic_dfsr & DFSR_VCATCH)
 255                         target->debug_reason = DBG_REASON_BREAKPOINT;
 256                 else /* EXTERNAL, HALTED, DWTTRAP w/o BKPT */
 257                         target->debug_reason = DBG_REASON_UNDEFINED;
 258         }
 259
 260         return ERROR_OK;
 261 }
 262
 263 static int cortex_m3_examine_exception_reason(target_t *target)
 264 {
 265         uint32_t shcsr, except_sr, cfsr = -1, except_ar = -1;
 266
 267         /* get pointers to arch-specific information */
 268         armv7m_common_t *armv7m = target->arch_info;
 269         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 270
 271         mem_ap_read_u32(swjdp, NVIC_SHCSR, &shcsr);
 272         switch (armv7m->exception_number)
 273         {
 274                 case 2: /* NMI */
 275                         break;
 276                 case 3: /* Hard Fault */
 277                         mem_ap_read_atomic_u32(swjdp, NVIC_HFSR, &except_sr);
 278                         if (except_sr & 0x40000000)
 279                         {
 280                                 mem_ap_read_u32(swjdp, NVIC_CFSR, &cfsr);
 281                         }
 282                         break;
 283                 case 4: /* Memory Management */
 284                         mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
 285                         mem_ap_read_u32(swjdp, NVIC_MMFAR, &except_ar);
 286                         break;
 287                 case 5: /* Bus Fault */
 288                         mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
 289                         mem_ap_read_u32(swjdp, NVIC_BFAR, &except_ar);
 290                         break;
 291                 case 6: /* Usage Fault */
 292                         mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
 293                         break;
 294                 case 11:        /* SVCall */
 295                         break;
 296                 case 12:        /* Debug Monitor */
 297                         mem_ap_read_u32(swjdp, NVIC_DFSR, &except_sr);
 298                         break;
 299                 case 14:        /* PendSV */
 300                         break;
 301                 case 15:        /* SysTick */
 302                         break;
 303                 default:
 304                         except_sr = 0;
 305                         break;
 306         }
 307         swjdp_transaction_endcheck(swjdp);
 308         LOG_DEBUG("%s SHCSR 0x%" PRIx32 ", SR 0x%" PRIx32 ", CFSR 0x%" PRIx32 ", AR 0x%" PRIx32 "", armv7m_exception_string(armv7m->exception_number), \
 309                 shcsr, except_sr, cfsr, except_ar);
 310         return ERROR_OK;
 311 }
 312
 313 static int cortex_m3_debug_entry(target_t *target)
 314 {
 315         int i;
 316         uint32_t xPSR;
 317         int retval;
 318
 319         /* get pointers to arch-specific information */
 320         armv7m_common_t *armv7m = target->arch_info;
 321         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 322         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 323
 324         LOG_DEBUG(" ");
 325         if (armv7m->pre_debug_entry)
 326                 armv7m->pre_debug_entry(target);
 327
 328         cortex_m3_clear_halt(target);
 329         mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 330
 331         if ((retval = armv7m->examine_debug_reason(target)) != ERROR_OK)
 332                 return retval;
 333
 334         /* Examine target state and mode */
 335         /* First load register acessible through core debug port*/
 336         int num_regs = armv7m->core_cache->num_regs;
 337
 338         for (i = 0; i < num_regs; i++)
 339         {
 340                 if (!armv7m->core_cache->reg_list[i].valid)
 341                         armv7m->read_core_reg(target, i);
 342         }
 343
 344         xPSR = buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32);
 345
 346 #ifdef ARMV7_GDB_HACKS
 347         /* copy real xpsr reg for gdb, setting thumb bit */
 348         buf_set_u32(armv7m_gdb_dummy_cpsr_value, 0, 32, xPSR);
 349         buf_set_u32(armv7m_gdb_dummy_cpsr_value, 5, 1, 1);
 350         armv7m_gdb_dummy_cpsr_reg.valid = armv7m->core_cache->reg_list[ARMV7M_xPSR].valid;
 351         armv7m_gdb_dummy_cpsr_reg.dirty = armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty;
 352 #endif
 353
 354         /* For IT instructions xPSR must be reloaded on resume and clear on debug exec */
 355         if (xPSR & 0xf00)
 356         {
 357                 armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty = armv7m->core_cache->reg_list[ARMV7M_xPSR].valid;
 358                 cortex_m3_store_core_reg_u32(target, ARMV7M_REGISTER_CORE_GP, 16, xPSR &~ 0xff);
 359         }
 360
 361         /* Are we in an exception handler */
 362         if (xPSR & 0x1FF)
 363         {
 364                 armv7m->core_mode = ARMV7M_MODE_HANDLER;
 365                 armv7m->exception_number = (xPSR & 0x1FF);
 366         }
 367         else
 368         {
 369                 armv7m->core_mode = buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_CONTROL].value, 0, 1);
 370                 armv7m->exception_number = 0;
 371         }
 372
 373         if (armv7m->exception_number)
 374         {
 375                 cortex_m3_examine_exception_reason(target);
 376         }
 377
 378         LOG_DEBUG("entered debug state in core mode: %s at PC 0x%" PRIx32 ", target->state: %s",
 379                 armv7m_mode_strings[armv7m->core_mode],
 380                 *(uint32_t*)(armv7m->core_cache->reg_list[15].value),
 381                 target_state_name(target));
 382
 383         if (armv7m->post_debug_entry)
 384                 armv7m->post_debug_entry(target);
 385
 386         return ERROR_OK;
 387 }
 388
 389 static int cortex_m3_poll(target_t *target)
 390 {
 391         int retval;
 392         enum target_state prev_target_state = target->state;
 393
 394         /* get pointers to arch-specific information */
 395         armv7m_common_t *armv7m = target->arch_info;
 396         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 397         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 398
 399         /* Read from Debug Halting Control and Status Register */
 400         retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 401         if (retval != ERROR_OK)
 402         {
 403                 target->state = TARGET_UNKNOWN;
 404                 return retval;
 405         }
 406
 407         if (cortex_m3->dcb_dhcsr & S_RESET_ST)
 408         {
 409                 /* check if still in reset */
 410                 mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 411
 412                 if (cortex_m3->dcb_dhcsr & S_RESET_ST)
 413                 {
 414                         target->state = TARGET_RESET;
 415                         return ERROR_OK;
 416                 }
 417         }
 418
 419         if (target->state == TARGET_RESET)
 420         {
 421                 /* Cannot switch context while running so endreset is called with target->state == TARGET_RESET */
 422                 LOG_DEBUG("Exit from reset with dcb_dhcsr 0x%" PRIx32 "", cortex_m3->dcb_dhcsr);
 423                 cortex_m3_endreset_event(target);
 424                 target->state = TARGET_RUNNING;
 425                 prev_target_state = TARGET_RUNNING;
 426         }
 427
 428         if (cortex_m3->dcb_dhcsr & S_HALT)
 429         {
 430                 target->state = TARGET_HALTED;
 431
 432                 if ((prev_target_state == TARGET_RUNNING) || (prev_target_state == TARGET_RESET))
 433                 {
 434                         if ((retval = cortex_m3_debug_entry(target)) != ERROR_OK)
 435                                 return retval;
 436
 437                         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 438                 }
 439                 if (prev_target_state == TARGET_DEBUG_RUNNING)
 440                 {
 441                         LOG_DEBUG(" ");
 442                         if ((retval = cortex_m3_debug_entry(target)) != ERROR_OK)
 443                                 return retval;
 444
 445                         target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 446                 }
 447         }
 448
 449         /* REVISIT when S_SLEEP is set, it's in a Sleep or DeepSleep state.
 450          * How best to model low power modes?
 451          */
 452
 453         if (target->state == TARGET_UNKNOWN)
 454         {
 455                 /* check if processor is retiring instructions */
 456                 if (cortex_m3->dcb_dhcsr & S_RETIRE_ST)
 457                 {
 458                         target->state = TARGET_RUNNING;
 459                         return ERROR_OK;
 460                 }
 461         }
 462
 463         return ERROR_OK;
 464 }
 465
 466 static int cortex_m3_halt(target_t *target)
 467 {
 468         LOG_DEBUG("target->state: %s",
 469                 target_state_name(target));
 470
 471         if (target->state == TARGET_HALTED)
 472         {
 473                 LOG_DEBUG("target was already halted");
 474                 return ERROR_OK;
 475         }
 476
 477         if (target->state == TARGET_UNKNOWN)
 478         {
 479                 LOG_WARNING("target was in unknown state when halt was requested");
 480         }
 481
 482         if (target->state == TARGET_RESET)
 483         {
 484                 if ((jtag_get_reset_config() & RESET_SRST_PULLS_TRST) && jtag_get_srst())
 485                 {
 486                         LOG_ERROR("can't request a halt while in reset if nSRST pulls nTRST");
 487                         return ERROR_TARGET_FAILURE;
 488                 }
 489                 else
 490                 {
 491                         /* we came here in a reset_halt or reset_init sequence
 492                          * debug entry was already prepared in cortex_m3_prepare_reset_halt()
 493                          */
 494                         target->debug_reason = DBG_REASON_DBGRQ;
 495
 496                         return ERROR_OK;
 497                 }
 498         }
 499
 500         /* Write to Debug Halting Control and Status Register */
 501         cortex_m3_write_debug_halt_mask(target, C_HALT, 0);
 502
 503         target->debug_reason = DBG_REASON_DBGRQ;
 504
 505         return ERROR_OK;
 506 }
 507
 508 static int cortex_m3_soft_reset_halt(struct target_s *target)
 509 {
 510         /* get pointers to arch-specific information */
 511         armv7m_common_t *armv7m = target->arch_info;
 512         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 513         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 514         uint32_t dcb_dhcsr = 0;
 515         int retval, timeout = 0;
 516
 517         /* Enter debug state on reset, cf. end_reset_event() */
 518         mem_ap_write_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
 519
 520         /* Request a reset */
 521         mem_ap_write_atomic_u32(swjdp, NVIC_AIRCR, AIRCR_VECTKEY | AIRCR_VECTRESET);
 522         target->state = TARGET_RESET;
 523
 524         /* registers are now invalid */
 525         armv7m_invalidate_core_regs(target);
 526
 527         while (timeout < 100)
 528         {
 529                 retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 530                 if (retval == ERROR_OK)
 531                 {
 532                         mem_ap_read_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
 533                         if ((dcb_dhcsr & S_HALT) && (cortex_m3->nvic_dfsr & DFSR_VCATCH))
 534                         {
 535                                 LOG_DEBUG("system reset-halted, dcb_dhcsr 0x%" PRIx32 ", nvic_dfsr 0x%" PRIx32 "", dcb_dhcsr, cortex_m3->nvic_dfsr);
 536                                 cortex_m3_poll(target);
 537                                 return ERROR_OK;
 538                         }
 539                         else
 540                                 LOG_DEBUG("waiting for system reset-halt, dcb_dhcsr 0x%" PRIx32 ", %i ms", dcb_dhcsr, timeout);
 541                 }
 542                 timeout++;
 543                 alive_sleep(1);
 544         }
 545
 546         return ERROR_OK;
 547 }
 548
 549 static void cortex_m3_enable_breakpoints(struct target_s *target)
 550 {
 551         breakpoint_t *breakpoint = target->breakpoints;
 552
 553         /* set any pending breakpoints */
 554         while (breakpoint)
 555         {
 556                 if (breakpoint->set == 0)
 557                         cortex_m3_set_breakpoint(target, breakpoint);
 558                 breakpoint = breakpoint->next;
 559         }
 560 }
 561
 562 static int cortex_m3_resume(struct target_s *target, int current,
 563                 uint32_t address, int handle_breakpoints, int debug_execution)
 564 {
 565         /* get pointers to arch-specific information */
 566         armv7m_common_t *armv7m = target->arch_info;
 567         breakpoint_t *breakpoint = NULL;
 568         uint32_t resume_pc;
 569
 570         if (target->state != TARGET_HALTED)
 571         {
 572                 LOG_WARNING("target not halted");
 573                 return ERROR_TARGET_NOT_HALTED;
 574         }
 575
 576         if (!debug_execution)
 577         {
 578                 target_free_all_working_areas(target);
 579                 cortex_m3_enable_breakpoints(target);
 580                 cortex_m3_enable_watchpoints(target);
 581         }
 582
 583         if (debug_execution)
 584         {
 585                 /* Disable interrupts */
 586                 /* We disable interrupts in the PRIMASK register instead of masking with C_MASKINTS,
 587                  * This is probably the same issue as Cortex-M3 Errata  377493:
 588                  * C_MASKINTS in parallel with disabled interrupts can cause local faults to not be taken. */
 589                 buf_set_u32(armv7m->core_cache->reg_list[ARMV7M_PRIMASK].value, 0, 32, 1);
 590                 armv7m->core_cache->reg_list[ARMV7M_PRIMASK].dirty = 1;
 591                 armv7m->core_cache->reg_list[ARMV7M_PRIMASK].valid = 1;
 592
 593                 /* Make sure we are in Thumb mode */
 594                 buf_set_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32,
 595                         buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32) | (1 << 24));
 596                 armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty = 1;
 597                 armv7m->core_cache->reg_list[ARMV7M_xPSR].valid = 1;
 598         }
 599
 600         /* current = 1: continue on current pc, otherwise continue at <address> */
 601         if (!current)
 602         {
 603                 buf_set_u32(armv7m->core_cache->reg_list[15].value, 0, 32, address);
 604                 armv7m->core_cache->reg_list[15].dirty = 1;
 605                 armv7m->core_cache->reg_list[15].valid = 1;
 606         }
 607
 608         resume_pc = buf_get_u32(armv7m->core_cache->reg_list[15].value, 0, 32);
 609
 610         armv7m_restore_context(target);
 611
 612         /* the front-end may request us not to handle breakpoints */
 613         if (handle_breakpoints)
 614         {
 615                 /* Single step past breakpoint at current address */
 616                 if ((breakpoint = breakpoint_find(target, resume_pc)))
 617                 {
 618                         LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %d)",
 619                                           breakpoint->address,
 620                                           breakpoint->unique_id );
 621                         cortex_m3_unset_breakpoint(target, breakpoint);
 622                         cortex_m3_single_step_core(target);
 623                         cortex_m3_set_breakpoint(target, breakpoint);
 624                 }
 625         }
 626
 627         /* Restart core */
 628         cortex_m3_write_debug_halt_mask(target, 0, C_HALT);
 629
 630         target->debug_reason = DBG_REASON_NOTHALTED;
 631
 632         /* registers are now invalid */
 633         armv7m_invalidate_core_regs(target);
 634         if (!debug_execution)
 635         {
 636                 target->state = TARGET_RUNNING;
 637                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 638                 LOG_DEBUG("target resumed at 0x%" PRIx32 "", resume_pc);
 639         }
 640         else
 641         {
 642                 target->state = TARGET_DEBUG_RUNNING;
 643                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
 644                 LOG_DEBUG("target debug resumed at 0x%" PRIx32 "", resume_pc);
 645         }
 646
 647         return ERROR_OK;
 648 }
 649
 650 /* int irqstepcount = 0; */
 651 static int cortex_m3_step(struct target_s *target, int current,
 652                 uint32_t address, int handle_breakpoints)
 653 {
 654         /* get pointers to arch-specific information */
 655         armv7m_common_t *armv7m = target->arch_info;
 656         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 657         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 658         breakpoint_t *breakpoint = NULL;
 659
 660         if (target->state != TARGET_HALTED)
 661         {
 662                 LOG_WARNING("target not halted");
 663                 return ERROR_TARGET_NOT_HALTED;
 664         }
 665
 666         /* current = 1: continue on current pc, otherwise continue at <address> */
 667         if (!current)
 668                 buf_set_u32(armv7m->core_cache->reg_list[15].value, 0, 32, address);
 669
 670         /* the front-end may request us not to handle breakpoints */
 671         if (handle_breakpoints)
 672                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv7m->core_cache->reg_list[15].value, 0, 32))))
 673                         cortex_m3_unset_breakpoint(target, breakpoint);
 674
 675         target->debug_reason = DBG_REASON_SINGLESTEP;
 676
 677         armv7m_restore_context(target);
 678
 679         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 680
 681         /* set step and clear halt */
 682         cortex_m3_write_debug_halt_mask(target, C_STEP, C_HALT);
 683         mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 684
 685         /* registers are now invalid */
 686         armv7m_invalidate_core_regs(target);
 687
 688         if (breakpoint)
 689                 cortex_m3_set_breakpoint(target, breakpoint);
 690
 691         LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32 " nvic_icsr = 0x%" PRIx32 "", cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr);
 692
 693         cortex_m3_debug_entry(target);
 694         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 695
 696         LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32 " nvic_icsr = 0x%" PRIx32 "", cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr);
 697         return ERROR_OK;
 698 }
 699
 700 static int cortex_m3_assert_reset(target_t *target)
 701 {
 702         armv7m_common_t *armv7m = target->arch_info;
 703         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 704         swjdp_common_t *swjdp = &armv7m->swjdp_info;
 705         int assert_srst = 1;
 706
 707         LOG_DEBUG("target->state: %s",
 708                 target_state_name(target));
 709
 710         enum reset_types jtag_reset_config = jtag_get_reset_config();
 711
 712         /*
 713          * We can reset Cortex-M3 targets using just the NVIC without
 714          * requiring SRST, getting a SoC reset (or a core-only reset)
 715          * instead of a system reset.
 716          */
 717         if (!(jtag_reset_config & RESET_HAS_SRST))
 718                 assert_srst = 0;
 719
 720         /* Enable debug requests */
 721         mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 722         if (!(cortex_m3->dcb_dhcsr & C_DEBUGEN))
 723                 mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN);
 724
 725         mem_ap_write_u32(swjdp, DCB_DCRDR, 0);
 726
 727         if (!target->reset_halt)
 728         {
 729                 /* Set/Clear C_MASKINTS in a separate operation */
 730                 if (cortex_m3->dcb_dhcsr & C_MASKINTS)
 731                         mem_ap_write_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN | C_HALT);
 732
 733                 /* clear any debug flags before resuming */
 734                 cortex_m3_clear_halt(target);
 735
 736                 /* clear C_HALT in dhcsr reg */
 737                 cortex_m3_write_debug_halt_mask(target, 0, C_HALT);
 738
 739                 /* Enter debug state on reset, cf. end_reset_event() */
 740                 mem_ap_write_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR);
 741         }
 742         else
 743         {
 744                 /* Enter debug state on reset, cf. end_reset_event() */
 745                 mem_ap_write_atomic_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
 746         }
 747
 748         /*
 749          * When nRST is asserted on most Stellaris devices, it clears some of
 750          * the debug state.  The ARMv7M and Cortex-M3 TRMs say that's wrong;
 751          * and OpenOCD depends on those TRMs.  So we won't use SRST on those
 752          * chips.  (Only power-on reset should affect debug state, beyond a
 753          * few specified bits; not the chip's nRST input, wired to SRST.)
 754          *
 755          * REVISIT current errata specs don't seem to cover this issue.
 756          * Do we have more details than this email?
 757          *   https://lists.berlios.de/pipermail
 758          *      /openocd-development/2008-August/003065.html
 759          */
 760         if (strcmp(target->variant, "lm3s") == 0)
 761         {
 762                 /* Check for silicon revisions with the issue. */
 763                 uint32_t did0;
 764
 765                 if (target_read_u32(target, 0x400fe000, &did0) == ERROR_OK)
 766                 {
 767                         switch ((did0 >> 16) & 0xff)
 768                         {
 769                                 case 0:
 770                                         /* all Sandstorm suffer issue */
 771                                         assert_srst = 0;
 772                                         break;
 773
 774                                 case 1:
 775                                 case 3:
 776                                         /* Fury and DustDevil rev A have
 777                                          * this nRST problem.  It should
 778                                          * be fixed in rev B silicon.
 779                                          */
 780                                         if (((did0 >> 8) & 0xff) == 0)
 781                                                 assert_srst = 0;
 782                                         break;
 783                                 case 4:
 784                                         /* Tempest should be fine. */
 785                                         break;
 786                         }
 787                 }
 788         }
 789
 790         if (assert_srst)
 791         {
 792                 /* default to asserting srst */
 793                 if (jtag_reset_config & RESET_SRST_PULLS_TRST)
 794                 {
 795                         jtag_add_reset(1, 1);
 796                 }
 797                 else
 798                 {
 799                         jtag_add_reset(0, 1);
 800                 }
 801         }
 802         else
 803         {
 804                 /* Use a standard Cortex-M3 software reset mechanism.
 805                  * SYSRESETREQ will reset SoC peripherals outside the
 806                  * core, like watchdog timers, if the SoC wires it up
 807                  * correctly.  Else VECRESET can reset just the core.
 808                  */
 809                 mem_ap_write_atomic_u32(swjdp, NVIC_AIRCR,
 810                                 AIRCR_VECTKEY | AIRCR_SYSRESETREQ);
 811                 LOG_DEBUG("Using Cortex-M3 SYSRESETREQ");
 812
 813                 {
 814                         /* I do not know why this is necessary, but it
 815                          * fixes strange effects (step/resume cause NMI
 816                          * after reset) on LM3S6918 -- Michael Schwingen
 817                          */
 818                         uint32_t tmp;
 819                         mem_ap_read_atomic_u32(swjdp, NVIC_AIRCR, &tmp);
 820                 }
 821         }
 822
 823         target->state = TARGET_RESET;
 824         jtag_add_sleep(50000);
 825
 826         armv7m_invalidate_core_regs(target);
 827
 828         if (target->reset_halt)
 829         {
 830                 int retval;
 831                 if ((retval = target_halt(target)) != ERROR_OK)
 832                         return retval;
 833         }
 834
 835         return ERROR_OK;
 836 }
 837
 838 static int cortex_m3_deassert_reset(target_t *target)
 839 {
 840         LOG_DEBUG("target->state: %s",
 841                 target_state_name(target));
 842
 843         /* deassert reset lines */
 844         jtag_add_reset(0, 0);
 845
 846         return ERROR_OK;
 847 }
 848
 849 static int
 850 cortex_m3_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 851 {
 852         int retval;
 853         int fp_num = 0;
 854         uint32_t hilo;
 855
 856         /* get pointers to arch-specific information */
 857         armv7m_common_t *armv7m = target->arch_info;
 858         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 859
 860         cortex_m3_fp_comparator_t * comparator_list = cortex_m3->fp_comparator_list;
 861
 862         if (breakpoint->set)
 863         {
 864                 LOG_WARNING("breakpoint (BPID: %d) already set", breakpoint->unique_id);
 865                 return ERROR_OK;
 866         }
 867
 868         if (cortex_m3->auto_bp_type)
 869         {
 870                 breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
 871         }
 872
 873         if (breakpoint->type == BKPT_HARD)
 874         {
 875                 while (comparator_list[fp_num].used && (fp_num < cortex_m3->fp_num_code))
 876                         fp_num++;
 877                 if (fp_num >= cortex_m3->fp_num_code)
 878                 {
 879                         LOG_DEBUG("ERROR Can not find free FP Comparator");
 880                         LOG_WARNING("ERROR Can not find free FP Comparator");
 881                         exit(-1);
 882                 }
 883                 breakpoint->set = fp_num + 1;
 884                 hilo = (breakpoint->address & 0x2) ? FPCR_REPLACE_BKPT_HIGH : FPCR_REPLACE_BKPT_LOW;
 885                 comparator_list[fp_num].used = 1;
 886                 comparator_list[fp_num].fpcr_value = (breakpoint->address & 0x1FFFFFFC) | hilo | 1;
 887                 target_write_u32(target, comparator_list[fp_num].fpcr_address, comparator_list[fp_num].fpcr_value);
 888                 LOG_DEBUG("fpc_num %i fpcr_value 0x%" PRIx32 "", fp_num, comparator_list[fp_num].fpcr_value);
 889                 if (!cortex_m3->fpb_enabled)
 890                 {
 891                         LOG_DEBUG("FPB wasn't enabled, do it now");
 892                         target_write_u32(target, FP_CTRL, 3);
 893                 }
 894         }
 895         else if (breakpoint->type == BKPT_SOFT)
 896         {
 897                 uint8_t code[4];
 898                 buf_set_u32(code, 0, 32, ARMV7M_T_BKPT(0x11));
 899                 if ((retval = target_read_memory(target, breakpoint->address & 0xFFFFFFFE, breakpoint->length, 1, breakpoint->orig_instr)) != ERROR_OK)
 900                 {
 901                         return retval;
 902                 }
 903                 if ((retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, breakpoint->length, 1, code)) != ERROR_OK)
 904                 {
 905                         return retval;
 906                 }
 907                 breakpoint->set = 0x11; /* Any nice value but 0 */
 908         }
 909
 910         LOG_DEBUG("BPID: %d, Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
 911                           breakpoint->unique_id,
 912                           (int)(breakpoint->type),
 913                           breakpoint->address,
 914                           breakpoint->length,
 915                           breakpoint->set);
 916
 917         return ERROR_OK;
 918 }
 919
 920 static int
 921 cortex_m3_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 922 {
 923         int retval;
 924         /* get pointers to arch-specific information */
 925         armv7m_common_t *armv7m = target->arch_info;
 926         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 927         cortex_m3_fp_comparator_t * comparator_list = cortex_m3->fp_comparator_list;
 928
 929         if (!breakpoint->set)
 930         {
 931                 LOG_WARNING("breakpoint not set");
 932                 return ERROR_OK;
 933         }
 934
 935         LOG_DEBUG("BPID: %d, Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
 936                           breakpoint->unique_id,
 937                           (int)(breakpoint->type),
 938                           breakpoint->address,
 939                           breakpoint->length,
 940                           breakpoint->set);
 941
 942         if (breakpoint->type == BKPT_HARD)
 943         {
 944                 int fp_num = breakpoint->set - 1;
 945                 if ((fp_num < 0) || (fp_num >= cortex_m3->fp_num_code))
 946                 {
 947                         LOG_DEBUG("Invalid FP Comparator number in breakpoint");
 948                         return ERROR_OK;
 949                 }
 950                 comparator_list[fp_num].used = 0;
 951                 comparator_list[fp_num].fpcr_value = 0;
 952                 target_write_u32(target, comparator_list[fp_num].fpcr_address, comparator_list[fp_num].fpcr_value);
 953         }
 954         else
 955         {
 956                 /* restore original instruction (kept in target endianness) */
 957                 if (breakpoint->length == 4)
 958                 {
 959                         if ((retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
 960                         {
 961                                 return retval;
 962                         }
 963                 }
 964                 else
 965                 {
 966                         if ((retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, 2, 1, breakpoint->orig_instr)) != ERROR_OK)
 967                         {
 968                                 return retval;
 969                         }
 970                 }
 971         }
 972         breakpoint->set = 0;
 973
 974         return ERROR_OK;
 975 }
 976
 977 static int
 978 cortex_m3_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 979 {
 980         /* get pointers to arch-specific information */
 981         armv7m_common_t *armv7m = target->arch_info;
 982         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 983
 984         if (cortex_m3->auto_bp_type)
 985         {
 986                 breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
 987 #ifdef ARMV7_GDB_HACKS
 988                 if (breakpoint->length != 2) {
 989                         /* XXX Hack: Replace all breakpoints with length != 2 with
 990                          * a hardware breakpoint. */
 991                         breakpoint->type = BKPT_HARD;
 992                         breakpoint->length = 2;
 993                 }
 994 #endif
 995         }
 996
 997         if ((breakpoint->type == BKPT_HARD) && (breakpoint->address >= 0x20000000))
 998         {
 999                 LOG_INFO("flash patch comparator requested outside code memory region");
1000                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1001         }
1002
1003         if ((breakpoint->type == BKPT_SOFT) && (breakpoint->address < 0x20000000))
1004         {
1005                 LOG_INFO("soft breakpoint requested in code (flash) memory region");
1006                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1007         }
1008
1009         if ((breakpoint->type == BKPT_HARD) && (cortex_m3->fp_code_available < 1))
1010         {
1011                 LOG_INFO("no flash patch comparator unit available for hardware breakpoint");
1012                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1013         }
1014
1015         if ((breakpoint->length != 2))
1016         {
1017                 LOG_INFO("only breakpoints of two bytes length supported");
1018                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1019         }
1020
1021         if (breakpoint->type == BKPT_HARD)
1022                 cortex_m3->fp_code_available--;
1023         cortex_m3_set_breakpoint(target, breakpoint);
1024
1025         return ERROR_OK;
1026 }
1027
1028 static int
1029 cortex_m3_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
1030 {
1031         /* get pointers to arch-specific information */
1032         armv7m_common_t *armv7m = target->arch_info;
1033         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1034
1035         if (target->state != TARGET_HALTED)
1036         {
1037                 LOG_WARNING("target not halted");
1038                 return ERROR_TARGET_NOT_HALTED;
1039         }
1040
1041         if (cortex_m3->auto_bp_type)
1042         {
1043                 breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
1044         }
1045
1046         if (breakpoint->set)
1047         {
1048                 cortex_m3_unset_breakpoint(target, breakpoint);
1049         }
1050
1051         if (breakpoint->type == BKPT_HARD)
1052                 cortex_m3->fp_code_available++;
1053
1054         return ERROR_OK;
1055 }
1056
1057 static int
1058 cortex_m3_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
1059 {
1060         int dwt_num = 0;
1061         uint32_t mask, temp;
1062
1063         /* get pointers to arch-specific information */
1064         armv7m_common_t *armv7m = target->arch_info;
1065         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1066         cortex_m3_dwt_comparator_t * comparator_list = cortex_m3->dwt_comparator_list;
1067
1068         if (watchpoint->set)
1069         {
1070                 LOG_WARNING("watchpoint (%d) already set", watchpoint->unique_id );
1071                 return ERROR_OK;
1072         }
1073
1074         if (watchpoint->mask == 0xffffffffu)
1075         {
1076                 while (comparator_list[dwt_num].used && (dwt_num < cortex_m3->dwt_num_comp))
1077                         dwt_num++;
1078                 if (dwt_num >= cortex_m3->dwt_num_comp)
1079                 {
1080                         LOG_DEBUG("ERROR Can not find free DWT Comparator");
1081                         LOG_WARNING("ERROR Can not find free DWT Comparator");
1082                         return -1;
1083                 }
1084                 watchpoint->set = dwt_num + 1;
1085                 mask = 0;
1086                 temp = watchpoint->length;
1087                 while (temp > 1)
1088                 {
1089                         temp = temp / 2;
1090                         mask++;
1091                 }
1092                 comparator_list[dwt_num].used = 1;
1093                 comparator_list[dwt_num].comp = watchpoint->address;
1094                 comparator_list[dwt_num].mask = mask;
1095                 comparator_list[dwt_num].function = watchpoint->rw + 5;
1096                 target_write_u32(target, comparator_list[dwt_num].dwt_comparator_address, comparator_list[dwt_num].comp);
1097                 target_write_u32(target, comparator_list[dwt_num].dwt_comparator_address | 0x4, comparator_list[dwt_num].mask);
1098                 target_write_u32(target, comparator_list[dwt_num].dwt_comparator_address | 0x8, comparator_list[dwt_num].function);
1099                 LOG_DEBUG("dwt_num %i 0x%" PRIx32 " 0x%" PRIx32 " 0x%" PRIx32 "", dwt_num, comparator_list[dwt_num].comp, comparator_list[dwt_num].mask, comparator_list[dwt_num].function);
1100         }
1101         else
1102         {
1103                 /* Move this test to add_watchpoint */
1104                 LOG_WARNING("Cannot watch data values (id: %d)",
1105                                   watchpoint->unique_id );
1106                 return ERROR_OK;
1107         }
1108         LOG_DEBUG("Watchpoint (ID: %d) address: 0x%08" PRIx32 " set=%d ",
1109                           watchpoint->unique_id, watchpoint->address, watchpoint->set );
1110         return ERROR_OK;
1111
1112 }
1113
1114 static int
1115 cortex_m3_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
1116 {
1117         /* get pointers to arch-specific information */
1118         armv7m_common_t *armv7m = target->arch_info;
1119         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1120         cortex_m3_dwt_comparator_t * comparator_list = cortex_m3->dwt_comparator_list;
1121         int dwt_num;
1122
1123         if (!watchpoint->set)
1124         {
1125                 LOG_WARNING("watchpoint (wpid: %d) not set", watchpoint->unique_id );
1126                 return ERROR_OK;
1127         }
1128
1129         LOG_DEBUG("Watchpoint (ID: %d) address: 0x%08" PRIx32 " set=%d ",
1130                           watchpoint->unique_id, watchpoint->address,watchpoint->set );
1131
1132         dwt_num = watchpoint->set - 1;
1133
1134         if ((dwt_num < 0) || (dwt_num >= cortex_m3->dwt_num_comp))
1135         {
1136                 LOG_DEBUG("Invalid DWT Comparator number in watchpoint");
1137                 return ERROR_OK;
1138         }
1139         comparator_list[dwt_num].used = 0;
1140         comparator_list[dwt_num].function = 0;
1141         target_write_u32(target, comparator_list[dwt_num].dwt_comparator_address | 0x8, comparator_list[dwt_num].function);
1142
1143         watchpoint->set = 0;
1144
1145         return ERROR_OK;
1146 }
1147
1148 static int
1149 cortex_m3_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
1150 {
1151         /* get pointers to arch-specific information */
1152         armv7m_common_t *armv7m = target->arch_info;
1153         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1154
1155         if (target->state != TARGET_HALTED)
1156         {
1157                 LOG_WARNING("target not halted");
1158                 return ERROR_TARGET_NOT_HALTED;
1159         }
1160
1161         if (cortex_m3->dwt_comp_available < 1)
1162         {
1163                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1164         }
1165
1166         if ((watchpoint->length != 1) && (watchpoint->length != 2) && (watchpoint->length != 4))
1167         {
1168                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1169         }
1170
1171         cortex_m3->dwt_comp_available--;
1172         LOG_DEBUG("dwt_comp_available: %d", cortex_m3->dwt_comp_available);
1173
1174         return ERROR_OK;
1175 }
1176
1177 static int
1178 cortex_m3_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
1179 {
1180         /* get pointers to arch-specific information */
1181         armv7m_common_t *armv7m = target->arch_info;
1182         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1183
1184         if (target->state != TARGET_HALTED)
1185         {
1186                 LOG_WARNING("target not halted");
1187                 return ERROR_TARGET_NOT_HALTED;
1188         }
1189
1190         if (watchpoint->set)
1191         {
1192                 cortex_m3_unset_watchpoint(target, watchpoint);
1193         }
1194
1195         cortex_m3->dwt_comp_available++;
1196         LOG_DEBUG("dwt_comp_available: %d", cortex_m3->dwt_comp_available);
1197
1198         return ERROR_OK;
1199 }
1200
1201 static void cortex_m3_enable_watchpoints(struct target_s *target)
1202 {
1203         watchpoint_t *watchpoint = target->watchpoints;
1204
1205         /* set any pending watchpoints */
1206         while (watchpoint)
1207         {
1208                 if (watchpoint->set == 0)
1209                         cortex_m3_set_watchpoint(target, watchpoint);
1210                 watchpoint = watchpoint->next;
1211         }
1212 }
1213
1214 static int cortex_m3_load_core_reg_u32(struct target_s *target,
1215                 enum armv7m_regtype type, uint32_t num, uint32_t * value)
1216 {
1217         int retval;
1218         /* get pointers to arch-specific information */
1219         armv7m_common_t *armv7m = target->arch_info;
1220         swjdp_common_t *swjdp = &armv7m->swjdp_info;
1221
1222         /* NOTE:  we "know" here that the register identifiers used
1223          * in the v7m header match the Cortex-M3 Debug Core Register
1224          * Selector values for R0..R15, xPSR, MSP, and PSP.
1225          */
1226         switch (num) {
1227         case 0 ... 18:
1228                 /* read a normal core register */
1229                 retval = cortexm3_dap_read_coreregister_u32(swjdp, value, num);
1230
1231                 if (retval != ERROR_OK)
1232                 {
1233                         LOG_ERROR("JTAG failure %i",retval);
1234                         return ERROR_JTAG_DEVICE_ERROR;
1235                 }
1236                 LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "",(int)num,*value);
1237                 break;
1238
1239         case ARMV7M_PRIMASK:
1240         case ARMV7M_BASEPRI:
1241         case ARMV7M_FAULTMASK:
1242         case ARMV7M_CONTROL:
1243                 /* Cortex-M3 packages these four registers as bitfields
1244                  * in one Debug Core register.  So say r0 and r2 docs;
1245                  * it was removed from r1 docs, but still works.
1246                  */
1247                 cortexm3_dap_read_coreregister_u32(swjdp, value, 20);
1248
1249                 switch (num)
1250                 {
1251                         case ARMV7M_PRIMASK:
1252                                 *value = buf_get_u32((uint8_t*)value, 0, 1);
1253                                 break;
1254
1255                         case ARMV7M_BASEPRI:
1256                                 *value = buf_get_u32((uint8_t*)value, 8, 8);
1257                                 break;
1258
1259                         case ARMV7M_FAULTMASK:
1260                                 *value = buf_get_u32((uint8_t*)value, 16, 1);
1261                                 break;
1262
1263                         case ARMV7M_CONTROL:
1264                                 *value = buf_get_u32((uint8_t*)value, 24, 2);
1265                                 break;
1266                 }
1267
1268                 LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "", (int)num, *value);
1269                 break;
1270
1271         default:
1272                 return ERROR_INVALID_ARGUMENTS;
1273         }
1274
1275         return ERROR_OK;
1276 }
1277
1278 static int cortex_m3_store_core_reg_u32(struct target_s *target,
1279                 enum armv7m_regtype type, uint32_t num, uint32_t value)
1280 {
1281         int retval;
1282         uint32_t reg;
1283
1284         /* get pointers to arch-specific information */
1285         armv7m_common_t *armv7m = target->arch_info;
1286         swjdp_common_t *swjdp = &armv7m->swjdp_info;
1287
1288 #ifdef ARMV7_GDB_HACKS
1289         /* If the LR register is being modified, make sure it will put us
1290          * in "thumb" mode, or an INVSTATE exception will occur. This is a
1291          * hack to deal with the fact that gdb will sometimes "forge"
1292          * return addresses, and doesn't set the LSB correctly (i.e., when
1293          * printing expressions containing function calls, it sets LR = 0.)
1294          * Valid exception return codes have bit 0 set too.
1295          */
1296         if (num == ARMV7M_R14)
1297                 value |= 0x01;
1298 #endif
1299
1300         /* NOTE:  we "know" here that the register identifiers used
1301          * in the v7m header match the Cortex-M3 Debug Core Register
1302          * Selector values for R0..R15, xPSR, MSP, and PSP.
1303          */
1304         switch (num) {
1305         case 0 ... 18:
1306                 retval = cortexm3_dap_write_coreregister_u32(swjdp, value, num);
1307                 if (retval != ERROR_OK)
1308                 {
1309                         LOG_ERROR("JTAG failure %i", retval);
1310                         armv7m->core_cache->reg_list[num].dirty = armv7m->core_cache->reg_list[num].valid;
1311                         return ERROR_JTAG_DEVICE_ERROR;
1312                 }
1313                 LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
1314                 break;
1315
1316         case ARMV7M_PRIMASK:
1317         case ARMV7M_BASEPRI:
1318         case ARMV7M_FAULTMASK:
1319         case ARMV7M_CONTROL:
1320                 /* Cortex-M3 packages these four registers as bitfields
1321                  * in one Debug Core register.  So say r0 and r2 docs;
1322                  * it was removed from r1 docs, but still works.
1323                  */
1324                 cortexm3_dap_read_coreregister_u32(swjdp, &reg, 20);
1325
1326                 switch (num)
1327                 {
1328                         case ARMV7M_PRIMASK:
1329                                 buf_set_u32((uint8_t*)&reg, 0, 1, value);
1330                                 break;
1331
1332                         case ARMV7M_BASEPRI:
1333                                 buf_set_u32((uint8_t*)&reg, 8, 8, value);
1334                                 break;
1335
1336                         case ARMV7M_FAULTMASK:
1337                                 buf_set_u32((uint8_t*)&reg, 16, 1, value);
1338                                 break;
1339
1340                         case ARMV7M_CONTROL:
1341                                 buf_set_u32((uint8_t*)&reg, 24, 2, value);
1342                                 break;
1343                 }
1344
1345                 cortexm3_dap_write_coreregister_u32(swjdp, reg, 20);
1346
1347                 LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
1348                 break;
1349
1350         default:
1351                 return ERROR_INVALID_ARGUMENTS;
1352         }
1353
1354         return ERROR_OK;
1355 }
1356
1357 static int cortex_m3_read_memory(struct target_s *target, uint32_t address,
1358                 uint32_t size, uint32_t count, uint8_t *buffer)
1359 {
1360         /* get pointers to arch-specific information */
1361         armv7m_common_t *armv7m = target->arch_info;
1362         swjdp_common_t *swjdp = &armv7m->swjdp_info;
1363         int retval;
1364
1365         /* sanitize arguments */
1366         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1367                 return ERROR_INVALID_ARGUMENTS;
1368
1369         /* cortex_m3 handles unaligned memory access */
1370
1371         switch (size)
1372         {
1373                 case 4:
1374                         retval = mem_ap_read_buf_u32(swjdp, buffer, 4 * count, address);
1375                         break;
1376                 case 2:
1377                         retval = mem_ap_read_buf_u16(swjdp, buffer, 2 * count, address);
1378                         break;
1379                 case 1:
1380                         retval = mem_ap_read_buf_u8(swjdp, buffer, count, address);
1381                         break;
1382                 default:
1383                         LOG_ERROR("BUG: we shouldn't get here");
1384                         exit(-1);
1385         }
1386
1387         return retval;
1388 }
1389
1390 static int cortex_m3_write_memory(struct target_s *target, uint32_t address,
1391                 uint32_t size, uint32_t count, uint8_t *buffer)
1392 {
1393         /* get pointers to arch-specific information */
1394         armv7m_common_t *armv7m = target->arch_info;
1395         swjdp_common_t *swjdp = &armv7m->swjdp_info;
1396         int retval;
1397
1398         /* sanitize arguments */
1399         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1400                 return ERROR_INVALID_ARGUMENTS;
1401
1402         switch (size)
1403         {
1404                 case 4:
1405                         retval = mem_ap_write_buf_u32(swjdp, buffer, 4 * count, address);
1406                         break;
1407                 case 2:
1408                         retval = mem_ap_write_buf_u16(swjdp, buffer, 2 * count, address);
1409                         break;
1410                 case 1:
1411                         retval = mem_ap_write_buf_u8(swjdp, buffer, count, address);
1412                         break;
1413                 default:
1414                         LOG_ERROR("BUG: we shouldn't get here");
1415                         exit(-1);
1416         }
1417
1418         return retval;
1419 }
1420
1421 static int cortex_m3_bulk_write_memory(target_t *target, uint32_t address,
1422                 uint32_t count, uint8_t *buffer)
1423 {
1424         return cortex_m3_write_memory(target, address, 4, count, buffer);
1425 }
1426
1427 static void cortex_m3_build_reg_cache(target_t *target)
1428 {
1429         armv7m_build_reg_cache(target);
1430 }
1431
1432 static int cortex_m3_init_target(struct command_context_s *cmd_ctx,
1433                 struct target_s *target)
1434 {
1435         cortex_m3_build_reg_cache(target);
1436         return ERROR_OK;
1437 }
1438
1439 static int cortex_m3_examine(struct target_s *target)
1440 {
1441         int retval;
1442         uint32_t cpuid, fpcr, dwtcr, ictr;
1443         int i;
1444
1445         /* get pointers to arch-specific information */
1446         armv7m_common_t *armv7m = target->arch_info;
1447         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1448         swjdp_common_t *swjdp = &armv7m->swjdp_info;
1449
1450         if ((retval = ahbap_debugport_init(swjdp)) != ERROR_OK)
1451                 return retval;
1452
1453         if (!target_was_examined(target))
1454         {
1455                 target_set_examined(target);
1456
1457                 /* Read from Device Identification Registers */
1458                 if ((retval = target_read_u32(target, CPUID, &cpuid)) != ERROR_OK)
1459                         return retval;
1460
1461                 if (((cpuid >> 4) & 0xc3f) == 0xc23)
1462                         LOG_DEBUG("CORTEX-M3 processor detected");
1463                 LOG_DEBUG("cpuid: 0x%8.8" PRIx32 "", cpuid);
1464
1465                 target_read_u32(target, NVIC_ICTR, &ictr);
1466                 cortex_m3->intlinesnum = (ictr & 0x1F) + 1;
1467                 cortex_m3->intsetenable = calloc(cortex_m3->intlinesnum, 4);
1468                 for (i = 0; i < cortex_m3->intlinesnum; i++)
1469                 {
1470                         target_read_u32(target, NVIC_ISE0 + 4 * i, cortex_m3->intsetenable + i);
1471                         LOG_DEBUG("interrupt enable[%i] = 0x%8.8" PRIx32 "", i, cortex_m3->intsetenable[i]);
1472                 }
1473
1474                 /* Setup FPB */
1475                 target_read_u32(target, FP_CTRL, &fpcr);
1476                 cortex_m3->auto_bp_type = 1;
1477                 cortex_m3->fp_num_code = ((fpcr >> 8) & 0x70) | ((fpcr >> 4) & 0xF); /* bits [14:12] and [7:4] */
1478                 cortex_m3->fp_num_lit = (fpcr >> 8) & 0xF;
1479                 cortex_m3->fp_code_available = cortex_m3->fp_num_code;
1480                 cortex_m3->fp_comparator_list = calloc(cortex_m3->fp_num_code + cortex_m3->fp_num_lit, sizeof(cortex_m3_fp_comparator_t));
1481                 cortex_m3->fpb_enabled = fpcr & 1;
1482                 for (i = 0; i < cortex_m3->fp_num_code + cortex_m3->fp_num_lit; i++)
1483                 {
1484                         cortex_m3->fp_comparator_list[i].type = (i < cortex_m3->fp_num_code) ? FPCR_CODE : FPCR_LITERAL;
1485                         cortex_m3->fp_comparator_list[i].fpcr_address = FP_COMP0 + 4 * i;
1486                 }
1487                 LOG_DEBUG("FPB fpcr 0x%" PRIx32 ", numcode %i, numlit %i", fpcr, cortex_m3->fp_num_code, cortex_m3->fp_num_lit);
1488
1489                 /* Setup DWT */
1490                 target_read_u32(target, DWT_CTRL, &dwtcr);
1491                 cortex_m3->dwt_num_comp = (dwtcr >> 28) & 0xF;
1492                 cortex_m3->dwt_comp_available = cortex_m3->dwt_num_comp;
1493                 cortex_m3->dwt_comparator_list = calloc(cortex_m3->dwt_num_comp, sizeof(cortex_m3_dwt_comparator_t));
1494                 for (i = 0; i < cortex_m3->dwt_num_comp; i++)
1495                 {
1496                         cortex_m3->dwt_comparator_list[i].dwt_comparator_address = DWT_COMP0 + 0x10 * i;
1497                 }
1498         }
1499
1500         return ERROR_OK;
1501 }
1502
1503 static int cortex_m3_quit(void)
1504 {
1505         return ERROR_OK;
1506 }
1507
1508 static int cortex_m3_dcc_read(swjdp_common_t *swjdp, uint8_t *value, uint8_t *ctrl)
1509 {
1510         uint16_t dcrdr;
1511
1512         mem_ap_read_buf_u16(swjdp, (uint8_t*)&dcrdr, 1, DCB_DCRDR);
1513         *ctrl = (uint8_t)dcrdr;
1514         *value = (uint8_t)(dcrdr >> 8);
1515
1516         LOG_DEBUG("data 0x%x ctrl 0x%x", *value, *ctrl);
1517
1518         /* write ack back to software dcc register
1519          * signify we have read data */
1520         if (dcrdr & (1 << 0))
1521         {
1522                 dcrdr = 0;
1523                 mem_ap_write_buf_u16(swjdp, (uint8_t*)&dcrdr, 1, DCB_DCRDR);
1524         }
1525
1526         return ERROR_OK;
1527 }
1528
1529 static int cortex_m3_target_request_data(target_t *target,
1530                 uint32_t size, uint8_t *buffer)
1531 {
1532         armv7m_common_t *armv7m = target->arch_info;
1533         swjdp_common_t *swjdp = &armv7m->swjdp_info;
1534         uint8_t data;
1535         uint8_t ctrl;
1536         uint32_t i;
1537
1538         for (i = 0; i < (size * 4); i++)
1539         {
1540                 cortex_m3_dcc_read(swjdp, &data, &ctrl);
1541                 buffer[i] = data;
1542         }
1543
1544         return ERROR_OK;
1545 }
1546
1547 static int cortex_m3_handle_target_request(void *priv)
1548 {
1549         target_t *target = priv;
1550         if (!target_was_examined(target))
1551                 return ERROR_OK;
1552         armv7m_common_t *armv7m = target->arch_info;
1553         swjdp_common_t *swjdp = &armv7m->swjdp_info;
1554
1555         if (!target->dbg_msg_enabled)
1556                 return ERROR_OK;
1557
1558         if (target->state == TARGET_RUNNING)
1559         {
1560                 uint8_t data;
1561                 uint8_t ctrl;
1562
1563                 cortex_m3_dcc_read(swjdp, &data, &ctrl);
1564
1565                 /* check if we have data */
1566                 if (ctrl & (1 << 0))
1567                 {
1568                         uint32_t request;
1569
1570                         /* we assume target is quick enough */
1571                         request = data;
1572                         cortex_m3_dcc_read(swjdp, &data, &ctrl);
1573                         request |= (data << 8);
1574                         cortex_m3_dcc_read(swjdp, &data, &ctrl);
1575                         request |= (data << 16);
1576                         cortex_m3_dcc_read(swjdp, &data, &ctrl);
1577                         request |= (data << 24);
1578                         target_request(target, request);
1579                 }
1580         }
1581
1582         return ERROR_OK;
1583 }
1584
1585 static int cortex_m3_init_arch_info(target_t *target,
1586                 cortex_m3_common_t *cortex_m3, jtag_tap_t *tap)
1587 {
1588         int retval;
1589         armv7m_common_t *armv7m;
1590         armv7m = &cortex_m3->armv7m;
1591
1592         armv7m_init_arch_info(target, armv7m);
1593
1594         /* prepare JTAG information for the new target */
1595         cortex_m3->jtag_info.tap = tap;
1596         cortex_m3->jtag_info.scann_size = 4;
1597
1598         armv7m->swjdp_info.dp_select_value = -1;
1599         armv7m->swjdp_info.ap_csw_value = -1;
1600         armv7m->swjdp_info.ap_tar_value = -1;
1601         armv7m->swjdp_info.jtag_info = &cortex_m3->jtag_info;
1602         armv7m->swjdp_info.memaccess_tck = 8;
1603         armv7m->swjdp_info.tar_autoincr_block = (1 << 12);      /* Cortex-M3 has 4096 bytes autoincrement range */
1604
1605         /* initialize arch-specific breakpoint handling */
1606
1607         cortex_m3->common_magic = CORTEX_M3_COMMON_MAGIC;
1608         cortex_m3->arch_info = NULL;
1609
1610         /* register arch-specific functions */
1611         armv7m->examine_debug_reason = cortex_m3_examine_debug_reason;
1612
1613         armv7m->pre_debug_entry = NULL;
1614         armv7m->post_debug_entry = NULL;
1615
1616         armv7m->pre_restore_context = NULL;
1617         armv7m->post_restore_context = NULL;
1618
1619         armv7m->arch_info = cortex_m3;
1620         armv7m->load_core_reg_u32 = cortex_m3_load_core_reg_u32;
1621         armv7m->store_core_reg_u32 = cortex_m3_store_core_reg_u32;
1622
1623         target_register_timer_callback(cortex_m3_handle_target_request, 1, 1, target);
1624
1625         if ((retval = arm_jtag_setup_connection(&cortex_m3->jtag_info)) != ERROR_OK)
1626         {
1627                 return retval;
1628         }
1629
1630         return ERROR_OK;
1631 }
1632
1633 static int cortex_m3_target_create(struct target_s *target, Jim_Interp *interp)
1634 {
1635         cortex_m3_common_t *cortex_m3 = calloc(1,sizeof(cortex_m3_common_t));
1636
1637         cortex_m3_init_arch_info(target, cortex_m3, target->tap);
1638
1639         return ERROR_OK;
1640 }
1641
1642 /*
1643  * REVISIT Thumb2 disassembly should work for all ARMv7 cores, as well
1644  * as at least ARM-1156T2.  The interesting thing about Cortex-M is
1645  * that *only* Thumb2 disassembly matters.  There are also some small
1646  * additions to Thumb2 that are specific to ARMv7-M.
1647  */
1648 static int
1649 handle_cortex_m3_disassemble_command(struct command_context_s *cmd_ctx,
1650                 char *cmd, char **args, int argc)
1651 {
1652         int retval = ERROR_OK;
1653         target_t *target = get_current_target(cmd_ctx);
1654         uint32_t address;
1655         unsigned long count = 1;
1656         arm_instruction_t cur_instruction;
1657
1658         errno = 0;
1659         switch (argc) {
1660         case 2:
1661                 count = strtoul(args[1], NULL, 0);
1662                 if (errno)
1663                         return ERROR_FAIL;
1664                 /* FALL THROUGH */
1665         case 1:
1666                 address = strtoul(args[0], NULL, 0);
1667                 if (errno)
1668                         return ERROR_FAIL;
1669                 break;
1670         default:
1671                 command_print(cmd_ctx,
1672                         "usage: cortex_m3 disassemble <address> [<count>]");
1673                 return ERROR_OK;
1674         }
1675
1676         while (count--) {
1677                 retval = thumb2_opcode(target, address, &cur_instruction);
1678                 if (retval != ERROR_OK)
1679                         return retval;
1680                 command_print(cmd_ctx, "%s", cur_instruction.text);
1681                 address += cur_instruction.instruction_size;
1682         }
1683
1684         return ERROR_OK;
1685 }
1686
1687 static const struct {
1688         char name[10];
1689         unsigned mask;
1690 } vec_ids[] = {
1691         { "hard_err",   VC_HARDERR, },
1692         { "int_err",    VC_INTERR, },
1693         { "bus_err",    VC_BUSERR, },
1694         { "state_err",  VC_STATERR, },
1695         { "chk_err",    VC_CHKERR, },
1696         { "nocp_err",   VC_NOCPERR, },
1697         { "mm_err",     VC_MMERR, },
1698         { "reset",      VC_CORERESET, },
1699 };
1700
1701 static int
1702 handle_cortex_m3_vector_catch_command(struct command_context_s *cmd_ctx,
1703                 char *cmd, char **argv, int argc)
1704 {
1705         target_t *target = get_current_target(cmd_ctx);
1706         armv7m_common_t *armv7m = target->arch_info;
1707         swjdp_common_t *swjdp = &armv7m->swjdp_info;
1708         uint32_t demcr = 0;
1709         int i;
1710
1711         mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
1712
1713         if (argc > 0) {
1714                 unsigned catch = 0;
1715
1716                 if (argc == 1) {
1717                         if (strcmp(argv[0], "all") == 0) {
1718                                 catch = VC_HARDERR | VC_INTERR | VC_BUSERR
1719                                         | VC_STATERR | VC_CHKERR | VC_NOCPERR
1720                                         | VC_MMERR | VC_CORERESET;
1721                                 goto write;
1722                         } else if (strcmp(argv[0], "none") == 0) {
1723                                 goto write;
1724                         }
1725                 }
1726                 while (argc-- > 0) {
1727                         for (i = 0; i < ARRAY_SIZE(vec_ids); i++) {
1728                                 if (strcmp(argv[argc], vec_ids[i].name) != 0)
1729                                         continue;
1730                                 catch |= vec_ids[i].mask;
1731                                 break;
1732                         }
1733                         if (i == ARRAY_SIZE(vec_ids)) {
1734                                 LOG_ERROR("No CM3 vector '%s'", argv[argc]);
1735                                 return ERROR_INVALID_ARGUMENTS;
1736                         }
1737                 }
1738 write:
1739                 demcr &= ~0xffff;
1740                 demcr |= catch;
1741
1742                 /* write, but don't assume it stuck */
1743                 mem_ap_write_u32(swjdp, DCB_DEMCR, demcr);
1744                 mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
1745         }
1746
1747         for (i = 0; i < ARRAY_SIZE(vec_ids); i++)
1748                 command_print(cmd_ctx, "%9s: %s", vec_ids[i].name,
1749                         (demcr & vec_ids[i].mask) ? "catch" : "ignore");
1750
1751         return ERROR_OK;
1752 }
1753
1754 static int
1755 handle_cortex_m3_mask_interrupts_command(struct command_context_s *cmd_ctx,
1756                 char *cmd, char **args, int argc)
1757 {
1758         target_t *target = get_current_target(cmd_ctx);
1759         armv7m_common_t *armv7m = target->arch_info;
1760         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1761
1762         if (target->state != TARGET_HALTED)
1763         {
1764                 command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
1765                 return ERROR_OK;
1766         }
1767
1768         if (argc > 0)
1769         {
1770                 if (!strcmp(args[0], "on"))
1771                 {
1772                         cortex_m3_write_debug_halt_mask(target, C_HALT | C_MASKINTS, 0);
1773                 }
1774                 else if (!strcmp(args[0], "off"))
1775                 {
1776                         cortex_m3_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
1777                 }
1778                 else
1779                 {
1780                         command_print(cmd_ctx, "usage: cortex_m3 maskisr ['on'|'off']");
1781                 }
1782         }
1783
1784         command_print(cmd_ctx, "cortex_m3 interrupt mask %s",
1785                         (cortex_m3->dcb_dhcsr & C_MASKINTS) ? "on" : "off");
1786
1787         return ERROR_OK;
1788 }
1789
1790 static int cortex_m3_register_commands(struct command_context_s *cmd_ctx)
1791 {
1792         int retval;
1793         command_t *cortex_m3_cmd;
1794
1795         retval = armv7m_register_commands(cmd_ctx);
1796
1797         cortex_m3_cmd = register_command(cmd_ctx, NULL, "cortex_m3",
1798                         NULL, COMMAND_ANY, "cortex_m3 specific commands");
1799
1800         register_command(cmd_ctx, cortex_m3_cmd, "disassemble",
1801                         handle_cortex_m3_disassemble_command, COMMAND_EXEC,
1802                         "disassemble Thumb2 instructions <address> [<count>]");
1803         register_command(cmd_ctx, cortex_m3_cmd, "maskisr",
1804                         handle_cortex_m3_mask_interrupts_command, COMMAND_EXEC,
1805                         "mask cortex_m3 interrupts ['on'|'off']");
1806         register_command(cmd_ctx, cortex_m3_cmd, "vector_catch",
1807                         handle_cortex_m3_vector_catch_command, COMMAND_EXEC,
1808                         "catch hardware vectors ['all'|'none'|<list>]");
1809
1810         return retval;
1811 }
1812
1813 target_type_t cortexm3_target =
1814 {
1815         .name = "cortex_m3",
1816
1817         .poll = cortex_m3_poll,
1818         .arch_state = armv7m_arch_state,
1819
1820         .target_request_data = cortex_m3_target_request_data,
1821
1822         .halt = cortex_m3_halt,
1823         .resume = cortex_m3_resume,
1824         .step = cortex_m3_step,
1825
1826         .assert_reset = cortex_m3_assert_reset,
1827         .deassert_reset = cortex_m3_deassert_reset,
1828         .soft_reset_halt = cortex_m3_soft_reset_halt,
1829
1830         .get_gdb_reg_list = armv7m_get_gdb_reg_list,
1831
1832         .read_memory = cortex_m3_read_memory,
1833         .write_memory = cortex_m3_write_memory,
1834         .bulk_write_memory = cortex_m3_bulk_write_memory,
1835         .checksum_memory = armv7m_checksum_memory,
1836         .blank_check_memory = armv7m_blank_check_memory,
1837
1838         .run_algorithm = armv7m_run_algorithm,
1839
1840         .add_breakpoint = cortex_m3_add_breakpoint,
1841         .remove_breakpoint = cortex_m3_remove_breakpoint,
1842         .add_watchpoint = cortex_m3_add_watchpoint,
1843         .remove_watchpoint = cortex_m3_remove_watchpoint,
1844
1845         .register_commands = cortex_m3_register_commands,
1846         .target_create = cortex_m3_target_create,
1847         .init_target = cortex_m3_init_target,
1848         .examine = cortex_m3_examine,
1849         .quit = cortex_m3_quit
1850 };
1851