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 == 3) {
1266                 LOG_DEBUG("Using a two byte breakpoint for 32bit Thumb-2 request");
1267                 breakpoint->length = 2;
1268         }
1269
1270         if ((breakpoint->length != 2)) {
1271                 LOG_INFO("only breakpoints of two bytes length supported");
1272                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1273         }
1274
1275         if (breakpoint->type == BKPT_HARD)
1276                 cortex_m->fp_code_available--;
1277
1278         return cortex_m_set_breakpoint(target, breakpoint);
1279 }
1280
1281 int cortex_m_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
1282 {
1283         struct cortex_m_common *cortex_m = target_to_cm(target);
1284
1285         /* REVISIT why check? FBP can be updated with core running ... */
1286         if (target->state != TARGET_HALTED) {
1287                 LOG_WARNING("target not halted");
1288                 return ERROR_TARGET_NOT_HALTED;
1289         }
1290
1291         if (cortex_m->auto_bp_type)
1292                 breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
1293
1294         if (breakpoint->set)
1295                 cortex_m_unset_breakpoint(target, breakpoint);
1296
1297         if (breakpoint->type == BKPT_HARD)
1298                 cortex_m->fp_code_available++;
1299
1300         return ERROR_OK;
1301 }
1302
1303 int cortex_m_set_watchpoint(struct target *target, struct watchpoint *watchpoint)
1304 {
1305         int dwt_num = 0;
1306         uint32_t mask, temp;
1307         struct cortex_m_common *cortex_m = target_to_cm(target);
1308
1309         /* watchpoint params were validated earlier */
1310         mask = 0;
1311         temp = watchpoint->length;
1312         while (temp) {
1313                 temp >>= 1;
1314                 mask++;
1315         }
1316         mask--;
1317
1318         /* REVISIT Don't fully trust these "not used" records ... users
1319          * may set up breakpoints by hand, e.g. dual-address data value
1320          * watchpoint using comparator #1; comparator #0 matching cycle
1321          * count; send data trace info through ITM and TPIU; etc
1322          */
1323         struct cortex_m_dwt_comparator *comparator;
1324
1325         for (comparator = cortex_m->dwt_comparator_list;
1326                 comparator->used && dwt_num < cortex_m->dwt_num_comp;
1327                 comparator++, dwt_num++)
1328                 continue;
1329         if (dwt_num >= cortex_m->dwt_num_comp) {
1330                 LOG_ERROR("Can not find free DWT Comparator");
1331                 return ERROR_FAIL;
1332         }
1333         comparator->used = 1;
1334         watchpoint->set = dwt_num + 1;
1335
1336         comparator->comp = watchpoint->address;
1337         target_write_u32(target, comparator->dwt_comparator_address + 0,
1338                 comparator->comp);
1339
1340         comparator->mask = mask;
1341         target_write_u32(target, comparator->dwt_comparator_address + 4,
1342                 comparator->mask);
1343
1344         switch (watchpoint->rw) {
1345                 case WPT_READ:
1346                         comparator->function = 5;
1347                         break;
1348                 case WPT_WRITE:
1349                         comparator->function = 6;
1350                         break;
1351                 case WPT_ACCESS:
1352                         comparator->function = 7;
1353                         break;
1354         }
1355         target_write_u32(target, comparator->dwt_comparator_address + 8,
1356                 comparator->function);
1357
1358         LOG_DEBUG("Watchpoint (ID %d) DWT%d 0x%08x 0x%x 0x%05x",
1359                 watchpoint->unique_id, dwt_num,
1360                 (unsigned) comparator->comp,
1361                 (unsigned) comparator->mask,
1362                 (unsigned) comparator->function);
1363         return ERROR_OK;
1364 }
1365
1366 int cortex_m_unset_watchpoint(struct target *target, struct watchpoint *watchpoint)
1367 {
1368         struct cortex_m_common *cortex_m = target_to_cm(target);
1369         struct cortex_m_dwt_comparator *comparator;
1370         int dwt_num;
1371
1372         if (!watchpoint->set) {
1373                 LOG_WARNING("watchpoint (wpid: %d) not set",
1374                         watchpoint->unique_id);
1375                 return ERROR_OK;
1376         }
1377
1378         dwt_num = watchpoint->set - 1;
1379
1380         LOG_DEBUG("Watchpoint (ID %d) DWT%d address: 0x%08x clear",
1381                 watchpoint->unique_id, dwt_num,
1382                 (unsigned) watchpoint->address);
1383
1384         if ((dwt_num < 0) || (dwt_num >= cortex_m->dwt_num_comp)) {
1385                 LOG_DEBUG("Invalid DWT Comparator number in watchpoint");
1386                 return ERROR_OK;
1387         }
1388
1389         comparator = cortex_m->dwt_comparator_list + dwt_num;
1390         comparator->used = 0;
1391         comparator->function = 0;
1392         target_write_u32(target, comparator->dwt_comparator_address + 8,
1393                 comparator->function);
1394
1395         watchpoint->set = false;
1396
1397         return ERROR_OK;
1398 }
1399
1400 int cortex_m_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
1401 {
1402         struct cortex_m_common *cortex_m = target_to_cm(target);
1403
1404         if (cortex_m->dwt_comp_available < 1) {
1405                 LOG_DEBUG("no comparators?");
1406                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1407         }
1408
1409         /* hardware doesn't support data value masking */
1410         if (watchpoint->mask != ~(uint32_t)0) {
1411                 LOG_DEBUG("watchpoint value masks not supported");
1412                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1413         }
1414
1415         /* hardware allows address masks of up to 32K */
1416         unsigned mask;
1417
1418         for (mask = 0; mask < 16; mask++) {
1419                 if ((1u << mask) == watchpoint->length)
1420                         break;
1421         }
1422         if (mask == 16) {
1423                 LOG_DEBUG("unsupported watchpoint length");
1424                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1425         }
1426         if (watchpoint->address & ((1 << mask) - 1)) {
1427                 LOG_DEBUG("watchpoint address is unaligned");
1428                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1429         }
1430
1431         /* Caller doesn't seem to be able to describe watching for data
1432          * values of zero; that flags "no value".
1433          *
1434          * REVISIT This DWT may well be able to watch for specific data
1435          * values.  Requires comparator #1 to set DATAVMATCH and match
1436          * the data, and another comparator (DATAVADDR0) matching addr.
1437          */
1438         if (watchpoint->value) {
1439                 LOG_DEBUG("data value watchpoint not YET supported");
1440                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1441         }
1442
1443         cortex_m->dwt_comp_available--;
1444         LOG_DEBUG("dwt_comp_available: %d", cortex_m->dwt_comp_available);
1445
1446         return ERROR_OK;
1447 }
1448
1449 int cortex_m_remove_watchpoint(struct target *target, struct watchpoint *watchpoint)
1450 {
1451         struct cortex_m_common *cortex_m = target_to_cm(target);
1452
1453         /* REVISIT why check? DWT can be updated with core running ... */
1454         if (target->state != TARGET_HALTED) {
1455                 LOG_WARNING("target not halted");
1456                 return ERROR_TARGET_NOT_HALTED;
1457         }
1458
1459         if (watchpoint->set)
1460                 cortex_m_unset_watchpoint(target, watchpoint);
1461
1462         cortex_m->dwt_comp_available++;
1463         LOG_DEBUG("dwt_comp_available: %d", cortex_m->dwt_comp_available);
1464
1465         return ERROR_OK;
1466 }
1467
1468 void cortex_m_enable_watchpoints(struct target *target)
1469 {
1470         struct watchpoint *watchpoint = target->watchpoints;
1471
1472         /* set any pending watchpoints */
1473         while (watchpoint) {
1474                 if (!watchpoint->set)
1475                         cortex_m_set_watchpoint(target, watchpoint);
1476                 watchpoint = watchpoint->next;
1477         }
1478 }
1479
1480 static int cortex_m_load_core_reg_u32(struct target *target,
1481                 uint32_t num, uint32_t *value)
1482 {
1483         int retval;
1484
1485         /* NOTE:  we "know" here that the register identifiers used
1486          * in the v7m header match the Cortex-M3 Debug Core Register
1487          * Selector values for R0..R15, xPSR, MSP, and PSP.
1488          */
1489         switch (num) {
1490                 case 0 ... 18:
1491                         /* read a normal core register */
1492                         retval = cortexm_dap_read_coreregister_u32(target, value, num);
1493
1494                         if (retval != ERROR_OK) {
1495                                 LOG_ERROR("JTAG failure %i", retval);
1496                                 return ERROR_JTAG_DEVICE_ERROR;
1497                         }
1498                         LOG_DEBUG("load from core reg %i  value 0x%" PRIx32 "", (int)num, *value);
1499                         break;
1500
1501                 case ARMV7M_PRIMASK:
1502                 case ARMV7M_BASEPRI:
1503                 case ARMV7M_FAULTMASK:
1504                 case ARMV7M_CONTROL:
1505                         /* Cortex-M3 packages these four registers as bitfields
1506                          * in one Debug Core register.  So say r0 and r2 docs;
1507                          * it was removed from r1 docs, but still works.
1508                          */
1509                         cortexm_dap_read_coreregister_u32(target, value, 20);
1510
1511                         switch (num) {
1512                                 case ARMV7M_PRIMASK:
1513                                         *value = buf_get_u32((uint8_t *)value, 0, 1);
1514                                         break;
1515
1516                                 case ARMV7M_BASEPRI:
1517                                         *value = buf_get_u32((uint8_t *)value, 8, 8);
1518                                         break;
1519
1520                                 case ARMV7M_FAULTMASK:
1521                                         *value = buf_get_u32((uint8_t *)value, 16, 1);
1522                                         break;
1523
1524                                 case ARMV7M_CONTROL:
1525                                         *value = buf_get_u32((uint8_t *)value, 24, 2);
1526                                         break;
1527                         }
1528
1529                         LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "", (int)num, *value);
1530                         break;
1531
1532                 default:
1533                         return ERROR_COMMAND_SYNTAX_ERROR;
1534         }
1535
1536         return ERROR_OK;
1537 }
1538
1539 static int cortex_m_store_core_reg_u32(struct target *target,
1540                 uint32_t num, uint32_t value)
1541 {
1542         int retval;
1543         uint32_t reg;
1544         struct armv7m_common *armv7m = target_to_armv7m(target);
1545
1546         /* NOTE:  we "know" here that the register identifiers used
1547          * in the v7m header match the Cortex-M3 Debug Core Register
1548          * Selector values for R0..R15, xPSR, MSP, and PSP.
1549          */
1550         switch (num) {
1551                 case 0 ... 18:
1552                         retval = cortexm_dap_write_coreregister_u32(target, value, num);
1553                         if (retval != ERROR_OK) {
1554                                 struct reg *r;
1555
1556                                 LOG_ERROR("JTAG failure");
1557                                 r = armv7m->arm.core_cache->reg_list + num;
1558                                 r->dirty = r->valid;
1559                                 return ERROR_JTAG_DEVICE_ERROR;
1560                         }
1561                         LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
1562                         break;
1563
1564                 case ARMV7M_PRIMASK:
1565                 case ARMV7M_BASEPRI:
1566                 case ARMV7M_FAULTMASK:
1567                 case ARMV7M_CONTROL:
1568                         /* Cortex-M3 packages these four registers as bitfields
1569                          * in one Debug Core register.  So say r0 and r2 docs;
1570                          * it was removed from r1 docs, but still works.
1571                          */
1572                         cortexm_dap_read_coreregister_u32(target, &reg, 20);
1573
1574                         switch (num) {
1575                                 case ARMV7M_PRIMASK:
1576                                         buf_set_u32((uint8_t *)&reg, 0, 1, value);
1577                                         break;
1578
1579                                 case ARMV7M_BASEPRI:
1580                                         buf_set_u32((uint8_t *)&reg, 8, 8, value);
1581                                         break;
1582
1583                                 case ARMV7M_FAULTMASK:
1584                                         buf_set_u32((uint8_t *)&reg, 16, 1, value);
1585                                         break;
1586
1587                                 case ARMV7M_CONTROL:
1588                                         buf_set_u32((uint8_t *)&reg, 24, 2, value);
1589                                         break;
1590                         }
1591
1592                         cortexm_dap_write_coreregister_u32(target, reg, 20);
1593
1594                         LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
1595                         break;
1596
1597                 default:
1598                         return ERROR_COMMAND_SYNTAX_ERROR;
1599         }
1600
1601         return ERROR_OK;
1602 }
1603
1604 static int cortex_m_read_memory(struct target *target, uint32_t address,
1605         uint32_t size, uint32_t count, uint8_t *buffer)
1606 {
1607         struct armv7m_common *armv7m = target_to_armv7m(target);
1608         struct adiv5_dap *swjdp = armv7m->arm.dap;
1609
1610         if (armv7m->arm.is_armv6m) {
1611                 /* armv6m does not handle unaligned memory access */
1612                 if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1613                         return ERROR_TARGET_UNALIGNED_ACCESS;
1614         }
1615
1616         return mem_ap_read(swjdp, buffer, size, count, address, true);
1617 }
1618
1619 static int cortex_m_write_memory(struct target *target, uint32_t address,
1620         uint32_t size, uint32_t count, const uint8_t *buffer)
1621 {
1622         struct armv7m_common *armv7m = target_to_armv7m(target);
1623         struct adiv5_dap *swjdp = armv7m->arm.dap;
1624
1625         if (armv7m->arm.is_armv6m) {
1626                 /* armv6m does not handle unaligned memory access */
1627                 if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1628                         return ERROR_TARGET_UNALIGNED_ACCESS;
1629         }
1630
1631         return mem_ap_write(swjdp, buffer, size, count, address, true);
1632 }
1633
1634 static int cortex_m_init_target(struct command_context *cmd_ctx,
1635         struct target *target)
1636 {
1637         armv7m_build_reg_cache(target);
1638         return ERROR_OK;
1639 }
1640
1641 /* REVISIT cache valid/dirty bits are unmaintained.  We could set "valid"
1642  * on r/w if the core is not running, and clear on resume or reset ... or
1643  * at least, in a post_restore_context() method.
1644  */
1645
1646 struct dwt_reg_state {
1647         struct target *target;
1648         uint32_t addr;
1649         uint32_t value;         /* scratch/cache */
1650 };
1651
1652 static int cortex_m_dwt_get_reg(struct reg *reg)
1653 {
1654         struct dwt_reg_state *state = reg->arch_info;
1655
1656         return target_read_u32(state->target, state->addr, &state->value);
1657 }
1658
1659 static int cortex_m_dwt_set_reg(struct reg *reg, uint8_t *buf)
1660 {
1661         struct dwt_reg_state *state = reg->arch_info;
1662
1663         return target_write_u32(state->target, state->addr,
1664                         buf_get_u32(buf, 0, reg->size));
1665 }
1666
1667 struct dwt_reg {
1668         uint32_t addr;
1669         char *name;
1670         unsigned size;
1671 };
1672
1673 static struct dwt_reg dwt_base_regs[] = {
1674         { DWT_CTRL, "dwt_ctrl", 32, },
1675         /* NOTE that Erratum 532314 (fixed r2p0) affects CYCCNT:  it wrongly
1676          * increments while the core is asleep.
1677          */
1678         { DWT_CYCCNT, "dwt_cyccnt", 32, },
1679         /* plus some 8 bit counters, useful for profiling with TPIU */
1680 };
1681
1682 static struct dwt_reg dwt_comp[] = {
1683 #define DWT_COMPARATOR(i) \
1684                 { DWT_COMP0 + 0x10 * (i), "dwt_" #i "_comp", 32, }, \
1685                 { DWT_MASK0 + 0x10 * (i), "dwt_" #i "_mask", 4, }, \
1686                 { DWT_FUNCTION0 + 0x10 * (i), "dwt_" #i "_function", 32, }
1687         DWT_COMPARATOR(0),
1688         DWT_COMPARATOR(1),
1689         DWT_COMPARATOR(2),
1690         DWT_COMPARATOR(3),
1691 #undef DWT_COMPARATOR
1692 };
1693
1694 static const struct reg_arch_type dwt_reg_type = {
1695         .get = cortex_m_dwt_get_reg,
1696         .set = cortex_m_dwt_set_reg,
1697 };
1698
1699 static void cortex_m_dwt_addreg(struct target *t, struct reg *r, struct dwt_reg *d)
1700 {
1701         struct dwt_reg_state *state;
1702
1703         state = calloc(1, sizeof *state);
1704         if (!state)
1705                 return;
1706         state->addr = d->addr;
1707         state->target = t;
1708
1709         r->name = d->name;
1710         r->size = d->size;
1711         r->value = &state->value;
1712         r->arch_info = state;
1713         r->type = &dwt_reg_type;
1714 }
1715
1716 void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
1717 {
1718         uint32_t dwtcr;
1719         struct reg_cache *cache;
1720         struct cortex_m_dwt_comparator *comparator;
1721         int reg, i;
1722
1723         target_read_u32(target, DWT_CTRL, &dwtcr);
1724         if (!dwtcr) {
1725                 LOG_DEBUG("no DWT");
1726                 return;
1727         }
1728
1729         cm->dwt_num_comp = (dwtcr >> 28) & 0xF;
1730         cm->dwt_comp_available = cm->dwt_num_comp;
1731         cm->dwt_comparator_list = calloc(cm->dwt_num_comp,
1732                         sizeof(struct cortex_m_dwt_comparator));
1733         if (!cm->dwt_comparator_list) {
1734 fail0:
1735                 cm->dwt_num_comp = 0;
1736                 LOG_ERROR("out of mem");
1737                 return;
1738         }
1739
1740         cache = calloc(1, sizeof *cache);
1741         if (!cache) {
1742 fail1:
1743                 free(cm->dwt_comparator_list);
1744                 goto fail0;
1745         }
1746         cache->name = "Cortex-M DWT registers";
1747         cache->num_regs = 2 + cm->dwt_num_comp * 3;
1748         cache->reg_list = calloc(cache->num_regs, sizeof *cache->reg_list);
1749         if (!cache->reg_list) {
1750                 free(cache);
1751                 goto fail1;
1752         }
1753
1754         for (reg = 0; reg < 2; reg++)
1755                 cortex_m_dwt_addreg(target, cache->reg_list + reg,
1756                         dwt_base_regs + reg);
1757
1758         comparator = cm->dwt_comparator_list;
1759         for (i = 0; i < cm->dwt_num_comp; i++, comparator++) {
1760                 int j;
1761
1762                 comparator->dwt_comparator_address = DWT_COMP0 + 0x10 * i;
1763                 for (j = 0; j < 3; j++, reg++)
1764                         cortex_m_dwt_addreg(target, cache->reg_list + reg,
1765                                 dwt_comp + 3 * i + j);
1766
1767                 /* make sure we clear any watchpoints enabled on the target */
1768                 target_write_u32(target, comparator->dwt_comparator_address + 8, 0);
1769         }
1770
1771         *register_get_last_cache_p(&target->reg_cache) = cache;
1772         cm->dwt_cache = cache;
1773
1774         LOG_DEBUG("DWT dwtcr 0x%" PRIx32 ", comp %d, watch%s",
1775                 dwtcr, cm->dwt_num_comp,
1776                 (dwtcr & (0xf << 24)) ? " only" : "/trigger");
1777
1778         /* REVISIT:  if num_comp > 1, check whether comparator #1 can
1779          * implement single-address data value watchpoints ... so we
1780          * won't need to check it later, when asked to set one up.
1781          */
1782 }
1783
1784 #define MVFR0 0xe000ef40
1785 #define MVFR1 0xe000ef44
1786
1787 #define MVFR0_DEFAULT_M4 0x10110021
1788 #define MVFR1_DEFAULT_M4 0x11000011
1789
1790 int cortex_m_examine(struct target *target)
1791 {
1792         int retval;
1793         uint32_t cpuid, fpcr, mvfr0, mvfr1;
1794         int i;
1795         struct cortex_m_common *cortex_m = target_to_cm(target);
1796         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
1797         struct armv7m_common *armv7m = target_to_armv7m(target);
1798
1799         /* stlink shares the examine handler but does not support
1800          * all its calls */
1801         if (!armv7m->stlink) {
1802                 retval = ahbap_debugport_init(swjdp);
1803                 if (retval != ERROR_OK)
1804                         return retval;
1805         }
1806
1807         if (!target_was_examined(target)) {
1808                 target_set_examined(target);
1809
1810                 /* Read from Device Identification Registers */
1811                 retval = target_read_u32(target, CPUID, &cpuid);
1812                 if (retval != ERROR_OK)
1813                         return retval;
1814
1815                 /* Get CPU Type */
1816                 i = (cpuid >> 4) & 0xf;
1817
1818                 LOG_DEBUG("Cortex-M%d r%" PRId8 "p%" PRId8 " processor detected",
1819                                 i, (uint8_t)((cpuid >> 20) & 0xf), (uint8_t)((cpuid >> 0) & 0xf));
1820                 LOG_DEBUG("cpuid: 0x%8.8" PRIx32 "", cpuid);
1821
1822                 /* test for floating point feature on cortex-m4 */
1823                 if (i == 4) {
1824                         target_read_u32(target, MVFR0, &mvfr0);
1825                         target_read_u32(target, MVFR1, &mvfr1);
1826
1827                         if ((mvfr0 == MVFR0_DEFAULT_M4) && (mvfr1 == MVFR1_DEFAULT_M4)) {
1828                                 LOG_DEBUG("Cortex-M%d floating point feature FPv4_SP found", i);
1829                                 armv7m->fp_feature = FPv4_SP;
1830                         }
1831                 } else if (i == 0) {
1832                         /* Cortex-M0 does not support unaligned memory access */
1833                         armv7m->arm.is_armv6m = true;
1834                 }
1835
1836                 if (i == 4 || i == 3) {
1837                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
1838                         armv7m->dap.tar_autoincr_block = (1 << 12);
1839                 }
1840
1841                 /* NOTE: FPB and DWT are both optional. */
1842
1843                 /* Setup FPB */
1844                 target_read_u32(target, FP_CTRL, &fpcr);
1845                 cortex_m->auto_bp_type = 1;
1846                 /* bits [14:12] and [7:4] */
1847                 cortex_m->fp_num_code = ((fpcr >> 8) & 0x70) | ((fpcr >> 4) & 0xF);
1848                 cortex_m->fp_num_lit = (fpcr >> 8) & 0xF;
1849                 cortex_m->fp_code_available = cortex_m->fp_num_code;
1850                 cortex_m->fp_comparator_list = calloc(
1851                                 cortex_m->fp_num_code + cortex_m->fp_num_lit,
1852                                 sizeof(struct cortex_m_fp_comparator));
1853                 cortex_m->fpb_enabled = fpcr & 1;
1854                 for (i = 0; i < cortex_m->fp_num_code + cortex_m->fp_num_lit; i++) {
1855                         cortex_m->fp_comparator_list[i].type =
1856                                 (i < cortex_m->fp_num_code) ? FPCR_CODE : FPCR_LITERAL;
1857                         cortex_m->fp_comparator_list[i].fpcr_address = FP_COMP0 + 4 * i;
1858
1859                         /* make sure we clear any breakpoints enabled on the target */
1860                         target_write_u32(target, cortex_m->fp_comparator_list[i].fpcr_address, 0);
1861                 }
1862                 LOG_DEBUG("FPB fpcr 0x%" PRIx32 ", numcode %i, numlit %i",
1863                         fpcr,
1864                         cortex_m->fp_num_code,
1865                         cortex_m->fp_num_lit);
1866
1867                 /* Setup DWT */
1868                 cortex_m_dwt_setup(cortex_m, target);
1869
1870                 /* These hardware breakpoints only work for code in flash! */
1871                 LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
1872                         target_name(target),
1873                         cortex_m->fp_num_code,
1874                         cortex_m->dwt_num_comp);
1875         }
1876
1877         return ERROR_OK;
1878 }
1879
1880 static int cortex_m_dcc_read(struct target *target, uint8_t *value, uint8_t *ctrl)
1881 {
1882         struct armv7m_common *armv7m = target_to_armv7m(target);
1883         struct adiv5_dap *swjdp = armv7m->arm.dap;
1884         uint16_t dcrdr;
1885         uint8_t buf[2];
1886         int retval;
1887
1888         retval = mem_ap_read(swjdp, buf, 2, 1, DCB_DCRDR, false);
1889         if (retval != ERROR_OK)
1890                 return retval;
1891
1892         dcrdr = target_buffer_get_u16(target, buf);
1893         *ctrl = (uint8_t)dcrdr;
1894         *value = (uint8_t)(dcrdr >> 8);
1895
1896         LOG_DEBUG("data 0x%x ctrl 0x%x", *value, *ctrl);
1897
1898         /* write ack back to software dcc register
1899          * signify we have read data */
1900         if (dcrdr & (1 << 0)) {
1901                 target_buffer_set_u16(target, buf, 0);
1902                 retval = mem_ap_write(swjdp, buf, 2, 1, DCB_DCRDR, false);
1903                 if (retval != ERROR_OK)
1904                         return retval;
1905         }
1906
1907         return ERROR_OK;
1908 }
1909
1910 static int cortex_m_target_request_data(struct target *target,
1911         uint32_t size, uint8_t *buffer)
1912 {
1913         uint8_t data;
1914         uint8_t ctrl;
1915         uint32_t i;
1916
1917         for (i = 0; i < (size * 4); i++) {
1918                 int retval = cortex_m_dcc_read(target, &data, &ctrl);
1919                 if (retval != ERROR_OK)
1920                         return retval;
1921                 buffer[i] = data;
1922         }
1923
1924         return ERROR_OK;
1925 }
1926
1927 static int cortex_m_handle_target_request(void *priv)
1928 {
1929         struct target *target = priv;
1930         if (!target_was_examined(target))
1931                 return ERROR_OK;
1932
1933         if (!target->dbg_msg_enabled)
1934                 return ERROR_OK;
1935
1936         if (target->state == TARGET_RUNNING) {
1937                 uint8_t data;
1938                 uint8_t ctrl;
1939                 int retval;
1940
1941                 retval = cortex_m_dcc_read(target, &data, &ctrl);
1942                 if (retval != ERROR_OK)
1943                         return retval;
1944
1945                 /* check if we have data */
1946                 if (ctrl & (1 << 0)) {
1947                         uint32_t request;
1948
1949                         /* we assume target is quick enough */
1950                         request = data;
1951                         for (int i = 1; i <= 3; i++) {
1952                                 retval = cortex_m_dcc_read(target, &data, &ctrl);
1953                                 if (retval != ERROR_OK)
1954                                         return retval;
1955                                 request |= ((uint32_t)data << (i * 8));
1956                         }
1957                         target_request(target, request);
1958                 }
1959         }
1960
1961         return ERROR_OK;
1962 }
1963
1964 static int cortex_m_init_arch_info(struct target *target,
1965         struct cortex_m_common *cortex_m, struct jtag_tap *tap)
1966 {
1967         int retval;
1968         struct armv7m_common *armv7m = &cortex_m->armv7m;
1969
1970         armv7m_init_arch_info(target, armv7m);
1971
1972         /* prepare JTAG information for the new target */
1973         cortex_m->jtag_info.tap = tap;
1974         cortex_m->jtag_info.scann_size = 4;
1975
1976         /* default reset mode is to use srst if fitted
1977          * if not it will use CORTEX_M3_RESET_VECTRESET */
1978         cortex_m->soft_reset_config = CORTEX_M_RESET_VECTRESET;
1979
1980         armv7m->arm.dap = &armv7m->dap;
1981
1982         /* Leave (only) generic DAP stuff for debugport_init(); */
1983         armv7m->dap.jtag_info = &cortex_m->jtag_info;
1984         armv7m->dap.memaccess_tck = 8;
1985
1986         /* Cortex-M3/M4 has 4096 bytes autoincrement range
1987          * but set a safe default to 1024 to support Cortex-M0
1988          * this will be changed in cortex_m3_examine if a M3/M4 is detected */
1989         armv7m->dap.tar_autoincr_block = (1 << 10);
1990
1991         /* register arch-specific functions */
1992         armv7m->examine_debug_reason = cortex_m_examine_debug_reason;
1993
1994         armv7m->post_debug_entry = NULL;
1995
1996         armv7m->pre_restore_context = NULL;
1997
1998         armv7m->load_core_reg_u32 = cortex_m_load_core_reg_u32;
1999         armv7m->store_core_reg_u32 = cortex_m_store_core_reg_u32;
2000
2001         target_register_timer_callback(cortex_m_handle_target_request, 1, 1, target);
2002
2003         retval = arm_jtag_setup_connection(&cortex_m->jtag_info);
2004         if (retval != ERROR_OK)
2005                 return retval;
2006
2007         return ERROR_OK;
2008 }
2009
2010 static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
2011 {
2012         struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
2013
2014         cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
2015         cortex_m_init_arch_info(target, cortex_m, target->tap);
2016
2017         return ERROR_OK;
2018 }
2019
2020 /*--------------------------------------------------------------------------*/
2021
2022 static int cortex_m_verify_pointer(struct command_context *cmd_ctx,
2023         struct cortex_m_common *cm)
2024 {
2025         if (cm->common_magic != CORTEX_M_COMMON_MAGIC) {
2026                 command_print(cmd_ctx, "target is not a Cortex-M");
2027                 return ERROR_TARGET_INVALID;
2028         }
2029         return ERROR_OK;
2030 }
2031
2032 /*
2033  * Only stuff below this line should need to verify that its target
2034  * is a Cortex-M3.  Everything else should have indirected through the
2035  * cortexm3_target structure, which is only used with CM3 targets.
2036  */
2037
2038 static const struct {
2039         char name[10];
2040         unsigned mask;
2041 } vec_ids[] = {
2042         { "hard_err",   VC_HARDERR, },
2043         { "int_err",    VC_INTERR, },
2044         { "bus_err",    VC_BUSERR, },
2045         { "state_err",  VC_STATERR, },
2046         { "chk_err",    VC_CHKERR, },
2047         { "nocp_err",   VC_NOCPERR, },
2048         { "mm_err",     VC_MMERR, },
2049         { "reset",      VC_CORERESET, },
2050 };
2051
2052 COMMAND_HANDLER(handle_cortex_m_vector_catch_command)
2053 {
2054         struct target *target = get_current_target(CMD_CTX);
2055         struct cortex_m_common *cortex_m = target_to_cm(target);
2056         struct armv7m_common *armv7m = &cortex_m->armv7m;
2057         struct adiv5_dap *swjdp = armv7m->arm.dap;
2058         uint32_t demcr = 0;
2059         int retval;
2060
2061         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2062         if (retval != ERROR_OK)
2063                 return retval;
2064
2065         retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
2066         if (retval != ERROR_OK)
2067                 return retval;
2068
2069         if (CMD_ARGC > 0) {
2070                 unsigned catch = 0;
2071
2072                 if (CMD_ARGC == 1) {
2073                         if (strcmp(CMD_ARGV[0], "all") == 0) {
2074                                 catch = VC_HARDERR | VC_INTERR | VC_BUSERR
2075                                         | VC_STATERR | VC_CHKERR | VC_NOCPERR
2076                                         | VC_MMERR | VC_CORERESET;
2077                                 goto write;
2078                         } else if (strcmp(CMD_ARGV[0], "none") == 0)
2079                                 goto write;
2080                 }
2081                 while (CMD_ARGC-- > 0) {
2082                         unsigned i;
2083                         for (i = 0; i < ARRAY_SIZE(vec_ids); i++) {
2084                                 if (strcmp(CMD_ARGV[CMD_ARGC], vec_ids[i].name) != 0)
2085                                         continue;
2086                                 catch |= vec_ids[i].mask;
2087                                 break;
2088                         }
2089                         if (i == ARRAY_SIZE(vec_ids)) {
2090                                 LOG_ERROR("No CM3 vector '%s'", CMD_ARGV[CMD_ARGC]);
2091                                 return ERROR_COMMAND_SYNTAX_ERROR;
2092                         }
2093                 }
2094 write:
2095                 /* For now, armv7m->demcr only stores vector catch flags. */
2096                 armv7m->demcr = catch;
2097
2098                 demcr &= ~0xffff;
2099                 demcr |= catch;
2100
2101                 /* write, but don't assume it stuck (why not??) */
2102                 retval = mem_ap_write_u32(swjdp, DCB_DEMCR, demcr);
2103                 if (retval != ERROR_OK)
2104                         return retval;
2105                 retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
2106                 if (retval != ERROR_OK)
2107                         return retval;
2108
2109                 /* FIXME be sure to clear DEMCR on clean server shutdown.
2110                  * Otherwise the vector catch hardware could fire when there's
2111                  * no debugger hooked up, causing much confusion...
2112                  */
2113         }
2114
2115         for (unsigned i = 0; i < ARRAY_SIZE(vec_ids); i++) {
2116                 command_print(CMD_CTX, "%9s: %s", vec_ids[i].name,
2117                         (demcr & vec_ids[i].mask) ? "catch" : "ignore");
2118         }
2119
2120         return ERROR_OK;
2121 }
2122
2123 COMMAND_HANDLER(handle_cortex_m_mask_interrupts_command)
2124 {
2125         struct target *target = get_current_target(CMD_CTX);
2126         struct cortex_m_common *cortex_m = target_to_cm(target);
2127         int retval;
2128
2129         static const Jim_Nvp nvp_maskisr_modes[] = {
2130                 { .name = "auto", .value = CORTEX_M_ISRMASK_AUTO },
2131                 { .name = "off", .value = CORTEX_M_ISRMASK_OFF },
2132                 { .name = "on", .value = CORTEX_M_ISRMASK_ON },
2133                 { .name = NULL, .value = -1 },
2134         };
2135         const Jim_Nvp *n;
2136
2137
2138         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2139         if (retval != ERROR_OK)
2140                 return retval;
2141
2142         if (target->state != TARGET_HALTED) {
2143                 command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME);
2144                 return ERROR_OK;
2145         }
2146
2147         if (CMD_ARGC > 0) {
2148                 n = Jim_Nvp_name2value_simple(nvp_maskisr_modes, CMD_ARGV[0]);
2149                 if (n->name == NULL)
2150                         return ERROR_COMMAND_SYNTAX_ERROR;
2151                 cortex_m->isrmasking_mode = n->value;
2152
2153
2154                 if (cortex_m->isrmasking_mode == CORTEX_M_ISRMASK_ON)
2155                         cortex_m_write_debug_halt_mask(target, C_HALT | C_MASKINTS, 0);
2156                 else
2157                         cortex_m_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
2158         }
2159
2160         n = Jim_Nvp_value2name_simple(nvp_maskisr_modes, cortex_m->isrmasking_mode);
2161         command_print(CMD_CTX, "cortex_m interrupt mask %s", n->name);
2162
2163         return ERROR_OK;
2164 }
2165
2166 COMMAND_HANDLER(handle_cortex_m_reset_config_command)
2167 {
2168         struct target *target = get_current_target(CMD_CTX);
2169         struct cortex_m_common *cortex_m = target_to_cm(target);
2170         int retval;
2171         char *reset_config;
2172
2173         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2174         if (retval != ERROR_OK)
2175                 return retval;
2176
2177         if (CMD_ARGC > 0) {
2178                 if (strcmp(*CMD_ARGV, "sysresetreq") == 0)
2179                         cortex_m->soft_reset_config = CORTEX_M_RESET_SYSRESETREQ;
2180                 else if (strcmp(*CMD_ARGV, "vectreset") == 0)
2181                         cortex_m->soft_reset_config = CORTEX_M_RESET_VECTRESET;
2182         }
2183
2184         switch (cortex_m->soft_reset_config) {
2185                 case CORTEX_M_RESET_SYSRESETREQ:
2186                         reset_config = "sysresetreq";
2187                         break;
2188
2189                 case CORTEX_M_RESET_VECTRESET:
2190                         reset_config = "vectreset";
2191                         break;
2192
2193                 default:
2194                         reset_config = "unknown";
2195                         break;
2196         }
2197
2198         command_print(CMD_CTX, "cortex_m reset_config %s", reset_config);
2199
2200         return ERROR_OK;
2201 }
2202
2203 static const struct command_registration cortex_m_exec_command_handlers[] = {
2204         {
2205                 .name = "maskisr",
2206                 .handler = handle_cortex_m_mask_interrupts_command,
2207                 .mode = COMMAND_EXEC,
2208                 .help = "mask cortex_m interrupts",
2209                 .usage = "['auto'|'on'|'off']",
2210         },
2211         {
2212                 .name = "vector_catch",
2213                 .handler = handle_cortex_m_vector_catch_command,
2214                 .mode = COMMAND_EXEC,
2215                 .help = "configure hardware vectors to trigger debug entry",
2216                 .usage = "['all'|'none'|('bus_err'|'chk_err'|...)*]",
2217         },
2218         {
2219                 .name = "reset_config",
2220                 .handler = handle_cortex_m_reset_config_command,
2221                 .mode = COMMAND_ANY,
2222                 .help = "configure software reset handling",
2223                 .usage = "['srst'|'sysresetreq'|'vectreset']",
2224         },
2225         COMMAND_REGISTRATION_DONE
2226 };
2227 static const struct command_registration cortex_m_command_handlers[] = {
2228         {
2229                 .chain = armv7m_command_handlers,
2230         },
2231         {
2232                 .name = "cortex_m",
2233                 .mode = COMMAND_EXEC,
2234                 .help = "Cortex-M command group",
2235                 .usage = "",
2236                 .chain = cortex_m_exec_command_handlers,
2237         },
2238         COMMAND_REGISTRATION_DONE
2239 };
2240
2241 struct target_type cortexm_target = {
2242         .name = "cortex_m",
2243         .deprecated_name = "cortex_m3",
2244
2245         .poll = cortex_m_poll,
2246         .arch_state = armv7m_arch_state,
2247
2248         .target_request_data = cortex_m_target_request_data,
2249
2250         .halt = cortex_m_halt,
2251         .resume = cortex_m_resume,
2252         .step = cortex_m_step,
2253
2254         .assert_reset = cortex_m_assert_reset,
2255         .deassert_reset = cortex_m_deassert_reset,
2256         .soft_reset_halt = cortex_m_soft_reset_halt,
2257
2258         .get_gdb_reg_list = armv7m_get_gdb_reg_list,
2259
2260         .read_memory = cortex_m_read_memory,
2261         .write_memory = cortex_m_write_memory,
2262         .checksum_memory = armv7m_checksum_memory,
2263         .blank_check_memory = armv7m_blank_check_memory,
2264
2265         .run_algorithm = armv7m_run_algorithm,
2266         .start_algorithm = armv7m_start_algorithm,
2267         .wait_algorithm = armv7m_wait_algorithm,
2268
2269         .add_breakpoint = cortex_m_add_breakpoint,
2270         .remove_breakpoint = cortex_m_remove_breakpoint,
2271         .add_watchpoint = cortex_m_add_watchpoint,
2272         .remove_watchpoint = cortex_m_remove_watchpoint,
2273
2274         .commands = cortex_m_command_handlers,
2275         .target_create = cortex_m_target_create,
2276         .init_target = cortex_m_init_target,
2277         .examine = cortex_m_examine,
2278 };