mips_m4k.c: EJTAGBOOT and NORMALBOOT are not supported on EJTAG 2.0
[fw/openocd] / src / target / mips_m4k.c
index aa9dbe7ad2732ad610c59e8225f6b7c699a0943a..5b740cc45242b614800d4e66a11fc4cc73467cb8 100644 (file)
@@ -22,7 +22,7 @@
  *   You should have received a copy of the GNU General Public License     *
  *   along with this program; if not, write to the                         *
  *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
@@ -42,36 +42,52 @@ static int mips_m4k_set_breakpoint(struct target *target,
                struct breakpoint *breakpoint);
 static int mips_m4k_unset_breakpoint(struct target *target,
                struct breakpoint *breakpoint);
+static int mips_m4k_internal_restore(struct target *target, int current,
+               uint32_t address, int handle_breakpoints,
+               int debug_execution);
+static int mips_m4k_halt(struct target *target);
+static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
+               uint32_t count, const uint8_t *buffer);
 
 static int mips_m4k_examine_debug_reason(struct target *target)
 {
+       struct mips32_common *mips32 = target_to_mips32(target);
+       struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
        uint32_t break_status;
        int retval;
 
        if ((target->debug_reason != DBG_REASON_DBGRQ)
-               && (target->debug_reason != DBG_REASON_SINGLESTEP)) {
-               /* get info about inst breakpoint support */
-               retval = target_read_u32(target, EJTAG_IBS, &break_status);
-               if (retval != ERROR_OK)
-                       return retval;
-               if (break_status & 0x1f) {
-                       /* we have halted on a  breakpoint */
-                       retval = target_write_u32(target, EJTAG_IBS, 0);
+                       && (target->debug_reason != DBG_REASON_SINGLESTEP)) {
+               if (ejtag_info->debug_caps & EJTAG_DCR_IB) {
+                       /* get info about inst breakpoint support */
+                       retval = target_read_u32(target,
+                               ejtag_info->ejtag_ibs_addr, &break_status);
                        if (retval != ERROR_OK)
                                return retval;
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
+                       if (break_status & 0x1f) {
+                               /* we have halted on a  breakpoint */
+                               retval = target_write_u32(target,
+                                       ejtag_info->ejtag_ibs_addr, 0);
+                               if (retval != ERROR_OK)
+                                       return retval;
+                               target->debug_reason = DBG_REASON_BREAKPOINT;
+                       }
                }
 
-               /* get info about data breakpoint support */
-               retval = target_read_u32(target, EJTAG_DBS, &break_status);
-               if (retval != ERROR_OK)
-                       return retval;
-               if (break_status & 0x1f) {
-                       /* we have halted on a  breakpoint */
-                       retval = target_write_u32(target, EJTAG_DBS, 0);
+               if (ejtag_info->debug_caps & EJTAG_DCR_DB) {
+                       /* get info about data breakpoint support */
+                       retval = target_read_u32(target,
+                               ejtag_info->ejtag_dbs_addr, &break_status);
                        if (retval != ERROR_OK)
                                return retval;
-                       target->debug_reason = DBG_REASON_WATCHPOINT;
+                       if (break_status & 0x1f) {
+                               /* we have halted on a  breakpoint */
+                               retval = target_write_u32(target,
+                                       ejtag_info->ejtag_dbs_addr, 0);
+                               if (retval != ERROR_OK)
+                                       return retval;
+                               target->debug_reason = DBG_REASON_WATCHPOINT;
+                       }
                }
        }
 
@@ -82,10 +98,11 @@ static int mips_m4k_debug_entry(struct target *target)
 {
        struct mips32_common *mips32 = target_to_mips32(target);
        struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
-       uint32_t debug_reg;
 
-       /* read debug register */
-       mips_ejtag_read_debug(ejtag_info, &debug_reg);
+       mips32_save_context(target);
+
+       /* make sure stepping disabled, SSt bit in CP0 debug register cleared */
+       mips_ejtag_config_step(ejtag_info, 0);
 
        /* make sure break unit configured */
        mips32_configure_break_unit(target);
@@ -93,14 +110,6 @@ static int mips_m4k_debug_entry(struct target *target)
        /* attempt to find halt reason */
        mips_m4k_examine_debug_reason(target);
 
-       /* clear single step if active */
-       if (debug_reg & EJTAG_DEBUG_DSS) {
-               /* stopped due to single step - clear step bit */
-               mips_ejtag_config_step(ejtag_info, 0);
-       }
-
-       mips32_save_context(target);
-
        /* default to mips32 isa, it will be changed below if required */
        mips32->isa_mode = MIPS32_ISA_MIPS32;
 
@@ -114,12 +123,73 @@ static int mips_m4k_debug_entry(struct target *target)
        return ERROR_OK;
 }
 
+static struct target *get_mips_m4k(struct target *target, int32_t coreid)
+{
+       struct target_list *head;
+       struct target *curr;
+
+       head = target->head;
+       while (head != (struct target_list *)NULL) {
+               curr = head->target;
+               if ((curr->coreid == coreid) && (curr->state == TARGET_HALTED))
+                       return curr;
+               head = head->next;
+       }
+       return target;
+}
+
+static int mips_m4k_halt_smp(struct target *target)
+{
+       int retval = ERROR_OK;
+       struct target_list *head;
+       struct target *curr;
+       head = target->head;
+       while (head != (struct target_list *)NULL) {
+               int ret = ERROR_OK;
+               curr = head->target;
+               if ((curr != target) && (curr->state != TARGET_HALTED))
+                       ret = mips_m4k_halt(curr);
+
+               if (ret != ERROR_OK) {
+                       LOG_ERROR("halt failed target->coreid: %" PRId32, curr->coreid);
+                       retval = ret;
+               }
+               head = head->next;
+       }
+       return retval;
+}
+
+static int update_halt_gdb(struct target *target)
+{
+       int retval = ERROR_OK;
+       if (target->gdb_service->core[0] == -1) {
+               target->gdb_service->target = target;
+               target->gdb_service->core[0] = target->coreid;
+               retval = mips_m4k_halt_smp(target);
+       }
+       return retval;
+}
+
 static int mips_m4k_poll(struct target *target)
 {
-       int retval;
+       int retval = ERROR_OK;
        struct mips32_common *mips32 = target_to_mips32(target);
        struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
        uint32_t ejtag_ctrl = ejtag_info->ejtag_ctrl;
+       enum target_state prev_target_state = target->state;
+
+       /*  toggle to another core is done by gdb as follow */
+       /*  maint packet J core_id */
+       /*  continue */
+       /*  the next polling trigger an halt event sent to gdb */
+       if ((target->state == TARGET_HALTED) && (target->smp) &&
+               (target->gdb_service) &&
+               (target->gdb_service->target == NULL)) {
+               target->gdb_service->target =
+                       get_mips_m4k(target, target->gdb_service->core[1]);
+               target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+               return retval;
+       }
 
        /* read ejtag control reg */
        mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
@@ -143,15 +213,29 @@ static int mips_m4k_poll(struct target *target)
 
        /* check for processor halted */
        if (ejtag_ctrl & EJTAG_CTRL_BRKST) {
-               if ((target->state == TARGET_RUNNING) || (target->state == TARGET_RESET)) {
+               if ((target->state != TARGET_HALTED)
+                   && (target->state != TARGET_DEBUG_RUNNING)) {
+                       if (target->state == TARGET_UNKNOWN)
+                               LOG_DEBUG("EJTAG_CTRL_BRKST already set during server startup.");
+
+                       /* OpenOCD was was probably started on the board with EJTAG_CTRL_BRKST already set
+                        * (maybe put on by HALT-ing the board in the previous session).
+                        *
+                        * Force enable debug entry for this session.
+                        */
                        mips_ejtag_set_instr(ejtag_info, EJTAG_INST_NORMALBOOT);
-
                        target->state = TARGET_HALTED;
-
                        retval = mips_m4k_debug_entry(target);
                        if (retval != ERROR_OK)
                                return retval;
 
+                       if (target->smp &&
+                               ((prev_target_state == TARGET_RUNNING)
+                            || (prev_target_state == TARGET_RESET))) {
+                               retval = update_halt_gdb(target);
+                               if (retval != ERROR_OK)
+                                       return retval;
+                       }
                        target_call_event_callbacks(target, TARGET_EVENT_HALTED);
                } else if (target->state == TARGET_DEBUG_RUNNING) {
                        target->state = TARGET_HALTED;
@@ -160,6 +244,12 @@ static int mips_m4k_poll(struct target *target)
                        if (retval != ERROR_OK)
                                return retval;
 
+                       if (target->smp) {
+                               retval = update_halt_gdb(target);
+                               if (retval != ERROR_OK)
+                                       return retval;
+                       }
+
                        target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
                }
        } else
@@ -191,7 +281,7 @@ static int mips_m4k_halt(struct target *target)
                        return ERROR_TARGET_FAILURE;
                } else {
                        /* we came here in a reset_halt or reset_init sequence
-                        * debug entry was already prepared in mips32_prepare_reset_halt()
+                        * debug entry was already prepared in mips_m4k_assert_reset()
                         */
                        target->debug_reason = DBG_REASON_DBGRQ;
 
@@ -211,27 +301,38 @@ static int mips_m4k_assert_reset(struct target *target)
 {
        struct mips_m4k_common *mips_m4k = target_to_m4k(target);
        struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info;
-       int assert_srst = 1;
 
        LOG_DEBUG("target->state: %s",
                target_state_name(target));
 
        enum reset_types jtag_reset_config = jtag_get_reset_config();
 
-       if (!(jtag_reset_config & RESET_HAS_SRST))
-               assert_srst = 0;
+       /* some cores support connecting while srst is asserted
+        * use that mode is it has been configured */
 
-       if (target->reset_halt) {
-               /* use hardware to catch reset */
-               mips_ejtag_set_instr(ejtag_info, EJTAG_INST_EJTAGBOOT);
-       } else
-               mips_ejtag_set_instr(ejtag_info, EJTAG_INST_NORMALBOOT);
+       bool srst_asserted = false;
+
+       if (!(jtag_reset_config & RESET_SRST_PULLS_TRST) &&
+                       (jtag_reset_config & RESET_SRST_NO_GATING)) {
+               jtag_add_reset(0, 1);
+               srst_asserted = true;
+       }
 
-       if (assert_srst) {
+
+       /* EJTAG before v2.5/2.6 does not support EJTAGBOOT or NORMALBOOT */
+       if (ejtag_info->ejtag_version != EJTAG_VERSION_20) {
+               if (target->reset_halt) {
+                       /* use hardware to catch reset */
+                       mips_ejtag_set_instr(ejtag_info, EJTAG_INST_EJTAGBOOT);
+               } else
+                       mips_ejtag_set_instr(ejtag_info, EJTAG_INST_NORMALBOOT);
+       }
+
+       if (jtag_reset_config & RESET_HAS_SRST) {
                /* here we should issue a srst only, but we may have to assert trst as well */
                if (jtag_reset_config & RESET_SRST_PULLS_TRST)
                        jtag_add_reset(1, 1);
-               else
+               else if (!srst_asserted)
                        jtag_add_reset(0, 1);
        } else {
                if (mips_m4k->is_pic32mx) {
@@ -277,12 +378,6 @@ static int mips_m4k_deassert_reset(struct target *target)
        return ERROR_OK;
 }
 
-static int mips_m4k_soft_reset_halt(struct target *target)
-{
-       /* TODO */
-       return ERROR_OK;
-}
-
 static int mips_m4k_single_step_core(struct target *target)
 {
        struct mips32_common *mips32 = target_to_mips32(target);
@@ -302,7 +397,33 @@ static int mips_m4k_single_step_core(struct target *target)
        return ERROR_OK;
 }
 
-static int mips_m4k_resume(struct target *target, int current,
+static int mips_m4k_restore_smp(struct target *target, uint32_t address, int handle_breakpoints)
+{
+       int retval = ERROR_OK;
+       struct target_list *head;
+       struct target *curr;
+
+       head = target->head;
+       while (head != (struct target_list *)NULL) {
+               int ret = ERROR_OK;
+               curr = head->target;
+               if ((curr != target) && (curr->state != TARGET_RUNNING)) {
+                       /*  resume current address , not in step mode */
+                       ret = mips_m4k_internal_restore(curr, 1, address,
+                                                  handle_breakpoints, 0);
+
+                       if (ret != ERROR_OK) {
+                               LOG_ERROR("target->coreid :%" PRId32 " failed to resume at address :0x%" PRIx32,
+                                                 curr->coreid, address);
+                               retval = ret;
+                       }
+               }
+               head = head->next;
+       }
+       return retval;
+}
+
+static int mips_m4k_internal_restore(struct target *target, int current,
                uint32_t address, int handle_breakpoints, int debug_execution)
 {
        struct mips32_common *mips32 = target_to_mips32(target);
@@ -331,7 +452,10 @@ static int mips_m4k_resume(struct target *target, int current,
        if (ejtag_info->impcode & EJTAG_IMP_MIPS16)
                buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1, mips32->isa_mode);
 
-       resume_pc = buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32);
+       if (!current)
+               resume_pc = address;
+       else
+               resume_pc = buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32);
 
        mips32_restore_context(target);
 
@@ -370,6 +494,33 @@ static int mips_m4k_resume(struct target *target, int current,
        return ERROR_OK;
 }
 
+static int mips_m4k_resume(struct target *target, int current,
+               uint32_t address, int handle_breakpoints, int debug_execution)
+{
+       int retval = ERROR_OK;
+
+       /* dummy resume for smp toggle in order to reduce gdb impact  */
+       if ((target->smp) && (target->gdb_service->core[1] != -1)) {
+               /*   simulate a start and halt of target */
+               target->gdb_service->target = NULL;
+               target->gdb_service->core[0] = target->gdb_service->core[1];
+               /*  fake resume at next poll we play the  target core[1], see poll*/
+               target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+               return retval;
+       }
+
+       retval = mips_m4k_internal_restore(target, current, address,
+                               handle_breakpoints,
+                               debug_execution);
+
+       if (retval == ERROR_OK && target->smp) {
+               target->gdb_service->core[0] = -1;
+               retval = mips_m4k_restore_smp(target, address, handle_breakpoints);
+       }
+
+       return retval;
+}
+
 static int mips_m4k_step(struct target *target, int current,
                uint32_t address, int handle_breakpoints)
 {
@@ -417,12 +568,12 @@ static int mips_m4k_step(struct target *target, int current,
        /* registers are now invalid */
        register_cache_invalidate(mips32->core_cache);
 
+       LOG_DEBUG("target stepped ");
+       mips_m4k_debug_entry(target);
+
        if (breakpoint)
                mips_m4k_set_breakpoint(target, breakpoint);
 
-       LOG_DEBUG("target stepped ");
-
-       mips_m4k_debug_entry(target);
        target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 
        return ERROR_OK;
@@ -444,6 +595,7 @@ static int mips_m4k_set_breakpoint(struct target *target,
                struct breakpoint *breakpoint)
 {
        struct mips32_common *mips32 = target_to_mips32(target);
+       struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
        struct mips32_comparator *comparator_list = mips32->inst_break_list;
        int retval;
 
@@ -458,22 +610,31 @@ static int mips_m4k_set_breakpoint(struct target *target,
                while (comparator_list[bp_num].used && (bp_num < mips32->num_inst_bpoints))
                        bp_num++;
                if (bp_num >= mips32->num_inst_bpoints) {
-                       LOG_ERROR("Can not find free FP Comparator(bpid: %d)",
+                       LOG_ERROR("Can not find free FP Comparator(bpid: %" PRIu32 ")",
                                        breakpoint->unique_id);
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }
                breakpoint->set = bp_num + 1;
                comparator_list[bp_num].used = 1;
                comparator_list[bp_num].bp_value = breakpoint->address;
+
+               /* EJTAG 2.0 uses 30bit IBA. First 2 bits are reserved.
+                * Warning: there is no IB ASID registers in 2.0.
+                * Do not set it! :) */
+               if (ejtag_info->ejtag_version == EJTAG_VERSION_20)
+                       comparator_list[bp_num].bp_value &= 0xFFFFFFFC;
+
                target_write_u32(target, comparator_list[bp_num].reg_address,
                                comparator_list[bp_num].bp_value);
-               target_write_u32(target, comparator_list[bp_num].reg_address + 0x08, 0x00000000);
-               target_write_u32(target, comparator_list[bp_num].reg_address + 0x18, 1);
-               LOG_DEBUG("bpid: %d, bp_num %i bp_value 0x%" PRIx32 "",
+               target_write_u32(target, comparator_list[bp_num].reg_address +
+                                ejtag_info->ejtag_ibm_offs, 0x00000000);
+               target_write_u32(target, comparator_list[bp_num].reg_address +
+                                ejtag_info->ejtag_ibc_offs, 1);
+               LOG_DEBUG("bpid: %" PRIu32 ", bp_num %i bp_value 0x%" PRIx32 "",
                                  breakpoint->unique_id,
                                  bp_num, comparator_list[bp_num].bp_value);
        } else if (breakpoint->type == BKPT_SOFT) {
-               LOG_DEBUG("bpid: %d", breakpoint->unique_id);
+               LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
                if (breakpoint->length == 4) {
                        uint32_t verify = 0xffffffff;
 
@@ -525,6 +686,7 @@ static int mips_m4k_unset_breakpoint(struct target *target,
 {
        /* get pointers to arch-specific information */
        struct mips32_common *mips32 = target_to_mips32(target);
+       struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
        struct mips32_comparator *comparator_list = mips32->inst_break_list;
        int retval;
 
@@ -536,20 +698,21 @@ static int mips_m4k_unset_breakpoint(struct target *target,
        if (breakpoint->type == BKPT_HARD) {
                int bp_num = breakpoint->set - 1;
                if ((bp_num < 0) || (bp_num >= mips32->num_inst_bpoints)) {
-                       LOG_DEBUG("Invalid FP Comparator number in breakpoint (bpid: %d)",
+                       LOG_DEBUG("Invalid FP Comparator number in breakpoint (bpid: %" PRIu32 ")",
                                          breakpoint->unique_id);
                        return ERROR_OK;
                }
-               LOG_DEBUG("bpid: %d - releasing hw: %d",
+               LOG_DEBUG("bpid: %" PRIu32 " - releasing hw: %d",
                                breakpoint->unique_id,
                                bp_num);
                comparator_list[bp_num].used = 0;
                comparator_list[bp_num].bp_value = 0;
-               target_write_u32(target, comparator_list[bp_num].reg_address + 0x18, 0);
+               target_write_u32(target, comparator_list[bp_num].reg_address +
+                                ejtag_info->ejtag_ibc_offs, 0);
 
        } else {
                /* restore original instruction (kept in target endianness) */
-               LOG_DEBUG("bpid: %d", breakpoint->unique_id);
+               LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
                if (breakpoint->length == 4) {
                        uint32_t current_instr;
 
@@ -634,6 +797,7 @@ static int mips_m4k_set_watchpoint(struct target *target,
                struct watchpoint *watchpoint)
 {
        struct mips32_common *mips32 = target_to_mips32(target);
+       struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
        struct mips32_comparator *comparator_list = mips32->data_break_list;
        int wp_num = 0;
        /*
@@ -683,11 +847,25 @@ static int mips_m4k_set_watchpoint(struct target *target,
        watchpoint->set = wp_num + 1;
        comparator_list[wp_num].used = 1;
        comparator_list[wp_num].bp_value = watchpoint->address;
-       target_write_u32(target, comparator_list[wp_num].reg_address, comparator_list[wp_num].bp_value);
-       target_write_u32(target, comparator_list[wp_num].reg_address + 0x08, 0x00000000);
-       target_write_u32(target, comparator_list[wp_num].reg_address + 0x10, 0x00000000);
-       target_write_u32(target, comparator_list[wp_num].reg_address + 0x18, enable);
-       target_write_u32(target, comparator_list[wp_num].reg_address + 0x20, 0);
+
+       /* EJTAG 2.0 uses 29bit DBA. First 3 bits are reserved.
+        * There is as well no ASID register support. */
+       if (ejtag_info->ejtag_version == EJTAG_VERSION_20)
+               comparator_list[wp_num].bp_value &= 0xFFFFFFF8;
+       else
+               target_write_u32(target, comparator_list[wp_num].reg_address +
+                        ejtag_info->ejtag_dbasid_offs, 0x00000000);
+
+       target_write_u32(target, comparator_list[wp_num].reg_address,
+                        comparator_list[wp_num].bp_value);
+       target_write_u32(target, comparator_list[wp_num].reg_address +
+                        ejtag_info->ejtag_dbm_offs, 0x00000000);
+
+       target_write_u32(target, comparator_list[wp_num].reg_address +
+                        ejtag_info->ejtag_dbc_offs, enable);
+       /* TODO: probably this value is ignored on 2.0 */
+       target_write_u32(target, comparator_list[wp_num].reg_address +
+                        ejtag_info->ejtag_dbv_offs, 0);
        LOG_DEBUG("wp_num %i bp_value 0x%" PRIx32 "", wp_num, comparator_list[wp_num].bp_value);
 
        return ERROR_OK;
@@ -698,6 +876,7 @@ static int mips_m4k_unset_watchpoint(struct target *target,
 {
        /* get pointers to arch-specific information */
        struct mips32_common *mips32 = target_to_mips32(target);
+       struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
        struct mips32_comparator *comparator_list = mips32->data_break_list;
 
        if (!watchpoint->set) {
@@ -712,7 +891,8 @@ static int mips_m4k_unset_watchpoint(struct target *target,
        }
        comparator_list[wp_num].used = 0;
        comparator_list[wp_num].bp_value = 0;
-       target_write_u32(target, comparator_list[wp_num].reg_address + 0x18, 0);
+       target_write_u32(target, comparator_list[wp_num].reg_address +
+                        ejtag_info->ejtag_dbc_offs, 0);
        watchpoint->set = 0;
 
        return ERROR_OK;
@@ -837,6 +1017,13 @@ static int mips_m4k_write_memory(struct target *target, uint32_t address,
                return ERROR_TARGET_NOT_HALTED;
        }
 
+       if (size == 4 && count > 32) {
+               int retval = mips_m4k_bulk_write_memory(target, address, count, buffer);
+               if (retval == ERROR_OK)
+                       return ERROR_OK;
+               LOG_WARNING("Falling back to non-bulk write");
+       }
+
        /* sanitize arguments */
        if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
                return ERROR_COMMAND_SYNTAX_ERROR;
@@ -869,9 +1056,9 @@ static int mips_m4k_write_memory(struct target *target, uint32_t address,
        /* if noDMA off, use DMAACC mode for memory write */
        int retval;
        if (ejtag_info->impcode & EJTAG_IMP_NODMA)
-               retval = mips32_pracc_write_mem(ejtag_info, address, size, count, (void *)buffer);
+               retval = mips32_pracc_write_mem(ejtag_info, address, size, count, buffer);
        else
-               retval = mips32_dmaacc_write_mem(ejtag_info, address, size, count, (void *)buffer);
+               retval = mips32_dmaacc_write_mem(ejtag_info, address, size, count, buffer);
 
        if (t != NULL)
                free(t);
@@ -952,16 +1139,12 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
 {
        struct mips32_common *mips32 = target_to_mips32(target);
        struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+       struct working_area *fast_data_area;
        int retval;
        int write_t = 1;
 
        LOG_DEBUG("address: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "", address, count);
 
-       if (target->state != TARGET_HALTED) {
-               LOG_WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
        /* check alignment */
        if (address & 0x3u)
                return ERROR_TARGET_UNALIGNED_ACCESS;
@@ -975,14 +1158,25 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
                                MIPS32_FASTDATA_HANDLER_SIZE,
                                &mips32->fast_data_area);
                if (retval != ERROR_OK) {
-                       LOG_WARNING("No working area available, falling back to non-bulk write");
-                       return mips_m4k_write_memory(target, address, 4, count, buffer);
+                       LOG_ERROR("No working area available");
+                       return retval;
                }
 
                /* reset fastadata state so the algo get reloaded */
                ejtag_info->fast_access_save = -1;
        }
 
+       fast_data_area = mips32->fast_data_area;
+
+       if (address <= fast_data_area->address + fast_data_area->size &&
+                       fast_data_area->address <= address + count) {
+               LOG_ERROR("fast_data (0x%8.8" PRIx32 ") is within write area "
+                         "(0x%8.8" PRIx32 "-0x%8.8" PRIx32 ").",
+                         fast_data_area->address, address, address + count);
+               LOG_ERROR("Change work-area-phys or load_image address!");
+               return ERROR_FAIL;
+       }
+
        /* mips32_pracc_fastdata_xfer requires uint32_t in host endianness, */
        /* but byte array represents target endianness                      */
        uint32_t *t = NULL;
@@ -1000,11 +1194,8 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
        if (t != NULL)
                free(t);
 
-       if (retval != ERROR_OK) {
-               /* FASTDATA access failed, try normal memory write */
-               LOG_DEBUG("Fastdata access Failed, falling back to non-bulk write");
-               retval = mips_m4k_write_memory(target, address, 4, count, buffer);
-       }
+       if (retval != ERROR_OK)
+               LOG_ERROR("Fastdata access Failed");
 
        return retval;
 }
@@ -1045,7 +1236,6 @@ COMMAND_HANDLER(mips_m4k_handle_cp0_command)
 
                if (CMD_ARGC == 2) {
                        uint32_t value;
-
                        retval = mips32_cp0_read(ejtag_info, &value, cp0_reg, cp0_sel);
                        if (retval != ERROR_OK) {
                                command_print(CMD_CTX,
@@ -1053,12 +1243,9 @@ COMMAND_HANDLER(mips_m4k_handle_cp0_command)
                                                cp0_reg);
                                return ERROR_OK;
                        }
-                       retval = jtag_execute_queue();
-                       if (retval != ERROR_OK)
-                               return retval;
-
                        command_print(CMD_CTX, "cp0 reg %" PRIi32 ", select %" PRIi32 ": %8.8" PRIx32,
                                        cp0_reg, cp0_sel, value);
+
                } else if (CMD_ARGC == 3) {
                        uint32_t value;
                        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], value);
@@ -1077,6 +1264,87 @@ COMMAND_HANDLER(mips_m4k_handle_cp0_command)
        return ERROR_OK;
 }
 
+COMMAND_HANDLER(mips_m4k_handle_smp_off_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       /* check target is an smp target */
+       struct target_list *head;
+       struct target *curr;
+       head = target->head;
+       target->smp = 0;
+       if (head != (struct target_list *)NULL) {
+               while (head != (struct target_list *)NULL) {
+                       curr = head->target;
+                       curr->smp = 0;
+                       head = head->next;
+               }
+               /*  fixes the target display to the debugger */
+               target->gdb_service->target = target;
+       }
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(mips_m4k_handle_smp_on_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       struct target_list *head;
+       struct target *curr;
+       head = target->head;
+       if (head != (struct target_list *)NULL) {
+               target->smp = 1;
+               while (head != (struct target_list *)NULL) {
+                       curr = head->target;
+                       curr->smp = 1;
+                       head = head->next;
+               }
+       }
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(mips_m4k_handle_smp_gdb_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       int retval = ERROR_OK;
+       struct target_list *head;
+       head = target->head;
+       if (head != (struct target_list *)NULL) {
+               if (CMD_ARGC == 1) {
+                       int coreid = 0;
+                       COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], coreid);
+                       if (ERROR_OK != retval)
+                               return retval;
+                       target->gdb_service->core[1] = coreid;
+
+               }
+               command_print(CMD_CTX, "gdb coreid  %" PRId32 " -> %" PRId32, target->gdb_service->core[0]
+                       , target->gdb_service->core[1]);
+       }
+       return ERROR_OK;
+}
+
+COMMAND_HANDLER(mips_m4k_handle_scan_delay_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       struct mips_m4k_common *mips_m4k = target_to_m4k(target);
+       struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info;
+
+       if (CMD_ARGC == 1)
+               COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], ejtag_info->scan_delay);
+       else if (CMD_ARGC > 1)
+                       return ERROR_COMMAND_SYNTAX_ERROR;
+
+       command_print(CMD_CTX, "scan delay: %d nsec", ejtag_info->scan_delay);
+       if (ejtag_info->scan_delay >= 20000000) {
+               ejtag_info->mode = 0;
+               command_print(CMD_CTX, "running in legacy mode");
+       } else {
+               ejtag_info->mode = 1;
+               command_print(CMD_CTX, "running in fast queued mode");
+       }
+
+       return ERROR_OK;
+}
+
 static const struct command_registration mips_m4k_exec_command_handlers[] = {
        {
                .name = "cp0",
@@ -1085,6 +1353,34 @@ static const struct command_registration mips_m4k_exec_command_handlers[] = {
                .usage = "regnum [value]",
                .help = "display/modify cp0 register",
        },
+       {
+               .name = "smp_off",
+               .handler = mips_m4k_handle_smp_off_command,
+               .mode = COMMAND_EXEC,
+               .help = "Stop smp handling",
+               .usage = "",},
+
+       {
+               .name = "smp_on",
+               .handler = mips_m4k_handle_smp_on_command,
+               .mode = COMMAND_EXEC,
+               .help = "Restart smp handling",
+               .usage = "",
+       },
+       {
+               .name = "smp_gdb",
+               .handler = mips_m4k_handle_smp_gdb_command,
+               .mode = COMMAND_EXEC,
+               .help = "display/fix current core played to gdb",
+               .usage = "",
+       },
+       {
+               .name = "scan_delay",
+               .handler = mips_m4k_handle_scan_delay_command,
+               .mode = COMMAND_ANY,
+               .help = "display/set scan delay in nano seconds",
+               .usage = "[value]",
+       },
        COMMAND_REGISTRATION_DONE
 };
 
@@ -1108,21 +1404,17 @@ struct target_type mips_m4k_target = {
        .poll = mips_m4k_poll,
        .arch_state = mips32_arch_state,
 
-       .target_request_data = NULL,
-
        .halt = mips_m4k_halt,
        .resume = mips_m4k_resume,
        .step = mips_m4k_step,
 
        .assert_reset = mips_m4k_assert_reset,
        .deassert_reset = mips_m4k_deassert_reset,
-       .soft_reset_halt = mips_m4k_soft_reset_halt,
 
        .get_gdb_reg_list = mips32_get_gdb_reg_list,
 
        .read_memory = mips_m4k_read_memory,
        .write_memory = mips_m4k_write_memory,
-       .bulk_write_memory = mips_m4k_bulk_write_memory,
        .checksum_memory = mips32_checksum_memory,
        .blank_check_memory = mips32_blank_check_memory,