git.gag.com Git - fw/openocd/blob - src/target/armv7m.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  *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
  12  *   oyvind.harboe@zylin.com                                               *
  13  *                                                                         *
  14  *   Copyright (C) 2018 by Liviu Ionescu                                   *
  15  *   <ilg@livius.net>                                                      *
  16  *                                                                         *
  17  *   Copyright (C) 2019 by Tomas Vanek                                     *
  18  *   vanekt@fbl.cz                                                         *
  19  *                                                                         *
  20  *   This program is free software; you can redistribute it and/or modify  *
  21  *   it under the terms of the GNU General Public License as published by  *
  22  *   the Free Software Foundation; either version 2 of the License, or     *
  23  *   (at your option) any later version.                                   *
  24  *                                                                         *
  25  *   This program is distributed in the hope that it will be useful,       *
  26  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  27  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  28  *   GNU General Public License for more details.                          *
  29  *                                                                         *
  30  *   You should have received a copy of the GNU General Public License     *
  31  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  32  *                                                                         *
  33  *   ARMv7-M Architecture, Application Level Reference Manual              *
  34  *              ARM DDI 0405C (September 2008)                             *
  35  *                                                                         *
  36  ***************************************************************************/
  37
  38 #ifdef HAVE_CONFIG_H
  39 #include "config.h"
  40 #endif
  41
  42 #include "breakpoints.h"
  43 #include "armv7m.h"
  44 #include "algorithm.h"
  45 #include "register.h"
  46 #include "semihosting_common.h"
  47
  48 #if 0
  49 #define _DEBUG_INSTRUCTION_EXECUTION_
  50 #endif
  51
  52 static const char * const armv7m_exception_strings[] = {
  53         "", "Reset", "NMI", "HardFault",
  54         "MemManage", "BusFault", "UsageFault", "SecureFault",
  55         "RESERVED", "RESERVED", "RESERVED", "SVCall",
  56         "DebugMonitor", "RESERVED", "PendSV", "SysTick"
  57 };
  58
  59 /* PSP is used in some thread modes */
  60 const int armv7m_psp_reg_map[ARMV7M_NUM_CORE_REGS] = {
  61         ARMV7M_R0, ARMV7M_R1, ARMV7M_R2, ARMV7M_R3,
  62         ARMV7M_R4, ARMV7M_R5, ARMV7M_R6, ARMV7M_R7,
  63         ARMV7M_R8, ARMV7M_R9, ARMV7M_R10, ARMV7M_R11,
  64         ARMV7M_R12, ARMV7M_PSP, ARMV7M_R14, ARMV7M_PC,
  65         ARMV7M_xPSR,
  66 };
  67
  68 /* MSP is used in handler and some thread modes */
  69 const int armv7m_msp_reg_map[ARMV7M_NUM_CORE_REGS] = {
  70         ARMV7M_R0, ARMV7M_R1, ARMV7M_R2, ARMV7M_R3,
  71         ARMV7M_R4, ARMV7M_R5, ARMV7M_R6, ARMV7M_R7,
  72         ARMV7M_R8, ARMV7M_R9, ARMV7M_R10, ARMV7M_R11,
  73         ARMV7M_R12, ARMV7M_MSP, ARMV7M_R14, ARMV7M_PC,
  74         ARMV7M_xPSR,
  75 };
  76
  77 /*
  78  * These registers are not memory-mapped.  The ARMv7-M profile includes
  79  * memory mapped registers too, such as for the NVIC (interrupt controller)
  80  * and SysTick (timer) modules; those can mostly be treated as peripherals.
  81  *
  82  * The ARMv6-M profile is almost identical in this respect, except that it
  83  * doesn't include basepri or faultmask registers.
  84  */
  85 static const struct {
  86         unsigned id;
  87         const char *name;
  88         unsigned bits;
  89         enum reg_type type;
  90         const char *group;
  91         const char *feature;
  92 } armv7m_regs[] = {
  93         { ARMV7M_R0, "r0", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
  94         { ARMV7M_R1, "r1", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
  95         { ARMV7M_R2, "r2", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
  96         { ARMV7M_R3, "r3", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
  97         { ARMV7M_R4, "r4", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
  98         { ARMV7M_R5, "r5", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
  99         { ARMV7M_R6, "r6", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 100         { ARMV7M_R7, "r7", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 101         { ARMV7M_R8, "r8", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 102         { ARMV7M_R9, "r9", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 103         { ARMV7M_R10, "r10", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 104         { ARMV7M_R11, "r11", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 105         { ARMV7M_R12, "r12", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 106         { ARMV7M_R13, "sp", 32, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.arm.m-profile" },
 107         { ARMV7M_R14, "lr", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 108         { ARMV7M_PC, "pc", 32, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.m-profile" },
 109         { ARMV7M_xPSR, "xPSR", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" },
 110
 111         { ARMV7M_MSP, "msp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" },
 112         { ARMV7M_PSP, "psp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" },
 113
 114         /* A working register for packing/unpacking special regs, hidden from gdb */
 115         { ARMV7M_PMSK_BPRI_FLTMSK_CTRL, "pmsk_bpri_fltmsk_ctrl", 32, REG_TYPE_INT, NULL, NULL },
 116
 117         /* WARNING: If you use armv7m_write_core_reg() on one of 4 following
 118          * special registers, the new data go to ARMV7M_PMSK_BPRI_FLTMSK_CTRL
 119          * cache only and are not flushed to CPU HW register.
 120          * To trigger write to CPU HW register, add
 121          *              armv7m_write_core_reg(,,ARMV7M_PMSK_BPRI_FLTMSK_CTRL,);
 122          */
 123         { ARMV7M_PRIMASK, "primask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 124         { ARMV7M_BASEPRI, "basepri", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 125         { ARMV7M_FAULTMASK, "faultmask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 126         { ARMV7M_CONTROL, "control", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" },
 127
 128         { ARMV7M_D0, "d0", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 129         { ARMV7M_D1, "d1", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 130         { ARMV7M_D2, "d2", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 131         { ARMV7M_D3, "d3", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 132         { ARMV7M_D4, "d4", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 133         { ARMV7M_D5, "d5", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 134         { ARMV7M_D6, "d6", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 135         { ARMV7M_D7, "d7", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 136         { ARMV7M_D8, "d8", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 137         { ARMV7M_D9, "d9", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 138         { ARMV7M_D10, "d10", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 139         { ARMV7M_D11, "d11", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 140         { ARMV7M_D12, "d12", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 141         { ARMV7M_D13, "d13", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 142         { ARMV7M_D14, "d14", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 143         { ARMV7M_D15, "d15", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" },
 144
 145         { ARMV7M_FPSCR, "fpscr", 32, REG_TYPE_INT, "float", "org.gnu.gdb.arm.vfp" },
 146 };
 147
 148 #define ARMV7M_NUM_REGS ARRAY_SIZE(armv7m_regs)
 149
 150 /**
 151  * Restores target context using the cache of core registers set up
 152  * by armv7m_build_reg_cache(), calling optional core-specific hooks.
 153  */
 154 int armv7m_restore_context(struct target *target)
 155 {
 156         int i;
 157         struct armv7m_common *armv7m = target_to_armv7m(target);
 158         struct reg_cache *cache = armv7m->arm.core_cache;
 159
 160         LOG_DEBUG(" ");
 161
 162         if (armv7m->pre_restore_context)
 163                 armv7m->pre_restore_context(target);
 164
 165         /* The descending order of register writes is crucial for correct
 166          * packing of ARMV7M_PMSK_BPRI_FLTMSK_CTRL!
 167          * See also comments in the register table above */
 168         for (i = cache->num_regs - 1; i >= 0; i--) {
 169                 if (cache->reg_list[i].dirty) {
 170                         armv7m->arm.write_core_reg(target, &cache->reg_list[i], i,
 171                                                    ARM_MODE_ANY, cache->reg_list[i].value);
 172                 }
 173         }
 174
 175         return ERROR_OK;
 176 }
 177
 178 /* Core state functions */
 179
 180 /**
 181  * Maps ISR number (from xPSR) to name.
 182  * Note that while names and meanings for the first sixteen are standardized
 183  * (with zero not a true exception), external interrupts are only numbered.
 184  * They are assigned by vendors, which generally assign different numbers to
 185  * peripherals (such as UART0 or a USB peripheral controller).
 186  */
 187 const char *armv7m_exception_string(int number)
 188 {
 189         static char enamebuf[32];
 190
 191         if ((number < 0) | (number > 511))
 192                 return "Invalid exception";
 193         if (number < 16)
 194                 return armv7m_exception_strings[number];
 195         sprintf(enamebuf, "External Interrupt(%i)", number - 16);
 196         return enamebuf;
 197 }
 198
 199 static int armv7m_get_core_reg(struct reg *reg)
 200 {
 201         int retval;
 202         struct arm_reg *armv7m_reg = reg->arch_info;
 203         struct target *target = armv7m_reg->target;
 204         struct arm *arm = target_to_arm(target);
 205
 206         if (target->state != TARGET_HALTED)
 207                 return ERROR_TARGET_NOT_HALTED;
 208
 209         retval = arm->read_core_reg(target, reg, reg->number, arm->core_mode);
 210
 211         return retval;
 212 }
 213
 214 static int armv7m_set_core_reg(struct reg *reg, uint8_t *buf)
 215 {
 216         struct arm_reg *armv7m_reg = reg->arch_info;
 217         struct target *target = armv7m_reg->target;
 218
 219         if (target->state != TARGET_HALTED)
 220                 return ERROR_TARGET_NOT_HALTED;
 221
 222         buf_cpy(buf, reg->value, reg->size);
 223         reg->dirty = true;
 224         reg->valid = true;
 225
 226         return ERROR_OK;
 227 }
 228
 229 static uint32_t armv7m_map_id_to_regsel(unsigned int arm_reg_id)
 230 {
 231         switch (arm_reg_id) {
 232         case ARMV7M_R0 ... ARMV7M_R14:
 233         case ARMV7M_PC:
 234         case ARMV7M_xPSR:
 235         case ARMV7M_MSP:
 236         case ARMV7M_PSP:
 237                 /* NOTE:  we "know" here that the register identifiers
 238                  * match the Cortex-M DCRSR.REGSEL selectors values
 239                  * for R0..R14, PC, xPSR, MSP, and PSP.
 240                  */
 241                 return arm_reg_id;
 242
 243         case ARMV7M_PMSK_BPRI_FLTMSK_CTRL:
 244                 return ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL;
 245
 246         case ARMV7M_FPSCR:
 247                 return ARMV7M_REGSEL_FPSCR;
 248
 249         case ARMV7M_D0 ... ARMV7M_D15:
 250                 return ARMV7M_REGSEL_S0 + 2 * (arm_reg_id - ARMV7M_D0);
 251
 252         default:
 253                 LOG_ERROR("Bad register ID %u", arm_reg_id);
 254                 return arm_reg_id;
 255         }
 256 }
 257
 258 static bool armv7m_map_reg_packing(unsigned int arm_reg_id,
 259                                         unsigned int *reg32_id, uint32_t *offset)
 260 {
 261         switch (arm_reg_id) {
 262
 263         case ARMV7M_PRIMASK:
 264                 *reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
 265                 *offset = 0;
 266                 return true;
 267         case ARMV7M_BASEPRI:
 268                 *reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
 269                 *offset = 1;
 270                 return true;
 271         case ARMV7M_FAULTMASK:
 272                 *reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
 273                 *offset = 2;
 274                 return true;
 275         case ARMV7M_CONTROL:
 276                 *reg32_id = ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
 277                 *offset = 3;
 278                 return true;
 279
 280         default:
 281                 return false;
 282         }
 283 }
 284
 285 static int armv7m_read_core_reg(struct target *target, struct reg *r,
 286         int num, enum arm_mode mode)
 287 {
 288         uint32_t reg_value;
 289         int retval;
 290         struct armv7m_common *armv7m = target_to_armv7m(target);
 291
 292         assert(num < (int)armv7m->arm.core_cache->num_regs);
 293         assert(num == (int)r->number);
 294
 295         /* If a code calls read_reg, it expects the cache is no more dirty.
 296          * Clear the dirty flag regardless of the later read succeeds or not
 297          * to prevent unwanted cache flush after a read error */
 298         r->dirty = false;
 299
 300         if (r->size <= 8) {
 301                 /* any 8-bit or shorter register is packed */
 302                 uint32_t offset = 0;    /* silence false gcc warning */
 303                 unsigned int reg32_id;
 304
 305                 bool is_packed = armv7m_map_reg_packing(num, &reg32_id, &offset);
 306                 assert(is_packed);
 307                 struct reg *r32 = &armv7m->arm.core_cache->reg_list[reg32_id];
 308
 309                 /* Read 32-bit container register if not cached */
 310                 if (!r32->valid) {
 311                         retval = armv7m_read_core_reg(target, r32, reg32_id, mode);
 312                         if (retval != ERROR_OK)
 313                                 return retval;
 314                 }
 315
 316                 /* Copy required bits of 32-bit container register */
 317                 buf_cpy(r32->value + offset, r->value, r->size);
 318
 319         } else {
 320                 assert(r->size == 32 || r->size == 64);
 321
 322                 struct arm_reg *armv7m_core_reg = r->arch_info;
 323                 uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
 324
 325                 retval = armv7m->load_core_reg_u32(target, regsel, &reg_value);
 326                 if (retval != ERROR_OK)
 327                         return retval;
 328                 buf_set_u32(r->value, 0, 32, reg_value);
 329
 330                 if (r->size == 64) {
 331                         retval = armv7m->load_core_reg_u32(target, regsel + 1, &reg_value);
 332                         if (retval != ERROR_OK) {
 333                                 r->valid = false;
 334                                 return retval;
 335                         }
 336                         buf_set_u32(r->value + 4, 0, 32, reg_value);
 337
 338                         uint64_t q = buf_get_u64(r->value, 0, 64);
 339                         LOG_DEBUG("read %s value 0x%016" PRIx64, r->name, q);
 340                 } else {
 341                         LOG_DEBUG("read %s value 0x%08" PRIx32, r->name, reg_value);
 342                 }
 343         }
 344
 345         r->valid = true;
 346
 347         return ERROR_OK;
 348 }
 349
 350 static int armv7m_write_core_reg(struct target *target, struct reg *r,
 351         int num, enum arm_mode mode, uint8_t *value)
 352 {
 353         int retval;
 354         uint32_t t;
 355         struct armv7m_common *armv7m = target_to_armv7m(target);
 356
 357         assert(num < (int)armv7m->arm.core_cache->num_regs);
 358         assert(num == (int)r->number);
 359
 360         if (value != r->value) {
 361                 /* If we are not flushing the cache, store the new value to the cache */
 362                 buf_cpy(value, r->value, r->size);
 363         }
 364
 365         if (r->size <= 8) {
 366                 /* any 8-bit or shorter register is packed */
 367                 uint32_t offset = 0;    /* silence false gcc warning */
 368                 unsigned int reg32_id;
 369
 370                 bool is_packed = armv7m_map_reg_packing(num, &reg32_id, &offset);
 371                 assert(is_packed);
 372                 struct reg *r32 = &armv7m->arm.core_cache->reg_list[reg32_id];
 373
 374                 if (!r32->valid) {
 375                         /* Before merging with other parts ensure the 32-bit register is valid */
 376                         retval = armv7m_read_core_reg(target, r32, reg32_id, mode);
 377                         if (retval != ERROR_OK)
 378                                 return retval;
 379                 }
 380
 381                 /* Write a part to the 32-bit container register */
 382                 buf_cpy(value, r32->value + offset, r->size);
 383                 r32->dirty = true;
 384
 385         } else {
 386                 assert(r->size == 32 || r->size == 64);
 387
 388                 struct arm_reg *armv7m_core_reg = r->arch_info;
 389                 uint32_t regsel = armv7m_map_id_to_regsel(armv7m_core_reg->num);
 390
 391                 t = buf_get_u32(value, 0, 32);
 392                 retval = armv7m->store_core_reg_u32(target, regsel, t);
 393                 if (retval != ERROR_OK)
 394                         goto out_error;
 395
 396                 if (r->size == 64) {
 397                         t = buf_get_u32(value + 4, 0, 32);
 398                         retval = armv7m->store_core_reg_u32(target, regsel + 1, t);
 399                         if (retval != ERROR_OK)
 400                                 goto out_error;
 401
 402                         uint64_t q = buf_get_u64(value, 0, 64);
 403                         LOG_DEBUG("write %s value 0x%016" PRIx64, r->name, q);
 404                 } else {
 405                         LOG_DEBUG("write %s value 0x%08" PRIx32, r->name, t);
 406                 }
 407         }
 408
 409         r->valid = true;
 410         r->dirty = false;
 411
 412         return ERROR_OK;
 413
 414 out_error:
 415         r->dirty = true;
 416         LOG_ERROR("Error setting register %s", r->name);
 417         return retval;
 418 }
 419
 420 /**
 421  * Returns generic ARM userspace registers to GDB.
 422  */
 423 int armv7m_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 424                 int *reg_list_size, enum target_register_class reg_class)
 425 {
 426         struct armv7m_common *armv7m = target_to_armv7m(target);
 427         int i, size;
 428
 429         if (reg_class == REG_CLASS_ALL)
 430                 size = armv7m->arm.core_cache->num_regs;
 431         else
 432                 size = ARMV7M_NUM_CORE_REGS;
 433
 434         *reg_list = malloc(sizeof(struct reg *) * size);
 435         if (*reg_list == NULL)
 436                 return ERROR_FAIL;
 437
 438         for (i = 0; i < size; i++)
 439                 (*reg_list)[i] = &armv7m->arm.core_cache->reg_list[i];
 440
 441         *reg_list_size = size;
 442
 443         return ERROR_OK;
 444 }
 445
 446 /** Runs a Thumb algorithm in the target. */
 447 int armv7m_run_algorithm(struct target *target,
 448         int num_mem_params, struct mem_param *mem_params,
 449         int num_reg_params, struct reg_param *reg_params,
 450         target_addr_t entry_point, target_addr_t exit_point,
 451         int timeout_ms, void *arch_info)
 452 {
 453         int retval;
 454
 455         retval = armv7m_start_algorithm(target,
 456                         num_mem_params, mem_params,
 457                         num_reg_params, reg_params,
 458                         entry_point, exit_point,
 459                         arch_info);
 460
 461         if (retval == ERROR_OK)
 462                 retval = armv7m_wait_algorithm(target,
 463                                 num_mem_params, mem_params,
 464                                 num_reg_params, reg_params,
 465                                 exit_point, timeout_ms,
 466                                 arch_info);
 467
 468         return retval;
 469 }
 470
 471 /** Starts a Thumb algorithm in the target. */
 472 int armv7m_start_algorithm(struct target *target,
 473         int num_mem_params, struct mem_param *mem_params,
 474         int num_reg_params, struct reg_param *reg_params,
 475         target_addr_t entry_point, target_addr_t exit_point,
 476         void *arch_info)
 477 {
 478         struct armv7m_common *armv7m = target_to_armv7m(target);
 479         struct armv7m_algorithm *armv7m_algorithm_info = arch_info;
 480         enum arm_mode core_mode = armv7m->arm.core_mode;
 481         int retval = ERROR_OK;
 482
 483         /* NOTE: armv7m_run_algorithm requires that each algorithm uses a software breakpoint
 484          * at the exit point */
 485
 486         if (armv7m_algorithm_info->common_magic != ARMV7M_COMMON_MAGIC) {
 487                 LOG_ERROR("current target isn't an ARMV7M target");
 488                 return ERROR_TARGET_INVALID;
 489         }
 490
 491         if (target->state != TARGET_HALTED) {
 492                 LOG_WARNING("target not halted");
 493                 return ERROR_TARGET_NOT_HALTED;
 494         }
 495
 496         /* refresh core register cache
 497          * Not needed if core register cache is always consistent with target process state */
 498         for (unsigned i = 0; i < armv7m->arm.core_cache->num_regs; i++) {
 499
 500                 armv7m_algorithm_info->context[i] = buf_get_u32(
 501                                 armv7m->arm.core_cache->reg_list[i].value,
 502                                 0,
 503                                 32);
 504         }
 505
 506         for (int i = 0; i < num_mem_params; i++) {
 507                 if (mem_params[i].direction == PARAM_IN)
 508                         continue;
 509                 retval = target_write_buffer(target, mem_params[i].address,
 510                                 mem_params[i].size,
 511                                 mem_params[i].value);
 512                 if (retval != ERROR_OK)
 513                         return retval;
 514         }
 515
 516         for (int i = 0; i < num_reg_params; i++) {
 517                 if (reg_params[i].direction == PARAM_IN)
 518                         continue;
 519
 520                 struct reg *reg =
 521                         register_get_by_name(armv7m->arm.core_cache, reg_params[i].reg_name, 0);
 522 /*              uint32_t regvalue; */
 523
 524                 if (!reg) {
 525                         LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
 526                         return ERROR_COMMAND_SYNTAX_ERROR;
 527                 }
 528
 529                 if (reg->size != reg_params[i].size) {
 530                         LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size",
 531                                 reg_params[i].reg_name);
 532                         return ERROR_COMMAND_SYNTAX_ERROR;
 533                 }
 534
 535 /*              regvalue = buf_get_u32(reg_params[i].value, 0, 32); */
 536                 armv7m_set_core_reg(reg, reg_params[i].value);
 537         }
 538
 539         {
 540                 /*
 541                  * Ensure xPSR.T is set to avoid trying to run things in arm
 542                  * (non-thumb) mode, which armv7m does not support.
 543                  *
 544                  * We do this by setting the entirety of xPSR, which should
 545                  * remove all the unknowns about xPSR state.
 546                  *
 547                  * Because xPSR.T is populated on reset from the vector table,
 548                  * it might be 0 if the vector table has "bad" data in it.
 549                  */
 550                 struct reg *reg = &armv7m->arm.core_cache->reg_list[ARMV7M_xPSR];
 551                 buf_set_u32(reg->value, 0, 32, 0x01000000);
 552                 reg->valid = true;
 553                 reg->dirty = true;
 554         }
 555
 556         if (armv7m_algorithm_info->core_mode != ARM_MODE_ANY &&
 557                         armv7m_algorithm_info->core_mode != core_mode) {
 558
 559                 /* we cannot set ARM_MODE_HANDLER, so use ARM_MODE_THREAD instead */
 560                 if (armv7m_algorithm_info->core_mode == ARM_MODE_HANDLER) {
 561                         armv7m_algorithm_info->core_mode = ARM_MODE_THREAD;
 562                         LOG_INFO("ARM_MODE_HANDLER not currently supported, using ARM_MODE_THREAD instead");
 563                 }
 564
 565                 LOG_DEBUG("setting core_mode: 0x%2.2x", armv7m_algorithm_info->core_mode);
 566                 buf_set_u32(armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].value,
 567                         0, 1, armv7m_algorithm_info->core_mode);
 568                 armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].dirty = true;
 569                 armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].valid = true;
 570         }
 571
 572         /* save previous core mode */
 573         armv7m_algorithm_info->core_mode = core_mode;
 574
 575         retval = target_resume(target, 0, entry_point, 1, 1);
 576
 577         return retval;
 578 }
 579
 580 /** Waits for an algorithm in the target. */
 581 int armv7m_wait_algorithm(struct target *target,
 582         int num_mem_params, struct mem_param *mem_params,
 583         int num_reg_params, struct reg_param *reg_params,
 584         target_addr_t exit_point, int timeout_ms,
 585         void *arch_info)
 586 {
 587         struct armv7m_common *armv7m = target_to_armv7m(target);
 588         struct armv7m_algorithm *armv7m_algorithm_info = arch_info;
 589         int retval = ERROR_OK;
 590
 591         /* NOTE: armv7m_run_algorithm requires that each algorithm uses a software breakpoint
 592          * at the exit point */
 593
 594         if (armv7m_algorithm_info->common_magic != ARMV7M_COMMON_MAGIC) {
 595                 LOG_ERROR("current target isn't an ARMV7M target");
 596                 return ERROR_TARGET_INVALID;
 597         }
 598
 599         retval = target_wait_state(target, TARGET_HALTED, timeout_ms);
 600         /* If the target fails to halt due to the breakpoint, force a halt */
 601         if (retval != ERROR_OK || target->state != TARGET_HALTED) {
 602                 retval = target_halt(target);
 603                 if (retval != ERROR_OK)
 604                         return retval;
 605                 retval = target_wait_state(target, TARGET_HALTED, 500);
 606                 if (retval != ERROR_OK)
 607                         return retval;
 608                 return ERROR_TARGET_TIMEOUT;
 609         }
 610
 611         if (exit_point) {
 612                 /* PC value has been cached in cortex_m_debug_entry() */
 613                 uint32_t pc = buf_get_u32(armv7m->arm.pc->value, 0, 32);
 614                 if (pc != exit_point) {
 615                         LOG_DEBUG("failed algorithm halted at 0x%" PRIx32 ", expected 0x%" TARGET_PRIxADDR,
 616                                           pc, exit_point);
 617                         return ERROR_TARGET_ALGO_EXIT;
 618                 }
 619         }
 620
 621         /* Read memory values to mem_params[] */
 622         for (int i = 0; i < num_mem_params; i++) {
 623                 if (mem_params[i].direction != PARAM_OUT) {
 624                         retval = target_read_buffer(target, mem_params[i].address,
 625                                         mem_params[i].size,
 626                                         mem_params[i].value);
 627                         if (retval != ERROR_OK)
 628                                 return retval;
 629                 }
 630         }
 631
 632         /* Copy core register values to reg_params[] */
 633         for (int i = 0; i < num_reg_params; i++) {
 634                 if (reg_params[i].direction != PARAM_OUT) {
 635                         struct reg *reg = register_get_by_name(armv7m->arm.core_cache,
 636                                         reg_params[i].reg_name,
 637                                         0);
 638
 639                         if (!reg) {
 640                                 LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
 641                                 return ERROR_COMMAND_SYNTAX_ERROR;
 642                         }
 643
 644                         if (reg->size != reg_params[i].size) {
 645                                 LOG_ERROR(
 646                                         "BUG: register '%s' size doesn't match reg_params[i].size",
 647                                         reg_params[i].reg_name);
 648                                 return ERROR_COMMAND_SYNTAX_ERROR;
 649                         }
 650
 651                         buf_set_u32(reg_params[i].value, 0, 32, buf_get_u32(reg->value, 0, 32));
 652                 }
 653         }
 654
 655         for (int i = armv7m->arm.core_cache->num_regs - 1; i >= 0; i--) {
 656                 uint32_t regvalue;
 657                 regvalue = buf_get_u32(armv7m->arm.core_cache->reg_list[i].value, 0, 32);
 658                 if (regvalue != armv7m_algorithm_info->context[i]) {
 659                         LOG_DEBUG("restoring register %s with value 0x%8.8" PRIx32,
 660                                         armv7m->arm.core_cache->reg_list[i].name,
 661                                 armv7m_algorithm_info->context[i]);
 662                         buf_set_u32(armv7m->arm.core_cache->reg_list[i].value,
 663                                 0, 32, armv7m_algorithm_info->context[i]);
 664                         armv7m->arm.core_cache->reg_list[i].valid = true;
 665                         armv7m->arm.core_cache->reg_list[i].dirty = true;
 666                 }
 667         }
 668
 669         /* restore previous core mode */
 670         if (armv7m_algorithm_info->core_mode != armv7m->arm.core_mode) {
 671                 LOG_DEBUG("restoring core_mode: 0x%2.2x", armv7m_algorithm_info->core_mode);
 672                 buf_set_u32(armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].value,
 673                         0, 1, armv7m_algorithm_info->core_mode);
 674                 armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].dirty = true;
 675                 armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].valid = true;
 676         }
 677
 678         armv7m->arm.core_mode = armv7m_algorithm_info->core_mode;
 679
 680         return retval;
 681 }
 682
 683 /** Logs summary of ARMv7-M state for a halted target. */
 684 int armv7m_arch_state(struct target *target)
 685 {
 686         struct armv7m_common *armv7m = target_to_armv7m(target);
 687         struct arm *arm = &armv7m->arm;
 688         uint32_t ctrl, sp;
 689
 690         /* avoid filling log waiting for fileio reply */
 691         if (target->semihosting && target->semihosting->hit_fileio)
 692                 return ERROR_OK;
 693
 694         ctrl = buf_get_u32(arm->core_cache->reg_list[ARMV7M_CONTROL].value, 0, 32);
 695         sp = buf_get_u32(arm->core_cache->reg_list[ARMV7M_R13].value, 0, 32);
 696
 697         LOG_USER("target halted due to %s, current mode: %s %s\n"
 698                 "xPSR: %#8.8" PRIx32 " pc: %#8.8" PRIx32 " %csp: %#8.8" PRIx32 "%s%s",
 699                 debug_reason_name(target),
 700                 arm_mode_name(arm->core_mode),
 701                 armv7m_exception_string(armv7m->exception_number),
 702                 buf_get_u32(arm->cpsr->value, 0, 32),
 703                 buf_get_u32(arm->pc->value, 0, 32),
 704                 (ctrl & 0x02) ? 'p' : 'm',
 705                 sp,
 706                 (target->semihosting && target->semihosting->is_active) ? ", semihosting" : "",
 707                 (target->semihosting && target->semihosting->is_fileio) ? " fileio" : "");
 708
 709         return ERROR_OK;
 710 }
 711
 712 static const struct reg_arch_type armv7m_reg_type = {
 713         .get = armv7m_get_core_reg,
 714         .set = armv7m_set_core_reg,
 715 };
 716
 717 /** Builds cache of architecturally defined registers.  */
 718 struct reg_cache *armv7m_build_reg_cache(struct target *target)
 719 {
 720         struct armv7m_common *armv7m = target_to_armv7m(target);
 721         struct arm *arm = &armv7m->arm;
 722         int num_regs = ARMV7M_NUM_REGS;
 723         struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
 724         struct reg_cache *cache = malloc(sizeof(struct reg_cache));
 725         struct reg *reg_list = calloc(num_regs, sizeof(struct reg));
 726         struct arm_reg *arch_info = calloc(num_regs, sizeof(struct arm_reg));
 727         struct reg_feature *feature;
 728         int i;
 729
 730         /* Build the process context cache */
 731         cache->name = "arm v7m registers";
 732         cache->next = NULL;
 733         cache->reg_list = reg_list;
 734         cache->num_regs = num_regs;
 735         (*cache_p) = cache;
 736
 737         for (i = 0; i < num_regs; i++) {
 738                 arch_info[i].num = armv7m_regs[i].id;
 739                 arch_info[i].target = target;
 740                 arch_info[i].arm = arm;
 741
 742                 reg_list[i].name = armv7m_regs[i].name;
 743                 reg_list[i].size = armv7m_regs[i].bits;
 744                 reg_list[i].value = arch_info[i].value;
 745                 reg_list[i].dirty = false;
 746                 reg_list[i].valid = false;
 747                 reg_list[i].hidden = i == ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
 748                 reg_list[i].type = &armv7m_reg_type;
 749                 reg_list[i].arch_info = &arch_info[i];
 750
 751                 reg_list[i].group = armv7m_regs[i].group;
 752                 reg_list[i].number = i;
 753                 reg_list[i].exist = true;
 754                 reg_list[i].caller_save = true; /* gdb defaults to true */
 755
 756                 if (reg_list[i].hidden)
 757                         continue;
 758
 759                 feature = calloc(1, sizeof(struct reg_feature));
 760                 if (feature) {
 761                         feature->name = armv7m_regs[i].feature;
 762                         reg_list[i].feature = feature;
 763                 } else
 764                         LOG_ERROR("unable to allocate feature list");
 765
 766                 reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
 767                 if (reg_list[i].reg_data_type)
 768                         reg_list[i].reg_data_type->type = armv7m_regs[i].type;
 769                 else
 770                         LOG_ERROR("unable to allocate reg type list");
 771         }
 772
 773         arm->cpsr = reg_list + ARMV7M_xPSR;
 774         arm->pc = reg_list + ARMV7M_PC;
 775         arm->core_cache = cache;
 776
 777         return cache;
 778 }
 779
 780 void armv7m_free_reg_cache(struct target *target)
 781 {
 782         struct armv7m_common *armv7m = target_to_armv7m(target);
 783         struct arm *arm = &armv7m->arm;
 784         struct reg_cache *cache;
 785         struct reg *reg;
 786         unsigned int i;
 787
 788         cache = arm->core_cache;
 789
 790         if (!cache)
 791                 return;
 792
 793         for (i = 0; i < cache->num_regs; i++) {
 794                 reg = &cache->reg_list[i];
 795
 796                 free(reg->feature);
 797                 free(reg->reg_data_type);
 798         }
 799
 800         free(cache->reg_list[0].arch_info);
 801         free(cache->reg_list);
 802         free(cache);
 803
 804         arm->core_cache = NULL;
 805 }
 806
 807 static int armv7m_setup_semihosting(struct target *target, int enable)
 808 {
 809         /* nothing todo for armv7m */
 810         return ERROR_OK;
 811 }
 812
 813 /** Sets up target as a generic ARMv7-M core */
 814 int armv7m_init_arch_info(struct target *target, struct armv7m_common *armv7m)
 815 {
 816         struct arm *arm = &armv7m->arm;
 817
 818         armv7m->common_magic = ARMV7M_COMMON_MAGIC;
 819         armv7m->fp_feature = FP_NONE;
 820         armv7m->trace_config.trace_bus_id = 1;
 821         /* Enable stimulus port #0 by default */
 822         armv7m->trace_config.itm_ter[0] = 1;
 823
 824         arm->core_type = ARM_CORE_TYPE_M_PROFILE;
 825         arm->arch_info = armv7m;
 826         arm->setup_semihosting = armv7m_setup_semihosting;
 827
 828         arm->read_core_reg = armv7m_read_core_reg;
 829         arm->write_core_reg = armv7m_write_core_reg;
 830
 831         return arm_init_arch_info(target, arm);
 832 }
 833
 834 /** Generates a CRC32 checksum of a memory region. */
 835 int armv7m_checksum_memory(struct target *target,
 836         target_addr_t address, uint32_t count, uint32_t *checksum)
 837 {
 838         struct working_area *crc_algorithm;
 839         struct armv7m_algorithm armv7m_info;
 840         struct reg_param reg_params[2];
 841         int retval;
 842
 843         static const uint8_t cortex_m_crc_code[] = {
 844 #include "../../contrib/loaders/checksum/armv7m_crc.inc"
 845         };
 846
 847         retval = target_alloc_working_area(target, sizeof(cortex_m_crc_code), &crc_algorithm);
 848         if (retval != ERROR_OK)
 849                 return retval;
 850
 851         retval = target_write_buffer(target, crc_algorithm->address,
 852                         sizeof(cortex_m_crc_code), (uint8_t *)cortex_m_crc_code);
 853         if (retval != ERROR_OK)
 854                 goto cleanup;
 855
 856         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 857         armv7m_info.core_mode = ARM_MODE_THREAD;
 858
 859         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
 860         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
 861
 862         buf_set_u32(reg_params[0].value, 0, 32, address);
 863         buf_set_u32(reg_params[1].value, 0, 32, count);
 864
 865         int timeout = 20000 * (1 + (count / (1024 * 1024)));
 866
 867         retval = target_run_algorithm(target, 0, NULL, 2, reg_params, crc_algorithm->address,
 868                         crc_algorithm->address + (sizeof(cortex_m_crc_code) - 6),
 869                         timeout, &armv7m_info);
 870
 871         if (retval == ERROR_OK)
 872                 *checksum = buf_get_u32(reg_params[0].value, 0, 32);
 873         else
 874                 LOG_ERROR("error executing cortex_m crc algorithm");
 875
 876         destroy_reg_param(&reg_params[0]);
 877         destroy_reg_param(&reg_params[1]);
 878
 879 cleanup:
 880         target_free_working_area(target, crc_algorithm);
 881
 882         return retval;
 883 }
 884
 885 /** Checks an array of memory regions whether they are erased. */
 886 int armv7m_blank_check_memory(struct target *target,
 887         struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value)
 888 {
 889         struct working_area *erase_check_algorithm;
 890         struct working_area *erase_check_params;
 891         struct reg_param reg_params[2];
 892         struct armv7m_algorithm armv7m_info;
 893         int retval;
 894
 895         static bool timed_out;
 896
 897         static const uint8_t erase_check_code[] = {
 898 #include "../../contrib/loaders/erase_check/armv7m_erase_check.inc"
 899         };
 900
 901         const uint32_t code_size = sizeof(erase_check_code);
 902
 903         /* make sure we have a working area */
 904         if (target_alloc_working_area(target, code_size,
 905                 &erase_check_algorithm) != ERROR_OK)
 906                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 907
 908         retval = target_write_buffer(target, erase_check_algorithm->address,
 909                         code_size, erase_check_code);
 910         if (retval != ERROR_OK)
 911                 goto cleanup1;
 912
 913         /* prepare blocks array for algo */
 914         struct algo_block {
 915                 union {
 916                         uint32_t size;
 917                         uint32_t result;
 918                 };
 919                 uint32_t address;
 920         };
 921
 922         uint32_t avail = target_get_working_area_avail(target);
 923         int blocks_to_check = avail / sizeof(struct algo_block) - 1;
 924         if (num_blocks < blocks_to_check)
 925                 blocks_to_check = num_blocks;
 926
 927         struct algo_block *params = malloc((blocks_to_check+1)*sizeof(struct algo_block));
 928         if (params == NULL) {
 929                 retval = ERROR_FAIL;
 930                 goto cleanup1;
 931         }
 932
 933         int i;
 934         uint32_t total_size = 0;
 935         for (i = 0; i < blocks_to_check; i++) {
 936                 total_size += blocks[i].size;
 937                 target_buffer_set_u32(target, (uint8_t *)&(params[i].size),
 938                                                 blocks[i].size / sizeof(uint32_t));
 939                 target_buffer_set_u32(target, (uint8_t *)&(params[i].address),
 940                                                 blocks[i].address);
 941         }
 942         target_buffer_set_u32(target, (uint8_t *)&(params[blocks_to_check].size), 0);
 943
 944         uint32_t param_size = (blocks_to_check + 1) * sizeof(struct algo_block);
 945         if (target_alloc_working_area(target, param_size,
 946                         &erase_check_params) != ERROR_OK) {
 947                 retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 948                 goto cleanup2;
 949         }
 950
 951         retval = target_write_buffer(target, erase_check_params->address,
 952                                 param_size, (uint8_t *)params);
 953         if (retval != ERROR_OK)
 954                 goto cleanup3;
 955
 956         uint32_t erased_word = erased_value | (erased_value << 8)
 957                                | (erased_value << 16) | (erased_value << 24);
 958
 959         LOG_DEBUG("Starting erase check of %d blocks, parameters@"
 960                  TARGET_ADDR_FMT, blocks_to_check, erase_check_params->address);
 961
 962         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 963         armv7m_info.core_mode = ARM_MODE_THREAD;
 964
 965         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
 966         buf_set_u32(reg_params[0].value, 0, 32, erase_check_params->address);
 967
 968         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
 969         buf_set_u32(reg_params[1].value, 0, 32, erased_word);
 970
 971         /* assume CPU clk at least 1 MHz */
 972         int timeout = (timed_out ? 30000 : 2000) + total_size * 3 / 1000;
 973
 974         retval = target_run_algorithm(target,
 975                                 0, NULL,
 976                                 ARRAY_SIZE(reg_params), reg_params,
 977                                 erase_check_algorithm->address,
 978                                 erase_check_algorithm->address + (code_size - 2),
 979                                 timeout,
 980                                 &armv7m_info);
 981
 982         timed_out = retval == ERROR_TARGET_TIMEOUT;
 983         if (retval != ERROR_OK && !timed_out)
 984                 goto cleanup4;
 985
 986         retval = target_read_buffer(target, erase_check_params->address,
 987                                 param_size, (uint8_t *)params);
 988         if (retval != ERROR_OK)
 989                 goto cleanup4;
 990
 991         for (i = 0; i < blocks_to_check; i++) {
 992                 uint32_t result = target_buffer_get_u32(target,
 993                                         (uint8_t *)&(params[i].result));
 994                 if (result != 0 && result != 1)
 995                         break;
 996
 997                 blocks[i].result = result;
 998         }
 999         if (i && timed_out)
1000                 LOG_INFO("Slow CPU clock: %d blocks checked, %d remain. Continuing...", i, num_blocks-i);
1001
1002         retval = i;             /* return number of blocks really checked */
1003
1004 cleanup4:
1005         destroy_reg_param(&reg_params[0]);
1006         destroy_reg_param(&reg_params[1]);
1007
1008 cleanup3:
1009         target_free_working_area(target, erase_check_params);
1010 cleanup2:
1011         free(params);
1012 cleanup1:
1013         target_free_working_area(target, erase_check_algorithm);
1014
1015         return retval;
1016 }
1017
1018 int armv7m_maybe_skip_bkpt_inst(struct target *target, bool *inst_found)
1019 {
1020         struct armv7m_common *armv7m = target_to_armv7m(target);
1021         struct reg *r = armv7m->arm.pc;
1022         bool result = false;
1023
1024
1025         /* if we halted last time due to a bkpt instruction
1026          * then we have to manually step over it, otherwise
1027          * the core will break again */
1028
1029         if (target->debug_reason == DBG_REASON_BREAKPOINT) {
1030                 uint16_t op;
1031                 uint32_t pc = buf_get_u32(r->value, 0, 32);
1032
1033                 pc &= ~1;
1034                 if (target_read_u16(target, pc, &op) == ERROR_OK) {
1035                         if ((op & 0xFF00) == 0xBE00) {
1036                                 pc = buf_get_u32(r->value, 0, 32) + 2;
1037                                 buf_set_u32(r->value, 0, 32, pc);
1038                                 r->dirty = true;
1039                                 r->valid = true;
1040                                 result = true;
1041                                 LOG_DEBUG("Skipping over BKPT instruction");
1042                         }
1043                 }
1044         }
1045
1046         if (inst_found)
1047                 *inst_found = result;
1048
1049         return ERROR_OK;
1050 }
1051
1052 const struct command_registration armv7m_command_handlers[] = {
1053         {
1054                 .chain = arm_command_handlers,
1055         },
1056         COMMAND_REGISTRATION_DONE
1057 };