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                 buf_get_u32(arm->pc->value, 0, 32),
 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_FPSCR:
1502                         /* Floating-point Status and Registers */
1503                         retval = target_write_u32(target, DCB_DCRSR, 0x21);
1504                         if (retval != ERROR_OK)
1505                                 return retval;
1506                         retval = target_read_u32(target, DCB_DCRDR, value);
1507                         if (retval != ERROR_OK)
1508                                 return retval;
1509                         LOG_DEBUG("load from FPSCR  value 0x%" PRIx32, *value);
1510                         break;
1511
1512                 case ARMV7M_S0 ... ARMV7M_S31:
1513                         /* Floating-point Status and Registers */
1514                         retval = target_write_u32(target, DCB_DCRSR, num - ARMV7M_S0 + 0x40);
1515                         if (retval != ERROR_OK)
1516                                 return retval;
1517                         retval = target_read_u32(target, DCB_DCRDR, value);
1518                         if (retval != ERROR_OK)
1519                                 return retval;
1520                         LOG_DEBUG("load from FPU reg S%d  value 0x%" PRIx32,
1521                                   (int)(num - ARMV7M_S0), *value);
1522                         break;
1523
1524                 case ARMV7M_PRIMASK:
1525                 case ARMV7M_BASEPRI:
1526                 case ARMV7M_FAULTMASK:
1527                 case ARMV7M_CONTROL:
1528                         /* Cortex-M3 packages these four registers as bitfields
1529                          * in one Debug Core register.  So say r0 and r2 docs;
1530                          * it was removed from r1 docs, but still works.
1531                          */
1532                         cortexm_dap_read_coreregister_u32(target, value, 20);
1533
1534                         switch (num) {
1535                                 case ARMV7M_PRIMASK:
1536                                         *value = buf_get_u32((uint8_t *)value, 0, 1);
1537                                         break;
1538
1539                                 case ARMV7M_BASEPRI:
1540                                         *value = buf_get_u32((uint8_t *)value, 8, 8);
1541                                         break;
1542
1543                                 case ARMV7M_FAULTMASK:
1544                                         *value = buf_get_u32((uint8_t *)value, 16, 1);
1545                                         break;
1546
1547                                 case ARMV7M_CONTROL:
1548                                         *value = buf_get_u32((uint8_t *)value, 24, 2);
1549                                         break;
1550                         }
1551
1552                         LOG_DEBUG("load from special reg %i value 0x%" PRIx32 "", (int)num, *value);
1553                         break;
1554
1555                 default:
1556                         return ERROR_COMMAND_SYNTAX_ERROR;
1557         }
1558
1559         return ERROR_OK;
1560 }
1561
1562 static int cortex_m_store_core_reg_u32(struct target *target,
1563                 uint32_t num, uint32_t value)
1564 {
1565         int retval;
1566         uint32_t reg;
1567         struct armv7m_common *armv7m = target_to_armv7m(target);
1568
1569         /* NOTE:  we "know" here that the register identifiers used
1570          * in the v7m header match the Cortex-M3 Debug Core Register
1571          * Selector values for R0..R15, xPSR, MSP, and PSP.
1572          */
1573         switch (num) {
1574                 case 0 ... 18:
1575                         retval = cortexm_dap_write_coreregister_u32(target, value, num);
1576                         if (retval != ERROR_OK) {
1577                                 struct reg *r;
1578
1579                                 LOG_ERROR("JTAG failure");
1580                                 r = armv7m->arm.core_cache->reg_list + num;
1581                                 r->dirty = r->valid;
1582                                 return ERROR_JTAG_DEVICE_ERROR;
1583                         }
1584                         LOG_DEBUG("write core reg %i value 0x%" PRIx32 "", (int)num, value);
1585                         break;
1586
1587                 case ARMV7M_FPSCR:
1588                         /* Floating-point Status and Registers */
1589                         retval = target_write_u32(target, DCB_DCRDR, value);
1590                         if (retval != ERROR_OK)
1591                                 return retval;
1592                         retval = target_write_u32(target, DCB_DCRSR, 0x21 | (1<<16));
1593                         if (retval != ERROR_OK)
1594                                 return retval;
1595                         LOG_DEBUG("write FPSCR value 0x%" PRIx32, value);
1596                         break;
1597
1598                 case ARMV7M_S0 ... ARMV7M_S31:
1599                         /* Floating-point Status and Registers */
1600                         retval = target_write_u32(target, DCB_DCRDR, value);
1601                         if (retval != ERROR_OK)
1602                                 return retval;
1603                         retval = target_write_u32(target, DCB_DCRSR, (num - ARMV7M_S0 + 0x40) | (1<<16));
1604                         if (retval != ERROR_OK)
1605                                 return retval;
1606                         LOG_DEBUG("write FPU reg S%d  value 0x%" PRIx32,
1607                                   (int)(num - ARMV7M_S0), value);
1608                         break;
1609
1610                 case ARMV7M_PRIMASK:
1611                 case ARMV7M_BASEPRI:
1612                 case ARMV7M_FAULTMASK:
1613                 case ARMV7M_CONTROL:
1614                         /* Cortex-M3 packages these four registers as bitfields
1615                          * in one Debug Core register.  So say r0 and r2 docs;
1616                          * it was removed from r1 docs, but still works.
1617                          */
1618                         cortexm_dap_read_coreregister_u32(target, &reg, 20);
1619
1620                         switch (num) {
1621                                 case ARMV7M_PRIMASK:
1622                                         buf_set_u32((uint8_t *)&reg, 0, 1, value);
1623                                         break;
1624
1625                                 case ARMV7M_BASEPRI:
1626                                         buf_set_u32((uint8_t *)&reg, 8, 8, value);
1627                                         break;
1628
1629                                 case ARMV7M_FAULTMASK:
1630                                         buf_set_u32((uint8_t *)&reg, 16, 1, value);
1631                                         break;
1632
1633                                 case ARMV7M_CONTROL:
1634                                         buf_set_u32((uint8_t *)&reg, 24, 2, value);
1635                                         break;
1636                         }
1637
1638                         cortexm_dap_write_coreregister_u32(target, reg, 20);
1639
1640                         LOG_DEBUG("write special reg %i value 0x%" PRIx32 " ", (int)num, value);
1641                         break;
1642
1643                 default:
1644                         return ERROR_COMMAND_SYNTAX_ERROR;
1645         }
1646
1647         return ERROR_OK;
1648 }
1649
1650 static int cortex_m_read_memory(struct target *target, uint32_t address,
1651         uint32_t size, uint32_t count, uint8_t *buffer)
1652 {
1653         struct armv7m_common *armv7m = target_to_armv7m(target);
1654         struct adiv5_dap *swjdp = armv7m->arm.dap;
1655
1656         if (armv7m->arm.is_armv6m) {
1657                 /* armv6m does not handle unaligned memory access */
1658                 if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1659                         return ERROR_TARGET_UNALIGNED_ACCESS;
1660         }
1661
1662         return mem_ap_read(swjdp, buffer, size, count, address, true);
1663 }
1664
1665 static int cortex_m_write_memory(struct target *target, uint32_t address,
1666         uint32_t size, uint32_t count, const uint8_t *buffer)
1667 {
1668         struct armv7m_common *armv7m = target_to_armv7m(target);
1669         struct adiv5_dap *swjdp = armv7m->arm.dap;
1670
1671         if (armv7m->arm.is_armv6m) {
1672                 /* armv6m does not handle unaligned memory access */
1673                 if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1674                         return ERROR_TARGET_UNALIGNED_ACCESS;
1675         }
1676
1677         return mem_ap_write(swjdp, buffer, size, count, address, true);
1678 }
1679
1680 static int cortex_m_init_target(struct command_context *cmd_ctx,
1681         struct target *target)
1682 {
1683         armv7m_build_reg_cache(target);
1684         return ERROR_OK;
1685 }
1686
1687 /* REVISIT cache valid/dirty bits are unmaintained.  We could set "valid"
1688  * on r/w if the core is not running, and clear on resume or reset ... or
1689  * at least, in a post_restore_context() method.
1690  */
1691
1692 struct dwt_reg_state {
1693         struct target *target;
1694         uint32_t addr;
1695         uint8_t value[4];               /* scratch/cache */
1696 };
1697
1698 static int cortex_m_dwt_get_reg(struct reg *reg)
1699 {
1700         struct dwt_reg_state *state = reg->arch_info;
1701
1702         uint32_t tmp;
1703         int retval = target_read_u32(state->target, state->addr, &tmp);
1704         if (retval != ERROR_OK)
1705                 return retval;
1706
1707         buf_set_u32(state->value, 0, 32, tmp);
1708         return ERROR_OK;
1709 }
1710
1711 static int cortex_m_dwt_set_reg(struct reg *reg, uint8_t *buf)
1712 {
1713         struct dwt_reg_state *state = reg->arch_info;
1714
1715         return target_write_u32(state->target, state->addr,
1716                         buf_get_u32(buf, 0, reg->size));
1717 }
1718
1719 struct dwt_reg {
1720         uint32_t addr;
1721         char *name;
1722         unsigned size;
1723 };
1724
1725 static struct dwt_reg dwt_base_regs[] = {
1726         { DWT_CTRL, "dwt_ctrl", 32, },
1727         /* NOTE that Erratum 532314 (fixed r2p0) affects CYCCNT:  it wrongly
1728          * increments while the core is asleep.
1729          */
1730         { DWT_CYCCNT, "dwt_cyccnt", 32, },
1731         /* plus some 8 bit counters, useful for profiling with TPIU */
1732 };
1733
1734 static struct dwt_reg dwt_comp[] = {
1735 #define DWT_COMPARATOR(i) \
1736                 { DWT_COMP0 + 0x10 * (i), "dwt_" #i "_comp", 32, }, \
1737                 { DWT_MASK0 + 0x10 * (i), "dwt_" #i "_mask", 4, }, \
1738                 { DWT_FUNCTION0 + 0x10 * (i), "dwt_" #i "_function", 32, }
1739         DWT_COMPARATOR(0),
1740         DWT_COMPARATOR(1),
1741         DWT_COMPARATOR(2),
1742         DWT_COMPARATOR(3),
1743 #undef DWT_COMPARATOR
1744 };
1745
1746 static const struct reg_arch_type dwt_reg_type = {
1747         .get = cortex_m_dwt_get_reg,
1748         .set = cortex_m_dwt_set_reg,
1749 };
1750
1751 static void cortex_m_dwt_addreg(struct target *t, struct reg *r, struct dwt_reg *d)
1752 {
1753         struct dwt_reg_state *state;
1754
1755         state = calloc(1, sizeof *state);
1756         if (!state)
1757                 return;
1758         state->addr = d->addr;
1759         state->target = t;
1760
1761         r->name = d->name;
1762         r->size = d->size;
1763         r->value = state->value;
1764         r->arch_info = state;
1765         r->type = &dwt_reg_type;
1766 }
1767
1768 void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target)
1769 {
1770         uint32_t dwtcr;
1771         struct reg_cache *cache;
1772         struct cortex_m_dwt_comparator *comparator;
1773         int reg, i;
1774
1775         target_read_u32(target, DWT_CTRL, &dwtcr);
1776         if (!dwtcr) {
1777                 LOG_DEBUG("no DWT");
1778                 return;
1779         }
1780
1781         cm->dwt_num_comp = (dwtcr >> 28) & 0xF;
1782         cm->dwt_comp_available = cm->dwt_num_comp;
1783         cm->dwt_comparator_list = calloc(cm->dwt_num_comp,
1784                         sizeof(struct cortex_m_dwt_comparator));
1785         if (!cm->dwt_comparator_list) {
1786 fail0:
1787                 cm->dwt_num_comp = 0;
1788                 LOG_ERROR("out of mem");
1789                 return;
1790         }
1791
1792         cache = calloc(1, sizeof *cache);
1793         if (!cache) {
1794 fail1:
1795                 free(cm->dwt_comparator_list);
1796                 goto fail0;
1797         }
1798         cache->name = "Cortex-M DWT registers";
1799         cache->num_regs = 2 + cm->dwt_num_comp * 3;
1800         cache->reg_list = calloc(cache->num_regs, sizeof *cache->reg_list);
1801         if (!cache->reg_list) {
1802                 free(cache);
1803                 goto fail1;
1804         }
1805
1806         for (reg = 0; reg < 2; reg++)
1807                 cortex_m_dwt_addreg(target, cache->reg_list + reg,
1808                         dwt_base_regs + reg);
1809
1810         comparator = cm->dwt_comparator_list;
1811         for (i = 0; i < cm->dwt_num_comp; i++, comparator++) {
1812                 int j;
1813
1814                 comparator->dwt_comparator_address = DWT_COMP0 + 0x10 * i;
1815                 for (j = 0; j < 3; j++, reg++)
1816                         cortex_m_dwt_addreg(target, cache->reg_list + reg,
1817                                 dwt_comp + 3 * i + j);
1818
1819                 /* make sure we clear any watchpoints enabled on the target */
1820                 target_write_u32(target, comparator->dwt_comparator_address + 8, 0);
1821         }
1822
1823         *register_get_last_cache_p(&target->reg_cache) = cache;
1824         cm->dwt_cache = cache;
1825
1826         LOG_DEBUG("DWT dwtcr 0x%" PRIx32 ", comp %d, watch%s",
1827                 dwtcr, cm->dwt_num_comp,
1828                 (dwtcr & (0xf << 24)) ? " only" : "/trigger");
1829
1830         /* REVISIT:  if num_comp > 1, check whether comparator #1 can
1831          * implement single-address data value watchpoints ... so we
1832          * won't need to check it later, when asked to set one up.
1833          */
1834 }
1835
1836 #define MVFR0 0xe000ef40
1837 #define MVFR1 0xe000ef44
1838
1839 #define MVFR0_DEFAULT_M4 0x10110021
1840 #define MVFR1_DEFAULT_M4 0x11000011
1841
1842 int cortex_m_examine(struct target *target)
1843 {
1844         int retval;
1845         uint32_t cpuid, fpcr, mvfr0, mvfr1;
1846         int i;
1847         struct cortex_m_common *cortex_m = target_to_cm(target);
1848         struct adiv5_dap *swjdp = cortex_m->armv7m.arm.dap;
1849         struct armv7m_common *armv7m = target_to_armv7m(target);
1850
1851         /* stlink shares the examine handler but does not support
1852          * all its calls */
1853         if (!armv7m->stlink) {
1854                 retval = ahbap_debugport_init(swjdp);
1855                 if (retval != ERROR_OK)
1856                         return retval;
1857         }
1858
1859         if (!target_was_examined(target)) {
1860                 target_set_examined(target);
1861
1862                 /* Read from Device Identification Registers */
1863                 retval = target_read_u32(target, CPUID, &cpuid);
1864                 if (retval != ERROR_OK)
1865                         return retval;
1866
1867                 /* Get CPU Type */
1868                 i = (cpuid >> 4) & 0xf;
1869
1870                 LOG_DEBUG("Cortex-M%d r%" PRId8 "p%" PRId8 " processor detected",
1871                                 i, (uint8_t)((cpuid >> 20) & 0xf), (uint8_t)((cpuid >> 0) & 0xf));
1872                 LOG_DEBUG("cpuid: 0x%8.8" PRIx32 "", cpuid);
1873
1874                 /* test for floating point feature on cortex-m4 */
1875                 if (i == 4) {
1876                         target_read_u32(target, MVFR0, &mvfr0);
1877                         target_read_u32(target, MVFR1, &mvfr1);
1878
1879                         if ((mvfr0 == MVFR0_DEFAULT_M4) && (mvfr1 == MVFR1_DEFAULT_M4)) {
1880                                 LOG_DEBUG("Cortex-M%d floating point feature FPv4_SP found", i);
1881                                 armv7m->fp_feature = FPv4_SP;
1882                         }
1883                 } else if (i == 0) {
1884                         /* Cortex-M0 does not support unaligned memory access */
1885                         armv7m->arm.is_armv6m = true;
1886                 }
1887
1888                 if (i == 4 || i == 3) {
1889                         /* Cortex-M3/M4 has 4096 bytes autoincrement range */
1890                         armv7m->dap.tar_autoincr_block = (1 << 12);
1891                 }
1892
1893                 /* NOTE: FPB and DWT are both optional. */
1894
1895                 /* Setup FPB */
1896                 target_read_u32(target, FP_CTRL, &fpcr);
1897                 cortex_m->auto_bp_type = 1;
1898                 /* bits [14:12] and [7:4] */
1899                 cortex_m->fp_num_code = ((fpcr >> 8) & 0x70) | ((fpcr >> 4) & 0xF);
1900                 cortex_m->fp_num_lit = (fpcr >> 8) & 0xF;
1901                 cortex_m->fp_code_available = cortex_m->fp_num_code;
1902                 cortex_m->fp_comparator_list = calloc(
1903                                 cortex_m->fp_num_code + cortex_m->fp_num_lit,
1904                                 sizeof(struct cortex_m_fp_comparator));
1905                 cortex_m->fpb_enabled = fpcr & 1;
1906                 for (i = 0; i < cortex_m->fp_num_code + cortex_m->fp_num_lit; i++) {
1907                         cortex_m->fp_comparator_list[i].type =
1908                                 (i < cortex_m->fp_num_code) ? FPCR_CODE : FPCR_LITERAL;
1909                         cortex_m->fp_comparator_list[i].fpcr_address = FP_COMP0 + 4 * i;
1910
1911                         /* make sure we clear any breakpoints enabled on the target */
1912                         target_write_u32(target, cortex_m->fp_comparator_list[i].fpcr_address, 0);
1913                 }
1914                 LOG_DEBUG("FPB fpcr 0x%" PRIx32 ", numcode %i, numlit %i",
1915                         fpcr,
1916                         cortex_m->fp_num_code,
1917                         cortex_m->fp_num_lit);
1918
1919                 /* Setup DWT */
1920                 cortex_m_dwt_setup(cortex_m, target);
1921
1922                 /* These hardware breakpoints only work for code in flash! */
1923                 LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
1924                         target_name(target),
1925                         cortex_m->fp_num_code,
1926                         cortex_m->dwt_num_comp);
1927         }
1928
1929         return ERROR_OK;
1930 }
1931
1932 static int cortex_m_dcc_read(struct target *target, uint8_t *value, uint8_t *ctrl)
1933 {
1934         struct armv7m_common *armv7m = target_to_armv7m(target);
1935         struct adiv5_dap *swjdp = armv7m->arm.dap;
1936         uint16_t dcrdr;
1937         uint8_t buf[2];
1938         int retval;
1939
1940         retval = mem_ap_read(swjdp, buf, 2, 1, DCB_DCRDR, false);
1941         if (retval != ERROR_OK)
1942                 return retval;
1943
1944         dcrdr = target_buffer_get_u16(target, buf);
1945         *ctrl = (uint8_t)dcrdr;
1946         *value = (uint8_t)(dcrdr >> 8);
1947
1948         LOG_DEBUG("data 0x%x ctrl 0x%x", *value, *ctrl);
1949
1950         /* write ack back to software dcc register
1951          * signify we have read data */
1952         if (dcrdr & (1 << 0)) {
1953                 target_buffer_set_u16(target, buf, 0);
1954                 retval = mem_ap_write(swjdp, buf, 2, 1, DCB_DCRDR, false);
1955                 if (retval != ERROR_OK)
1956                         return retval;
1957         }
1958
1959         return ERROR_OK;
1960 }
1961
1962 static int cortex_m_target_request_data(struct target *target,
1963         uint32_t size, uint8_t *buffer)
1964 {
1965         uint8_t data;
1966         uint8_t ctrl;
1967         uint32_t i;
1968
1969         for (i = 0; i < (size * 4); i++) {
1970                 int retval = cortex_m_dcc_read(target, &data, &ctrl);
1971                 if (retval != ERROR_OK)
1972                         return retval;
1973                 buffer[i] = data;
1974         }
1975
1976         return ERROR_OK;
1977 }
1978
1979 static int cortex_m_handle_target_request(void *priv)
1980 {
1981         struct target *target = priv;
1982         if (!target_was_examined(target))
1983                 return ERROR_OK;
1984
1985         if (!target->dbg_msg_enabled)
1986                 return ERROR_OK;
1987
1988         if (target->state == TARGET_RUNNING) {
1989                 uint8_t data;
1990                 uint8_t ctrl;
1991                 int retval;
1992
1993                 retval = cortex_m_dcc_read(target, &data, &ctrl);
1994                 if (retval != ERROR_OK)
1995                         return retval;
1996
1997                 /* check if we have data */
1998                 if (ctrl & (1 << 0)) {
1999                         uint32_t request;
2000
2001                         /* we assume target is quick enough */
2002                         request = data;
2003                         for (int i = 1; i <= 3; i++) {
2004                                 retval = cortex_m_dcc_read(target, &data, &ctrl);
2005                                 if (retval != ERROR_OK)
2006                                         return retval;
2007                                 request |= ((uint32_t)data << (i * 8));
2008                         }
2009                         target_request(target, request);
2010                 }
2011         }
2012
2013         return ERROR_OK;
2014 }
2015
2016 static int cortex_m_init_arch_info(struct target *target,
2017         struct cortex_m_common *cortex_m, struct jtag_tap *tap)
2018 {
2019         int retval;
2020         struct armv7m_common *armv7m = &cortex_m->armv7m;
2021
2022         armv7m_init_arch_info(target, armv7m);
2023
2024         /* prepare JTAG information for the new target */
2025         cortex_m->jtag_info.tap = tap;
2026         cortex_m->jtag_info.scann_size = 4;
2027
2028         /* default reset mode is to use srst if fitted
2029          * if not it will use CORTEX_M3_RESET_VECTRESET */
2030         cortex_m->soft_reset_config = CORTEX_M_RESET_VECTRESET;
2031
2032         armv7m->arm.dap = &armv7m->dap;
2033
2034         /* Leave (only) generic DAP stuff for debugport_init(); */
2035         armv7m->dap.jtag_info = &cortex_m->jtag_info;
2036         armv7m->dap.memaccess_tck = 8;
2037
2038         /* Cortex-M3/M4 has 4096 bytes autoincrement range
2039          * but set a safe default to 1024 to support Cortex-M0
2040          * this will be changed in cortex_m3_examine if a M3/M4 is detected */
2041         armv7m->dap.tar_autoincr_block = (1 << 10);
2042
2043         /* register arch-specific functions */
2044         armv7m->examine_debug_reason = cortex_m_examine_debug_reason;
2045
2046         armv7m->post_debug_entry = NULL;
2047
2048         armv7m->pre_restore_context = NULL;
2049
2050         armv7m->load_core_reg_u32 = cortex_m_load_core_reg_u32;
2051         armv7m->store_core_reg_u32 = cortex_m_store_core_reg_u32;
2052
2053         target_register_timer_callback(cortex_m_handle_target_request, 1, 1, target);
2054
2055         retval = arm_jtag_setup_connection(&cortex_m->jtag_info);
2056         if (retval != ERROR_OK)
2057                 return retval;
2058
2059         return ERROR_OK;
2060 }
2061
2062 static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
2063 {
2064         struct cortex_m_common *cortex_m = calloc(1, sizeof(struct cortex_m_common));
2065
2066         cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
2067         cortex_m_init_arch_info(target, cortex_m, target->tap);
2068
2069         return ERROR_OK;
2070 }
2071
2072 /*--------------------------------------------------------------------------*/
2073
2074 static int cortex_m_verify_pointer(struct command_context *cmd_ctx,
2075         struct cortex_m_common *cm)
2076 {
2077         if (cm->common_magic != CORTEX_M_COMMON_MAGIC) {
2078                 command_print(cmd_ctx, "target is not a Cortex-M");
2079                 return ERROR_TARGET_INVALID;
2080         }
2081         return ERROR_OK;
2082 }
2083
2084 /*
2085  * Only stuff below this line should need to verify that its target
2086  * is a Cortex-M3.  Everything else should have indirected through the
2087  * cortexm3_target structure, which is only used with CM3 targets.
2088  */
2089
2090 static const struct {
2091         char name[10];
2092         unsigned mask;
2093 } vec_ids[] = {
2094         { "hard_err",   VC_HARDERR, },
2095         { "int_err",    VC_INTERR, },
2096         { "bus_err",    VC_BUSERR, },
2097         { "state_err",  VC_STATERR, },
2098         { "chk_err",    VC_CHKERR, },
2099         { "nocp_err",   VC_NOCPERR, },
2100         { "mm_err",     VC_MMERR, },
2101         { "reset",      VC_CORERESET, },
2102 };
2103
2104 COMMAND_HANDLER(handle_cortex_m_vector_catch_command)
2105 {
2106         struct target *target = get_current_target(CMD_CTX);
2107         struct cortex_m_common *cortex_m = target_to_cm(target);
2108         struct armv7m_common *armv7m = &cortex_m->armv7m;
2109         struct adiv5_dap *swjdp = armv7m->arm.dap;
2110         uint32_t demcr = 0;
2111         int retval;
2112
2113         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2114         if (retval != ERROR_OK)
2115                 return retval;
2116
2117         retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
2118         if (retval != ERROR_OK)
2119                 return retval;
2120
2121         if (CMD_ARGC > 0) {
2122                 unsigned catch = 0;
2123
2124                 if (CMD_ARGC == 1) {
2125                         if (strcmp(CMD_ARGV[0], "all") == 0) {
2126                                 catch = VC_HARDERR | VC_INTERR | VC_BUSERR
2127                                         | VC_STATERR | VC_CHKERR | VC_NOCPERR
2128                                         | VC_MMERR | VC_CORERESET;
2129                                 goto write;
2130                         } else if (strcmp(CMD_ARGV[0], "none") == 0)
2131                                 goto write;
2132                 }
2133                 while (CMD_ARGC-- > 0) {
2134                         unsigned i;
2135                         for (i = 0; i < ARRAY_SIZE(vec_ids); i++) {
2136                                 if (strcmp(CMD_ARGV[CMD_ARGC], vec_ids[i].name) != 0)
2137                                         continue;
2138                                 catch |= vec_ids[i].mask;
2139                                 break;
2140                         }
2141                         if (i == ARRAY_SIZE(vec_ids)) {
2142                                 LOG_ERROR("No CM3 vector '%s'", CMD_ARGV[CMD_ARGC]);
2143                                 return ERROR_COMMAND_SYNTAX_ERROR;
2144                         }
2145                 }
2146 write:
2147                 /* For now, armv7m->demcr only stores vector catch flags. */
2148                 armv7m->demcr = catch;
2149
2150                 demcr &= ~0xffff;
2151                 demcr |= catch;
2152
2153                 /* write, but don't assume it stuck (why not??) */
2154                 retval = mem_ap_write_u32(swjdp, DCB_DEMCR, demcr);
2155                 if (retval != ERROR_OK)
2156                         return retval;
2157                 retval = mem_ap_read_atomic_u32(swjdp, DCB_DEMCR, &demcr);
2158                 if (retval != ERROR_OK)
2159                         return retval;
2160
2161                 /* FIXME be sure to clear DEMCR on clean server shutdown.
2162                  * Otherwise the vector catch hardware could fire when there's
2163                  * no debugger hooked up, causing much confusion...
2164                  */
2165         }
2166
2167         for (unsigned i = 0; i < ARRAY_SIZE(vec_ids); i++) {
2168                 command_print(CMD_CTX, "%9s: %s", vec_ids[i].name,
2169                         (demcr & vec_ids[i].mask) ? "catch" : "ignore");
2170         }
2171
2172         return ERROR_OK;
2173 }
2174
2175 COMMAND_HANDLER(handle_cortex_m_mask_interrupts_command)
2176 {
2177         struct target *target = get_current_target(CMD_CTX);
2178         struct cortex_m_common *cortex_m = target_to_cm(target);
2179         int retval;
2180
2181         static const Jim_Nvp nvp_maskisr_modes[] = {
2182                 { .name = "auto", .value = CORTEX_M_ISRMASK_AUTO },
2183                 { .name = "off", .value = CORTEX_M_ISRMASK_OFF },
2184                 { .name = "on", .value = CORTEX_M_ISRMASK_ON },
2185                 { .name = NULL, .value = -1 },
2186         };
2187         const Jim_Nvp *n;
2188
2189
2190         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2191         if (retval != ERROR_OK)
2192                 return retval;
2193
2194         if (target->state != TARGET_HALTED) {
2195                 command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME);
2196                 return ERROR_OK;
2197         }
2198
2199         if (CMD_ARGC > 0) {
2200                 n = Jim_Nvp_name2value_simple(nvp_maskisr_modes, CMD_ARGV[0]);
2201                 if (n->name == NULL)
2202                         return ERROR_COMMAND_SYNTAX_ERROR;
2203                 cortex_m->isrmasking_mode = n->value;
2204
2205
2206                 if (cortex_m->isrmasking_mode == CORTEX_M_ISRMASK_ON)
2207                         cortex_m_write_debug_halt_mask(target, C_HALT | C_MASKINTS, 0);
2208                 else
2209                         cortex_m_write_debug_halt_mask(target, C_HALT, C_MASKINTS);
2210         }
2211
2212         n = Jim_Nvp_value2name_simple(nvp_maskisr_modes, cortex_m->isrmasking_mode);
2213         command_print(CMD_CTX, "cortex_m interrupt mask %s", n->name);
2214
2215         return ERROR_OK;
2216 }
2217
2218 COMMAND_HANDLER(handle_cortex_m_reset_config_command)
2219 {
2220         struct target *target = get_current_target(CMD_CTX);
2221         struct cortex_m_common *cortex_m = target_to_cm(target);
2222         int retval;
2223         char *reset_config;
2224
2225         retval = cortex_m_verify_pointer(CMD_CTX, cortex_m);
2226         if (retval != ERROR_OK)
2227                 return retval;
2228
2229         if (CMD_ARGC > 0) {
2230                 if (strcmp(*CMD_ARGV, "sysresetreq") == 0)
2231                         cortex_m->soft_reset_config = CORTEX_M_RESET_SYSRESETREQ;
2232                 else if (strcmp(*CMD_ARGV, "vectreset") == 0)
2233                         cortex_m->soft_reset_config = CORTEX_M_RESET_VECTRESET;
2234         }
2235
2236         switch (cortex_m->soft_reset_config) {
2237                 case CORTEX_M_RESET_SYSRESETREQ:
2238                         reset_config = "sysresetreq";
2239                         break;
2240
2241                 case CORTEX_M_RESET_VECTRESET:
2242                         reset_config = "vectreset";
2243                         break;
2244
2245                 default:
2246                         reset_config = "unknown";
2247                         break;
2248         }
2249
2250         command_print(CMD_CTX, "cortex_m reset_config %s", reset_config);
2251
2252         return ERROR_OK;
2253 }
2254
2255 static const struct command_registration cortex_m_exec_command_handlers[] = {
2256         {
2257                 .name = "maskisr",
2258                 .handler = handle_cortex_m_mask_interrupts_command,
2259                 .mode = COMMAND_EXEC,
2260                 .help = "mask cortex_m interrupts",
2261                 .usage = "['auto'|'on'|'off']",
2262         },
2263         {
2264                 .name = "vector_catch",
2265                 .handler = handle_cortex_m_vector_catch_command,
2266                 .mode = COMMAND_EXEC,
2267                 .help = "configure hardware vectors to trigger debug entry",
2268                 .usage = "['all'|'none'|('bus_err'|'chk_err'|...)*]",
2269         },
2270         {
2271                 .name = "reset_config",
2272                 .handler = handle_cortex_m_reset_config_command,
2273                 .mode = COMMAND_ANY,
2274                 .help = "configure software reset handling",
2275                 .usage = "['srst'|'sysresetreq'|'vectreset']",
2276         },
2277         COMMAND_REGISTRATION_DONE
2278 };
2279 static const struct command_registration cortex_m_command_handlers[] = {
2280         {
2281                 .chain = armv7m_command_handlers,
2282         },
2283         {
2284                 .name = "cortex_m",
2285                 .mode = COMMAND_EXEC,
2286                 .help = "Cortex-M command group",
2287                 .usage = "",
2288                 .chain = cortex_m_exec_command_handlers,
2289         },
2290         COMMAND_REGISTRATION_DONE
2291 };
2292
2293 struct target_type cortexm_target = {
2294         .name = "cortex_m",
2295         .deprecated_name = "cortex_m3",
2296
2297         .poll = cortex_m_poll,
2298         .arch_state = armv7m_arch_state,
2299
2300         .target_request_data = cortex_m_target_request_data,
2301
2302         .halt = cortex_m_halt,
2303         .resume = cortex_m_resume,
2304         .step = cortex_m_step,
2305
2306         .assert_reset = cortex_m_assert_reset,
2307         .deassert_reset = cortex_m_deassert_reset,
2308         .soft_reset_halt = cortex_m_soft_reset_halt,
2309
2310         .get_gdb_reg_list = armv7m_get_gdb_reg_list,
2311
2312         .read_memory = cortex_m_read_memory,
2313         .write_memory = cortex_m_write_memory,
2314         .checksum_memory = armv7m_checksum_memory,
2315         .blank_check_memory = armv7m_blank_check_memory,
2316
2317         .run_algorithm = armv7m_run_algorithm,
2318         .start_algorithm = armv7m_start_algorithm,
2319         .wait_algorithm = armv7m_wait_algorithm,
2320
2321         .add_breakpoint = cortex_m_add_breakpoint,
2322         .remove_breakpoint = cortex_m_remove_breakpoint,
2323         .add_watchpoint = cortex_m_add_watchpoint,
2324         .remove_watchpoint = cortex_m_remove_watchpoint,
2325
2326         .commands = cortex_m_command_handlers,
2327         .target_create = cortex_m_target_create,
2328         .init_target = cortex_m_init_target,
2329         .examine = cortex_m_examine,
2330 };