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