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         /* Store all non-debug execution registers to armv7m_algorithm_info context */
 497         for (unsigned i = 0; i < armv7m->arm.core_cache->num_regs; i++) {
 498
 499                 armv7m_algorithm_info->context[i] = buf_get_u32(
 500                                 armv7m->arm.core_cache->reg_list[i].value,
 501                                 0,
 502                                 32);
 503         }
 504
 505         for (int i = 0; i < num_mem_params; i++) {
 506                 if (mem_params[i].direction == PARAM_IN)
 507                         continue;
 508                 retval = target_write_buffer(target, mem_params[i].address,
 509                                 mem_params[i].size,
 510                                 mem_params[i].value);
 511                 if (retval != ERROR_OK)
 512                         return retval;
 513         }
 514
 515         for (int i = 0; i < num_reg_params; i++) {
 516                 if (reg_params[i].direction == PARAM_IN)
 517                         continue;
 518
 519                 struct reg *reg =
 520                         register_get_by_name(armv7m->arm.core_cache, reg_params[i].reg_name, 0);
 521 /*              uint32_t regvalue; */
 522
 523                 if (!reg) {
 524                         LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
 525                         return ERROR_COMMAND_SYNTAX_ERROR;
 526                 }
 527
 528                 if (reg->size != reg_params[i].size) {
 529                         LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size",
 530                                 reg_params[i].reg_name);
 531                         return ERROR_COMMAND_SYNTAX_ERROR;
 532                 }
 533
 534 /*              regvalue = buf_get_u32(reg_params[i].value, 0, 32); */
 535                 armv7m_set_core_reg(reg, reg_params[i].value);
 536         }
 537
 538         {
 539                 /*
 540                  * Ensure xPSR.T is set to avoid trying to run things in arm
 541                  * (non-thumb) mode, which armv7m does not support.
 542                  *
 543                  * We do this by setting the entirety of xPSR, which should
 544                  * remove all the unknowns about xPSR state.
 545                  *
 546                  * Because xPSR.T is populated on reset from the vector table,
 547                  * it might be 0 if the vector table has "bad" data in it.
 548                  */
 549                 struct reg *reg = &armv7m->arm.core_cache->reg_list[ARMV7M_xPSR];
 550                 buf_set_u32(reg->value, 0, 32, 0x01000000);
 551                 reg->valid = true;
 552                 reg->dirty = true;
 553         }
 554
 555         if (armv7m_algorithm_info->core_mode != ARM_MODE_ANY &&
 556                         armv7m_algorithm_info->core_mode != core_mode) {
 557
 558                 /* we cannot set ARM_MODE_HANDLER, so use ARM_MODE_THREAD instead */
 559                 if (armv7m_algorithm_info->core_mode == ARM_MODE_HANDLER) {
 560                         armv7m_algorithm_info->core_mode = ARM_MODE_THREAD;
 561                         LOG_INFO("ARM_MODE_HANDLER not currently supported, using ARM_MODE_THREAD instead");
 562                 }
 563
 564                 LOG_DEBUG("setting core_mode: 0x%2.2x", armv7m_algorithm_info->core_mode);
 565                 buf_set_u32(armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].value,
 566                         0, 1, armv7m_algorithm_info->core_mode);
 567                 armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].dirty = true;
 568                 armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].valid = true;
 569         }
 570
 571         /* save previous core mode */
 572         armv7m_algorithm_info->core_mode = core_mode;
 573
 574         retval = target_resume(target, 0, entry_point, 1, 1);
 575
 576         return retval;
 577 }
 578
 579 /** Waits for an algorithm in the target. */
 580 int armv7m_wait_algorithm(struct target *target,
 581         int num_mem_params, struct mem_param *mem_params,
 582         int num_reg_params, struct reg_param *reg_params,
 583         target_addr_t exit_point, int timeout_ms,
 584         void *arch_info)
 585 {
 586         struct armv7m_common *armv7m = target_to_armv7m(target);
 587         struct armv7m_algorithm *armv7m_algorithm_info = arch_info;
 588         int retval = ERROR_OK;
 589
 590         /* NOTE: armv7m_run_algorithm requires that each algorithm uses a software breakpoint
 591          * at the exit point */
 592
 593         if (armv7m_algorithm_info->common_magic != ARMV7M_COMMON_MAGIC) {
 594                 LOG_ERROR("current target isn't an ARMV7M target");
 595                 return ERROR_TARGET_INVALID;
 596         }
 597
 598         retval = target_wait_state(target, TARGET_HALTED, timeout_ms);
 599         /* If the target fails to halt due to the breakpoint, force a halt */
 600         if (retval != ERROR_OK || target->state != TARGET_HALTED) {
 601                 retval = target_halt(target);
 602                 if (retval != ERROR_OK)
 603                         return retval;
 604                 retval = target_wait_state(target, TARGET_HALTED, 500);
 605                 if (retval != ERROR_OK)
 606                         return retval;
 607                 return ERROR_TARGET_TIMEOUT;
 608         }
 609
 610         if (exit_point) {
 611                 /* PC value has been cached in cortex_m_debug_entry() */
 612                 uint32_t pc = buf_get_u32(armv7m->arm.pc->value, 0, 32);
 613                 if (pc != exit_point) {
 614                         LOG_DEBUG("failed algorithm halted at 0x%" PRIx32 ", expected 0x%" TARGET_PRIxADDR,
 615                                           pc, exit_point);
 616                         return ERROR_TARGET_ALGO_EXIT;
 617                 }
 618         }
 619
 620         /* Read memory values to mem_params[] */
 621         for (int i = 0; i < num_mem_params; i++) {
 622                 if (mem_params[i].direction != PARAM_OUT) {
 623                         retval = target_read_buffer(target, mem_params[i].address,
 624                                         mem_params[i].size,
 625                                         mem_params[i].value);
 626                         if (retval != ERROR_OK)
 627                                 return retval;
 628                 }
 629         }
 630
 631         /* Copy core register values to reg_params[] */
 632         for (int i = 0; i < num_reg_params; i++) {
 633                 if (reg_params[i].direction != PARAM_OUT) {
 634                         struct reg *reg = register_get_by_name(armv7m->arm.core_cache,
 635                                         reg_params[i].reg_name,
 636                                         0);
 637
 638                         if (!reg) {
 639                                 LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
 640                                 return ERROR_COMMAND_SYNTAX_ERROR;
 641                         }
 642
 643                         if (reg->size != reg_params[i].size) {
 644                                 LOG_ERROR(
 645                                         "BUG: register '%s' size doesn't match reg_params[i].size",
 646                                         reg_params[i].reg_name);
 647                                 return ERROR_COMMAND_SYNTAX_ERROR;
 648                         }
 649
 650                         buf_set_u32(reg_params[i].value, 0, 32, buf_get_u32(reg->value, 0, 32));
 651                 }
 652         }
 653
 654         for (int i = armv7m->arm.core_cache->num_regs - 1; i >= 0; i--) {
 655                 uint32_t regvalue;
 656                 regvalue = buf_get_u32(armv7m->arm.core_cache->reg_list[i].value, 0, 32);
 657                 if (regvalue != armv7m_algorithm_info->context[i]) {
 658                         LOG_DEBUG("restoring register %s with value 0x%8.8" PRIx32,
 659                                         armv7m->arm.core_cache->reg_list[i].name,
 660                                 armv7m_algorithm_info->context[i]);
 661                         buf_set_u32(armv7m->arm.core_cache->reg_list[i].value,
 662                                 0, 32, armv7m_algorithm_info->context[i]);
 663                         armv7m->arm.core_cache->reg_list[i].valid = true;
 664                         armv7m->arm.core_cache->reg_list[i].dirty = true;
 665                 }
 666         }
 667
 668         /* restore previous core mode */
 669         if (armv7m_algorithm_info->core_mode != armv7m->arm.core_mode) {
 670                 LOG_DEBUG("restoring core_mode: 0x%2.2x", armv7m_algorithm_info->core_mode);
 671                 buf_set_u32(armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].value,
 672                         0, 1, armv7m_algorithm_info->core_mode);
 673                 armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].dirty = true;
 674                 armv7m->arm.core_cache->reg_list[ARMV7M_CONTROL].valid = true;
 675         }
 676
 677         armv7m->arm.core_mode = armv7m_algorithm_info->core_mode;
 678
 679         return retval;
 680 }
 681
 682 /** Logs summary of ARMv7-M state for a halted target. */
 683 int armv7m_arch_state(struct target *target)
 684 {
 685         struct armv7m_common *armv7m = target_to_armv7m(target);
 686         struct arm *arm = &armv7m->arm;
 687         uint32_t ctrl, sp;
 688
 689         /* avoid filling log waiting for fileio reply */
 690         if (target->semihosting && target->semihosting->hit_fileio)
 691                 return ERROR_OK;
 692
 693         ctrl = buf_get_u32(arm->core_cache->reg_list[ARMV7M_CONTROL].value, 0, 32);
 694         sp = buf_get_u32(arm->core_cache->reg_list[ARMV7M_R13].value, 0, 32);
 695
 696         LOG_USER("target halted due to %s, current mode: %s %s\n"
 697                 "xPSR: %#8.8" PRIx32 " pc: %#8.8" PRIx32 " %csp: %#8.8" PRIx32 "%s%s",
 698                 debug_reason_name(target),
 699                 arm_mode_name(arm->core_mode),
 700                 armv7m_exception_string(armv7m->exception_number),
 701                 buf_get_u32(arm->cpsr->value, 0, 32),
 702                 buf_get_u32(arm->pc->value, 0, 32),
 703                 (ctrl & 0x02) ? 'p' : 'm',
 704                 sp,
 705                 (target->semihosting && target->semihosting->is_active) ? ", semihosting" : "",
 706                 (target->semihosting && target->semihosting->is_fileio) ? " fileio" : "");
 707
 708         return ERROR_OK;
 709 }
 710
 711 static const struct reg_arch_type armv7m_reg_type = {
 712         .get = armv7m_get_core_reg,
 713         .set = armv7m_set_core_reg,
 714 };
 715
 716 /** Builds cache of architecturally defined registers.  */
 717 struct reg_cache *armv7m_build_reg_cache(struct target *target)
 718 {
 719         struct armv7m_common *armv7m = target_to_armv7m(target);
 720         struct arm *arm = &armv7m->arm;
 721         int num_regs = ARMV7M_NUM_REGS;
 722         struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
 723         struct reg_cache *cache = malloc(sizeof(struct reg_cache));
 724         struct reg *reg_list = calloc(num_regs, sizeof(struct reg));
 725         struct arm_reg *arch_info = calloc(num_regs, sizeof(struct arm_reg));
 726         struct reg_feature *feature;
 727         int i;
 728
 729         /* Build the process context cache */
 730         cache->name = "arm v7m registers";
 731         cache->next = NULL;
 732         cache->reg_list = reg_list;
 733         cache->num_regs = num_regs;
 734         (*cache_p) = cache;
 735
 736         for (i = 0; i < num_regs; i++) {
 737                 arch_info[i].num = armv7m_regs[i].id;
 738                 arch_info[i].target = target;
 739                 arch_info[i].arm = arm;
 740
 741                 reg_list[i].name = armv7m_regs[i].name;
 742                 reg_list[i].size = armv7m_regs[i].bits;
 743                 reg_list[i].value = arch_info[i].value;
 744                 reg_list[i].dirty = false;
 745                 reg_list[i].valid = false;
 746                 reg_list[i].hidden = i == ARMV7M_PMSK_BPRI_FLTMSK_CTRL;
 747                 reg_list[i].type = &armv7m_reg_type;
 748                 reg_list[i].arch_info = &arch_info[i];
 749
 750                 reg_list[i].group = armv7m_regs[i].group;
 751                 reg_list[i].number = i;
 752                 reg_list[i].exist = true;
 753                 reg_list[i].caller_save = true; /* gdb defaults to true */
 754
 755                 if (reg_list[i].hidden)
 756                         continue;
 757
 758                 feature = calloc(1, sizeof(struct reg_feature));
 759                 if (feature) {
 760                         feature->name = armv7m_regs[i].feature;
 761                         reg_list[i].feature = feature;
 762                 } else
 763                         LOG_ERROR("unable to allocate feature list");
 764
 765                 reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
 766                 if (reg_list[i].reg_data_type)
 767                         reg_list[i].reg_data_type->type = armv7m_regs[i].type;
 768                 else
 769                         LOG_ERROR("unable to allocate reg type list");
 770         }
 771
 772         arm->cpsr = reg_list + ARMV7M_xPSR;
 773         arm->pc = reg_list + ARMV7M_PC;
 774         arm->core_cache = cache;
 775
 776         return cache;
 777 }
 778
 779 void armv7m_free_reg_cache(struct target *target)
 780 {
 781         struct armv7m_common *armv7m = target_to_armv7m(target);
 782         struct arm *arm = &armv7m->arm;
 783         struct reg_cache *cache;
 784         struct reg *reg;
 785         unsigned int i;
 786
 787         cache = arm->core_cache;
 788
 789         if (!cache)
 790                 return;
 791
 792         for (i = 0; i < cache->num_regs; i++) {
 793                 reg = &cache->reg_list[i];
 794
 795                 free(reg->feature);
 796                 free(reg->reg_data_type);
 797         }
 798
 799         free(cache->reg_list[0].arch_info);
 800         free(cache->reg_list);
 801         free(cache);
 802
 803         arm->core_cache = NULL;
 804 }
 805
 806 static int armv7m_setup_semihosting(struct target *target, int enable)
 807 {
 808         /* nothing todo for armv7m */
 809         return ERROR_OK;
 810 }
 811
 812 /** Sets up target as a generic ARMv7-M core */
 813 int armv7m_init_arch_info(struct target *target, struct armv7m_common *armv7m)
 814 {
 815         struct arm *arm = &armv7m->arm;
 816
 817         armv7m->common_magic = ARMV7M_COMMON_MAGIC;
 818         armv7m->fp_feature = FP_NONE;
 819         armv7m->trace_config.trace_bus_id = 1;
 820         /* Enable stimulus port #0 by default */
 821         armv7m->trace_config.itm_ter[0] = 1;
 822
 823         arm->core_type = ARM_CORE_TYPE_M_PROFILE;
 824         arm->arch_info = armv7m;
 825         arm->setup_semihosting = armv7m_setup_semihosting;
 826
 827         arm->read_core_reg = armv7m_read_core_reg;
 828         arm->write_core_reg = armv7m_write_core_reg;
 829
 830         return arm_init_arch_info(target, arm);
 831 }
 832
 833 /** Generates a CRC32 checksum of a memory region. */
 834 int armv7m_checksum_memory(struct target *target,
 835         target_addr_t address, uint32_t count, uint32_t *checksum)
 836 {
 837         struct working_area *crc_algorithm;
 838         struct armv7m_algorithm armv7m_info;
 839         struct reg_param reg_params[2];
 840         int retval;
 841
 842         static const uint8_t cortex_m_crc_code[] = {
 843 #include "../../contrib/loaders/checksum/armv7m_crc.inc"
 844         };
 845
 846         retval = target_alloc_working_area(target, sizeof(cortex_m_crc_code), &crc_algorithm);
 847         if (retval != ERROR_OK)
 848                 return retval;
 849
 850         retval = target_write_buffer(target, crc_algorithm->address,
 851                         sizeof(cortex_m_crc_code), (uint8_t *)cortex_m_crc_code);
 852         if (retval != ERROR_OK)
 853                 goto cleanup;
 854
 855         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 856         armv7m_info.core_mode = ARM_MODE_THREAD;
 857
 858         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
 859         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
 860
 861         buf_set_u32(reg_params[0].value, 0, 32, address);
 862         buf_set_u32(reg_params[1].value, 0, 32, count);
 863
 864         int timeout = 20000 * (1 + (count / (1024 * 1024)));
 865
 866         retval = target_run_algorithm(target, 0, NULL, 2, reg_params, crc_algorithm->address,
 867                         crc_algorithm->address + (sizeof(cortex_m_crc_code) - 6),
 868                         timeout, &armv7m_info);
 869
 870         if (retval == ERROR_OK)
 871                 *checksum = buf_get_u32(reg_params[0].value, 0, 32);
 872         else
 873                 LOG_ERROR("error executing cortex_m crc algorithm");
 874
 875         destroy_reg_param(&reg_params[0]);
 876         destroy_reg_param(&reg_params[1]);
 877
 878 cleanup:
 879         target_free_working_area(target, crc_algorithm);
 880
 881         return retval;
 882 }
 883
 884 /** Checks an array of memory regions whether they are erased. */
 885 int armv7m_blank_check_memory(struct target *target,
 886         struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value)
 887 {
 888         struct working_area *erase_check_algorithm;
 889         struct working_area *erase_check_params;
 890         struct reg_param reg_params[2];
 891         struct armv7m_algorithm armv7m_info;
 892         int retval;
 893
 894         static bool timed_out;
 895
 896         static const uint8_t erase_check_code[] = {
 897 #include "../../contrib/loaders/erase_check/armv7m_erase_check.inc"
 898         };
 899
 900         const uint32_t code_size = sizeof(erase_check_code);
 901
 902         /* make sure we have a working area */
 903         if (target_alloc_working_area(target, code_size,
 904                 &erase_check_algorithm) != ERROR_OK)
 905                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 906
 907         retval = target_write_buffer(target, erase_check_algorithm->address,
 908                         code_size, erase_check_code);
 909         if (retval != ERROR_OK)
 910                 goto cleanup1;
 911
 912         /* prepare blocks array for algo */
 913         struct algo_block {
 914                 union {
 915                         uint32_t size;
 916                         uint32_t result;
 917                 };
 918                 uint32_t address;
 919         };
 920
 921         uint32_t avail = target_get_working_area_avail(target);
 922         int blocks_to_check = avail / sizeof(struct algo_block) - 1;
 923         if (num_blocks < blocks_to_check)
 924                 blocks_to_check = num_blocks;
 925
 926         struct algo_block *params = malloc((blocks_to_check+1)*sizeof(struct algo_block));
 927         if (params == NULL) {
 928                 retval = ERROR_FAIL;
 929                 goto cleanup1;
 930         }
 931
 932         int i;
 933         uint32_t total_size = 0;
 934         for (i = 0; i < blocks_to_check; i++) {
 935                 total_size += blocks[i].size;
 936                 target_buffer_set_u32(target, (uint8_t *)&(params[i].size),
 937                                                 blocks[i].size / sizeof(uint32_t));
 938                 target_buffer_set_u32(target, (uint8_t *)&(params[i].address),
 939                                                 blocks[i].address);
 940         }
 941         target_buffer_set_u32(target, (uint8_t *)&(params[blocks_to_check].size), 0);
 942
 943         uint32_t param_size = (blocks_to_check + 1) * sizeof(struct algo_block);
 944         if (target_alloc_working_area(target, param_size,
 945                         &erase_check_params) != ERROR_OK) {
 946                 retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 947                 goto cleanup2;
 948         }
 949
 950         retval = target_write_buffer(target, erase_check_params->address,
 951                                 param_size, (uint8_t *)params);
 952         if (retval != ERROR_OK)
 953                 goto cleanup3;
 954
 955         uint32_t erased_word = erased_value | (erased_value << 8)
 956                                | (erased_value << 16) | (erased_value << 24);
 957
 958         LOG_DEBUG("Starting erase check of %d blocks, parameters@"
 959                  TARGET_ADDR_FMT, blocks_to_check, erase_check_params->address);
 960
 961         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 962         armv7m_info.core_mode = ARM_MODE_THREAD;
 963
 964         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
 965         buf_set_u32(reg_params[0].value, 0, 32, erase_check_params->address);
 966
 967         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
 968         buf_set_u32(reg_params[1].value, 0, 32, erased_word);
 969
 970         /* assume CPU clk at least 1 MHz */
 971         int timeout = (timed_out ? 30000 : 2000) + total_size * 3 / 1000;
 972
 973         retval = target_run_algorithm(target,
 974                                 0, NULL,
 975                                 ARRAY_SIZE(reg_params), reg_params,
 976                                 erase_check_algorithm->address,
 977                                 erase_check_algorithm->address + (code_size - 2),
 978                                 timeout,
 979                                 &armv7m_info);
 980
 981         timed_out = retval == ERROR_TARGET_TIMEOUT;
 982         if (retval != ERROR_OK && !timed_out)
 983                 goto cleanup4;
 984
 985         retval = target_read_buffer(target, erase_check_params->address,
 986                                 param_size, (uint8_t *)params);
 987         if (retval != ERROR_OK)
 988                 goto cleanup4;
 989
 990         for (i = 0; i < blocks_to_check; i++) {
 991                 uint32_t result = target_buffer_get_u32(target,
 992                                         (uint8_t *)&(params[i].result));
 993                 if (result != 0 && result != 1)
 994                         break;
 995
 996                 blocks[i].result = result;
 997         }
 998         if (i && timed_out)
 999                 LOG_INFO("Slow CPU clock: %d blocks checked, %d remain. Continuing...", i, num_blocks-i);
1000
1001         retval = i;             /* return number of blocks really checked */
1002
1003 cleanup4:
1004         destroy_reg_param(&reg_params[0]);
1005         destroy_reg_param(&reg_params[1]);
1006
1007 cleanup3:
1008         target_free_working_area(target, erase_check_params);
1009 cleanup2:
1010         free(params);
1011 cleanup1:
1012         target_free_working_area(target, erase_check_algorithm);
1013
1014         return retval;
1015 }
1016
1017 int armv7m_maybe_skip_bkpt_inst(struct target *target, bool *inst_found)
1018 {
1019         struct armv7m_common *armv7m = target_to_armv7m(target);
1020         struct reg *r = armv7m->arm.pc;
1021         bool result = false;
1022
1023
1024         /* if we halted last time due to a bkpt instruction
1025          * then we have to manually step over it, otherwise
1026          * the core will break again */
1027
1028         if (target->debug_reason == DBG_REASON_BREAKPOINT) {
1029                 uint16_t op;
1030                 uint32_t pc = buf_get_u32(r->value, 0, 32);
1031
1032                 pc &= ~1;
1033                 if (target_read_u16(target, pc, &op) == ERROR_OK) {
1034                         if ((op & 0xFF00) == 0xBE00) {
1035                                 pc = buf_get_u32(r->value, 0, 32) + 2;
1036                                 buf_set_u32(r->value, 0, 32, pc);
1037                                 r->dirty = true;
1038                                 r->valid = true;
1039                                 result = true;
1040                                 LOG_DEBUG("Skipping over BKPT instruction");
1041                         }
1042                 }
1043         }
1044
1045         if (inst_found)
1046                 *inst_found = result;
1047
1048         return ERROR_OK;
1049 }
1050
1051 const struct command_registration armv7m_command_handlers[] = {
1052         {
1053                 .chain = arm_command_handlers,
1054         },
1055         COMMAND_REGISTRATION_DONE
1056 };