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

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