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