git.gag.com Git - fw/openocd/blob - src/target/cortex_m3.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  *   This program is free software; you can redistribute it and/or modify  *
   9  *   it under the terms of the GNU General Public License as published by  *
  10  *   the Free Software Foundation; either version 2 of the License, or     *
  11  *   (at your option) any later version.                                   *
  12  *                                                                         *
  13  *   This program is distributed in the hope that it will be useful,       *
  14  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  15  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  16  *   GNU General Public License for more details.                          *
  17  *                                                                         *
  18  *   You should have received a copy of the GNU General Public License     *
  19  *   along with this program; if not, write to the                         *
  20  *   Free Software Foundation, Inc.,                                       *
  21  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  22  ***************************************************************************/
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 #include "replacements.h"
  28
  29 #include "cortex_m3.h"
  30 #include "armv7m.h"
  31
  32 #include "register.h"
  33 #include "target.h"
  34 #include "target_request.h"
  35 #include "log.h"
  36 #include "jtag.h"
  37 #include "arm_jtag.h"
  38
  39 #include <stdlib.h>
  40 #include <string.h>
  41
  42 /* cli handling */
  43 int cortex_m3_register_commands(struct command_context_s *cmd_ctx);
  44
  45 /* forward declarations */
  46 void cortex_m3_enable_breakpoints(struct target_s *target);
  47 void cortex_m3_enable_watchpoints(struct target_s *target);
  48 int cortex_m3_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
  49 int cortex_m3_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
  50 int cortex_m3_quit();
  51 int cortex_m3_load_core_reg_u32(target_t *target, enum armv7m_regtype type, u32 num, u32 *value);
  52 int cortex_m3_store_core_reg_u32(target_t *target, enum armv7m_regtype type, u32 num, u32 value);
  53 int cortex_m3_target_request_data(target_t *target, u32 size, u8 *buffer);
  54 int cortex_m3_examine(struct command_context_s *cmd_ctx, struct target_s *target);
  55
  56 target_type_t cortexm3_target =
  57 {
  58         .name = "cortex_m3",
  59
  60         .poll = cortex_m3_poll,
  61         .arch_state = armv7m_arch_state,
  62
  63         .target_request_data = cortex_m3_target_request_data,
  64
  65         .halt = cortex_m3_halt,
  66         .resume = cortex_m3_resume,
  67         .step = cortex_m3_step,
  68
  69         .assert_reset = cortex_m3_assert_reset,
  70         .deassert_reset = cortex_m3_deassert_reset,
  71         .soft_reset_halt = cortex_m3_soft_reset_halt,
  72
  73         .get_gdb_reg_list = armv7m_get_gdb_reg_list,
  74
  75         .read_memory = cortex_m3_read_memory,
  76         .write_memory = cortex_m3_write_memory,
  77         .bulk_write_memory = cortex_m3_bulk_write_memory,
  78         .checksum_memory = armv7m_checksum_memory,
  79
  80         .run_algorithm = armv7m_run_algorithm,
  81
  82         .add_breakpoint = cortex_m3_add_breakpoint,
  83         .remove_breakpoint = cortex_m3_remove_breakpoint,
  84         .add_watchpoint = cortex_m3_add_watchpoint,
  85         .remove_watchpoint = cortex_m3_remove_watchpoint,
  86
  87         .register_commands = cortex_m3_register_commands,
  88         .target_command = cortex_m3_target_command,
  89         .init_target = cortex_m3_init_target,
  90         .examine = cortex_m3_examine,
  91         .quit = cortex_m3_quit
  92 };
  93
  94 int cortex_m3_clear_halt(target_t *target)
  95 {
  96         /* get pointers to arch-specific information */
  97         armv7m_common_t *armv7m = target->arch_info;
  98         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
  99         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 100
 101     /* Read Debug Fault Status Register */
 102     ahbap_read_system_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
 103     /* Write Debug Fault Status Register to enable processing to resume ?? Try with and without this !! */
 104     ahbap_write_system_atomic_u32(swjdp, NVIC_DFSR, cortex_m3->nvic_dfsr);
 105     LOG_DEBUG(" NVIC_DFSR 0x%x", cortex_m3->nvic_dfsr);
 106
 107     return ERROR_OK;
 108 }
 109
 110 int cortex_m3_single_step_core(target_t *target)
 111 {
 112         /* get pointers to arch-specific information */
 113         armv7m_common_t *armv7m = target->arch_info;
 114         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 115         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 116
 117         if (!(cortex_m3->dcb_dhcsr & C_MASKINTS))
 118                 ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_MASKINTS | C_HALT | C_DEBUGEN );
 119         ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_MASKINTS | C_STEP | C_DEBUGEN );
 120         cortex_m3->dcb_dhcsr |= C_MASKINTS;
 121         LOG_DEBUG(" ");
 122         cortex_m3_clear_halt(target);
 123
 124         return ERROR_OK;
 125 }
 126
 127 int cortex_m3_exec_opcode(target_t *target,u32 opcode, int len /* MODE, r0_invalue, &r0_outvalue */ )
 128 {
 129         /* get pointers to arch-specific information */
 130         armv7m_common_t *armv7m = target->arch_info;
 131         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 132         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 133         u32 savedram;
 134         int retvalue;
 135
 136         ahbap_read_system_u32(swjdp, 0x20000000, &savedram);
 137         ahbap_write_system_u32(swjdp, 0x20000000, opcode);
 138         ahbap_write_coreregister_u32(swjdp, 0x20000000, 15);
 139         cortex_m3_single_step_core(target);
 140         armv7m->core_cache->reg_list[15].dirty = armv7m->core_cache->reg_list[15].valid;
 141         retvalue = ahbap_write_system_atomic_u32(swjdp, 0x20000000, savedram);
 142
 143         return retvalue;
 144 }
 145
 146 #if 0
 147 /* Enable interrupts */
 148 int cortex_m3_cpsie(target_t *target, u32 IF)
 149 {
 150         return cortex_m3_exec_opcode(target, ARMV7M_T_CPSIE(IF), 2);
 151 }
 152
 153 /* Disable interrupts */
 154 int cortex_m3_cpsid(target_t *target, u32 IF)
 155 {
 156         return cortex_m3_exec_opcode(target, ARMV7M_T_CPSID(IF), 2);
 157 }
 158 #endif
 159
 160 int cortex_m3_endreset_event(target_t *target)
 161 {
 162         int i;
 163         u32 dcb_demcr;
 164
 165         /* get pointers to arch-specific information */
 166         armv7m_common_t *armv7m = target->arch_info;
 167         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 168         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 169         cortex_m3_fp_comparator_t *fp_list = cortex_m3->fp_comparator_list;
 170         cortex_m3_dwt_comparator_t *dwt_list = cortex_m3->dwt_comparator_list;
 171
 172         ahbap_read_system_atomic_u32(swjdp, DCB_DEMCR, &dcb_demcr);
 173         LOG_DEBUG("DCB_DEMCR = 0x%8.8x",dcb_demcr);
 174
 175         ahbap_write_system_u32(swjdp, DCB_DCRDR, 0 );
 176
 177         /* Enable debug requests */
 178         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 179         if (!(cortex_m3->dcb_dhcsr & C_DEBUGEN))
 180                 ahbap_write_system_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN );
 181         /* Enable trace and dwt */
 182         ahbap_write_system_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR );
 183         /* Monitor bus faults */
 184         ahbap_write_system_u32(swjdp, NVIC_SHCSR, SHCSR_BUSFAULTENA );
 185
 186         /* Enable FPB */
 187         target_write_u32(target, FP_CTRL, 3);
 188
 189         /* Restore FPB registers */
 190         for (i = 0; i < cortex_m3->fp_num_code + cortex_m3->fp_num_lit; i++)
 191         {
 192                 target_write_u32(target, fp_list[i].fpcr_address, fp_list[i].fpcr_value);
 193         }
 194
 195         /* Restore DWT registers */
 196         for (i = 0; i < cortex_m3->dwt_num_comp; i++)
 197         {
 198                 target_write_u32(target, dwt_list[i].dwt_comparator_address, dwt_list[i].comp);
 199                 target_write_u32(target, dwt_list[i].dwt_comparator_address | 0x4, dwt_list[i].mask);
 200                 target_write_u32(target, dwt_list[i].dwt_comparator_address | 0x8, dwt_list[i].function);
 201         }
 202         swjdp_transaction_endcheck(swjdp);
 203
 204         armv7m_invalidate_core_regs(target);
 205         return ERROR_OK;
 206 }
 207
 208 int cortex_m3_examine_debug_reason(target_t *target)
 209 {
 210         /* get pointers to arch-specific information */
 211         armv7m_common_t *armv7m = target->arch_info;
 212         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 213
 214         /* THIS IS NOT GOOD, TODO - better logic for detection of debug state reason */
 215         /* only check the debug reason if we don't know it already */
 216
 217         if ((target->debug_reason != DBG_REASON_DBGRQ)
 218                 && (target->debug_reason != DBG_REASON_SINGLESTEP))
 219         {
 220                 /*  INCOMPLETE */
 221
 222                 if (cortex_m3->nvic_dfsr & DFSR_BKPT)
 223                 {
 224                         target->debug_reason = DBG_REASON_BREAKPOINT;
 225                         if (cortex_m3->nvic_dfsr & DFSR_DWTTRAP)
 226                                 target->debug_reason = DBG_REASON_WPTANDBKPT;
 227                 }
 228                 else if (cortex_m3->nvic_dfsr & DFSR_DWTTRAP)
 229                         target->debug_reason = DBG_REASON_WATCHPOINT;
 230         }
 231
 232         return ERROR_OK;
 233 }
 234
 235 int cortex_m3_examine_exception_reason(target_t *target)
 236 {
 237         u32 shcsr, except_sr, cfsr = -1, except_ar = -1;
 238
 239         /* get pointers to arch-specific information */
 240         armv7m_common_t *armv7m = target->arch_info;
 241         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 242         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 243
 244         ahbap_read_system_u32(swjdp, NVIC_SHCSR, &shcsr);
 245         switch (armv7m->exception_number)
 246         {
 247                 case 2: /* NMI */
 248                         break;
 249                 case 3: /* Hard Fault */
 250                         ahbap_read_system_atomic_u32(swjdp, NVIC_HFSR, &except_sr);
 251                         if (except_sr & 0x40000000)
 252                         {
 253                                 ahbap_read_system_u32(swjdp, NVIC_CFSR, &cfsr);
 254                         }
 255                         break;
 256                 case 4: /* Memory Management */
 257                         ahbap_read_system_u32(swjdp, NVIC_CFSR, &except_sr);
 258                         ahbap_read_system_u32(swjdp, NVIC_MMFAR, &except_ar);
 259                         break;
 260                 case 5: /* Bus Fault */
 261                         ahbap_read_system_u32(swjdp, NVIC_CFSR, &except_sr);
 262                         ahbap_read_system_u32(swjdp, NVIC_BFAR, &except_ar);
 263                         break;
 264                 case 6: /* Usage Fault */
 265                         ahbap_read_system_u32(swjdp, NVIC_CFSR, &except_sr);
 266                         break;
 267                 case 11:        /* SVCall */
 268                         break;
 269                 case 12:        /* Debug Monitor */
 270                         ahbap_read_system_u32(swjdp, NVIC_DFSR, &except_sr);
 271                         break;
 272                 case 14:        /* PendSV */
 273                         break;
 274                 case 15:        /* SysTick */
 275                         break;
 276                 default:
 277                         except_sr = 0;
 278                         break;
 279         }
 280         swjdp_transaction_endcheck(swjdp);
 281     LOG_DEBUG("%s SHCSR 0x%x, SR 0x%x, CFSR 0x%x, AR 0x%x", armv7m_exception_string(armv7m->exception_number), \
 282         shcsr, except_sr, cfsr, except_ar);
 283         return ERROR_OK;
 284 }
 285
 286 int cortex_m3_debug_entry(target_t *target)
 287 {
 288         int i;
 289         u32 xPSR;
 290         int retval;
 291
 292         /* get pointers to arch-specific information */
 293         armv7m_common_t *armv7m = target->arch_info;
 294         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 295         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 296
 297         LOG_DEBUG(" ");
 298         if (armv7m->pre_debug_entry)
 299                 armv7m->pre_debug_entry(target);
 300
 301         cortex_m3_clear_halt(target);
 302         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 303
 304         if ((retval = armv7m->examine_debug_reason(target)) != ERROR_OK)
 305                 return retval;
 306
 307         /* Examine target state and mode */
 308         /* First load register acessible through core debug port*/
 309         for (i = 0; i < ARMV7M_PRIMASK; i++)
 310         {
 311                 if (!armv7m->core_cache->reg_list[i].valid)
 312                         armv7m->read_core_reg(target, i);
 313         }
 314
 315         xPSR = buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32);
 316
 317         /* For IT instructions xPSR must be reloaded on resume and clear on debug exec */
 318         if (xPSR & 0xf00)
 319         {
 320                 armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty = armv7m->core_cache->reg_list[ARMV7M_xPSR].valid;
 321                 cortex_m3_store_core_reg_u32(target, ARMV7M_REGISTER_CORE_GP, 16, xPSR &~ 0xff);
 322         }
 323
 324         /* Now we can load SP core registers */
 325         for (i = ARMV7M_PRIMASK; i < ARMV7NUMCOREREGS; i++)
 326         {
 327                 if (!armv7m->core_cache->reg_list[i].valid)
 328                         armv7m->read_core_reg(target, i);
 329         }
 330
 331         /* Are we in an exception handler */
 332         if (xPSR & 0x1FF)
 333         {
 334                 armv7m->core_mode = ARMV7M_MODE_HANDLER;
 335                 armv7m->exception_number = (xPSR & 0x1FF);
 336         }
 337         else
 338         {
 339                 armv7m->core_mode = buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_CONTROL].value, 0, 1);
 340                 armv7m->exception_number = 0;
 341         }
 342
 343         if (armv7m->exception_number)
 344         {
 345                 cortex_m3_examine_exception_reason(target);
 346         }
 347
 348         LOG_DEBUG("entered debug state in core mode: %s at PC 0x%x, target->state: %s", armv7m_mode_strings[armv7m->core_mode], \
 349                 *(u32*)(armv7m->core_cache->reg_list[15].value), target_state_strings[target->state]);
 350
 351         if (armv7m->post_debug_entry)
 352                 armv7m->post_debug_entry(target);
 353
 354         return ERROR_OK;
 355 }
 356
 357 int cortex_m3_poll(target_t *target)
 358 {
 359         int retval;
 360         u32 prev_target_state = target->state;
 361
 362         /* get pointers to arch-specific information */
 363         armv7m_common_t *armv7m = target->arch_info;
 364         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 365         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 366
 367         /* Read from Debug Halting Control and Status Register */
 368         retval = ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 369         if (retval != ERROR_OK)
 370         {
 371                 target->state = TARGET_UNKNOWN;
 372                 return retval;
 373         }
 374
 375         if (cortex_m3->dcb_dhcsr & S_RESET_ST)
 376         {
 377                 /* check if still in reset */
 378                 ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 379
 380                 if (cortex_m3->dcb_dhcsr & S_RESET_ST)
 381                 {
 382                         target->state = TARGET_RESET;
 383                         return ERROR_OK;
 384                 }
 385         }
 386
 387         if (target->state == TARGET_RESET)
 388         {
 389                 /* Cannot switch context while running so endreset is called with target->state == TARGET_RESET */
 390                 LOG_DEBUG("Exit from reset with dcb_dhcsr 0x%x", cortex_m3->dcb_dhcsr);
 391                 cortex_m3_endreset_event(target);
 392                 target->state = TARGET_RUNNING;
 393                 prev_target_state = TARGET_RUNNING;
 394         }
 395
 396         if (cortex_m3->dcb_dhcsr & S_HALT)
 397         {
 398                 target->state = TARGET_HALTED;
 399
 400                 if ((prev_target_state == TARGET_RUNNING) || (prev_target_state == TARGET_RESET))
 401                 {
 402                         if ((retval = cortex_m3_debug_entry(target)) != ERROR_OK)
 403                                 return retval;
 404
 405                         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 406                 }
 407                 if (prev_target_state == TARGET_DEBUG_RUNNING)
 408                 {
 409                         LOG_DEBUG(" ");
 410                         if ((retval = cortex_m3_debug_entry(target)) != ERROR_OK)
 411                                 return retval;
 412
 413                         target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 414                 }
 415         }
 416
 417         /*
 418         if (cortex_m3->dcb_dhcsr & S_SLEEP)
 419                 target->state = TARGET_SLEEP;
 420         */
 421
 422 #if 0
 423     /* Read Debug Fault Status Register, added to figure out the lockup when running flashtest.script  */
 424         ahbap_read_system_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
 425         LOG_DEBUG("dcb_dhcsr 0x%x, nvic_dfsr 0x%x, target->state: %s", cortex_m3->dcb_dhcsr, cortex_m3->nvic_dfsr, target_state_strings[target->state]);
 426 #endif
 427
 428         return ERROR_OK;
 429 }
 430
 431 int cortex_m3_halt(target_t *target)
 432 {
 433         /* get pointers to arch-specific information */
 434         armv7m_common_t *armv7m = target->arch_info;
 435         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 436         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 437
 438         LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
 439
 440         if (target->state == TARGET_HALTED)
 441         {
 442                 LOG_DEBUG("target was already halted");
 443                 return ERROR_OK;
 444         }
 445
 446         if (target->state == TARGET_UNKNOWN)
 447         {
 448                 LOG_WARNING("target was in unknown state when halt was requested");
 449         }
 450
 451         if (target->state == TARGET_RESET)
 452         {
 453                 if ((jtag_reset_config & RESET_SRST_PULLS_TRST) && jtag_srst)
 454                 {
 455                         LOG_ERROR("can't request a halt while in reset if nSRST pulls nTRST");
 456                         return ERROR_TARGET_FAILURE;
 457                 }
 458                 else
 459                 {
 460                         /* we came here in a reset_halt or reset_init sequence
 461                          * debug entry was already prepared in cortex_m3_prepare_reset_halt()
 462                          */
 463                         target->debug_reason = DBG_REASON_DBGRQ;
 464
 465                         return ERROR_OK;
 466                 }
 467         }
 468
 469         /* Write to Debug Halting Control and Status Register */
 470         ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN | C_HALT );
 471
 472         target->debug_reason = DBG_REASON_DBGRQ;
 473
 474         return ERROR_OK;
 475 }
 476
 477 int cortex_m3_soft_reset_halt(struct target_s *target)
 478 {
 479         /* get pointers to arch-specific information */
 480         armv7m_common_t *armv7m = target->arch_info;
 481         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 482         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 483         u32 dcb_dhcsr = 0;
 484         int retval, timeout = 0;
 485
 486         /* Enter debug state on reset, cf. end_reset_event() */
 487         ahbap_write_system_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET );
 488
 489         /* Request a reset */
 490         ahbap_write_system_atomic_u32(swjdp, NVIC_AIRCR, AIRCR_VECTKEY | AIRCR_VECTRESET );
 491         target->state = TARGET_RESET;
 492
 493         /* registers are now invalid */
 494         armv7m_invalidate_core_regs(target);
 495
 496         while (timeout < 100)
 497         {
 498                 retval = ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
 499                 if (retval == ERROR_OK)
 500                 {
 501                     ahbap_read_system_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
 502                         if ((dcb_dhcsr & S_HALT) && (cortex_m3->nvic_dfsr & DFSR_VCATCH))
 503                         {
 504                                 LOG_DEBUG("system reset-halted, dcb_dhcsr 0x%x, nvic_dfsr 0x%x", dcb_dhcsr, cortex_m3->nvic_dfsr);
 505                                 cortex_m3_poll(target);
 506                                 return ERROR_OK;
 507                         }
 508                         else
 509                                 LOG_DEBUG("waiting for system reset-halt, dcb_dhcsr 0x%x, %i ms", dcb_dhcsr, timeout);
 510                 }
 511                 timeout++;
 512                 usleep(1000);
 513         }
 514
 515         return ERROR_OK;
 516 }
 517
 518 int cortex_m3_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
 519 {
 520         /* get pointers to arch-specific information */
 521         armv7m_common_t *armv7m = target->arch_info;
 522         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 523         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 524         breakpoint_t *breakpoint = NULL;
 525         u32 dcb_dhcsr, resume_pc;
 526
 527         if (target->state != TARGET_HALTED)
 528         {
 529                 LOG_WARNING("target not halted");
 530                 return ERROR_TARGET_NOT_HALTED;
 531         }
 532
 533         if (!debug_execution)
 534         {
 535                 target_free_all_working_areas(target);
 536                 cortex_m3_enable_breakpoints(target);
 537                 cortex_m3_enable_watchpoints(target);
 538
 539                 /* TODOLATER Interrupt handling/disable for debug execution, cache ... ... */
 540         }
 541
 542         dcb_dhcsr = DBGKEY | C_DEBUGEN;
 543         if (debug_execution)
 544         {
 545                 /* Disable interrupts */
 546                 /*
 547                    We disable interrupts in the PRIMASK register instead of masking with C_MASKINTS,
 548                    This is probably the same inssue as Cortex-M3 Errata 377493:
 549                    C_MASKINTS in parallel with disabled interrupts can cause local faults to not be taken.
 550                 */
 551                 buf_set_u32(armv7m->core_cache->reg_list[ARMV7M_PRIMASK].value, 0, 32, 1);
 552                 /* Make sure we are in Thumb mode */
 553                 buf_set_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32,
 554                         buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32) | (1<<24));
 555         }
 556
 557         /* current = 1: continue on current pc, otherwise continue at <address> */
 558         if (!current)
 559         {
 560                 buf_set_u32(armv7m->core_cache->reg_list[15].value, 0, 32, address);
 561                 armv7m->core_cache->reg_list[15].dirty = 1;
 562                 armv7m->core_cache->reg_list[15].valid = 1;
 563         }
 564
 565         resume_pc = buf_get_u32(armv7m->core_cache->reg_list[15].value, 0, 32);
 566
 567         armv7m_restore_context(target);
 568
 569         /* the front-end may request us not to handle breakpoints */
 570         if (handle_breakpoints)
 571         {
 572                 /* Single step past breakpoint at current address */
 573                 if ((breakpoint = breakpoint_find(target, resume_pc)))
 574                 {
 575                         LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
 576                         cortex_m3_unset_breakpoint(target, breakpoint);
 577                         cortex_m3_single_step_core(target);
 578                         cortex_m3_set_breakpoint(target, breakpoint);
 579                 }
 580         }
 581
 582         /* Set/Clear C_MASKINTS in a separate operation */
 583         if ((cortex_m3->dcb_dhcsr & C_MASKINTS) != (dcb_dhcsr & C_MASKINTS))
 584                 ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, dcb_dhcsr | C_HALT );
 585
 586         /* Restart core */
 587         ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, dcb_dhcsr );
 588         target->debug_reason = DBG_REASON_NOTHALTED;
 589
 590         /* registers are now invalid */
 591         armv7m_invalidate_core_regs(target);
 592         if (!debug_execution)
 593         {
 594                 target->state = TARGET_RUNNING;
 595                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 596                 LOG_DEBUG("target resumed at 0x%x",resume_pc);
 597         }
 598         else
 599         {
 600                 target->state = TARGET_DEBUG_RUNNING;
 601                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
 602                 LOG_DEBUG("target debug resumed at 0x%x",resume_pc);
 603         }
 604
 605         return ERROR_OK;
 606 }
 607
 608 /* int irqstepcount=0; */
 609 int cortex_m3_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
 610 {
 611         /* get pointers to arch-specific information */
 612         armv7m_common_t *armv7m = target->arch_info;
 613         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 614         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 615         breakpoint_t *breakpoint = NULL;
 616
 617         if (target->state != TARGET_HALTED)
 618         {
 619                 LOG_WARNING("target not halted");
 620                 return ERROR_TARGET_NOT_HALTED;
 621         }
 622
 623         /* current = 1: continue on current pc, otherwise continue at <address> */
 624         if (!current)
 625                 buf_set_u32(armv7m->core_cache->reg_list[15].value, 0, 32, address);
 626
 627         /* the front-end may request us not to handle breakpoints */
 628         if (handle_breakpoints)
 629                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv7m->core_cache->reg_list[15].value, 0, 32))))
 630                         cortex_m3_unset_breakpoint(target, breakpoint);
 631
 632         target->debug_reason = DBG_REASON_SINGLESTEP;
 633
 634         armv7m_restore_context(target);
 635
 636         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 637
 638         if (cortex_m3->dcb_dhcsr & C_MASKINTS)
 639                 ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN );
 640         ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, DBGKEY| C_STEP | C_DEBUGEN);
 641         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 642
 643         /* registers are now invalid */
 644         armv7m_invalidate_core_regs(target);
 645
 646         if (breakpoint)
 647                 cortex_m3_set_breakpoint(target, breakpoint);
 648
 649         LOG_DEBUG("target stepped dcb_dhcsr = 0x%x nvic_icsr = 0x%x", cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr);
 650
 651         cortex_m3_debug_entry(target);
 652         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 653
 654         LOG_DEBUG("target stepped dcb_dhcsr = 0x%x nvic_icsr = 0x%x", cortex_m3->dcb_dhcsr, cortex_m3->nvic_icsr);
 655         return ERROR_OK;
 656 }
 657
 658 int cortex_m3_assert_reset(target_t *target)
 659 {
 660         armv7m_common_t *armv7m = target->arch_info;
 661         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 662         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 663
 664         LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
 665
 666         if (!(jtag_reset_config & RESET_HAS_SRST))
 667         {
 668                 LOG_ERROR("Can't assert SRST");
 669                 return ERROR_FAIL;
 670         }
 671
 672         /* Enable debug requests */
 673         ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 674         if (!(cortex_m3->dcb_dhcsr & C_DEBUGEN))
 675                 ahbap_write_system_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN );
 676
 677         ahbap_write_system_u32(swjdp, DCB_DCRDR, 0 );
 678
 679         if (target->reset_mode == RESET_RUN)
 680         {
 681                 /* Set/Clear C_MASKINTS in a separate operation */
 682                 if (cortex_m3->dcb_dhcsr & C_MASKINTS)
 683                         ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN | C_HALT );
 684
 685                 cortex_m3_clear_halt(target);
 686
 687                 /* Enter debug state on reset, cf. end_reset_event() */
 688                 ahbap_write_system_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR);
 689         }
 690         else
 691         {
 692                 /* Enter debug state on reset, cf. end_reset_event() */
 693                 ahbap_write_system_atomic_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET );
 694         }
 695
 696         if (jtag_reset_config & RESET_SRST_PULLS_TRST)
 697         {
 698                 jtag_add_reset(1, 1);
 699         }
 700         else
 701         {
 702                 jtag_add_reset(0, 1);
 703         }
 704
 705         target->state = TARGET_RESET;
 706         jtag_add_sleep(50000);
 707
 708         armv7m_invalidate_core_regs(target);
 709
 710         return ERROR_OK;
 711 }
 712
 713 int cortex_m3_deassert_reset(target_t *target)
 714 {
 715         LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
 716
 717         /* deassert reset lines */
 718         jtag_add_reset(0, 0);
 719
 720         return ERROR_OK;
 721 }
 722
 723 void cortex_m3_enable_breakpoints(struct target_s *target)
 724 {
 725         breakpoint_t *breakpoint = target->breakpoints;
 726
 727         /* set any pending breakpoints */
 728         while (breakpoint)
 729         {
 730                 if (breakpoint->set == 0)
 731                         cortex_m3_set_breakpoint(target, breakpoint);
 732                 breakpoint = breakpoint->next;
 733         }
 734 }
 735
 736 int cortex_m3_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 737 {
 738         int fp_num=0;
 739         u32 hilo;
 740
 741         /* get pointers to arch-specific information */
 742         armv7m_common_t *armv7m = target->arch_info;
 743         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 744
 745         cortex_m3_fp_comparator_t * comparator_list = cortex_m3->fp_comparator_list;
 746
 747         if (breakpoint->set)
 748         {
 749                 LOG_WARNING("breakpoint already set");
 750                 return ERROR_OK;
 751         }
 752
 753         if (cortex_m3->auto_bp_type)
 754         {
 755                 breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
 756         }
 757
 758         if (breakpoint->type == BKPT_HARD)
 759         {
 760                 while(comparator_list[fp_num].used && (fp_num < cortex_m3->fp_num_code))
 761                         fp_num++;
 762                 if (fp_num >= cortex_m3->fp_num_code)
 763                 {
 764                         LOG_DEBUG("ERROR Can not find free FP Comparator");
 765                         LOG_WARNING("ERROR Can not find free FP Comparator");
 766                         exit(-1);
 767                 }
 768                 breakpoint->set = fp_num + 1;
 769                 hilo = (breakpoint->address & 0x2) ? FPCR_REPLACE_BKPT_HIGH : FPCR_REPLACE_BKPT_LOW;
 770                 comparator_list[fp_num].used = 1;
 771                 comparator_list[fp_num].fpcr_value = (breakpoint->address & 0x1FFFFFFC) | hilo | 1;
 772                 target_write_u32(target, comparator_list[fp_num].fpcr_address, comparator_list[fp_num].fpcr_value);
 773                 LOG_DEBUG("fpc_num %i fpcr_value 0x%x", fp_num, comparator_list[fp_num].fpcr_value);
 774         }
 775         else if (breakpoint->type == BKPT_SOFT)
 776         {
 777                 u8 code[4];
 778                 buf_set_u32(code, 0, 32, ARMV7M_T_BKPT(0x11));
 779                 target->type->read_memory(target, breakpoint->address & 0xFFFFFFFE, breakpoint->length, 1, breakpoint->orig_instr);
 780                 target->type->write_memory(target, breakpoint->address & 0xFFFFFFFE, breakpoint->length, 1, code);
 781                 breakpoint->set = 0x11; /* Any nice value but 0 */
 782         }
 783
 784         return ERROR_OK;
 785 }
 786
 787 int cortex_m3_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 788 {
 789         /* get pointers to arch-specific information */
 790         armv7m_common_t *armv7m = target->arch_info;
 791         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 792         cortex_m3_fp_comparator_t * comparator_list = cortex_m3->fp_comparator_list;
 793
 794         if (!breakpoint->set)
 795         {
 796                 LOG_WARNING("breakpoint not set");
 797                 return ERROR_OK;
 798         }
 799
 800         if (breakpoint->type == BKPT_HARD)
 801         {
 802                 int fp_num = breakpoint->set - 1;
 803                 if ((fp_num < 0) || (fp_num >= cortex_m3->fp_num_code))
 804                 {
 805                         LOG_DEBUG("Invalid FP Comparator number in breakpoint");
 806                         return ERROR_OK;
 807                 }
 808                 comparator_list[fp_num].used = 0;
 809                 comparator_list[fp_num].fpcr_value = 0;
 810                 target_write_u32(target, comparator_list[fp_num].fpcr_address, comparator_list[fp_num].fpcr_value);
 811         }
 812         else
 813         {
 814                 /* restore original instruction (kept in target endianness) */
 815                 if (breakpoint->length == 4)
 816                 {
 817                         target->type->write_memory(target, breakpoint->address & 0xFFFFFFFE, 4, 1, breakpoint->orig_instr);
 818                 }
 819                 else
 820                 {
 821                         target->type->write_memory(target, breakpoint->address & 0xFFFFFFFE, 2, 1, breakpoint->orig_instr);
 822                 }
 823         }
 824         breakpoint->set = 0;
 825
 826         return ERROR_OK;
 827 }
 828
 829 int cortex_m3_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 830 {
 831         /* get pointers to arch-specific information */
 832         armv7m_common_t *armv7m = target->arch_info;
 833         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 834
 835         if (cortex_m3->auto_bp_type)
 836         {
 837                 breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
 838         }
 839
 840         if ((breakpoint->type == BKPT_HARD) && (breakpoint->address >= 0x20000000))
 841         {
 842                 LOG_INFO("flash patch comparator requested outside code memory region");
 843                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 844         }
 845
 846         if ((breakpoint->type == BKPT_SOFT) && (breakpoint->address < 0x20000000))
 847         {
 848                 LOG_INFO("soft breakpoint requested in code (flash) memory region");
 849                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 850         }
 851
 852         if ((breakpoint->type == BKPT_HARD) && (cortex_m3->fp_code_available < 1))
 853         {
 854                 LOG_INFO("no flash patch comparator unit available for hardware breakpoint");
 855                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 856         }
 857
 858         if ((breakpoint->length != 2))
 859         {
 860                 LOG_INFO("only breakpoints of two bytes length supported");
 861                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 862         }
 863
 864         if (breakpoint->type == BKPT_HARD)
 865                 cortex_m3->fp_code_available--;
 866         cortex_m3_set_breakpoint(target, breakpoint);
 867
 868         return ERROR_OK;
 869 }
 870
 871 int cortex_m3_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 872 {
 873         /* get pointers to arch-specific information */
 874         armv7m_common_t *armv7m = target->arch_info;
 875         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 876
 877         if (target->state != TARGET_HALTED)
 878         {
 879                 LOG_WARNING("target not halted");
 880                 return ERROR_TARGET_NOT_HALTED;
 881         }
 882
 883         if (cortex_m3->auto_bp_type)
 884         {
 885                 breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
 886         }
 887
 888         if (breakpoint->set)
 889         {
 890                 cortex_m3_unset_breakpoint(target, breakpoint);
 891         }
 892
 893         if (breakpoint->type == BKPT_HARD)
 894                 cortex_m3->fp_code_available++;
 895
 896         return ERROR_OK;
 897 }
 898
 899 int cortex_m3_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 900 {
 901         int dwt_num=0;
 902         u32 mask, temp;
 903
 904         /* get pointers to arch-specific information */
 905         armv7m_common_t *armv7m = target->arch_info;
 906         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 907         cortex_m3_dwt_comparator_t * comparator_list = cortex_m3->dwt_comparator_list;
 908
 909         if (watchpoint->set)
 910         {
 911                 LOG_WARNING("watchpoint already set");
 912                 return ERROR_OK;
 913         }
 914
 915         if (watchpoint->mask == 0xffffffffu)
 916         {
 917                 while(comparator_list[dwt_num].used && (dwt_num < cortex_m3->dwt_num_comp))
 918                         dwt_num++;
 919                 if (dwt_num >= cortex_m3->dwt_num_comp)
 920                 {
 921                         LOG_DEBUG("ERROR Can not find free DWT Comparator");
 922                         LOG_WARNING("ERROR Can not find free DWT Comparator");
 923                         return -1;
 924                 }
 925                 watchpoint->set = dwt_num + 1;
 926                 mask = 0;
 927                 temp = watchpoint->length;
 928                 while (temp > 1)
 929                 {
 930                         temp = temp / 2;
 931                         mask++;
 932                 }
 933                 comparator_list[dwt_num].used = 1;
 934                 comparator_list[dwt_num].comp = watchpoint->address;
 935                 comparator_list[dwt_num].mask = mask;
 936                 comparator_list[dwt_num].function = watchpoint->rw + 5;
 937                 target_write_u32(target, comparator_list[dwt_num].dwt_comparator_address, comparator_list[dwt_num].comp);
 938                 target_write_u32(target, comparator_list[dwt_num].dwt_comparator_address|0x4, comparator_list[dwt_num].mask);
 939                 target_write_u32(target, comparator_list[dwt_num].dwt_comparator_address|0x8, comparator_list[dwt_num].function);
 940                 LOG_DEBUG("dwt_num %i 0x%x 0x%x 0x%x", dwt_num, comparator_list[dwt_num].comp, comparator_list[dwt_num].mask, comparator_list[dwt_num].function);
 941         }
 942         else
 943         {
 944                 LOG_WARNING("Cannot watch data values");  /* Move this test to add_watchpoint */
 945                 return ERROR_OK;
 946         }
 947
 948         return ERROR_OK;
 949
 950 }
 951
 952 int cortex_m3_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 953 {
 954         /* get pointers to arch-specific information */
 955         armv7m_common_t *armv7m = target->arch_info;
 956         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 957         cortex_m3_dwt_comparator_t * comparator_list = cortex_m3->dwt_comparator_list;
 958         int dwt_num;
 959
 960         if (!watchpoint->set)
 961         {
 962                 LOG_WARNING("watchpoint not set");
 963                 return ERROR_OK;
 964         }
 965
 966         dwt_num = watchpoint->set - 1;
 967
 968         if ((dwt_num < 0) || (dwt_num >= cortex_m3->dwt_num_comp))
 969         {
 970                 LOG_DEBUG("Invalid DWT Comparator number in watchpoint");
 971                 return ERROR_OK;
 972         }
 973         comparator_list[dwt_num].used = 0;
 974         comparator_list[dwt_num].function = 0;
 975         target_write_u32(target, comparator_list[dwt_num].dwt_comparator_address|0x8, comparator_list[dwt_num].function);
 976
 977         watchpoint->set = 0;
 978
 979         return ERROR_OK;
 980 }
 981
 982 int cortex_m3_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 983 {
 984         /* get pointers to arch-specific information */
 985         armv7m_common_t *armv7m = target->arch_info;
 986         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
 987
 988         if (target->state != TARGET_HALTED)
 989         {
 990                 LOG_WARNING("target not halted");
 991                 return ERROR_TARGET_NOT_HALTED;
 992         }
 993
 994         if (cortex_m3->dwt_comp_available < 1)
 995         {
 996                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 997         }
 998
 999         if ((watchpoint->length != 1) && (watchpoint->length != 2) && (watchpoint->length != 4))
1000         {
1001                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1002         }
1003
1004         cortex_m3->dwt_comp_available--;
1005
1006         return ERROR_OK;
1007 }
1008
1009 int cortex_m3_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
1010 {
1011         /* get pointers to arch-specific information */
1012         armv7m_common_t *armv7m = target->arch_info;
1013         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1014
1015         if (target->state != TARGET_HALTED)
1016         {
1017                 LOG_WARNING("target not halted");
1018                 return ERROR_TARGET_NOT_HALTED;
1019         }
1020
1021         if (watchpoint->set)
1022         {
1023                 cortex_m3_unset_watchpoint(target, watchpoint);
1024         }
1025
1026         cortex_m3->dwt_comp_available++;
1027
1028         return ERROR_OK;
1029 }
1030
1031 void cortex_m3_enable_watchpoints(struct target_s *target)
1032 {
1033         watchpoint_t *watchpoint = target->watchpoints;
1034
1035         /* set any pending watchpoints */
1036         while (watchpoint)
1037         {
1038                 if (watchpoint->set == 0)
1039                         cortex_m3_set_watchpoint(target, watchpoint);
1040                 watchpoint = watchpoint->next;
1041         }
1042 }
1043
1044 int cortex_m3_load_core_reg_u32(struct target_s *target, enum armv7m_regtype type, u32 num, u32 * value)
1045 {
1046         int retval;
1047         /* get pointers to arch-specific information */
1048         armv7m_common_t *armv7m = target->arch_info;
1049         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1050         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
1051
1052         if ((type == ARMV7M_REGISTER_CORE_GP) && (num <= ARMV7M_PSP))
1053         {
1054                 /* read a normal core register */
1055                 retval = ahbap_read_coreregister_u32(swjdp, value, num);
1056
1057                 if (retval != ERROR_OK)
1058                 {
1059                         LOG_ERROR("JTAG failure %i",retval);
1060                         return ERROR_JTAG_DEVICE_ERROR;
1061                 }
1062                 LOG_DEBUG("load from core reg %i  value 0x%x",num,*value);
1063         }
1064         else if (type == ARMV7M_REGISTER_CORE_SP) /* Special purpose core register */
1065         {
1066                 /* read other registers */
1067                 ahbap_read_coreregister_u32(swjdp, value, 20);
1068
1069                 switch (num)
1070                 {
1071                         case 19:
1072                                 *value = buf_get_u32((u8*)value, 0, 8);
1073                                 break;
1074
1075                         case 20:
1076                                 *value = buf_get_u32((u8*)value, 8, 8);
1077                                 break;
1078
1079                         case 21:
1080                                 *value = buf_get_u32((u8*)value, 16, 8);
1081                                 break;
1082
1083                         case 22:
1084                                 *value = buf_get_u32((u8*)value, 24, 8);
1085                                 break;
1086                 }
1087
1088                 LOG_DEBUG("load from special reg %i value 0x%x", num, *value);
1089         }
1090         else
1091         {
1092                 return ERROR_INVALID_ARGUMENTS;
1093         }
1094
1095         return ERROR_OK;
1096 }
1097
1098 int cortex_m3_store_core_reg_u32(struct target_s *target, enum armv7m_regtype type, u32 num, u32 value)
1099 {
1100         int retval;
1101         u32 reg;
1102
1103         /* get pointers to arch-specific information */
1104         armv7m_common_t *armv7m = target->arch_info;
1105         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1106         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
1107
1108         if ((type == ARMV7M_REGISTER_CORE_GP) && (num <= ARMV7M_PSP))
1109         {
1110                 retval = ahbap_write_coreregister_u32(swjdp, value, num);
1111                 if (retval != ERROR_OK)
1112                 {
1113                         LOG_ERROR("JTAG failure %i", retval);
1114                         armv7m->core_cache->reg_list[num].dirty = armv7m->core_cache->reg_list[num].valid;
1115                         return ERROR_JTAG_DEVICE_ERROR;
1116                 }
1117                 LOG_DEBUG("write core reg %i value 0x%x", num, value);
1118         }
1119         else if (type == ARMV7M_REGISTER_CORE_SP) /* Special purpose core register */
1120         {
1121                 /* write other registers */
1122
1123                 ahbap_read_coreregister_u32(swjdp, &reg, 20);
1124
1125                 switch (num)
1126                 {
1127                         case 19:
1128                                 buf_set_u32((u8*)&reg, 0, 8, value);
1129                                 break;
1130
1131                         case 20:
1132                                 buf_set_u32((u8*)&reg, 8, 8, value);
1133                                 break;
1134
1135                         case 21:
1136                                 buf_set_u32((u8*)&reg, 16, 8, value);
1137                                 break;
1138
1139                         case 22:
1140                                 buf_set_u32((u8*)&reg, 24, 8, value);
1141                                 break;
1142                 }
1143
1144                 ahbap_write_coreregister_u32(swjdp, reg, 20);
1145
1146                 LOG_DEBUG("write special reg %i value 0x%x ", num, value);
1147         }
1148         else
1149         {
1150                 return ERROR_INVALID_ARGUMENTS;
1151         }
1152
1153         return ERROR_OK;
1154 }
1155
1156 int cortex_m3_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1157 {
1158         /* get pointers to arch-specific information */
1159         armv7m_common_t *armv7m = target->arch_info;
1160         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1161         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
1162         int retval;
1163
1164         /* sanitize arguments */
1165         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1166                 return ERROR_INVALID_ARGUMENTS;
1167
1168         /* cortex_m3 handles unaligned memory access */
1169
1170         switch (size)
1171         {
1172                 case 4:
1173                         retval = ahbap_read_buf_u32(swjdp, buffer, 4 * count, address);
1174                         break;
1175                 case 2:
1176                         retval = ahbap_read_buf_u16(swjdp, buffer, 2 * count, address);
1177                         break;
1178                 case 1:
1179                         retval = ahbap_read_buf_u8(swjdp, buffer, count, address);
1180                         break;
1181                 default:
1182                         LOG_ERROR("BUG: we shouldn't get here");
1183                         exit(-1);
1184         }
1185
1186         return retval;
1187 }
1188
1189 int cortex_m3_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1190 {
1191         /* get pointers to arch-specific information */
1192         armv7m_common_t *armv7m = target->arch_info;
1193         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1194         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
1195         int retval;
1196
1197         /* sanitize arguments */
1198         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1199                 return ERROR_INVALID_ARGUMENTS;
1200
1201         switch (size)
1202         {
1203                 case 4:
1204                         retval = ahbap_write_buf_u32(swjdp, buffer, 4 * count, address);
1205                         break;
1206                 case 2:
1207                         retval = ahbap_write_buf_u16(swjdp, buffer, 2 * count, address);
1208                         break;
1209                 case 1:
1210                         retval = ahbap_write_buf_u8(swjdp, buffer, count, address);
1211                         break;
1212                 default:
1213                         LOG_ERROR("BUG: we shouldn't get here");
1214                         exit(-1);
1215         }
1216
1217         return retval;
1218 }
1219
1220 int cortex_m3_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)
1221 {
1222         return cortex_m3_write_memory(target, address, 4, count, buffer);
1223 }
1224
1225 void cortex_m3_build_reg_cache(target_t *target)
1226 {
1227         armv7m_build_reg_cache(target);
1228 }
1229
1230 int cortex_m3_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
1231 {
1232         cortex_m3_build_reg_cache(target);
1233         return ERROR_OK;
1234 }
1235
1236 int cortex_m3_examine(struct command_context_s *cmd_ctx, struct target_s *target)
1237 {
1238         int retval;
1239         u32 cpuid, fpcr, dwtcr, ictr;
1240         int i;
1241
1242         /* get pointers to arch-specific information */
1243         armv7m_common_t *armv7m = target->arch_info;
1244         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1245         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
1246
1247         target->type->examined = 1;
1248
1249         if ((retval=ahbap_debugport_init(swjdp))!=ERROR_OK)
1250                 return retval;
1251
1252         /* Read from Device Identification Registers */
1253         if ((retval=target_read_u32(target, CPUID, &cpuid))!=ERROR_OK)
1254                 return retval;
1255
1256         if (((cpuid >> 4) & 0xc3f) == 0xc23)
1257                 LOG_DEBUG("CORTEX-M3 processor detected");
1258         LOG_DEBUG("cpuid: 0x%8.8x", cpuid);
1259
1260         target_read_u32(target, NVIC_ICTR, &ictr);
1261         cortex_m3->intlinesnum = (ictr & 0x1F) + 1;
1262         cortex_m3->intsetenable = calloc(cortex_m3->intlinesnum, 4);
1263         for (i = 0; i < cortex_m3->intlinesnum; i++)
1264         {
1265                 target_read_u32(target, NVIC_ISE0 + 4 * i, cortex_m3->intsetenable + i);
1266                 LOG_DEBUG("interrupt enable[%i] = 0x%8.8x", i, cortex_m3->intsetenable[i]);
1267         }
1268
1269         /* Setup FPB */
1270         target_read_u32(target, FP_CTRL, &fpcr);
1271         cortex_m3->auto_bp_type = 1;
1272         cortex_m3->fp_num_code = (fpcr >> 4) & 0xF;
1273         cortex_m3->fp_num_lit = (fpcr >> 8) & 0xF;
1274         cortex_m3->fp_code_available = cortex_m3->fp_num_code;
1275         cortex_m3->fp_comparator_list = calloc(cortex_m3->fp_num_code + cortex_m3->fp_num_lit, sizeof(cortex_m3_fp_comparator_t));
1276         for (i = 0; i < cortex_m3->fp_num_code + cortex_m3->fp_num_lit; i++)
1277         {
1278                 cortex_m3->fp_comparator_list[i].type = (i < cortex_m3->fp_num_code) ? FPCR_CODE : FPCR_LITERAL;
1279                 cortex_m3->fp_comparator_list[i].fpcr_address = FP_COMP0 + 4 * i;
1280         }
1281         LOG_DEBUG("FPB fpcr 0x%x, numcode %i, numlit %i", fpcr, cortex_m3->fp_num_code, cortex_m3->fp_num_lit);
1282
1283         /* Setup DWT */
1284         target_read_u32(target, DWT_CTRL, &dwtcr);
1285         cortex_m3->dwt_num_comp = (dwtcr >> 28) & 0xF;
1286         cortex_m3->dwt_comp_available = cortex_m3->dwt_num_comp;
1287         cortex_m3->dwt_comparator_list=calloc(cortex_m3->dwt_num_comp, sizeof(cortex_m3_dwt_comparator_t));
1288         for (i = 0; i < cortex_m3->dwt_num_comp; i++)
1289         {
1290                 cortex_m3->dwt_comparator_list[i].dwt_comparator_address = DWT_COMP0 + 0x10 * i;
1291         }
1292
1293         return ERROR_OK;
1294 }
1295
1296
1297 int cortex_m3_quit()
1298 {
1299
1300         return ERROR_OK;
1301 }
1302
1303 int cortex_m3_dcc_read(swjdp_common_t *swjdp, u8 *value, u8 *ctrl)
1304 {
1305         u16 dcrdr;
1306
1307         ahbap_read_buf_u16( swjdp, (u8*)&dcrdr, 1, DCB_DCRDR);
1308         *ctrl = (u8)dcrdr;
1309         *value = (u8)(dcrdr >> 8);
1310
1311         LOG_DEBUG("data 0x%x ctrl 0x%x", *value, *ctrl);
1312
1313         /* write ack back to software dcc register
1314          * signify we have read data */
1315         if (dcrdr & (1 << 0))
1316         {
1317                 dcrdr = 0;
1318                 ahbap_write_buf_u16( swjdp, (u8*)&dcrdr, 1, DCB_DCRDR);
1319         }
1320
1321         return ERROR_OK;
1322 }
1323
1324 int cortex_m3_target_request_data(target_t *target, u32 size, u8 *buffer)
1325 {
1326         armv7m_common_t *armv7m = target->arch_info;
1327         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1328         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
1329         u8 data;
1330         u8 ctrl;
1331         int i;
1332
1333         for (i = 0; i < (size * 4); i++)
1334         {
1335                 cortex_m3_dcc_read(swjdp, &data, &ctrl);
1336                 buffer[i] = data;
1337         }
1338
1339         return ERROR_OK;
1340 }
1341
1342 int cortex_m3_handle_target_request(void *priv)
1343 {
1344         target_t *target = priv;
1345         if (!target->type->examined)
1346                 return ERROR_OK;
1347         armv7m_common_t *armv7m = target->arch_info;
1348         cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
1349         swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
1350
1351         if (!target->dbg_msg_enabled)
1352                 return ERROR_OK;
1353
1354         if (target->state == TARGET_RUNNING)
1355         {
1356                 u8 data;
1357                 u8 ctrl;
1358
1359                 cortex_m3_dcc_read(swjdp, &data, &ctrl);
1360
1361                 /* check if we have data */
1362                 if (ctrl & (1 << 0))
1363                 {
1364                         u32 request;
1365
1366                         /* we assume target is quick enough */
1367                         request = data;
1368                         cortex_m3_dcc_read(swjdp, &data, &ctrl);
1369                         request |= (data << 8);
1370                         cortex_m3_dcc_read(swjdp, &data, &ctrl);
1371                         request |= (data << 16);
1372                         cortex_m3_dcc_read(swjdp, &data, &ctrl);
1373                         request |= (data << 24);
1374                         target_request(target, request);
1375                 }
1376         }
1377
1378         return ERROR_OK;
1379 }
1380
1381 int cortex_m3_init_arch_info(target_t *target, cortex_m3_common_t *cortex_m3, int chain_pos, char *variant)
1382 {
1383         armv7m_common_t *armv7m;
1384         armv7m = &cortex_m3->armv7m;
1385
1386         /* prepare JTAG information for the new target */
1387         cortex_m3->jtag_info.chain_pos = chain_pos;
1388         cortex_m3->jtag_info.scann_size = 4;
1389
1390         cortex_m3->swjdp_info.dp_select_value = -1;
1391         cortex_m3->swjdp_info.ap_csw_value = -1;
1392         cortex_m3->swjdp_info.ap_tar_value = -1;
1393         cortex_m3->swjdp_info.jtag_info = &cortex_m3->jtag_info;
1394
1395         /* initialize arch-specific breakpoint handling */
1396
1397         cortex_m3->common_magic = CORTEX_M3_COMMON_MAGIC;
1398         cortex_m3->arch_info = NULL;
1399
1400         /* register arch-specific functions */
1401         armv7m->examine_debug_reason = cortex_m3_examine_debug_reason;
1402
1403         armv7m->pre_debug_entry = NULL;
1404         armv7m->post_debug_entry = NULL;
1405
1406         armv7m->pre_restore_context = NULL;
1407         armv7m->post_restore_context = NULL;
1408
1409         armv7m_init_arch_info(target, armv7m);
1410         armv7m->arch_info = cortex_m3;
1411         armv7m->load_core_reg_u32 = cortex_m3_load_core_reg_u32;
1412         armv7m->store_core_reg_u32 = cortex_m3_store_core_reg_u32;
1413
1414         target_register_timer_callback(cortex_m3_handle_target_request, 1, 1, target);
1415
1416         return ERROR_OK;
1417 }
1418
1419 /* target cortex_m3 <endianess> <startup_mode> <chain_pos> <variant>*/
1420 int cortex_m3_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
1421 {
1422         int chain_pos;
1423         char *variant = NULL;
1424         cortex_m3_common_t *cortex_m3 = malloc(sizeof(cortex_m3_common_t));
1425         memset(cortex_m3, 0, sizeof(*cortex_m3));
1426
1427         if (argc < 4)
1428         {
1429                 LOG_ERROR("'target cortex_m3' requires at least one additional argument");
1430                 exit(-1);
1431         }
1432
1433         chain_pos = strtoul(args[3], NULL, 0);
1434
1435         if (argc >= 5)
1436                 variant = args[4];
1437
1438         cortex_m3_init_arch_info(target, cortex_m3, chain_pos, variant);
1439         cortex_m3_register_commands(cmd_ctx);
1440
1441         return ERROR_OK;
1442 }
1443
1444 int cortex_m3_register_commands(struct command_context_s *cmd_ctx)
1445 {
1446         int retval;
1447
1448         retval = armv7m_register_commands(cmd_ctx);
1449
1450         return retval;
1451 }