git.sur5r.net Git - openocd/blob - src/target/cortex_m.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  *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 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 "jtag/interface.h"
  35 #include "breakpoints.h"
  36 #include "cortex_m.h"
  37 #include "target_request.h"
  38 #include "target_type.h"
  39 #include "arm_disassembler.h"
  40 #include "register.h"
  41 #include "arm_opcodes.h"
  42 #include "arm_semihosting.h"
  43 #include <helper/time_support.h>
  44
  45 /* NOTE:  most of this should work fine for the Cortex-M1 and
  46  * Cortex-M0 cores too, although they're ARMv6-M not ARMv7-M.
  47  * Some differences:  M0/M1 doesn't have FBP remapping or the
  48  * DWT tracing/profiling support.  (So the cycle counter will
  49  * not be usable; the other stuff isn't currently used here.)
  50  *
  51  * Although there are some workarounds for errata seen only in r0p0
  52  * silicon, such old parts are hard to find and thus not much tested
  53  * any longer.
  54  */
  55
  56 /**
  57  * Returns the type of a break point required by address location
  58  */
  59 #define BKPT_TYPE_BY_ADDR(addr) ((addr) < 0x20000000 ? BKPT_HARD : BKPT_SOFT)
  60
  61 /* forward declarations */
  62 static int cortex_m_store_core_reg_u32(struct target *target,
  63                 uint32_t num, uint32_t value);
  64
  65 static int cortexm_dap_read_coreregister_u32(struct target *target,
  66         uint32_t *value, int regnum)
  67 {
  68         struct armv7m_common *armv7m = target_to_armv7m(target);
  69         struct adiv5_dap *swjdp = armv7m->arm.dap;
  70         int retval;
  71         uint32_t dcrdr;
  72
  73         /* because the DCB_DCRDR is used for the emulated dcc channel
  74          * we have to save/restore the DCB_DCRDR when used */
  75         if (target->dbg_msg_enabled) {
  76                 retval = mem_ap_read_u32(swjdp, DCB_DCRDR, &dcrdr);
  77                 if (retval != ERROR_OK)
  78                         return retval;
  79         }
  80
  81         retval = mem_ap_write_u32(swjdp, DCB_DCRSR, regnum);
  82         if (retval != ERROR_OK)
  83                 return retval;
  84
  85         retval = mem_ap_read_atomic_u32(swjdp, DCB_DCRDR, value);
  86         if (retval != ERROR_OK)
  87                 return retval;
  88
  89         if (target->dbg_msg_enabled) {
  90                 /* restore DCB_DCRDR - this needs to be in a separate
  91                  * transaction otherwise the emulated DCC channel breaks */
  92                 if (retval == ERROR_OK)
  93                         retval = mem_ap_write_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
  94         }
  95
  96         return retval;
  97 }
  98
  99 static int cortexm_dap_write_coreregister_u32(struct target *target,
 100         uint32_t value, int regnum)
 101 {
 102         struct armv7m_common *armv7m = target_to_armv7m(target);
 103         struct adiv5_dap *swjdp = armv7m->arm.dap;
 104         int retval;
 105         uint32_t dcrdr;
 106
 107         /* because the DCB_DCRDR is used for the emulated dcc channel
 108          * we have to save/restore the DCB_DCRDR when used */
 109         if (target->dbg_msg_enabled) {
 110                 retval = mem_ap_read_u32(swjdp, DCB_DCRDR, &dcrdr);
 111                 if (retval != ERROR_OK)
 112                         return retval;
 113         }
 114
 115         retval = mem_ap_write_u32(swjdp, DCB_DCRDR, value);
 116         if (retval != ERROR_OK)
 117                 return retval;
 118
 119         retval = mem_ap_write_atomic_u32(swjdp, DCB_DCRSR, regnum | DCRSR_WnR);
 120         if (retval != ERROR_OK)
 121                 return retval;
 122
 123         if (target->dbg_msg_enabled) {
 124                 /* restore DCB_DCRDR - this needs to be in a seperate
 125                  * transaction otherwise the emulated DCC channel breaks */
 126                 if (retval == ERROR_OK)
 127                         retval = mem_ap_write_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
 128         }
 129
 130         return retval;
 131 }
 132
 133 static int cortex_m_write_debug_halt_mask(struct target *target,
 134         uint32_t mask_on, uint32_t mask_off)
 135 {
 136         struct cortex_m_common *cortex_m = target_to_cm(target);
 137         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
 138
 139         /* mask off status bits */
 140         cortex_m->dcb_dhcsr &= ~((0xFFFF << 16) | mask_off);
 141         /* create new register mask */
 142         cortex_m->dcb_dhcsr |= DBGKEY | C_DEBUGEN | mask_on;
 143
 144         return mem_ap_write_atomic_u32(swjdp, DCB_DHCSR, cortex_m->dcb_dhcsr);
 145 }
 146
 147 static int cortex_m_clear_halt(struct target *target)
 148 {
 149         struct cortex_m_common *cortex_m = target_to_cm(target);
 150         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
 151         int retval;
 152
 153         /* clear step if any */
 154         cortex_m_write_debug_halt_mask(target, C_HALT, C_STEP);
 155
 156         /* Read Debug Fault Status Register */
 157         retval = mem_ap_read_atomic_u32(swjdp, NVIC_DFSR, &cortex_m->nvic_dfsr);
 158         if (retval != ERROR_OK)
 159                 return retval;
 160
 161         /* Clear Debug Fault Status */
 162         retval = mem_ap_write_atomic_u32(swjdp, NVIC_DFSR, cortex_m->nvic_dfsr);
 163         if (retval != ERROR_OK)
 164                 return retval;
 165         LOG_DEBUG(" NVIC_DFSR 0x%" PRIx32 "", cortex_m->nvic_dfsr);
 166
 167         return ERROR_OK;
 168 }
 169
 170 static int cortex_m_single_step_core(struct target *target)
 171 {
 172         struct cortex_m_common *cortex_m = target_to_cm(target);
 173         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
 174         uint32_t dhcsr_save;
 175         int retval;
 176
 177         /* backup dhcsr reg */
 178         dhcsr_save = cortex_m->dcb_dhcsr;
 179
 180         /* Mask interrupts before clearing halt, if done already.  This avoids
 181          * Erratum 377497 (fixed in r1p0) where setting MASKINTS while clearing
 182          * HALT can put the core into an unknown state.
 183          */
 184         if (!(cortex_m->dcb_dhcsr & C_MASKINTS)) {
 185                 retval = mem_ap_write_atomic_u32(swjdp, DCB_DHCSR,
 186                                 DBGKEY | C_MASKINTS | C_HALT | C_DEBUGEN);
 187                 if (retval != ERROR_OK)
 188                         return retval;
 189         }
 190         retval = mem_ap_write_atomic_u32(swjdp, DCB_DHCSR,
 191                         DBGKEY | C_MASKINTS | C_STEP | C_DEBUGEN);
 192         if (retval != ERROR_OK)
 193                 return retval;
 194         LOG_DEBUG(" ");
 195
 196         /* restore dhcsr reg */
 197         cortex_m->dcb_dhcsr = dhcsr_save;
 198         cortex_m_clear_halt(target);
 199
 200         return ERROR_OK;
 201 }
 202
 203 static int cortex_m_enable_fpb(struct target *target)
 204 {
 205         int retval = target_write_u32(target, FP_CTRL, 3);
 206         if (retval != ERROR_OK)
 207                 return retval;
 208
 209         /* check the fpb is actually enabled */
 210         uint32_t fpctrl;
 211         retval = target_read_u32(target, FP_CTRL, &fpctrl);
 212         if (retval != ERROR_OK)
 213                 return retval;
 214
 215         if (fpctrl & 1)
 216                 return ERROR_OK;
 217
 218         return ERROR_FAIL;
 219 }
 220
 221 static int cortex_m_endreset_event(struct target *target)
 222 {
 223         int i;
 224         int retval;
 225         uint32_t dcb_demcr;
 226         struct cortex_m_common *cortex_m = target_to_cm(target);
 227         struct armv7m_common *armv7m = &cortex_m->armv7m;
 228         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
 229         struct cortex_m_fp_comparator *fp_list = cortex_m->fp_comparator_list;
 230         struct cortex_m_dwt_comparator *dwt_list = cortex_m->dwt_comparator_list;
 231
 232         /* REVISIT The four debug monitor bits are currently ignored... */
 233         retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &dcb_demcr);
 234         if (retval != ERROR_OK)
 235                 return retval;
 236         LOG_DEBUG("DCB_DEMCR = 0x%8.8" PRIx32 "", dcb_demcr);
 237
 238         /* this register is used for emulated dcc channel */
 239         retval = mem_ap_write_u32(swjdp, DCB_DCRDR, 0);
 240         if (retval != ERROR_OK)
 241                 return retval;
 242
 243         /* Enable debug requests */
 244         retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m->dcb_dhcsr);
 245         if (retval != ERROR_OK)
 246                 return retval;
 247         if (!(cortex_m->dcb_dhcsr & C_DEBUGEN)) {
 248                 retval = mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN);
 249                 if (retval != ERROR_OK)
 250                         return retval;
 251         }
 252
 253         /* clear any interrupt masking */
 254         cortex_m_write_debug_halt_mask(target, 0, C_MASKINTS);
 255
 256         /* Enable features controlled by ITM and DWT blocks, and catch only
 257          * the vectors we were told to pay attention to.
 258          *
 259          * Target firmware is responsible for all fault handling policy
 260          * choices *EXCEPT* explicitly scripted overrides like "vector_catch"
 261          * or manual updates to the NVIC SHCSR and CCR registers.
 262          */
 263         retval = mem_ap_write_u32(swjdp, DCB_DEMCR, TRCENA | armv7m->demcr);
 264         if (retval != ERROR_OK)
 265                 return retval;
 266
 267         /* Paranoia: evidently some (early?) chips don't preserve all the
 268          * debug state (including FBP, DWT, etc) across reset...
 269          */
 270
 271         /* Enable FPB */
 272         retval = cortex_m_enable_fpb(target);
 273         if (retval != ERROR_OK) {
 274                 LOG_ERROR("Failed to enable the FPB");
 275                 return retval;
 276         }
 277
 278         cortex_m->fpb_enabled = 1;
 279
 280         /* Restore FPB registers */
 281         for (i = 0; i < cortex_m->fp_num_code + cortex_m->fp_num_lit; i++) {
 282                 retval = target_write_u32(target, fp_list[i].fpcr_address, fp_list[i].fpcr_value);
 283                 if (retval != ERROR_OK)
 284                         return retval;
 285         }
 286
 287         /* Restore DWT registers */
 288         for (i = 0; i < cortex_m->dwt_num_comp; i++) {
 289                 retval = target_write_u32(target, dwt_list[i].dwt_comparator_address + 0,
 290                                 dwt_list[i].comp);
 291                 if (retval != ERROR_OK)
 292                         return retval;
 293                 retval = target_write_u32(target, dwt_list[i].dwt_comparator_address + 4,
 294                                 dwt_list[i].mask);
 295                 if (retval != ERROR_OK)
 296                         return retval;
 297                 retval = target_write_u32(target, dwt_list[i].dwt_comparator_address + 8,
 298                                 dwt_list[i].function);
 299                 if (retval != ERROR_OK)
 300                         return retval;
 301         }
 302         retval = dap_run(swjdp);
 303         if (retval != ERROR_OK)
 304                 return retval;
 305
 306         register_cache_invalidate(armv7m->arm.core_cache);
 307
 308         /* make sure we have latest dhcsr flags */
 309         retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m->dcb_dhcsr);
 310
 311         return retval;
 312 }
 313
 314 static int cortex_m_examine_debug_reason(struct target *target)
 315 {
 316         struct cortex_m_common *cortex_m = target_to_cm(target);
 317
 318         /* THIS IS NOT GOOD, TODO - better logic for detection of debug state reason
 319          * only check the debug reason if we don't know it already */
 320
 321         if ((target->debug_reason != DBG_REASON_DBGRQ)
 322                 && (target->debug_reason != DBG_REASON_SINGLESTEP)) {
 323                 if (cortex_m->nvic_dfsr & DFSR_BKPT) {
 324                         target->debug_reason = DBG_REASON_BREAKPOINT;
 325                         if (cortex_m->nvic_dfsr & DFSR_DWTTRAP)
 326                                 target->debug_reason = DBG_REASON_WPTANDBKPT;
 327                 } else if (cortex_m->nvic_dfsr & DFSR_DWTTRAP)
 328                         target->debug_reason = DBG_REASON_WATCHPOINT;
 329                 else if (cortex_m->nvic_dfsr & DFSR_VCATCH)
 330                         target->debug_reason = DBG_REASON_BREAKPOINT;
 331                 else    /* EXTERNAL, HALTED */
 332                         target->debug_reason = DBG_REASON_UNDEFINED;
 333         }
 334
 335         return ERROR_OK;
 336 }
 337
 338 static int cortex_m_examine_exception_reason(struct target *target)
 339 {
 340         uint32_t shcsr = 0, except_sr = 0, cfsr = -1, except_ar = -1;
 341         struct armv7m_common *armv7m = target_to_armv7m(target);
 342         struct adiv5_dap *swjdp = armv7m->arm.dap;
 343         int retval;
 344
 345         retval = mem_ap_read_u32(swjdp, NVIC_SHCSR, &shcsr);
 346         if (retval != ERROR_OK)
 347                 return retval;
 348         switch (armv7m->exception_number) {
 349                 case 2: /* NMI */
 350                         break;
 351                 case 3: /* Hard Fault */
 352                         retval = mem_ap_read_atomic_u32(swjdp, NVIC_HFSR, &except_sr);
 353                         if (retval != ERROR_OK)
 354                                 return retval;
 355                         if (except_sr & 0x40000000) {
 356                                 retval = mem_ap_read_u32(swjdp, NVIC_CFSR, &cfsr);
 357                                 if (retval != ERROR_OK)
 358                                         return retval;
 359                         }
 360                         break;
 361                 case 4: /* Memory Management */
 362                         retval = mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
 363                         if (retval != ERROR_OK)
 364                                 return retval;
 365                         retval = mem_ap_read_u32(swjdp, NVIC_MMFAR, &except_ar);
 366                         if (retval != ERROR_OK)
 367                                 return retval;
 368                         break;
 369                 case 5: /* Bus Fault */
 370                         retval = mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
 371                         if (retval != ERROR_OK)
 372                                 return retval;
 373                         retval = mem_ap_read_u32(swjdp, NVIC_BFAR, &except_ar);
 374                         if (retval != ERROR_OK)
 375                                 return retval;
 376                         break;
 377                 case 6: /* Usage Fault */
 378                         retval = mem_ap_read_u32(swjdp, NVIC_CFSR, &except_sr);
 379                         if (retval != ERROR_OK)
 380                                 return retval;
 381                         break;
 382                 case 11:        /* SVCall */
 383                         break;
 384                 case 12:        /* Debug Monitor */
 385                         retval = mem_ap_read_u32(swjdp, NVIC_DFSR, &except_sr);
 386                         if (retval != ERROR_OK)
 387                                 return retval;
 388                         break;
 389                 case 14:        /* PendSV */
 390                         break;
 391                 case 15:        /* SysTick */
 392                         break;
 393                 default:
 394                         except_sr = 0;
 395                         break;
 396         }
 397         retval = dap_run(swjdp);
 398         if (retval == ERROR_OK)
 399                 LOG_DEBUG("%s SHCSR 0x%" PRIx32 ", SR 0x%" PRIx32
 400                         ", CFSR 0x%" PRIx32 ", AR 0x%" PRIx32,
 401                         armv7m_exception_string(armv7m->exception_number),
 402                         shcsr, except_sr, cfsr, except_ar);
 403         return retval;
 404 }
 405
 406 static int cortex_m_debug_entry(struct target *target)
 407 {
 408         int i;
 409         uint32_t xPSR;
 410         int retval;
 411         struct cortex_m_common *cortex_m = target_to_cm(target);
 412         struct armv7m_common *armv7m = &cortex_m->armv7m;
 413         struct arm *arm = &armv7m->arm;
 414         struct adiv5_dap *swjdp = armv7m->arm.dap;
 415         struct reg *r;
 416
 417         LOG_DEBUG(" ");
 418
 419         cortex_m_clear_halt(target);
 420         retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m->dcb_dhcsr);
 421         if (retval != ERROR_OK)
 422                 return retval;
 423
 424         retval = armv7m->examine_debug_reason(target);
 425         if (retval != ERROR_OK)
 426                 return retval;
 427
 428         /* Examine target state and mode
 429          * First load register accessible through core debug port */
 430         int num_regs = arm->core_cache->num_regs;
 431
 432         for (i = 0; i < num_regs; i++) {
 433                 r = &armv7m->arm.core_cache->reg_list[i];
 434                 if (!r->valid)
 435                         arm->read_core_reg(target, r, i, ARM_MODE_ANY);
 436         }
 437
 438         r = arm->cpsr;
 439         xPSR = buf_get_u32(r->value, 0, 32);
 440
 441         /* For IT instructions xPSR must be reloaded on resume and clear on debug exec */
 442         if (xPSR & 0xf00) {
 443                 r->dirty = r->valid;
 444                 cortex_m_store_core_reg_u32(target, 16, xPSR & ~0xff);
 445         }
 446
 447         /* Are we in an exception handler */
 448         if (xPSR & 0x1FF) {
 449                 armv7m->exception_number = (xPSR & 0x1FF);
 450
 451                 arm->core_mode = ARM_MODE_HANDLER;
 452                 arm->map = armv7m_msp_reg_map;
 453         } else {
 454                 unsigned control = buf_get_u32(arm->core_cache
 455                                 ->reg_list[ARMV7M_CONTROL].value, 0, 2);
 456
 457                 /* is this thread privileged? */
 458                 arm->core_mode = control & 1
 459                         ? ARM_MODE_USER_THREAD
 460                         : ARM_MODE_THREAD;
 461
 462                 /* which stack is it using? */
 463                 if (control & 2)
 464                         arm->map = armv7m_psp_reg_map;
 465                 else
 466                         arm->map = armv7m_msp_reg_map;
 467
 468                 armv7m->exception_number = 0;
 469         }
 470
 471         if (armv7m->exception_number)
 472                 cortex_m_examine_exception_reason(target);
 473
 474         LOG_DEBUG("entered debug state in core mode: %s at PC 0x%" PRIx32 ", target->state: %s",
 475                 arm_mode_name(arm->core_mode),
 476                 *(uint32_t *)(arm->pc->value),
 477                 target_state_name(target));
 478
 479         if (armv7m->post_debug_entry) {
 480                 retval = armv7m->post_debug_entry(target);
 481                 if (retval != ERROR_OK)
 482                         return retval;
 483         }
 484
 485         return ERROR_OK;
 486 }
 487
 488 static int cortex_m_poll(struct target *target)
 489 {
 490         int detected_failure = ERROR_OK;
 491         int retval = ERROR_OK;
 492         enum target_state prev_target_state = target->state;
 493         struct cortex_m_common *cortex_m = target_to_cm(target);
 494         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
 495
 496         /* Read from Debug Halting Control and Status Register */
 497         retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m->dcb_dhcsr);
 498         if (retval != ERROR_OK) {
 499                 target->state = TARGET_UNKNOWN;
 500                 return retval;
 501         }
 502
 503         /* Recover from lockup.  See ARMv7-M architecture spec,
 504          * section B1.5.15 "Unrecoverable exception cases".
 505          */
 506         if (cortex_m->dcb_dhcsr & S_LOCKUP) {
 507                 LOG_ERROR("%s -- clearing lockup after double fault",
 508                         target_name(target));
 509                 cortex_m_write_debug_halt_mask(target, C_HALT, 0);
 510                 target->debug_reason = DBG_REASON_DBGRQ;
 511
 512                 /* We have to execute the rest (the "finally" equivalent, but
 513                  * still throw this exception again).
 514                  */
 515                 detected_failure = ERROR_FAIL;
 516
 517                 /* refresh status bits */
 518                 retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m->dcb_dhcsr);
 519                 if (retval != ERROR_OK)
 520                         return retval;
 521         }
 522
 523         if (cortex_m->dcb_dhcsr & S_RESET_ST) {
 524                 target->state = TARGET_RESET;
 525                 return ERROR_OK;
 526         }
 527
 528         if (target->state == TARGET_RESET) {
 529                 /* Cannot switch context while running so endreset is
 530                  * called with target->state == TARGET_RESET
 531                  */
 532                 LOG_DEBUG("Exit from reset with dcb_dhcsr 0x%" PRIx32,
 533                         cortex_m->dcb_dhcsr);
 534                 retval = cortex_m_endreset_event(target);
 535                 if (retval != ERROR_OK) {
 536                         target->state = TARGET_UNKNOWN;
 537                         return retval;
 538                 }
 539                 target->state = TARGET_RUNNING;
 540                 prev_target_state = TARGET_RUNNING;
 541         }
 542
 543         if (cortex_m->dcb_dhcsr & S_HALT) {
 544                 target->state = TARGET_HALTED;
 545
 546                 if ((prev_target_state == TARGET_RUNNING) || (prev_target_state == TARGET_RESET)) {
 547                         retval = cortex_m_debug_entry(target);
 548                         if (retval != ERROR_OK)
 549                                 return retval;
 550
 551                         if (arm_semihosting(target, &retval) != 0)
 552                                 return retval;
 553
 554                         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 555                 }
 556                 if (prev_target_state == TARGET_DEBUG_RUNNING) {
 557                         LOG_DEBUG(" ");
 558                         retval = cortex_m_debug_entry(target);
 559                         if (retval != ERROR_OK)
 560                                 return retval;
 561
 562                         target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 563                 }
 564         }
 565
 566         /* REVISIT when S_SLEEP is set, it's in a Sleep or DeepSleep state.
 567          * How best to model low power modes?
 568          */
 569
 570         if (target->state == TARGET_UNKNOWN) {
 571                 /* check if processor is retiring instructions */
 572                 if (cortex_m->dcb_dhcsr & S_RETIRE_ST) {
 573                         target->state = TARGET_RUNNING;
 574                         retval = ERROR_OK;
 575                 }
 576         }
 577
 578         /* Did we detect a failure condition that we cleared? */
 579         if (detected_failure != ERROR_OK)
 580                 retval = detected_failure;
 581         return retval;
 582 }
 583
 584 static int cortex_m_halt(struct target *target)
 585 {
 586         LOG_DEBUG("target->state: %s",
 587                 target_state_name(target));
 588
 589         if (target->state == TARGET_HALTED) {
 590                 LOG_DEBUG("target was already halted");
 591                 return ERROR_OK;
 592         }
 593
 594         if (target->state == TARGET_UNKNOWN)
 595                 LOG_WARNING("target was in unknown state when halt was requested");
 596
 597         if (target->state == TARGET_RESET) {
 598                 if ((jtag_get_reset_config() & RESET_SRST_PULLS_TRST) && jtag_get_srst()) {
 599                         LOG_ERROR("can't request a halt while in reset if nSRST pulls nTRST");
 600                         return ERROR_TARGET_FAILURE;
 601                 } else {
 602                         /* we came here in a reset_halt or reset_init sequence
 603                          * debug entry was already prepared in cortex_m3_assert_reset()
 604                          */
 605                         target->debug_reason = DBG_REASON_DBGRQ;
 606
 607                         return ERROR_OK;
 608                 }
 609         }
 610
 611         /* Write to Debug Halting Control and Status Register */
 612         cortex_m_write_debug_halt_mask(target, C_HALT, 0);
 613
 614         target->debug_reason = DBG_REASON_DBGRQ;
 615
 616         return ERROR_OK;
 617 }
 618
 619 static int cortex_m_soft_reset_halt(struct target *target)
 620 {
 621         struct cortex_m_common *cortex_m = target_to_cm(target);
 622         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
 623         uint32_t dcb_dhcsr = 0;
 624         int retval, timeout = 0;
 625
 626         /* soft_reset_halt is deprecated on cortex_m as the same functionality
 627          * can be obtained by using 'reset halt' and 'cortex_m reset_config vectreset'
 628          * As this reset only used VC_CORERESET it would only ever reset the cortex_m
 629          * core, not the peripherals */
 630         LOG_WARNING("soft_reset_halt is deprecated, please use 'reset halt' instead.");
 631
 632         /* Enter debug state on reset; restore DEMCR in endreset_event() */
 633         retval = mem_ap_write_u32(swjdp, DCB_DEMCR,
 634                         TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
 635         if (retval != ERROR_OK)
 636                 return retval;
 637
 638         /* Request a core-only reset */
 639         retval = mem_ap_write_atomic_u32(swjdp, NVIC_AIRCR,
 640                         AIRCR_VECTKEY | AIRCR_VECTRESET);
 641         if (retval != ERROR_OK)
 642                 return retval;
 643         target->state = TARGET_RESET;
 644
 645         /* registers are now invalid */
 646         register_cache_invalidate(cortex_m->armv7m.arm.core_cache);
 647
 648         while (timeout < 100) {
 649                 retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 650                 if (retval == ERROR_OK) {
 651                         retval = mem_ap_read_atomic_u32(swjdp, NVIC_DFSR,
 652                                         &cortex_m->nvic_dfsr);
 653                         if (retval != ERROR_OK)
 654                                 return retval;
 655                         if ((dcb_dhcsr & S_HALT)
 656                                 && (cortex_m->nvic_dfsr & DFSR_VCATCH)) {
 657                                 LOG_DEBUG("system reset-halted, DHCSR 0x%08x, "
 658                                         "DFSR 0x%08x",
 659                                         (unsigned) dcb_dhcsr,
 660                                         (unsigned) cortex_m->nvic_dfsr);
 661                                 cortex_m_poll(target);
 662                                 /* FIXME restore user's vector catch config */
 663                                 return ERROR_OK;
 664                         } else
 665                                 LOG_DEBUG("waiting for system reset-halt, "
 666                                         "DHCSR 0x%08x, %d ms",
 667                                         (unsigned) dcb_dhcsr, timeout);
 668                 }
 669                 timeout++;
 670                 alive_sleep(1);
 671         }
 672
 673         return ERROR_OK;
 674 }
 675
 676 void cortex_m_enable_breakpoints(struct target *target)
 677 {
 678         struct breakpoint *breakpoint = target->breakpoints;
 679
 680         /* set any pending breakpoints */
 681         while (breakpoint) {
 682                 if (!breakpoint->set)
 683                         cortex_m_set_breakpoint(target, breakpoint);
 684                 breakpoint = breakpoint->next;
 685         }
 686 }
 687
 688 static int cortex_m_resume(struct target *target, int current,
 689         uint32_t address, int handle_breakpoints, int debug_execution)
 690 {
 691         struct armv7m_common *armv7m = target_to_armv7m(target);
 692         struct breakpoint *breakpoint = NULL;
 693         uint32_t resume_pc;
 694         struct reg *r;
 695
 696         if (target->state != TARGET_HALTED) {
 697                 LOG_WARNING("target not halted");
 698                 return ERROR_TARGET_NOT_HALTED;
 699         }
 700
 701         if (!debug_execution) {
 702                 target_free_all_working_areas(target);
 703                 cortex_m_enable_breakpoints(target);
 704                 cortex_m_enable_watchpoints(target);
 705         }
 706
 707         if (debug_execution) {
 708                 r = armv7m->arm.core_cache->reg_list + ARMV7M_PRIMASK;
 709
 710                 /* Disable interrupts */
 711                 /* We disable interrupts in the PRIMASK register instead of
 712                  * masking with C_MASKINTS.  This is probably the same issue
 713                  * as Cortex-M3 Erratum 377493 (fixed in r1p0):  C_MASKINTS
 714                  * in parallel with disabled interrupts can cause local faults
 715                  * to not be taken.
 716                  *
 717                  * REVISIT this clearly breaks non-debug execution, since the
 718                  * PRIMASK register state isn't saved/restored...  workaround
 719                  * by never resuming app code after debug execution.
 720                  */
 721                 buf_set_u32(r->value, 0, 1, 1);
 722                 r->dirty = true;
 723                 r->valid = true;
 724
 725                 /* Make sure we are in Thumb mode */
 726                 r = armv7m->arm.cpsr;
 727                 buf_set_u32(r->value, 24, 1, 1);
 728                 r->dirty = true;
 729                 r->valid = true;
 730         }
 731
 732         /* current = 1: continue on current pc, otherwise continue at <address> */
 733         r = armv7m->arm.pc;
 734         if (!current) {
 735                 buf_set_u32(r->value, 0, 32, address);
 736                 r->dirty = true;
 737                 r->valid = true;
 738         }
 739
 740         /* if we halted last time due to a bkpt instruction
 741          * then we have to manually step over it, otherwise
 742          * the core will break again */
 743
 744         if (!breakpoint_find(target, buf_get_u32(r->value, 0, 32))
 745                 && !debug_execution)
 746                 armv7m_maybe_skip_bkpt_inst(target, NULL);
 747
 748         resume_pc = buf_get_u32(r->value, 0, 32);
 749
 750         armv7m_restore_context(target);
 751
 752         /* the front-end may request us not to handle breakpoints */
 753         if (handle_breakpoints) {
 754                 /* Single step past breakpoint at current address */
 755                 breakpoint = breakpoint_find(target, resume_pc);
 756                 if (breakpoint) {
 757                         LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %" PRIu32 ")",
 758                                 breakpoint->address,
 759                                 breakpoint->unique_id);
 760                         cortex_m_unset_breakpoint(target, breakpoint);
 761                         cortex_m_single_step_core(target);
 762                         cortex_m_set_breakpoint(target, breakpoint);
 763                 }
 764         }
 765
 766         /* Restart core */
 767         cortex_m_write_debug_halt_mask(target, 0, C_HALT);
 768
 769         target->debug_reason = DBG_REASON_NOTHALTED;
 770
 771         /* registers are now invalid */
 772         register_cache_invalidate(armv7m->arm.core_cache);
 773
 774         if (!debug_execution) {
 775                 target->state = TARGET_RUNNING;
 776                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 777                 LOG_DEBUG("target resumed at 0x%" PRIx32 "", resume_pc);
 778         } else {
 779                 target->state = TARGET_DEBUG_RUNNING;
 780                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
 781                 LOG_DEBUG("target debug resumed at 0x%" PRIx32 "", resume_pc);
 782         }
 783
 784         return ERROR_OK;
 785 }
 786
 787 /* int irqstepcount = 0; */
 788 static int cortex_m_step(struct target *target, int current,
 789         uint32_t address, int handle_breakpoints)
 790 {
 791         struct cortex_m_common *cortex_m = target_to_cm(target);
 792         struct armv7m_common *armv7m = &cortex_m->armv7m;
 793         struct adiv5_dap *swjdp = armv7m->arm.dap;
 794         struct breakpoint *breakpoint = NULL;
 795         struct reg *pc = armv7m->arm.pc;
 796         bool bkpt_inst_found = false;
 797         int retval;
 798         bool isr_timed_out = false;
 799
 800         if (target->state != TARGET_HALTED) {
 801                 LOG_WARNING("target not halted");
 802                 return ERROR_TARGET_NOT_HALTED;
 803         }
 804
 805         /* current = 1: continue on current pc, otherwise continue at <address> */
 806         if (!current)
 807                 buf_set_u32(pc->value, 0, 32, address);
 808
 809         uint32_t pc_value = buf_get_u32(pc->value, 0, 32);
 810
 811         /* the front-end may request us not to handle breakpoints */
 812         if (handle_breakpoints) {
 813                 breakpoint = breakpoint_find(target, pc_value);
 814                 if (breakpoint)
 815                         cortex_m_unset_breakpoint(target, breakpoint);
 816         }
 817
 818         armv7m_maybe_skip_bkpt_inst(target, &bkpt_inst_found);
 819
 820         target->debug_reason = DBG_REASON_SINGLESTEP;
 821
 822         armv7m_restore_context(target);
 823
 824         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 825
 826         /* if no bkpt instruction is found at pc then we can perform
 827          * a normal step, otherwise we have to manually step over the bkpt
 828          * instruction - as such simulate a step */
 829         if (bkpt_inst_found == false) {
 830                 /* Automatic ISR masking mode off: Just step over the next instruction */
 831                 if ((cortex_m->isrmasking_mode != CORTEX_M_ISRMASK_AUTO))
 832                         cortex_m_write_debug_halt_mask(target, C_STEP, C_HALT);
 833                 else {
 834                         /* Process interrupts during stepping in a way they don't interfere
 835                          * debugging.
 836                          *
 837                          * Principle:
 838                          *
 839                          * Set a temporary break point at the current pc and let the core run
 840                          * with interrupts enabled. Pending interrupts get served and we run
 841                          * into the breakpoint again afterwards. Then we step over the next
 842                          * instruction with interrupts disabled.
 843                          *
 844                          * If the pending interrupts don't complete within time, we leave the
 845                          * core running. This may happen if the interrupts trigger faster
 846                          * than the core can process them or the handler doesn't return.
 847                          *
 848                          * If no more breakpoints are available we simply do a step with
 849                          * interrupts enabled.
 850                          *
 851                          */
 852
 853                         /* 2012-09-29 ph
 854                          *
 855                          * If a break point is already set on the lower half word then a break point on
 856                          * the upper half word will not break again when the core is restarted. So we
 857                          * just step over the instruction with interrupts disabled.
 858                          *
 859                          * The documentation has no information about this, it was found by observation
 860                          * on STM32F1 and STM32F2. Proper explanation welcome. STM32F0 dosen't seem to
 861                          * suffer from this problem.
 862                          *
 863                          * To add some confusion: pc_value has bit 0 always set, while the breakpoint
 864                          * address has it always cleared. The former is done to indicate thumb mode
 865                          * to gdb.
 866                          *
 867                          */
 868                         if ((pc_value & 0x02) && breakpoint_find(target, pc_value & ~0x03)) {
 869                                 LOG_DEBUG("Stepping over next instruction with interrupts disabled");
 870                                 cortex_m_write_debug_halt_mask(target, C_HALT | C_MASKINTS, 0);
 871                                 cortex_m_write_debug_halt_mask(target, C_STEP, C_HALT);
 872                                 /* Re-enable interrupts */
 873                                 cortex_m_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
 874                         }
 875                         else {
 876
 877                                 /* Set a temporary break point */
 878                                 if (breakpoint)
 879                                         retval = cortex_m_set_breakpoint(target, breakpoint);
 880                                 else
 881                                         retval = breakpoint_add(target, pc_value, 2, BKPT_TYPE_BY_ADDR(pc_value));
 882                                 bool tmp_bp_set = (retval == ERROR_OK);
 883
 884                                 /* No more breakpoints left, just do a step */
 885                                 if (!tmp_bp_set)
 886                                         cortex_m_write_debug_halt_mask(target, C_STEP, C_HALT);
 887                                 else {
 888                                         /* Start the core */
 889                                         LOG_DEBUG("Starting core to serve pending interrupts");
 890                                         int64_t t_start = timeval_ms();
 891                                         cortex_m_write_debug_halt_mask(target, 0, C_HALT | C_STEP);
 892
 893                                         /* Wait for pending handlers to complete or timeout */
 894                                         do {
 895                                                 retval = mem_ap_read_atomic_u32(swjdp,
 896                                                                 DCB_DHCSR,
 897                                                                 &cortex_m->dcb_dhcsr);
 898                                                 if (retval != ERROR_OK) {
 899                                                         target->state = TARGET_UNKNOWN;
 900                                                         return retval;
 901                                                 }
 902                                                 isr_timed_out = ((timeval_ms() - t_start) > 500);
 903                                         } while (!((cortex_m->dcb_dhcsr & S_HALT) || isr_timed_out));
 904
 905                                         /* only remove breakpoint if we created it */
 906                                         if (breakpoint)
 907                                                 cortex_m_unset_breakpoint(target, breakpoint);
 908                                         else {
 909                                                 /* Remove the temporary breakpoint */
 910                                                 breakpoint_remove(target, pc_value);
 911                                         }
 912
 913                                         if (isr_timed_out) {
 914                                                 LOG_DEBUG("Interrupt handlers didn't complete within time, "
 915                                                         "leaving target running");
 916                                         } else {
 917                                                 /* Step over next instruction with interrupts disabled */
 918                                                 cortex_m_write_debug_halt_mask(target,
 919                                                         C_HALT | C_MASKINTS,
 920                                                         0);
 921                                                 cortex_m_write_debug_halt_mask(target, C_STEP, C_HALT);
 922                                                 /* Re-enable interrupts */
 923                                                 cortex_m_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
 924                                         }
 925                                 }
 926                         }
 927                 }
 928         }
 929
 930         retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m->dcb_dhcsr);
 931         if (retval != ERROR_OK)
 932                 return retval;
 933
 934         /* registers are now invalid */
 935         register_cache_invalidate(armv7m->arm.core_cache);
 936
 937         if (breakpoint)
 938                 cortex_m_set_breakpoint(target, breakpoint);
 939
 940         if (isr_timed_out) {
 941                 /* Leave the core running. The user has to stop execution manually. */
 942                 target->debug_reason = DBG_REASON_NOTHALTED;
 943                 target->state = TARGET_RUNNING;
 944                 return ERROR_OK;
 945         }
 946
 947         LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32
 948                 " nvic_icsr = 0x%" PRIx32,
 949                 cortex_m->dcb_dhcsr, cortex_m->nvic_icsr);
 950
 951         retval = cortex_m_debug_entry(target);
 952         if (retval != ERROR_OK)
 953                 return retval;
 954         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 955
 956         LOG_DEBUG("target stepped dcb_dhcsr = 0x%" PRIx32
 957                 " nvic_icsr = 0x%" PRIx32,
 958                 cortex_m->dcb_dhcsr, cortex_m->nvic_icsr);
 959
 960         return ERROR_OK;
 961 }
 962
 963 static int cortex_m_assert_reset(struct target *target)
 964 {
 965         struct cortex_m_common *cortex_m = target_to_cm(target);
 966         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
 967         enum cortex_m_soft_reset_config reset_config = cortex_m->soft_reset_config;
 968
 969         LOG_DEBUG("target->state: %s",
 970                 target_state_name(target));
 971
 972         enum reset_types jtag_reset_config = jtag_get_reset_config();
 973
 974         if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT)) {
 975                 /* allow scripts to override the reset event */
 976
 977                 target_handle_event(target, TARGET_EVENT_RESET_ASSERT);
 978                 register_cache_invalidate(cortex_m->armv7m.arm.core_cache);
 979                 target->state = TARGET_RESET;
 980
 981                 return ERROR_OK;
 982         }
 983
 984         /* some cores support connecting while srst is asserted
 985          * use that mode is it has been configured */
 986
 987         bool srst_asserted = false;
 988
 989         if ((jtag_reset_config & RESET_HAS_SRST) &&
 990             (jtag_reset_config & RESET_SRST_NO_GATING)) {
 991                 adapter_assert_reset();
 992                 srst_asserted = true;
 993         }
 994
 995         /* Enable debug requests */
 996         int retval;
 997         retval = mem_ap_read_atomic_u32(swjdp, DCB_DHCSR, &cortex_m->dcb_dhcsr);
 998         if (retval != ERROR_OK)
 999                 return retval;
1000         if (!(cortex_m->dcb_dhcsr & C_DEBUGEN)) {
1001                 retval = mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN);
1002                 if (retval != ERROR_OK)
1003                         return retval;
1004         }
1005
1006         /* If the processor is sleeping in a WFI or WFE instruction, the
1007          * C_HALT bit must be asserted to regain control */
1008         if (cortex_m->dcb_dhcsr & S_SLEEP) {
1009                 retval = mem_ap_write_u32(swjdp, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
1010                 if (retval != ERROR_OK)
1011                         return retval;
1012         }
1013
1014         retval = mem_ap_write_u32(swjdp, DCB_DCRDR, 0);
1015         if (retval != ERROR_OK)
1016                 return retval;
1017
1018         if (!target->reset_halt) {
1019                 /* Set/Clear C_MASKINTS in a separate operation */
1020                 if (cortex_m->dcb_dhcsr & C_MASKINTS) {
1021                         retval = mem_ap_write_atomic_u32(swjdp, DCB_DHCSR,
1022                                         DBGKEY | C_DEBUGEN | C_HALT);
1023                         if (retval != ERROR_OK)
1024                                 return retval;
1025                 }
1026
1027                 /* clear any debug flags before resuming */
1028                 cortex_m_clear_halt(target);
1029
1030                 /* clear C_HALT in dhcsr reg */
1031                 cortex_m_write_debug_halt_mask(target, 0, C_HALT);
1032         } else {
1033                 /* Halt in debug on reset; endreset_event() restores DEMCR.
1034                  *
1035                  * REVISIT catching BUSERR presumably helps to defend against
1036                  * bad vector table entries.  Should this include MMERR or
1037                  * other flags too?
1038                  */
1039                 retval = mem_ap_write_atomic_u32(swjdp, DCB_DEMCR,
1040                                 TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
1041                 if (retval != ERROR_OK)
1042                         return retval;
1043         }
1044
1045         if (jtag_reset_config & RESET_HAS_SRST) {
1046                 /* default to asserting srst */
1047                 if (!srst_asserted)
1048                         adapter_assert_reset();
1049         } else {
1050                 /* Use a standard Cortex-M3 software reset mechanism.
1051                  * We default to using VECRESET as it is supported on all current cores.
1052                  * This has the disadvantage of not resetting the peripherals, so a
1053                  * reset-init event handler is needed to perform any peripheral resets.
1054                  */
1055                 retval = mem_ap_write_atomic_u32(swjdp, NVIC_AIRCR,
1056                                 AIRCR_VECTKEY | ((reset_config == CORTEX_M_RESET_SYSRESETREQ)
1057                                 ? AIRCR_SYSRESETREQ : AIRCR_VECTRESET));
1058                 if (retval != ERROR_OK)
1059                         return retval;
1060
1061                 LOG_DEBUG("Using Cortex-M %s", (reset_config == CORTEX_M_RESET_SYSRESETREQ)
1062                         ? "SYSRESETREQ" : "VECTRESET");
1063
1064                 if (reset_config == CORTEX_M_RESET_VECTRESET) {
1065                         LOG_WARNING("Only resetting the Cortex-M core, use a reset-init event "
1066                                 "handler to reset any peripherals or configure hardware srst support.");
1067                 }
1068
1069                 /*
1070                   SAM4L needs to execute security initalization
1071                   startup sequence before AP access would be enabled.
1072                   During the intialization CDBGPWRUPACK is pulled low and we
1073                   need to wait for it to be set to 1 again.
1074                 */
1075                 retval = dap_dp_poll_register(swjdp, DP_CTRL_STAT,
1076                                               CDBGPWRUPACK, CDBGPWRUPACK, 100);
1077                 if (retval != ERROR_OK) {
1078                         LOG_ERROR("Failed waitnig for CDBGPWRUPACK");
1079                         return ERROR_FAIL;
1080                 }
1081
1082                 {
1083                         /* I do not know why this is necessary, but it
1084                          * fixes strange effects (step/resume cause NMI
1085                          * after reset) on LM3S6918 -- Michael Schwingen
1086                          */
1087                         uint32_t tmp;
1088                         retval = mem_ap_read_atomic_u32(swjdp, NVIC_AIRCR, &tmp);
1089                         if (retval != ERROR_OK)
1090                                 return retval;
1091                 }
1092         }
1093
1094         target->state = TARGET_RESET;
1095         jtag_add_sleep(50000);
1096
1097         register_cache_invalidate(cortex_m->armv7m.arm.core_cache);
1098
1099         if (target->reset_halt) {
1100                 retval = target_halt(target);
1101                 if (retval != ERROR_OK)
1102                         return retval;
1103         }
1104
1105         return ERROR_OK;
1106 }
1107
1108 static int cortex_m_deassert_reset(struct target *target)
1109 {
1110         LOG_DEBUG("target->state: %s",
1111                 target_state_name(target));
1112
1113         /* deassert reset lines */
1114         adapter_deassert_reset();
1115
1116         return ERROR_OK;
1117 }
1118
1119 int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint)
1120 {
1121         int retval;
1122         int fp_num = 0;
1123         uint32_t hilo;
1124         struct cortex_m_common *cortex_m = target_to_cm(target);
1125         struct cortex_m_fp_comparator *comparator_list = cortex_m->fp_comparator_list;
1126
1127         if (breakpoint->set) {
1128                 LOG_WARNING("breakpoint (BPID: %" PRIu32 ") already set", breakpoint->unique_id);
1129                 return ERROR_OK;
1130         }
1131
1132         if (cortex_m->auto_bp_type)
1133                 breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
1134
1135         if (breakpoint->type == BKPT_HARD) {
1136                 while (comparator_list[fp_num].used && (fp_num < cortex_m->fp_num_code))
1137                         fp_num++;
1138                 if (fp_num >= cortex_m->fp_num_code) {
1139                         LOG_ERROR("Can not find free FPB Comparator!");
1140                         return ERROR_FAIL;
1141                 }
1142                 breakpoint->set = fp_num + 1;
1143                 hilo = (breakpoint->address & 0x2) ? FPCR_REPLACE_BKPT_HIGH : FPCR_REPLACE_BKPT_LOW;
1144                 comparator_list[fp_num].used = 1;
1145                 comparator_list[fp_num].fpcr_value = (breakpoint->address & 0x1FFFFFFC) | hilo | 1;
1146                 target_write_u32(target, comparator_list[fp_num].fpcr_address,
1147                         comparator_list[fp_num].fpcr_value);
1148                 LOG_DEBUG("fpc_num %i fpcr_value 0x%" PRIx32 "",
1149                         fp_num,
1150                         comparator_list[fp_num].fpcr_value);
1151                 if (!cortex_m->fpb_enabled) {
1152                         LOG_DEBUG("FPB wasn't enabled, do it now");
1153                         retval = cortex_m_enable_fpb(target);
1154                         if (retval != ERROR_OK) {
1155                                 LOG_ERROR("Failed to enable the FPB");
1156                                 return retval;
1157                         }
1158
1159                         cortex_m->fpb_enabled = 1;
1160                 }
1161         } else if (breakpoint->type == BKPT_SOFT) {
1162                 uint8_t code[4];
1163
1164                 /* NOTE: on ARMv6-M and ARMv7-M, BKPT(0xab) is used for
1165                  * semihosting; don't use that.  Otherwise the BKPT
1166                  * parameter is arbitrary.
1167                  */
1168                 buf_set_u32(code, 0, 32, ARMV5_T_BKPT(0x11));
1169                 retval = target_read_memory(target,
1170                                 breakpoint->address & 0xFFFFFFFE,
1171                                 breakpoint->length, 1,
1172                                 breakpoint->orig_instr);
1173                 if (retval != ERROR_OK)
1174                         return retval;
1175                 retval = target_write_memory(target,
1176                                 breakpoint->address & 0xFFFFFFFE,
1177                                 breakpoint->length, 1,
1178                                 code);
1179                 if (retval != ERROR_OK)
1180                         return retval;
1181                 breakpoint->set = true;
1182         }
1183
1184         LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
1185                 breakpoint->unique_id,
1186                 (int)(breakpoint->type),
1187                 breakpoint->address,
1188                 breakpoint->length,
1189                 breakpoint->set);
1190
1191         return ERROR_OK;
1192 }
1193
1194 int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
1195 {
1196         int retval;
1197         struct cortex_m_common *cortex_m = target_to_cm(target);
1198         struct cortex_m_fp_comparator *comparator_list = cortex_m->fp_comparator_list;
1199
1200         if (!breakpoint->set) {
1201                 LOG_WARNING("breakpoint not set");
1202                 return ERROR_OK;
1203         }
1204
1205         LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
1206                 breakpoint->unique_id,
1207                 (int)(breakpoint->type),
1208                 breakpoint->address,
1209                 breakpoint->length,
1210                 breakpoint->set);
1211
1212         if (breakpoint->type == BKPT_HARD) {
1213                 int fp_num = breakpoint->set - 1;
1214                 if ((fp_num < 0) || (fp_num >= cortex_m->fp_num_code)) {
1215                         LOG_DEBUG("Invalid FP Comparator number in breakpoint");
1216                         return ERROR_OK;
1217                 }
1218                 comparator_list[fp_num].used = 0;
1219                 comparator_list[fp_num].fpcr_value = 0;
1220                 target_write_u32(target, comparator_list[fp_num].fpcr_address,
1221                         comparator_list[fp_num].fpcr_value);
1222         } else {
1223                 /* restore original instruction (kept in target endianness) */
1224                 if (breakpoint->length == 4) {
1225                         retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, 4, 1,
1226                                         breakpoint->orig_instr);
1227                         if (retval != ERROR_OK)
1228                                 return retval;
1229                 } else {
1230                         retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, 2, 1,
1231                                         breakpoint->orig_instr);
1232                         if (retval != ERROR_OK)
1233                                 return retval;
1234                 }
1235         }
1236         breakpoint->set = false;
1237
1238         return ERROR_OK;
1239 }
1240
1241 int cortex_m_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
1242 {
1243         struct cortex_m_common *cortex_m = target_to_cm(target);
1244
1245         if (cortex_m->auto_bp_type)
1246                 breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
1247
1248         if (breakpoint->type != BKPT_TYPE_BY_ADDR(breakpoint->address)) {
1249                 if (breakpoint->type == BKPT_HARD) {
1250                         LOG_INFO("flash patch comparator requested outside code memory region");
1251                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1252                 }
1253
1254                 if (breakpoint->type == BKPT_SOFT) {
1255                         LOG_INFO("soft breakpoint requested in code (flash) memory region");
1256                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1257                 }
1258         }
1259
1260         if ((breakpoint->type == BKPT_HARD) && (cortex_m->fp_code_available < 1)) {
1261                 LOG_INFO("no flash patch comparator unit available for hardware breakpoint");
1262                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1263         }
1264
1265         if ((breakpoint->length != 2)) {
1266                 LOG_INFO("only breakpoints of two bytes length supported");
1267                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1268         }
1269
1270         if (breakpoint->type == BKPT_HARD)
1271                 cortex_m->fp_code_available--;
1272
1273         return cortex_m_set_breakpoint(target, breakpoint);
1274 }
1275
1276 int cortex_m_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
1277 {
1278         struct cortex_m_common *cortex_m = target_to_cm(target);
1279
1280         /* REVISIT why check? FBP can be updated with core running ... */
1281         if (target->state != TARGET_HALTED) {
1282                 LOG_WARNING("target not halted");
1283                 return ERROR_TARGET_NOT_HALTED;
1284         }
1285
1286         if (cortex_m->auto_bp_type)
1287                 breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
1288
1289         if (breakpoint->set)
1290                 cortex_m_unset_breakpoint(target, breakpoint);
1291
1292         if (breakpoint->type == BKPT_HARD)
1293                 cortex_m->fp_code_available++;
1294
1295         return ERROR_OK;
1296 }
1297
1298 int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
1299 {
1300         int dwt_num = 0;
1301         uint32_t mask, temp;
1302         struct cortex_m_common *cortex_m = target_to_cm(target);
1303
1304         /* watchpoint params were validated earlier */
1305         mask = 0;
1306         temp = watchpoint->length;
1307         while (temp) {
1308                 temp >>= 1;
1309                 mask++;
1310         }
1311         mask--;
1312
1313         /* REVISIT Don't fully trust these "not used" records ... users
1314          * may set up breakpoints by hand, e.g. dual-address data value
1315          * watchpoint using comparator #1; comparator #0 matching cycle
1316          * count; send data trace info through ITM and TPIU; etc
1317          */
1318         struct cortex_m_dwt_comparator *comparator;
1319
1320         for (comparator = cortex_m->dwt_comparator_list;
1321                 comparator->used && dwt_num < cortex_m->dwt_num_comp;
1322                 comparator++, dwt_num++)
1323                 continue;
1324         if (dwt_num >= cortex_m->dwt_num_comp) {
1325                 LOG_ERROR("Can not find free DWT Comparator");
1326                 return ERROR_FAIL;
1327         }
1328         comparator->used = 1;
1329         watchpoint->set = dwt_num + 1;
1330
1331         comparator->comp = watchpoint->address;
1332         target_write_u32(target, comparator->dwt_comparator_address + 0,
1333                 comparator->comp);
1334
1335         comparator->mask = mask;
1336         target_write_u32(target, comparator->dwt_comparator_address + 4,
1337                 comparator->mask);
1338
1339         switch (watchpoint->rw) {
1340                 case WPT_READ:
1341                         comparator->function = 5;
1342                         break;
1343                 case WPT_WRITE:
1344                         comparator->function = 6;
1345                         break;
1346                 case WPT_ACCESS:
1347                         comparator->function = 7;
1348                         break;
1349         }
1350         target_write_u32(target, comparator->dwt_comparator_address + 8,
1351                 comparator->function);
1352
1353         LOG_DEBUG("Watchpoint (ID %d) DWT%d 0x%08x 0x%x 0x%05x",
1354                 watchpoint->unique_id, dwt_num,
1355                 (unsigned) comparator->comp,
1356                 (unsigned) comparator->mask,
1357                 (unsigned) comparator->function);
1358         return ERROR_OK;
1359 }
1360
1361 int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
1362 {
1363         struct cortex_m_common *cortex_m = target_to_cm(target);
1364         struct cortex_m_dwt_comparator *comparator;
1365         int dwt_num;
1366
1367         if (!watchpoint->set) {
1368                 LOG_WARNING("watchpoint (wpid: %d) not set",
1369                         watchpoint->unique_id);
1370                 return ERROR_OK;
1371         }
1372
1373         dwt_num = watchpoint->set - 1;
1374
1375         LOG_DEBUG("Watchpoint (ID %d) DWT%d address: 0x%08x clear",
1376                 watchpoint->unique_id, dwt_num,
1377                 (unsigned) watchpoint->address);
1378
1379         if ((dwt_num < 0) || (dwt_num >= cortex_m->dwt_num_comp)) {
1380                 LOG_DEBUG("Invalid DWT Comparator number in watchpoint");
1381                 return ERROR_OK;
1382         }
1383
1384         comparator = cortex_m->dwt_comparator_list + dwt_num;
1385         comparator->used = 0;
1386         comparator->function = 0;
1387         target_write_u32(target, comparator->dwt_comparator_address + 8,
1388                 comparator->function);
1389
1390         watchpoint->set = false;
1391
1392         return ERROR_OK;
1393 }
1394
1395 int cortex_m_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
1396 {
1397         struct cortex_m_common *cortex_m = target_to_cm(target);
1398
1399         if (cortex_m->dwt_comp_available < 1) {
1400                 LOG_DEBUG("no comparators?");
1401                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1402         }
1403
1404         /* hardware doesn't support data value masking */
1405         if (watchpoint->mask != ~(uint32_t)0) {
1406                 LOG_DEBUG("watchpoint value masks not supported");
1407                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1408         }
1409
1410         /* hardware allows address masks of up to 32K */
1411         unsigned mask;
1412
1413         for (mask = 0; mask < 16; mask++) {
1414                 if ((1u << mask) == watchpoint->length)
1415                         break;
1416         }
1417         if (mask == 16) {
1418                 LOG_DEBUG("unsupported watchpoint length");
1419                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1420         }
1421         if (watchpoint->address & ((1 << mask) - 1)) {
1422                 LOG_DEBUG("watchpoint address is unaligned");
1423                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1424         }
1425
1426         /* Caller doesn't seem to be able to describe watching for data
1427          * values of zero; that flags "no value".
1428          *
1429          * REVISIT This DWT may well be able to watch for specific data
1430          * values.  Requires comparator #1 to set DATAVMATCH and match
1431          * the data, and another comparator (DATAVADDR0) matching addr.
1432          */
1433         if (watchpoint->value) {
1434                 LOG_DEBUG("data value watchpoint not YET supported");
1435                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1436         }
1437
1438         cortex_m->dwt_comp_available--;
1439         LOG_DEBUG("dwt_comp_available: %d", cortex_m->dwt_comp_available);
1440
1441         return ERROR_OK;
1442 }
1443
1444 int cortex_m_remove_watchpoint(struct target *target, struct watchpoint *watchpoint)
1445 {
1446         struct cortex_m_common *cortex_m = target_to_cm(target);
1447
1448         /* REVISIT why check? DWT can be updated with core running ... */
1449         if (target->state != TARGET_HALTED) {
1450                 LOG_WARNING("target not halted");
1451                 return ERROR_TARGET_NOT_HALTED;
1452         }
1453
1454         if (watchpoint->set)
1455                 cortex_m_unset_watchpoint(target, watchpoint);
1456
1457         cortex_m->dwt_comp_available++;
1458         LOG_DEBUG("dwt_comp_available: %d", cortex_m->dwt_comp_available);
1459
1460         return ERROR_OK;
1461 }
1462
1463 void cortex_m_enable_watchpoints(struct target *target)
1464 {
1465         struct watchpoint *watchpoint = target->watchpoints;
1466
1467         /* set any pending watchpoints */
1468         while (watchpoint) {
1469                 if (!watchpoint->set)
1470                         cortex_m_set_watchpoint(target, watchpoint);
1471                 watchpoint = watchpoint->next;
1472         }
1473 }
1474
1475 static int cortex_m_load_core_reg_u32(struct target *target,
1476                 uint32_t num, uint32_t *value)
1477 {
1478         int retval;
1479
1480         /* NOTE:  we "know" here that the register identifiers used
1481          * in the v7m header match the Cortex-M3 Debug Core Register
1482          * Selector values for R0..R15, xPSR, MSP, and PSP.
1483          */
1484         switch (num) {
1485                 case 0 ... 18:
1486                         /* read a normal core register */
1487                         retval = cortexm_dap_read_coreregister_u32(target, value, num);
1488
1489                         if (retval != ERROR_OK) {
1490                                 LOG_ERROR("JTAG failure %i", retval);
1491                                 return ERROR_JTAG_DEVICE_ERROR;
1492                         }
1493                         LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "", (int)num, *value);
1494                         break;
1495
1496                 case ARMV7M_PRIMASK:
1497                 case ARMV7M_BASEPRI:
1498                 case ARMV7M_FAULTMASK:
1499                 case ARMV7M_CONTROL:
1500                         /* Cortex-M3 packages these four registers as bitfields
1501                          * in one Debug Core register.  So say r0 and r2 docs;
1502                          * it was removed from r1 docs, but still works.
1503                          */
1504                         cortexm_dap_read_coreregister_u32(target, value, 20);
1505
1506                         switch (num) {
1507                                 case ARMV7M_PRIMASK:
1508                                         *value = buf_get_u32((uint8_t *)value, 0, 1);
1509                                         break;
1510
1511                                 case ARMV7M_BASEPRI:
1512                                         *value = buf_get_u32((uint8_t *)value, 8, 8);
1513                                         break;
1514
1515                                 case ARMV7M_FAULTMASK:
1516                                         *value = buf_get_u32((uint8_t *)value, 16, 1);
1517                                         break;
1518
1519                                 case ARMV7M_CONTROL:
1520                                         *value = buf_get_u32((uint8_t *)value, 24, 2);
1521                                         break;
1522                         }
1523
1524                         LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "", (int)num, *value);
1525                         break;
1526
1527                 default:
1528                         return ERROR_COMMAND_SYNTAX_ERROR;
1529         }
1530
1531         return ERROR_OK;
1532 }
1533
1534 static int cortex_m_store_core_reg_u32(struct target *target,
1535                 uint32_t num, uint32_t value)
1536 {
1537         int retval;
1538         uint32_t reg;
1539         struct armv7m_common *armv7m = target_to_armv7m(target);
1540
1541         /* NOTE:  we "know" here that the register identifiers used
1542          * in the v7m header match the Cortex-M3 Debug Core Register
1543          * Selector values for R0..R15, xPSR, MSP, and PSP.
1544          */
1545         switch (num) {
1546                 case 0 ... 18:
1547                         retval = cortexm_dap_write_coreregister_u32(target, value, num);
1548                         if (retval != ERROR_OK) {
1549                                 struct reg *r;
1550
1551                                 LOG_ERROR("JTAG failure");
1552                                 r = armv7m->arm.core_cache->reg_list + num;
1553                                 r->dirty = r->valid;
1554                                 return ERROR_JTAG_DEVICE_ERROR;
1555                         }
1556                         LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
1557                         break;
1558
1559                 case ARMV7M_PRIMASK:
1560                 case ARMV7M_BASEPRI:
1561                 case ARMV7M_FAULTMASK:
1562                 case ARMV7M_CONTROL:
1563                         /* Cortex-M3 packages these four registers as bitfields
1564                          * in one Debug Core register.  So say r0 and r2 docs;
1565                          * it was removed from r1 docs, but still works.
1566                          */
1567                         cortexm_dap_read_coreregister_u32(target, &reg, 20);
1568
1569                         switch (num) {
1570                                 case ARMV7M_PRIMASK:
1571                                         buf_set_u32((uint8_t *)&reg, 0, 1, value);
1572                                         break;
1573
1574                                 case ARMV7M_BASEPRI:
1575                                         buf_set_u32((uint8_t *)&reg, 8, 8, value);
1576                                         break;
1577
1578                                 case ARMV7M_FAULTMASK:
1579                                         buf_set_u32((uint8_t *)&reg, 16, 1, value);
1580                                         break;
1581
1582                                 case ARMV7M_CONTROL:
1583                                         buf_set_u32((uint8_t *)&reg, 24, 2, value);
1584                                         break;
1585                         }
1586
1587                         cortexm_dap_write_coreregister_u32(target, reg, 20);
1588
1589                         LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
1590                         break;
1591
1592                 default:
1593                         return ERROR_COMMAND_SYNTAX_ERROR;
1594         }
1595
1596         return ERROR_OK;
1597 }
1598
1599 static int cortex_m_read_memory(struct target *target, uint32_t address,
1600         uint32_t size, uint32_t count, uint8_t *buffer)
1601 {
1602         struct armv7m_common *armv7m = target_to_armv7m(target);
1603         struct adiv5_dap *swjdp = armv7m->arm.dap;
1604
1605         if (armv7m->arm.is_armv6m) {
1606                 /* armv6m does not handle unaligned memory access */
1607                 if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1608                         return ERROR_TARGET_UNALIGNED_ACCESS;
1609         }
1610
1611         return mem_ap_read(swjdp, buffer, size, count, address, true);
1612 }
1613
1614 static int cortex_m_write_memory(struct target *target, uint32_t address,
1615         uint32_t size, uint32_t count, const uint8_t *buffer)
1616 {
1617         struct armv7m_common *armv7m = target_to_armv7m(target);
1618         struct adiv5_dap *swjdp = armv7m->arm.dap;
1619
1620         if (armv7m->arm.is_armv6m) {
1621                 /* armv6m does not handle unaligned memory access */
1622                 if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1623                         return ERROR_TARGET_UNALIGNED_ACCESS;
1624         }
1625
1626         return mem_ap_write(swjdp, buffer, size, count, address, true);
1627 }
1628
1629 static int cortex_m_init_target(struct command_context *cmd_ctx,
1630         struct target *target)
1631 {
1632         armv7m_build_reg_cache(target);
1633         return ERROR_OK;
1634 }
1635
1636 /* REVISIT cache valid/dirty bits are unmaintained.  We could set "valid"
1637  * on r/w if the core is not running, and clear on resume or reset ... or
1638  * at least, in a post_restore_context() method.
1639  */
1640
1641 struct dwt_reg_state {
1642         struct target *target;
1643         uint32_t addr;
1644         uint32_t value;         /* scratch/cache */
1645 };
1646
1647 static int cortex_m_dwt_get_reg(struct reg *reg)
1648 {
1649         struct dwt_reg_state *state = reg->arch_info;
1650
1651         return target_read_u32(state->target, state->addr, &state->value);
1652 }
1653
1654 static int cortex_m_dwt_set_reg(struct reg *reg, uint8_t *buf)
1655 {
1656         struct dwt_reg_state *state = reg->arch_info;
1657
1658         return target_write_u32(state->target, state->addr,
1659                         buf_get_u32(buf, 0, reg->size));
1660 }
1661
1662 struct dwt_reg {
1663         uint32_t addr;
1664         char *name;
1665         unsigned size;
1666 };
1667
1668 static struct dwt_reg dwt_base_regs[] = {
1669         { DWT_CTRL, "dwt_ctrl", 32, },
1670         /* NOTE that Erratum 532314 (fixed r2p0) affects CYCCNT:  it wrongly
1671          * increments while the core is asleep.
1672          */
1673         { DWT_CYCCNT, "dwt_cyccnt", 32, },
1674         /* plus some 8 bit counters, useful for profiling with TPIU */
1675 };
1676
1677 static struct dwt_reg dwt_comp[] = {
1678 #define DWT_COMPARATOR(i) \
1679                 { DWT_COMP0 + 0x10 * (i), "dwt_" #i "_comp", 32, }, \
1680                 { DWT_MASK0 + 0x10 * (i), "dwt_" #i "_mask", 4, }, \
1681                 { DWT_FUNCTION0 + 0x10 * (i), "dwt_" #i "_function", 32, }
1682         DWT_COMPARATOR(0),
1683         DWT_COMPARATOR(1),
1684         DWT_COMPARATOR(2),
1685         DWT_COMPARATOR(3),
1686 #undef DWT_COMPARATOR
1687 };
1688
1689 static const struct reg_arch_type dwt_reg_type = {
1690         .get = cortex_m_dwt_get_reg,
1691         .set = cortex_m_dwt_set_reg,
1692 };
1693
1694 static void cortex_m_dwt_addreg(struct target *t, struct reg *r, struct dwt_reg *d)
1695 {
1696         struct dwt_reg_state *state;
1697
1698         state = calloc(1, sizeof *state);
1699         if (!state)
1700                 return;
1701         state->addr = d->addr;
1702         state->target = t;
1703
1704         r->name = d->name;
1705         r->size = d->size;
1706         r->value = &state->value;
1707         r->arch_info = state;
1708         r->type = &dwt_reg_type;
1709 }
1710
1711 void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
1712 {
1713         uint32_t dwtcr;
1714         struct reg_cache *cache;
1715         struct cortex_m_dwt_comparator *comparator;
1716         int reg, i;
1717
1718         target_read_u32(target, DWT_CTRL, &dwtcr);
1719         if (!dwtcr) {
1720                 LOG_DEBUG("no DWT");
1721                 return;
1722         }
1723
1724         cm->dwt_num_comp = (dwtcr >> 28) & 0xF;
1725         cm->dwt_comp_available = cm->dwt_num_comp;
1726         cm->dwt_comparator_list = calloc(cm->dwt_num_comp,
1727                         sizeof(struct cortex_m_dwt_comparator));
1728         if (!cm->dwt_comparator_list) {
1729 fail0:
1730                 cm->dwt_num_comp = 0;
1731                 LOG_ERROR("out of mem");
1732                 return;
1733         }
1734
1735         cache = calloc(1, sizeof *cache);
1736         if (!cache) {
1737 fail1:
1738                 free(cm->dwt_comparator_list);
1739                 goto fail0;
1740         }
1741         cache->name = "Cortex-M DWT registers";
1742         cache->num_regs = 2 + cm->dwt_num_comp * 3;
1743         cache->reg_list = calloc(cache->num_regs, sizeof *cache->reg_list);
1744         if (!cache->reg_list) {
1745                 free(cache);
1746                 goto fail1;
1747         }
1748
1749         for (reg = 0; reg < 2; reg++)
1750                 cortex_m_dwt_addreg(target, cache->reg_list + reg,
1751                         dwt_base_regs + reg);
1752
1753         comparator = cm->dwt_comparator_list;
1754         for (i = 0; i < cm->dwt_num_comp; i++, comparator++) {
1755                 int j;
1756
1757                 comparator->dwt_comparator_address = DWT_COMP0 + 0x10 * i;
1758                 for (j = 0; j < 3; j++, reg++)
1759                         cortex_m_dwt_addreg(target, cache->reg_list + reg,
1760                                 dwt_comp + 3 * i + j);
1761
1762                 /* make sure we clear any watchpoints enabled on the target */
1763                 target_write_u32(target, comparator->dwt_comparator_address + 8, 0);
1764         }
1765
1766         *register_get_last_cache_p(&target->reg_cache) = cache;
1767         cm->dwt_cache = cache;
1768
1769         LOG_DEBUG("DWT dwtcr 0x%" PRIx32 ", comp %d, watch%s",
1770                 dwtcr, cm->dwt_num_comp,
1771                 (dwtcr & (0xf << 24)) ? " only" : "/trigger");
1772
1773         /* REVISIT:  if num_comp > 1, check whether comparator #1 can
1774          * implement single-address data value watchpoints ... so we
1775          * won't need to check it later, when asked to set one up.
1776          */
1777 }
1778
1779 #define MVFR0 0xe000ef40
1780 #define MVFR1 0xe000ef44
1781
1782 #define MVFR0_DEFAULT_M4 0x10110021
1783 #define MVFR1_DEFAULT_M4 0x11000011
1784
1785 int cortex_m_examine(struct target *target)
1786 {
1787         int retval;
1788         uint32_t cpuid, fpcr, mvfr0, mvfr1;
1789         int i;
1790         struct cortex_m_common *cortex_m = target_to_cm(target);
1791         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
1792         struct armv7m_common *armv7m = target_to_armv7m(target);
1793
1794         /* stlink shares the examine handler but does not support
1795          * all its calls */
1796         if (!armv7m->stlink) {
1797                 retval = ahbap_debugport_init(swjdp);
1798                 if (retval != ERROR_OK)
1799                         return retval;
1800         }
1801
1802         if (!target_was_examined(target)) {
1803                 target_set_examined(target);
1804
1805                 /* Read from Device Identification Registers */
1806                 retval = target_read_u32(target, CPUID, &cpuid);
1807                 if (retval != ERROR_OK)
1808                         return retval;
1809
1810                 /* Get CPU Type */
1811                 i = (cpuid >> 4) & 0xf;
1812
1813                 LOG_DEBUG("Cortex-M%d r%" PRId8 "p%" PRId8 " processor detected",
1814                                 i, (uint8_t)((cpuid >> 20) & 0xf), (uint8_t)((cpuid >> 0) & 0xf));
1815                 LOG_DEBUG("cpuid: 0x%8.8" PRIx32 "", cpuid);
1816
1817                 /* test for floating point feature on cortex-m4 */
1818                 if (i == 4) {
1819                         target_read_u32(target, MVFR0, &mvfr0);
1820                         target_read_u32(target, MVFR1, &mvfr1);
1821
1822                         if ((mvfr0 == MVFR0_DEFAULT_M4) && (mvfr1 == MVFR1_DEFAULT_M4)) {
1823                                 LOG_DEBUG("Cortex-M%d floating point feature FPv4_SP found", i);
1824                                 armv7m->fp_feature = FPv4_SP;
1825                         }
1826                 } else if (i == 0) {
1827                         /* Cortex-M0 does not support unaligned memory access */
1828                         armv7m->arm.is_armv6m = true;
1829                 }
1830
1831                 if (i == 4 || i == 3) {
1832                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
1833                         armv7m->dap.tar_autoincr_block = (1 << 12);
1834                 }
1835
1836                 /* NOTE: FPB and DWT are both optional. */
1837
1838                 /* Setup FPB */
1839                 target_read_u32(target, FP_CTRL, &fpcr);
1840                 cortex_m->auto_bp_type = 1;
1841                 /* bits [14:12] and [7:4] */
1842                 cortex_m->fp_num_code = ((fpcr >> 8) & 0x70) | ((fpcr >> 4) & 0xF);
1843                 cortex_m->fp_num_lit = (fpcr >> 8) & 0xF;
1844                 cortex_m->fp_code_available = cortex_m->fp_num_code;
1845                 cortex_m->fp_comparator_list = calloc(
1846                                 cortex_m->fp_num_code + cortex_m->fp_num_lit,
1847                                 sizeof(struct cortex_m_fp_comparator));
1848                 cortex_m->fpb_enabled = fpcr & 1;
1849                 for (i = 0; i < cortex_m->fp_num_code + cortex_m->fp_num_lit; i++) {
1850                         cortex_m->fp_comparator_list[i].type =
1851                                 (i < cortex_m->fp_num_code) ? FPCR_CODE : FPCR_LITERAL;
1852                         cortex_m->fp_comparator_list[i].fpcr_address = FP_COMP0 + 4 * i;
1853
1854                         /* make sure we clear any breakpoints enabled on the target */
1855                         target_write_u32(target, cortex_m->fp_comparator_list[i].fpcr_address, 0);
1856                 }
1857                 LOG_DEBUG("FPB fpcr 0x%" PRIx32 ", numcode %i, numlit %i",
1858                         fpcr,
1859                         cortex_m->fp_num_code,
1860                         cortex_m->fp_num_lit);
1861
1862                 /* Setup DWT */
1863                 cortex_m_dwt_setup(cortex_m, target);
1864
1865                 /* These hardware breakpoints only work for code in flash! */
1866                 LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
1867                         target_name(target),
1868                         cortex_m->fp_num_code,
1869                         cortex_m->dwt_num_comp);
1870         }
1871
1872         return ERROR_OK;
1873 }
1874
1875 static int cortex_m_dcc_read(struct target *target, uint8_t *value, uint8_t *ctrl)
1876 {
1877         struct armv7m_common *armv7m = target_to_armv7m(target);
1878         struct adiv5_dap *swjdp = armv7m->arm.dap;
1879         uint16_t dcrdr;
1880         uint8_t buf[2];
1881         int retval;
1882
1883         retval = mem_ap_read(swjdp, buf, 2, 1, DCB_DCRDR, false);
1884         if (retval != ERROR_OK)
1885                 return retval;
1886
1887         dcrdr = target_buffer_get_u16(target, buf);
1888         *ctrl = (uint8_t)dcrdr;
1889         *value = (uint8_t)(dcrdr >> 8);
1890
1891         LOG_DEBUG("data 0x%x ctrl 0x%x", *value, *ctrl);
1892
1893         /* write ack back to software dcc register
1894          * signify we have read data */
1895         if (dcrdr & (1 << 0)) {
1896                 target_buffer_set_u16(target, buf, 0);
1897                 retval = mem_ap_write(swjdp, buf, 2, 1, DCB_DCRDR, false);
1898                 if (retval != ERROR_OK)
1899                         return retval;
1900         }
1901
1902         return ERROR_OK;
1903 }
1904
1905 static int cortex_m_target_request_data(struct target *target,
1906         uint32_t size, uint8_t *buffer)
1907 {
1908         uint8_t data;
1909         uint8_t ctrl;
1910         uint32_t i;
1911
1912         for (i = 0; i < (size * 4); i++) {
1913                 cortex_m_dcc_read(target, &data, &ctrl);
1914                 buffer[i] = data;
1915         }
1916
1917         return ERROR_OK;
1918 }
1919
1920 static int cortex_m_handle_target_request(void *priv)
1921 {
1922         struct target *target = priv;
1923         if (!target_was_examined(target))
1924                 return ERROR_OK;
1925
1926         if (!target->dbg_msg_enabled)
1927                 return ERROR_OK;
1928
1929         if (target->state == TARGET_RUNNING) {
1930                 uint8_t data;
1931                 uint8_t ctrl;
1932
1933                 cortex_m_dcc_read(target, &data, &ctrl);
1934
1935                 /* check if we have data */
1936                 if (ctrl & (1 << 0)) {
1937                         uint32_t request;
1938
1939                         /* we assume target is quick enough */
1940                         request = data;
1941                         cortex_m_dcc_read(target, &data, &ctrl);
1942                         request |= (data << 8);
1943                         cortex_m_dcc_read(target, &data, &ctrl);
1944                         request |= (data << 16);
1945                         cortex_m_dcc_read(target, &data, &ctrl);
1946                         request |= (data << 24);
1947                         target_request(target, request);
1948                 }
1949         }
1950
1951         return ERROR_OK;
1952 }
1953
1954 static int cortex_m_init_arch_info(struct target *target,
1955         struct cortex_m_common *cortex_m, struct jtag_tap *tap)
1956 {
1957         int retval;
1958         struct armv7m_common *armv7m = &cortex_m->armv7m;
1959
1960         armv7m_init_arch_info(target, armv7m);
1961
1962         /* prepare JTAG information for the new target */
1963         cortex_m->jtag_info.tap = tap;
1964         cortex_m->jtag_info.scann_size = 4;
1965
1966         /* default reset mode is to use srst if fitted
1967          * if not it will use CORTEX_M3_RESET_VECTRESET */
1968         cortex_m->soft_reset_config = CORTEX_M_RESET_VECTRESET;
1969
1970         armv7m->arm.dap = &armv7m->dap;
1971
1972         /* Leave (only) generic DAP stuff for debugport_init(); */
1973         armv7m->dap.jtag_info = &cortex_m->jtag_info;
1974         armv7m->dap.memaccess_tck = 8;
1975
1976         /* Cortex-M3/M4 has 4096 bytes autoincrement range
1977          * but set a safe default to 1024 to support Cortex-M0
1978          * this will be changed in cortex_m3_examine if a M3/M4 is detected */
1979         armv7m->dap.tar_autoincr_block = (1 << 10);
1980
1981         /* register arch-specific functions */
1982         armv7m->examine_debug_reason = cortex_m_examine_debug_reason;
1983
1984         armv7m->post_debug_entry = NULL;
1985
1986         armv7m->pre_restore_context = NULL;
1987
1988         armv7m->load_core_reg_u32 = cortex_m_load_core_reg_u32;
1989         armv7m->store_core_reg_u32 = cortex_m_store_core_reg_u32;
1990
1991         target_register_timer_callback(cortex_m_handle_target_request, 1, 1, target);
1992
1993         retval = arm_jtag_setup_connection(&cortex_m->jtag_info);
1994         if (retval != ERROR_OK)
1995                 return retval;
1996
1997         return ERROR_OK;
1998 }
1999
2000 static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
2001 {
2002         struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
2003
2004         cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
2005         cortex_m_init_arch_info(target, cortex_m, target->tap);
2006
2007         return ERROR_OK;
2008 }
2009
2010 /*--------------------------------------------------------------------------*/
2011
2012 static int cortex_m_verify_pointer(struct command_context *cmd_ctx,
2013         struct cortex_m_common *cm)
2014 {
2015         if (cm->common_magic != CORTEX_M_COMMON_MAGIC) {
2016                 command_print(cmd_ctx, "target is not a Cortex-M");
2017                 return ERROR_TARGET_INVALID;
2018         }
2019         return ERROR_OK;
2020 }
2021
2022 /*
2023  * Only stuff below this line should need to verify that its target
2024  * is a Cortex-M3.  Everything else should have indirected through the
2025  * cortexm3_target structure, which is only used with CM3 targets.
2026  */
2027
2028 static const struct {
2029         char name[10];
2030         unsigned mask;
2031 } vec_ids[] = {
2032         { "hard_err",   VC_HARDERR, },
2033         { "int_err",    VC_INTERR, },
2034         { "bus_err",    VC_BUSERR, },
2035         { "state_err",  VC_STATERR, },
2036         { "chk_err",    VC_CHKERR, },
2037         { "nocp_err",   VC_NOCPERR, },
2038         { "mm_err",     VC_MMERR, },
2039         { "reset",      VC_CORERESET, },
2040 };
2041
2042 COMMAND_HANDLER(handle_cortex_m_vector_catch_command)
2043 {
2044         struct target *target = get_current_target(CMD_CTX);
2045         struct cortex_m_common *cortex_m = target_to_cm(target);
2046         struct armv7m_common *armv7m = &cortex_m->armv7m;
2047         struct adiv5_dap *swjdp = armv7m->arm.dap;
2048         uint32_t demcr = 0;
2049         int retval;
2050
2051         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2052         if (retval != ERROR_OK)
2053                 return retval;
2054
2055         retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
2056         if (retval != ERROR_OK)
2057                 return retval;
2058
2059         if (CMD_ARGC > 0) {
2060                 unsigned catch = 0;
2061
2062                 if (CMD_ARGC == 1) {
2063                         if (strcmp(CMD_ARGV[0], "all") == 0) {
2064                                 catch = VC_HARDERR | VC_INTERR | VC_BUSERR
2065                                         | VC_STATERR | VC_CHKERR | VC_NOCPERR
2066                                         | VC_MMERR | VC_CORERESET;
2067                                 goto write;
2068                         } else if (strcmp(CMD_ARGV[0], "none") == 0)
2069                                 goto write;
2070                 }
2071                 while (CMD_ARGC-- > 0) {
2072                         unsigned i;
2073                         for (i = 0; i < ARRAY_SIZE(vec_ids); i++) {
2074                                 if (strcmp(CMD_ARGV[CMD_ARGC], vec_ids[i].name) != 0)
2075                                         continue;
2076                                 catch |= vec_ids[i].mask;
2077                                 break;
2078                         }
2079                         if (i == ARRAY_SIZE(vec_ids)) {
2080                                 LOG_ERROR("No CM3 vector '%s'", CMD_ARGV[CMD_ARGC]);
2081                                 return ERROR_COMMAND_SYNTAX_ERROR;
2082                         }
2083                 }
2084 write:
2085                 /* For now, armv7m->demcr only stores vector catch flags. */
2086                 armv7m->demcr = catch;
2087
2088                 demcr &= ~0xffff;
2089                 demcr |= catch;
2090
2091                 /* write, but don't assume it stuck (why not??) */
2092                 retval = mem_ap_write_u32(swjdp, DCB_DEMCR, demcr);
2093                 if (retval != ERROR_OK)
2094                         return retval;
2095                 retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
2096                 if (retval != ERROR_OK)
2097                         return retval;
2098
2099                 /* FIXME be sure to clear DEMCR on clean server shutdown.
2100                  * Otherwise the vector catch hardware could fire when there's
2101                  * no debugger hooked up, causing much confusion...
2102                  */
2103         }
2104
2105         for (unsigned i = 0; i < ARRAY_SIZE(vec_ids); i++) {
2106                 command_print(CMD_CTX, "%9s: %s", vec_ids[i].name,
2107                         (demcr & vec_ids[i].mask) ? "catch" : "ignore");
2108         }
2109
2110         return ERROR_OK;
2111 }
2112
2113 COMMAND_HANDLER(handle_cortex_m_mask_interrupts_command)
2114 {
2115         struct target *target = get_current_target(CMD_CTX);
2116         struct cortex_m_common *cortex_m = target_to_cm(target);
2117         int retval;
2118
2119         static const Jim_Nvp nvp_maskisr_modes[] = {
2120                 { .name = "auto", .value = CORTEX_M_ISRMASK_AUTO },
2121                 { .name = "off", .value = CORTEX_M_ISRMASK_OFF },
2122                 { .name = "on", .value = CORTEX_M_ISRMASK_ON },
2123                 { .name = NULL, .value = -1 },
2124         };
2125         const Jim_Nvp *n;
2126
2127
2128         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2129         if (retval != ERROR_OK)
2130                 return retval;
2131
2132         if (target->state != TARGET_HALTED) {
2133                 command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME);
2134                 return ERROR_OK;
2135         }
2136
2137         if (CMD_ARGC > 0) {
2138                 n = Jim_Nvp_name2value_simple(nvp_maskisr_modes, CMD_ARGV[0]);
2139                 if (n->name == NULL)
2140                         return ERROR_COMMAND_SYNTAX_ERROR;
2141                 cortex_m->isrmasking_mode = n->value;
2142
2143
2144                 if (cortex_m->isrmasking_mode == CORTEX_M_ISRMASK_ON)
2145                         cortex_m_write_debug_halt_mask(target, C_HALT | C_MASKINTS, 0);
2146                 else
2147                         cortex_m_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
2148         }
2149
2150         n = Jim_Nvp_value2name_simple(nvp_maskisr_modes, cortex_m->isrmasking_mode);
2151         command_print(CMD_CTX, "cortex_m interrupt mask %s", n->name);
2152
2153         return ERROR_OK;
2154 }
2155
2156 COMMAND_HANDLER(handle_cortex_m_reset_config_command)
2157 {
2158         struct target *target = get_current_target(CMD_CTX);
2159         struct cortex_m_common *cortex_m = target_to_cm(target);
2160         int retval;
2161         char *reset_config;
2162
2163         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2164         if (retval != ERROR_OK)
2165                 return retval;
2166
2167         if (CMD_ARGC > 0) {
2168                 if (strcmp(*CMD_ARGV, "sysresetreq") == 0)
2169                         cortex_m->soft_reset_config = CORTEX_M_RESET_SYSRESETREQ;
2170                 else if (strcmp(*CMD_ARGV, "vectreset") == 0)
2171                         cortex_m->soft_reset_config = CORTEX_M_RESET_VECTRESET;
2172         }
2173
2174         switch (cortex_m->soft_reset_config) {
2175                 case CORTEX_M_RESET_SYSRESETREQ:
2176                         reset_config = "sysresetreq";
2177                         break;
2178
2179                 case CORTEX_M_RESET_VECTRESET:
2180                         reset_config = "vectreset";
2181                         break;
2182
2183                 default:
2184                         reset_config = "unknown";
2185                         break;
2186         }
2187
2188         command_print(CMD_CTX, "cortex_m reset_config %s", reset_config);
2189
2190         return ERROR_OK;
2191 }
2192
2193 static const struct command_registration cortex_m_exec_command_handlers[] = {
2194         {
2195                 .name = "maskisr",
2196                 .handler = handle_cortex_m_mask_interrupts_command,
2197                 .mode = COMMAND_EXEC,
2198                 .help = "mask cortex_m interrupts",
2199                 .usage = "['auto'|'on'|'off']",
2200         },
2201         {
2202                 .name = "vector_catch",
2203                 .handler = handle_cortex_m_vector_catch_command,
2204                 .mode = COMMAND_EXEC,
2205                 .help = "configure hardware vectors to trigger debug entry",
2206                 .usage = "['all'|'none'|('bus_err'|'chk_err'|...)*]",
2207         },
2208         {
2209                 .name = "reset_config",
2210                 .handler = handle_cortex_m_reset_config_command,
2211                 .mode = COMMAND_ANY,
2212                 .help = "configure software reset handling",
2213                 .usage = "['srst'|'sysresetreq'|'vectreset']",
2214         },
2215         COMMAND_REGISTRATION_DONE
2216 };
2217 static const struct command_registration cortex_m_command_handlers[] = {
2218         {
2219                 .chain = armv7m_command_handlers,
2220         },
2221         {
2222                 .name = "cortex_m",
2223                 .mode = COMMAND_EXEC,
2224                 .help = "Cortex-M command group",
2225                 .usage = "",
2226                 .chain = cortex_m_exec_command_handlers,
2227         },
2228         COMMAND_REGISTRATION_DONE
2229 };
2230
2231 struct target_type cortexm_target = {
2232         .name = "cortex_m",
2233         .deprecated_name = "cortex_m3",
2234
2235         .poll = cortex_m_poll,
2236         .arch_state = armv7m_arch_state,
2237
2238         .target_request_data = cortex_m_target_request_data,
2239
2240         .halt = cortex_m_halt,
2241         .resume = cortex_m_resume,
2242         .step = cortex_m_step,
2243
2244         .assert_reset = cortex_m_assert_reset,
2245         .deassert_reset = cortex_m_deassert_reset,
2246         .soft_reset_halt = cortex_m_soft_reset_halt,
2247
2248         .get_gdb_reg_list = armv7m_get_gdb_reg_list,
2249
2250         .read_memory = cortex_m_read_memory,
2251         .write_memory = cortex_m_write_memory,
2252         .checksum_memory = armv7m_checksum_memory,
2253         .blank_check_memory = armv7m_blank_check_memory,
2254
2255         .run_algorithm = armv7m_run_algorithm,
2256         .start_algorithm = armv7m_start_algorithm,
2257         .wait_algorithm = armv7m_wait_algorithm,
2258
2259         .add_breakpoint = cortex_m_add_breakpoint,
2260         .remove_breakpoint = cortex_m_remove_breakpoint,
2261         .add_watchpoint = cortex_m_add_watchpoint,
2262         .remove_watchpoint = cortex_m_remove_watchpoint,
2263
2264         .commands = cortex_m_command_handlers,
2265         .target_create = cortex_m_target_create,
2266         .init_target = cortex_m_init_target,
2267         .examine = cortex_m_examine,
2268 };