Duane Ellis: "target as an [tcl] object" feature.
[fw/openocd] / src / target / cortex_m3.c
index aba15e7f547033160af1509e70a706b5d99b039c..e034189a07978a6addbc1845f1e99c2da9fb66a1 100644 (file)
 int cortex_m3_register_commands(struct command_context_s *cmd_ctx);
 
 /* forward declarations */
-void cortex_m3_unset_all_breakpoints_and_watchpoints(struct target_s *target);
 void cortex_m3_enable_breakpoints(struct target_s *target);
 void cortex_m3_enable_watchpoints(struct target_s *target);
-void cortex_m3_disable_bkpts_and_wpts(struct target_s *target);
-int cortex_m3_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
+int cortex_m3_target_create(struct target_s *target, Jim_Interp *interp);
 int cortex_m3_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
-int cortex_m3_quit();
+int cortex_m3_quit(void);
 int cortex_m3_load_core_reg_u32(target_t *target, enum armv7m_regtype type, u32 num, u32 *value);
 int cortex_m3_store_core_reg_u32(target_t *target, enum armv7m_regtype type, u32 num, u32 value);
 int cortex_m3_target_request_data(target_t *target, u32 size, u8 *buffer);
+int cortex_m3_examine(struct target_s *target);
+
+#ifdef ARMV7_GDB_HACKS
+extern u8 armv7m_gdb_dummy_cpsr_value[];
+extern reg_t armv7m_gdb_dummy_cpsr_reg;
+#endif
 
 target_type_t cortexm3_target =
 {
@@ -70,7 +74,6 @@ target_type_t cortexm3_target =
        .assert_reset = cortex_m3_assert_reset,
        .deassert_reset = cortex_m3_deassert_reset,
        .soft_reset_halt = cortex_m3_soft_reset_halt,
-       .prepare_reset_halt = cortex_m3_prepare_reset_halt,
        
        .get_gdb_reg_list = armv7m_get_gdb_reg_list,
 
@@ -78,6 +81,7 @@ target_type_t cortexm3_target =
        .write_memory = cortex_m3_write_memory,
        .bulk_write_memory = cortex_m3_bulk_write_memory,
        .checksum_memory = armv7m_checksum_memory,
+       .blank_check_memory = armv7m_blank_check_memory,
        
        .run_algorithm = armv7m_run_algorithm,
        
@@ -87,8 +91,9 @@ target_type_t cortexm3_target =
        .remove_watchpoint = cortex_m3_remove_watchpoint,
 
        .register_commands = cortex_m3_register_commands,
-       .target_command = cortex_m3_target_command,
+       .target_create = cortex_m3_target_create,
        .init_target = cortex_m3_init_target,
+       .examine = cortex_m3_examine,
        .quit = cortex_m3_quit
 };
 
@@ -202,11 +207,6 @@ int cortex_m3_endreset_event(target_t *target)
        }
        swjdp_transaction_endcheck(swjdp);
        
-       /* Make sure working_areas are all free */
-       target_free_all_working_areas(target);
-       
-       /* We are in process context */
-       armv7m_use_context(target, ARMV7M_PROCESS_CONTEXT);
        armv7m_invalidate_core_regs(target);
        return ERROR_OK;
 }
@@ -319,7 +319,15 @@ int cortex_m3_debug_entry(target_t *target)
        }
 
        xPSR = buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32);
-       
+
+#ifdef ARMV7_GDB_HACKS
+       /* copy real xpsr reg for gdb, setting thumb bit */
+       buf_set_u32(armv7m_gdb_dummy_cpsr_value, 0, 32, xPSR);
+       buf_set_u32(armv7m_gdb_dummy_cpsr_value, 5, 1, 1);
+       armv7m_gdb_dummy_cpsr_reg.valid = armv7m->core_cache->reg_list[ARMV7M_xPSR].valid;
+       armv7m_gdb_dummy_cpsr_reg.dirty = armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty;
+#endif
+
        /* For IT instructions xPSR must be reloaded on resume and clear on debug exec */
        if (xPSR & 0xf00)
        {
@@ -327,23 +335,34 @@ int cortex_m3_debug_entry(target_t *target)
                cortex_m3_store_core_reg_u32(target, ARMV7M_REGISTER_CORE_GP, 16, xPSR &~ 0xff);
        }
 
-       /* Now we can load SP core registers */ 
+       /* Now we can load SP core registers */
        for (i = ARMV7M_PRIMASK; i < ARMV7NUMCOREREGS; i++)
        {
                if (!armv7m->core_cache->reg_list[i].valid)
-                       armv7m->read_core_reg(target, i);               
+                       armv7m->read_core_reg(target, i);
        }
 
        /* Are we in an exception handler */
-    armv7m->core_mode = (xPSR & 0x1FF) ? ARMV7M_MODE_HANDLER : ARMV7M_MODE_THREAD;
-    armv7m->exception_number = xPSR & 0x1FF;
+       if (xPSR & 0x1FF)
+       {
+               armv7m->core_mode = ARMV7M_MODE_HANDLER;
+               armv7m->exception_number = (xPSR & 0x1FF);
+       }
+       else
+       {
+               armv7m->core_mode = buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_CONTROL].value, 0, 1);
+               armv7m->exception_number = 0;
+       }
+       
        if (armv7m->exception_number)
        {
                cortex_m3_examine_exception_reason(target);
        }
 
-       LOG_DEBUG("entered debug state in core mode: %s at PC 0x%x, target->state: %s", armv7m_mode_strings[armv7m->core_mode], \
-               *(u32*)(armv7m->core_cache->reg_list[15].value), target_state_strings[target->state]);
+       LOG_DEBUG("entered debug state in core mode: %s at PC 0x%x, target->state: %s", 
+                 armv7m_mode_strings[armv7m->core_mode],
+                 *(u32*)(armv7m->core_cache->reg_list[15].value), 
+                 Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name);
 
        if (armv7m->post_debug_entry)
                armv7m->post_debug_entry(target);
@@ -416,9 +435,12 @@ int cortex_m3_poll(target_t *target)
                target->state = TARGET_SLEEP;
        */
 
+#if 0
     /* Read Debug Fault Status Register, added to figure out the lockup when running flashtest.script  */
-    ahbap_read_system_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
-       LOG_DEBUG("dcb_dhcsr 0x%x, nvic_dfsr 0x%x, target->state: %s", cortex_m3->dcb_dhcsr, cortex_m3->nvic_dfsr, target_state_strings[target->state]);        
+       ahbap_read_system_atomic_u32(swjdp, NVIC_DFSR, &cortex_m3->nvic_dfsr);
+       LOG_DEBUG("dcb_dhcsr 0x%x, nvic_dfsr 0x%x, target->state: %s", cortex_m3->dcb_dhcsr, cortex_m3->nvic_dfsr, Jim_Nvp_value2name( nvp_target_state, target->state )->name );
+#endif
+       
        return ERROR_OK;
 }
 
@@ -429,11 +451,12 @@ int cortex_m3_halt(target_t *target)
        cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
        swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
        
-       LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
+       LOG_DEBUG("target->state: %s", 
+                 Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name );
        
        if (target->state == TARGET_HALTED)
        {
-               LOG_WARNING("target was already halted");
+               LOG_DEBUG("target was already halted");
                return ERROR_OK;
        }
        
@@ -476,13 +499,6 @@ int cortex_m3_soft_reset_halt(struct target_s *target)
        swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
        u32 dcb_dhcsr = 0;
        int retval, timeout = 0;
-       
-       /* Check that we are using process_context, or change and print warning */
-       if (armv7m_get_context(target) != ARMV7M_PROCESS_CONTEXT)
-       {
-               LOG_DEBUG("Changing to process contex registers");
-               armv7m_use_context(target, ARMV7M_PROCESS_CONTEXT);
-       }
 
        /* Enter debug state on reset, cf. end_reset_event() */
        ahbap_write_system_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET );
@@ -510,34 +526,12 @@ int cortex_m3_soft_reset_halt(struct target_s *target)
                                LOG_DEBUG("waiting for system reset-halt, dcb_dhcsr 0x%x, %i ms", dcb_dhcsr, timeout);
                }
                timeout++;
-               usleep(1000);
+               alive_sleep(1);
        }
                
        return ERROR_OK;
 }
 
-int cortex_m3_prepare_reset_halt(struct target_s *target)
-{
-       armv7m_common_t *armv7m = target->arch_info;
-       cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
-       swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
-       u32 dcb_demcr, dcb_dhcsr;
-       
-       /* Enable debug requests */
-       ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
-       if (!(cortex_m3->dcb_dhcsr & C_DEBUGEN))
-               ahbap_write_system_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN );
-       
-       /* Enter debug state on reset, cf. end_reset_event() */
-       ahbap_write_system_atomic_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET );
-       
-       ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
-       ahbap_read_system_atomic_u32(swjdp, DCB_DEMCR, &dcb_demcr);
-       LOG_DEBUG("dcb_dhcsr 0x%x, dcb_demcr 0x%x, ", dcb_dhcsr, dcb_demcr);
-       
-       return ERROR_OK;
-}
-
 int cortex_m3_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
 {
        /* get pointers to arch-specific information */
@@ -555,13 +549,6 @@ int cortex_m3_resume(struct target_s *target, int current, u32 address, int hand
        
        if (!debug_execution)
        {
-               /* Check that we are using process_context, or change and print warning */
-               if (armv7m_get_context(target) != ARMV7M_PROCESS_CONTEXT)
-               {
-                       LOG_DEBUG("Incorrect context in resume");
-                       armv7m_use_context(target, ARMV7M_PROCESS_CONTEXT);
-               }
-               
                target_free_all_working_areas(target);
                cortex_m3_enable_breakpoints(target);
                cortex_m3_enable_watchpoints(target);
@@ -572,12 +559,6 @@ int cortex_m3_resume(struct target_s *target, int current, u32 address, int hand
        dcb_dhcsr = DBGKEY | C_DEBUGEN;
        if (debug_execution)
        {
-               /* Check that we are using debug_context, or change and print warning */
-               if (armv7m_get_context(target) != ARMV7M_DEBUG_CONTEXT)
-               {
-                       LOG_DEBUG("Incorrect context in debug_exec resume");
-                       armv7m_use_context(target, ARMV7M_DEBUG_CONTEXT);
-               }
                /* Disable interrupts */
                /* 
                   We disable interrupts in the PRIMASK register instead of masking with C_MASKINTS,
@@ -608,10 +589,10 @@ int cortex_m3_resume(struct target_s *target, int current, u32 address, int hand
                /* Single step past breakpoint at current address */
                if ((breakpoint = breakpoint_find(target, resume_pc)))
                {
-                               LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
-                               cortex_m3_unset_breakpoint(target, breakpoint);
-                               cortex_m3_single_step_core(target);
-                               cortex_m3_set_breakpoint(target, breakpoint);
+                       LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
+                       cortex_m3_unset_breakpoint(target, breakpoint);
+                       cortex_m3_single_step_core(target);
+                       cortex_m3_set_breakpoint(target, breakpoint);
                }
        }
 
@@ -655,13 +636,6 @@ int cortex_m3_step(struct target_s *target, int current, u32 address, int handle
                LOG_WARNING("target not halted");
                return ERROR_TARGET_NOT_HALTED;
        }
-       
-       /* Check that we are using process_context, or change and print warning */
-       if (armv7m_get_context(target) != ARMV7M_PROCESS_CONTEXT)
-       {
-               LOG_WARNING("Incorrect context in step, must be process");
-               armv7m_use_context(target, ARMV7M_PROCESS_CONTEXT);
-       }
 
        /* current = 1: continue on current pc, otherwise continue at <address> */
        if (!current)
@@ -683,9 +657,8 @@ int cortex_m3_step(struct target_s *target, int current, u32 address, int handle
        ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, DBGKEY| C_STEP | C_DEBUGEN);
        ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
 
-       /* If we run in process context then registers are now invalid */
-       if (armv7m_get_context(target) == ARMV7M_PROCESS_CONTEXT)
-               armv7m_invalidate_core_regs(target);
+       /* registers are now invalid */
+       armv7m_invalidate_core_regs(target);
        
        if (breakpoint)
                cortex_m3_set_breakpoint(target, breakpoint);
@@ -701,86 +674,112 @@ int cortex_m3_step(struct target_s *target, int current, u32 address, int handle
 
 int cortex_m3_assert_reset(target_t *target)
 {
-       int retval;
        armv7m_common_t *armv7m = target->arch_info;
        cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
        swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
+       int assert_srst = 1;
+       
+       LOG_DEBUG("target->state: %s", 
+                 Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name );
        
-       LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
+       if (!(jtag_reset_config & RESET_HAS_SRST))
+       {
+               LOG_ERROR("Can't assert SRST");
+               return ERROR_FAIL;
+       }
        
+       /* Enable debug requests */
+       ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &cortex_m3->dcb_dhcsr);
+       if (!(cortex_m3->dcb_dhcsr & C_DEBUGEN))
+               ahbap_write_system_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN );
+               
        ahbap_write_system_u32(swjdp, DCB_DCRDR, 0 );
        
-       if (target->reset_mode == RESET_RUN)
+       if (!target->reset_halt)
        {
                /* Set/Clear C_MASKINTS in a separate operation */
                if (cortex_m3->dcb_dhcsr & C_MASKINTS)
                        ahbap_write_system_atomic_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN | C_HALT );
-               
+       
                cortex_m3_clear_halt(target);
                                                        
                /* Enter debug state on reset, cf. end_reset_event() */ 
-               ahbap_write_system_u32(swjdp, DCB_DHCSR, DBGKEY | C_DEBUGEN );
                ahbap_write_system_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR);
        }
+       else
+       {
+               /* Enter debug state on reset, cf. end_reset_event() */
+               ahbap_write_system_atomic_u32(swjdp, DCB_DEMCR, TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET );
+       }
        
-       if (target->state == TARGET_HALTED || target->state == TARGET_UNKNOWN)
+       /* following hack is to handle luminary reset
+        * when srst is asserted the luminary device seesm to also clear the debug registers
+        * which does not match the armv7 debug TRM */
+               
+       if (strcmp(cortex_m3->variant, "lm3s") == 0)
        {
-               /* assert SRST and TRST */
-               /* system would get ouf sync if we didn't reset test-logic, too */
-               if ((retval = jtag_add_reset(1, 1)) != ERROR_OK)
+               /* get revision of lm3s target, only early silicon has this issue
+                * Fury Rev B, DustDevil Rev B, Tempest all ok */
+               
+               u32 did0;
+               
+               if (target_read_u32(target, 0x400fe000, &did0) == ERROR_OK)
                {
-                       if (retval == ERROR_JTAG_RESET_CANT_SRST)
-                       {
-                               return retval;
-                       }
-                       else
+                       switch ((did0 >> 16) & 0xff)
                        {
-                               LOG_ERROR("unknown error");
-                               exit(-1);
+                               case 0:
+                                       /* all Sandstorm suffer issue */
+                                       assert_srst = 0;
+                                       break;
+                               
+                               case 1:
+                               case 3:
+                                       /* only Fury/DustDevil rev A suffer reset problems */
+                                       if (((did0 >> 8) & 0xff) == 0)
+                                               assert_srst = 0;
+                                       break;
                        }
                }
-               jtag_add_sleep(5000);
-               if ((retval = jtag_add_reset(0, 1)) != ERROR_OK)
+       }
+       
+       if (assert_srst)
+       {
+               /* default to asserting srst */
+               if (jtag_reset_config & RESET_SRST_PULLS_TRST)
                {
-                       if (retval == ERROR_JTAG_RESET_WOULD_ASSERT_TRST)
-                       {
-                               retval = jtag_add_reset(1, 1);
-                       }
+                       jtag_add_reset(1, 1);
+               }
+               else
+               {
+                       jtag_add_reset(0, 1);
                }
        }
        else
        {
-               if ((retval = jtag_add_reset(0, 1)) != ERROR_OK)
-               {
-                       if (retval == ERROR_JTAG_RESET_WOULD_ASSERT_TRST)
-                       {
-                               retval = jtag_add_reset(1, 1);
-                       }
-                       
-                       if (retval == ERROR_JTAG_RESET_CANT_SRST)
-                       {
-                               return retval;
-                       }
-                       else if (retval != ERROR_OK)
-                       {
-                               LOG_ERROR("unknown error");
-                               exit(-1);
-                       }
-               }
+               /* this causes the luminary device to reset using the watchdog */
+               ahbap_write_system_atomic_u32(swjdp, NVIC_AIRCR, AIRCR_VECTKEY | AIRCR_SYSRESETREQ );
+               LOG_DEBUG("Using Luminary Reset: SYSRESETREQ");
        }
        
        target->state = TARGET_RESET;
        jtag_add_sleep(50000);
        
-       armv7m_use_context(target, ARMV7M_PROCESS_CONTEXT);
        armv7m_invalidate_core_regs(target);
 
+    if (target->reset_halt)
+    {
+       int retval;
+               if ((retval = target_halt(target))!=ERROR_OK)
+                       return retval;
+    }
+       
        return ERROR_OK;
 }
 
 int cortex_m3_deassert_reset(target_t *target)
 {              
-       LOG_DEBUG("target->state: %s", target_state_strings[target->state]);
+       LOG_DEBUG("target->state: %s", 
+                 Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name);
        
        /* deassert reset lines */
        jtag_add_reset(0, 0);
@@ -788,11 +787,6 @@ int cortex_m3_deassert_reset(target_t *target)
        return ERROR_OK;
 }
 
-void cortex_m3_unset_all_breakpoints_and_watchpoints(struct target_s *target)
-{
-
-}
-
 void cortex_m3_enable_breakpoints(struct target_s *target)
 {
        breakpoint_t *breakpoint = target->breakpoints;
@@ -822,7 +816,7 @@ int cortex_m3_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
                LOG_WARNING("breakpoint already set");
                return ERROR_OK;
        }
-
+    
        if (cortex_m3->auto_bp_type)
        {
                breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
@@ -904,10 +898,18 @@ int cortex_m3_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
        /* get pointers to arch-specific information */
        armv7m_common_t *armv7m = target->arch_info;
        cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
-       
+
        if (cortex_m3->auto_bp_type)
        {
                breakpoint->type = (breakpoint->address < 0x20000000) ? BKPT_HARD : BKPT_SOFT;
+#ifdef ARMV7_GDB_HACKS
+               if (breakpoint->length != 2) {
+                       /* XXX Hack: Replace all breakpoints with length != 2 with
+                        * a hardware breakpoint. */ 
+                       breakpoint->type = BKPT_HARD;
+                       breakpoint->length = 2;
+               }
+#endif
        }
 
        if ((breakpoint->type == BKPT_HARD) && (breakpoint->address >= 0x20000000))
@@ -1137,22 +1139,28 @@ int cortex_m3_load_core_reg_u32(struct target_s *target, enum armv7m_regtype typ
        else if (type == ARMV7M_REGISTER_CORE_SP) /* Special purpose core register */
        {
                /* read other registers */
-               u32 savedram;
-               u32 SYSm;
-               u32 instr;
-               SYSm = num & 0x1F;
+               ahbap_read_coreregister_u32(swjdp, value, 20);
+               
+               switch (num)
+               {
+                       case 19:
+                               *value = buf_get_u32((u8*)value, 0, 8);
+                               break;
+                               
+                       case 20:
+                               *value = buf_get_u32((u8*)value, 8, 8);
+                               break;
+                               
+                       case 21:
+                               *value = buf_get_u32((u8*)value, 16, 8);
+                               break;
+                               
+                       case 22:
+                               *value = buf_get_u32((u8*)value, 24, 8);
+                               break;
+               }
                
-               ahbap_read_system_u32(swjdp, 0x20000000, &savedram);
-               instr = ARMV7M_T_MRS(0, SYSm);
-               ahbap_write_system_u32(swjdp, 0x20000000, ARMV7M_T_MRS(0, SYSm));
-               ahbap_write_coreregister_u32(swjdp, 0x20000000, 15);
-               cortex_m3_single_step_core(target);
-               ahbap_read_coreregister_u32(swjdp, value, 0);
-               armv7m->core_cache->reg_list[0].dirty = armv7m->core_cache->reg_list[0].valid;
-               armv7m->core_cache->reg_list[15].dirty = armv7m->core_cache->reg_list[15].valid;
-               ahbap_write_system_u32(swjdp, 0x20000000, savedram);
-               swjdp_transaction_endcheck(swjdp);
-               LOG_DEBUG("load from special reg %i value 0x%x", SYSm, *value);
+               LOG_DEBUG("load from special reg %i value 0x%x", num, *value);
        }
        else
        {
@@ -1165,12 +1173,24 @@ int cortex_m3_load_core_reg_u32(struct target_s *target, enum armv7m_regtype typ
 int cortex_m3_store_core_reg_u32(struct target_s *target, enum armv7m_regtype type, u32 num, u32 value)
 {
        int retval;
+       u32 reg;
        
        /* get pointers to arch-specific information */
        armv7m_common_t *armv7m = target->arch_info;
        cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
        swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
 
+#ifdef ARMV7_GDB_HACKS
+       /* If the LR register is being modified, make sure it will put us
+        * in "thumb" mode, or an INVSTATE exception will occur. This is a
+        * hack to deal with the fact that gdb will sometimes "forge"
+        * return addresses, and doesn't set the LSB correctly (i.e., when
+        * printing expressions containing function calls, it sets LR=0.) */
+       
+       if (num == 14)
+               value |= 0x01;
+#endif
+        
        if ((type == ARMV7M_REGISTER_CORE_GP) && (num <= ARMV7M_PSP))
        {
                retval = ahbap_write_coreregister_u32(swjdp, value, num);
@@ -1185,23 +1205,31 @@ int cortex_m3_store_core_reg_u32(struct target_s *target, enum armv7m_regtype ty
        else if (type == ARMV7M_REGISTER_CORE_SP) /* Special purpose core register */
        {
                /* write other registers */
-               u32 savedram , tempr0;
-               u32 SYSm;
-               u32 instr;
-               SYSm = num & 0x1F;
                
-               ahbap_read_system_u32(swjdp, 0x20000000, &savedram);
-               instr = ARMV7M_T_MSR(SYSm, 0);
-               ahbap_write_system_u32(swjdp, 0x20000000, ARMV7M_T_MSR(SYSm, 0));
-               ahbap_read_coreregister_u32(swjdp, &tempr0, 0);
-               ahbap_write_coreregister_u32(swjdp, value, 0);
-               ahbap_write_coreregister_u32(swjdp, 0x20000000, 15);
-               cortex_m3_single_step_core(target);
-               ahbap_write_coreregister_u32(swjdp, tempr0, 0);
-               armv7m->core_cache->reg_list[15].dirty = armv7m->core_cache->reg_list[15].valid;
-               ahbap_write_system_u32(swjdp, 0x20000000, savedram);
-               swjdp_transaction_endcheck(swjdp);
-               LOG_DEBUG("write special reg %i value 0x%x ", SYSm, value);
+               ahbap_read_coreregister_u32(swjdp, &reg, 20);
+               
+               switch (num)
+               {
+                       case 19:
+                               buf_set_u32((u8*)&reg, 0, 8, value);
+                               break;
+                               
+                       case 20:
+                               buf_set_u32((u8*)&reg, 8, 8, value);
+                               break;
+                               
+                       case 21:
+                               buf_set_u32((u8*)&reg, 16, 8, value);
+                               break;
+                               
+                       case 22:
+                               buf_set_u32((u8*)&reg, 24, 8, value);
+                               break;
+               }
+               
+               ahbap_write_coreregister_u32(swjdp, reg, 20);
+               
+               LOG_DEBUG("write special reg %i value 0x%x ", num, value);
        }
        else
        {
@@ -1287,6 +1315,13 @@ void cortex_m3_build_reg_cache(target_t *target)
 
 int cortex_m3_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
 {
+       cortex_m3_build_reg_cache(target);
+       return ERROR_OK;
+}
+
+int cortex_m3_examine(struct target_s *target)
+{
+       int retval;
        u32 cpuid, fpcr, dwtcr, ictr;
        int i;
        
@@ -1294,12 +1329,16 @@ int cortex_m3_init_target(struct command_context_s *cmd_ctx, struct target_s *ta
        armv7m_common_t *armv7m = target->arch_info;
        cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
        swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
+       
+       target->type->examined = 1;
 
-       cortex_m3_build_reg_cache(target);
-       ahbap_debugport_init(swjdp);
+       if ((retval=ahbap_debugport_init(swjdp))!=ERROR_OK)
+               return retval;
 
        /* Read from Device Identification Registers */
-       target_read_u32(target, CPUID, &cpuid);
+       if ((retval=target_read_u32(target, CPUID, &cpuid))!=ERROR_OK)
+               return retval;
+       
        if (((cpuid >> 4) & 0xc3f) == 0xc23)
                LOG_DEBUG("CORTEX-M3 processor detected");
        LOG_DEBUG("cpuid: 0x%8.8x", cpuid);
@@ -1340,7 +1379,7 @@ int cortex_m3_init_target(struct command_context_s *cmd_ctx, struct target_s *ta
        return ERROR_OK;
 }
 
-int cortex_m3_quit()
+int cortex_m3_quit(void)
 {
        
        return ERROR_OK;
@@ -1388,6 +1427,8 @@ int cortex_m3_target_request_data(target_t *target, u32 size, u8 *buffer)
 int cortex_m3_handle_target_request(void *priv)
 {
        target_t *target = priv;
+       if (!target->type->examined)
+               return ERROR_OK;
        armv7m_common_t *armv7m = target->arch_info;
        cortex_m3_common_t *cortex_m3 = armv7m->arch_info;
        swjdp_common_t *swjdp = &cortex_m3->swjdp_info;
@@ -1422,7 +1463,7 @@ int cortex_m3_handle_target_request(void *priv)
        return ERROR_OK;
 }
 
-int cortex_m3_init_arch_info(target_t *target, cortex_m3_common_t *cortex_m3, int chain_pos, char *variant)
+int cortex_m3_init_arch_info(target_t *target, cortex_m3_common_t *cortex_m3, int chain_pos, const char *variant)
 {
        armv7m_common_t *armv7m;
        armv7m = &cortex_m3->armv7m;
@@ -1450,6 +1491,15 @@ int cortex_m3_init_arch_info(target_t *target, cortex_m3_common_t *cortex_m3, in
        armv7m->pre_restore_context = NULL;
        armv7m->post_restore_context = NULL;
        
+       if (variant)
+       {
+               cortex_m3->variant = strdup(variant);
+       }
+       else
+       {
+               cortex_m3->variant = strdup("");
+       }
+       
        armv7m_init_arch_info(target, armv7m);  
        armv7m->arch_info = cortex_m3;
        armv7m->load_core_reg_u32 = cortex_m3_load_core_reg_u32;
@@ -1460,27 +1510,11 @@ int cortex_m3_init_arch_info(target_t *target, cortex_m3_common_t *cortex_m3, in
        return ERROR_OK;
 }
 
-/* target cortex_m3 <endianess> <startup_mode> <chain_pos> <variant>*/
-int cortex_m3_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
+int cortex_m3_target_create(struct target_s *target, Jim_Interp *interp)
 {
-       int chain_pos;
-       char *variant = NULL;
-       cortex_m3_common_t *cortex_m3 = malloc(sizeof(cortex_m3_common_t));
-       memset(cortex_m3, 0, sizeof(*cortex_m3));
-
-       if (argc < 4)
-       {
-               LOG_ERROR("'target cortex_m3' requires at least one additional argument");
-               exit(-1);
-       }
+       cortex_m3_common_t *cortex_m3 = calloc(1,sizeof(cortex_m3_common_t));
        
-       chain_pos = strtoul(args[3], NULL, 0);
-       
-       if (argc >= 5)
-               variant = args[4];
-       
-       cortex_m3_init_arch_info(target, cortex_m3, chain_pos, variant);
-       cortex_m3_register_commands(cmd_ctx);
+       cortex_m3_init_arch_info(target, cortex_m3, target->chain_position, target->variant);
        
        return ERROR_OK;
 }
@@ -1493,3 +1527,11 @@ int cortex_m3_register_commands(struct command_context_s *cmd_ctx)
        
        return retval;
 }
+
+
+/*
+ * Local Variables: ***
+ * c-basic-offset: 4 ***
+ * tab-width: 4 ***
+ * End: ***
+ */