git.sur5r.net Git - openocd/blob - src/target/mips_m4k.c

   1 /***************************************************************************
   2  *   Copyright (C) 2008 by Spencer Oliver                                  *
   3  *   spen@spen-soft.co.uk                                                  *
   4  *                                                                         *
   5  *   Copyright (C) 2008 by David T.L. Wong                                 *
   6  *                                                                         *
   7  *   Copyright (C) 2009 by David N. Claffey <dnclaffey@gmail.com>          *
   8  *                                                                         *
   9  *   Copyright (C) 2011 by Drasko DRASKOVIC                                *
  10  *   drasko.draskovic@gmail.com                                            *
  11  *                                                                         *
  12  *   This program is free software; you can redistribute it and/or modify  *
  13  *   it under the terms of the GNU General Public License as published by  *
  14  *   the Free Software Foundation; either version 2 of the License, or     *
  15  *   (at your option) any later version.                                   *
  16  *                                                                         *
  17  *   This program is distributed in the hope that it will be useful,       *
  18  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  19  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  20  *   GNU General Public License for more details.                          *
  21  *                                                                         *
  22  *   You should have received a copy of the GNU General Public License     *
  23  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  24  ***************************************************************************/
  25
  26 #ifdef HAVE_CONFIG_H
  27 #include "config.h"
  28 #endif
  29
  30 #include "breakpoints.h"
  31 #include "mips32.h"
  32 #include "mips_m4k.h"
  33 #include "mips32_dmaacc.h"
  34 #include "target_type.h"
  35 #include "register.h"
  36
  37 static void mips_m4k_enable_breakpoints(struct target *target);
  38 static void mips_m4k_enable_watchpoints(struct target *target);
  39 static int mips_m4k_set_breakpoint(struct target *target,
  40                 struct breakpoint *breakpoint);
  41 static int mips_m4k_unset_breakpoint(struct target *target,
  42                 struct breakpoint *breakpoint);
  43 static int mips_m4k_internal_restore(struct target *target, int current,
  44                 target_addr_t address, int handle_breakpoints,
  45                 int debug_execution);
  46 static int mips_m4k_halt(struct target *target);
  47 static int mips_m4k_bulk_write_memory(struct target *target, target_addr_t address,
  48                 uint32_t count, const uint8_t *buffer);
  49
  50 static int mips_m4k_examine_debug_reason(struct target *target)
  51 {
  52         struct mips32_common *mips32 = target_to_mips32(target);
  53         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
  54         uint32_t break_status;
  55         int retval;
  56
  57         if ((target->debug_reason != DBG_REASON_DBGRQ)
  58                         && (target->debug_reason != DBG_REASON_SINGLESTEP)) {
  59                 if (ejtag_info->debug_caps & EJTAG_DCR_IB) {
  60                         /* get info about inst breakpoint support */
  61                         retval = target_read_u32(target,
  62                                 ejtag_info->ejtag_ibs_addr, &break_status);
  63                         if (retval != ERROR_OK)
  64                                 return retval;
  65                         if (break_status & 0x1f) {
  66                                 /* we have halted on a  breakpoint */
  67                                 retval = target_write_u32(target,
  68                                         ejtag_info->ejtag_ibs_addr, 0);
  69                                 if (retval != ERROR_OK)
  70                                         return retval;
  71                                 target->debug_reason = DBG_REASON_BREAKPOINT;
  72                         }
  73                 }
  74
  75                 if (ejtag_info->debug_caps & EJTAG_DCR_DB) {
  76                         /* get info about data breakpoint support */
  77                         retval = target_read_u32(target,
  78                                 ejtag_info->ejtag_dbs_addr, &break_status);
  79                         if (retval != ERROR_OK)
  80                                 return retval;
  81                         if (break_status & 0x1f) {
  82                                 /* we have halted on a  breakpoint */
  83                                 retval = target_write_u32(target,
  84                                         ejtag_info->ejtag_dbs_addr, 0);
  85                                 if (retval != ERROR_OK)
  86                                         return retval;
  87                                 target->debug_reason = DBG_REASON_WATCHPOINT;
  88                         }
  89                 }
  90         }
  91
  92         return ERROR_OK;
  93 }
  94
  95 static int mips_m4k_debug_entry(struct target *target)
  96 {
  97         struct mips32_common *mips32 = target_to_mips32(target);
  98         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
  99
 100         mips32_save_context(target);
 101
 102         /* make sure stepping disabled, SSt bit in CP0 debug register cleared */
 103         mips_ejtag_config_step(ejtag_info, 0);
 104
 105         /* make sure break unit configured */
 106         mips32_configure_break_unit(target);
 107
 108         /* attempt to find halt reason */
 109         mips_m4k_examine_debug_reason(target);
 110
 111         mips32_read_config_regs(target);
 112
 113         /* default to mips32 isa, it will be changed below if required */
 114         mips32->isa_mode = MIPS32_ISA_MIPS32;
 115
 116         /* other than mips32 only and isa bit set ? */
 117         if (mips32->isa_imp && buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1))
 118                 mips32->isa_mode = mips32->isa_imp == 2 ? MIPS32_ISA_MIPS16E : MIPS32_ISA_MMIPS32;
 119
 120         LOG_DEBUG("entered debug state at PC 0x%" PRIx32 ", target->state: %s",
 121                         buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32),
 122                         target_state_name(target));
 123
 124         return ERROR_OK;
 125 }
 126
 127 static struct target *get_mips_m4k(struct target *target, int32_t coreid)
 128 {
 129         struct target_list *head;
 130         struct target *curr;
 131
 132         head = target->head;
 133         while (head != (struct target_list *)NULL) {
 134                 curr = head->target;
 135                 if ((curr->coreid == coreid) && (curr->state == TARGET_HALTED))
 136                         return curr;
 137                 head = head->next;
 138         }
 139         return target;
 140 }
 141
 142 static int mips_m4k_halt_smp(struct target *target)
 143 {
 144         int retval = ERROR_OK;
 145         struct target_list *head;
 146         struct target *curr;
 147         head = target->head;
 148         while (head != (struct target_list *)NULL) {
 149                 int ret = ERROR_OK;
 150                 curr = head->target;
 151                 if ((curr != target) && (curr->state != TARGET_HALTED))
 152                         ret = mips_m4k_halt(curr);
 153
 154                 if (ret != ERROR_OK) {
 155                         LOG_ERROR("halt failed target->coreid: %" PRId32, curr->coreid);
 156                         retval = ret;
 157                 }
 158                 head = head->next;
 159         }
 160         return retval;
 161 }
 162
 163 static int update_halt_gdb(struct target *target)
 164 {
 165         int retval = ERROR_OK;
 166         if (target->gdb_service->core[0] == -1) {
 167                 target->gdb_service->target = target;
 168                 target->gdb_service->core[0] = target->coreid;
 169                 retval = mips_m4k_halt_smp(target);
 170         }
 171         return retval;
 172 }
 173
 174 static int mips_m4k_poll(struct target *target)
 175 {
 176         int retval = ERROR_OK;
 177         struct mips32_common *mips32 = target_to_mips32(target);
 178         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 179         uint32_t ejtag_ctrl = ejtag_info->ejtag_ctrl;
 180         enum target_state prev_target_state = target->state;
 181
 182         /*  toggle to another core is done by gdb as follow */
 183         /*  maint packet J core_id */
 184         /*  continue */
 185         /*  the next polling trigger an halt event sent to gdb */
 186         if ((target->state == TARGET_HALTED) && (target->smp) &&
 187                 (target->gdb_service) &&
 188                 (target->gdb_service->target == NULL)) {
 189                 target->gdb_service->target =
 190                         get_mips_m4k(target, target->gdb_service->core[1]);
 191                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 192                 return retval;
 193         }
 194
 195         /* read ejtag control reg */
 196         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 197         retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
 198         if (retval != ERROR_OK)
 199                 return retval;
 200
 201         ejtag_info->isa = (ejtag_ctrl & EJTAG_CTRL_DBGISA) ? 1 : 0;
 202
 203         /* clear this bit before handling polling
 204          * as after reset registers will read zero */
 205         if (ejtag_ctrl & EJTAG_CTRL_ROCC) {
 206                 /* we have detected a reset, clear flag
 207                  * otherwise ejtag will not work */
 208                 ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_ROCC;
 209
 210                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 211                 retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
 212                 if (retval != ERROR_OK)
 213                         return retval;
 214                 LOG_DEBUG("Reset Detected");
 215         }
 216
 217         /* check for processor halted */
 218         if (ejtag_ctrl & EJTAG_CTRL_BRKST) {
 219                 if ((target->state != TARGET_HALTED)
 220                     && (target->state != TARGET_DEBUG_RUNNING)) {
 221                         if (target->state == TARGET_UNKNOWN)
 222                                 LOG_DEBUG("EJTAG_CTRL_BRKST already set during server startup.");
 223
 224                         /* OpenOCD was was probably started on the board with EJTAG_CTRL_BRKST already set
 225                          * (maybe put on by HALT-ing the board in the previous session).
 226                          *
 227                          * Force enable debug entry for this session.
 228                          */
 229                         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_NORMALBOOT);
 230                         target->state = TARGET_HALTED;
 231                         retval = mips_m4k_debug_entry(target);
 232                         if (retval != ERROR_OK)
 233                                 return retval;
 234
 235                         if (target->smp &&
 236                                 ((prev_target_state == TARGET_RUNNING)
 237                              || (prev_target_state == TARGET_RESET))) {
 238                                 retval = update_halt_gdb(target);
 239                                 if (retval != ERROR_OK)
 240                                         return retval;
 241                         }
 242                         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 243                 } else if (target->state == TARGET_DEBUG_RUNNING) {
 244                         target->state = TARGET_HALTED;
 245
 246                         retval = mips_m4k_debug_entry(target);
 247                         if (retval != ERROR_OK)
 248                                 return retval;
 249
 250                         if (target->smp) {
 251                                 retval = update_halt_gdb(target);
 252                                 if (retval != ERROR_OK)
 253                                         return retval;
 254                         }
 255
 256                         target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 257                 }
 258         } else
 259                 target->state = TARGET_RUNNING;
 260
 261 /*      LOG_DEBUG("ctrl = 0x%08X", ejtag_ctrl); */
 262
 263         return ERROR_OK;
 264 }
 265
 266 static int mips_m4k_halt(struct target *target)
 267 {
 268         struct mips32_common *mips32 = target_to_mips32(target);
 269         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 270
 271         LOG_DEBUG("target->state: %s", target_state_name(target));
 272
 273         if (target->state == TARGET_HALTED) {
 274                 LOG_DEBUG("target was already halted");
 275                 return ERROR_OK;
 276         }
 277
 278         if (target->state == TARGET_UNKNOWN)
 279                 LOG_WARNING("target was in unknown state when halt was requested");
 280
 281         if (target->state == TARGET_RESET) {
 282                 if ((jtag_get_reset_config() & RESET_SRST_PULLS_TRST) && jtag_get_srst()) {
 283                         LOG_ERROR("can't request a halt while in reset if nSRST pulls nTRST");
 284                         return ERROR_TARGET_FAILURE;
 285                 } else {
 286                         /* we came here in a reset_halt or reset_init sequence
 287                          * debug entry was already prepared in mips_m4k_assert_reset()
 288                          */
 289                         target->debug_reason = DBG_REASON_DBGRQ;
 290
 291                         return ERROR_OK;
 292                 }
 293         }
 294
 295         /* break processor */
 296         mips_ejtag_enter_debug(ejtag_info);
 297
 298         target->debug_reason = DBG_REASON_DBGRQ;
 299
 300         return ERROR_OK;
 301 }
 302
 303 static int mips_m4k_assert_reset(struct target *target)
 304 {
 305         struct mips_m4k_common *mips_m4k = target_to_m4k(target);
 306         struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info;
 307
 308         /* TODO: apply hw reset signal in not examined state */
 309         if (!(target_was_examined(target))) {
 310                 LOG_WARNING("Reset is not asserted because the target is not examined.");
 311                 LOG_WARNING("Use a reset button or power cycle the target.");
 312                 return ERROR_TARGET_NOT_EXAMINED;
 313         }
 314
 315         LOG_DEBUG("target->state: %s",
 316                 target_state_name(target));
 317
 318         enum reset_types jtag_reset_config = jtag_get_reset_config();
 319
 320         /* some cores support connecting while srst is asserted
 321          * use that mode is it has been configured */
 322
 323         bool srst_asserted = false;
 324
 325         if (!(jtag_reset_config & RESET_SRST_PULLS_TRST) &&
 326                         (jtag_reset_config & RESET_SRST_NO_GATING)) {
 327                 jtag_add_reset(0, 1);
 328                 srst_asserted = true;
 329         }
 330
 331
 332         /* EJTAG before v2.5/2.6 does not support EJTAGBOOT or NORMALBOOT */
 333         if (ejtag_info->ejtag_version != EJTAG_VERSION_20) {
 334                 if (target->reset_halt) {
 335                         /* use hardware to catch reset */
 336                         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_EJTAGBOOT);
 337                 } else
 338                         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_NORMALBOOT);
 339         }
 340
 341         if (jtag_reset_config & RESET_HAS_SRST) {
 342                 /* here we should issue a srst only, but we may have to assert trst as well */
 343                 if (jtag_reset_config & RESET_SRST_PULLS_TRST)
 344                         jtag_add_reset(1, 1);
 345                 else if (!srst_asserted)
 346                         jtag_add_reset(0, 1);
 347         } else if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT)) {
 348                 target_handle_event(target, TARGET_EVENT_RESET_ASSERT);
 349         } else {
 350                 if (mips_m4k->is_pic32mx) {
 351                         LOG_DEBUG("Using MTAP reset to reset processor...");
 352
 353                         /* use microchip specific MTAP reset */
 354                         mips_ejtag_set_instr(ejtag_info, MTAP_SW_MTAP);
 355                         mips_ejtag_set_instr(ejtag_info, MTAP_COMMAND);
 356
 357                         mips_ejtag_drscan_8_out(ejtag_info, MCHP_ASERT_RST);
 358                         mips_ejtag_drscan_8_out(ejtag_info, MCHP_DE_ASSERT_RST);
 359                         mips_ejtag_set_instr(ejtag_info, MTAP_SW_ETAP);
 360                 } else {
 361                         /* use ejtag reset - not supported by all cores */
 362                         uint32_t ejtag_ctrl = ejtag_info->ejtag_ctrl | EJTAG_CTRL_PRRST | EJTAG_CTRL_PERRST;
 363                         LOG_DEBUG("Using EJTAG reset (PRRST) to reset processor...");
 364                         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 365                         mips_ejtag_drscan_32_out(ejtag_info, ejtag_ctrl);
 366                 }
 367         }
 368
 369         target->state = TARGET_RESET;
 370         jtag_add_sleep(50000);
 371
 372         register_cache_invalidate(mips_m4k->mips32.core_cache);
 373
 374         if (target->reset_halt) {
 375                 int retval = target_halt(target);
 376                 if (retval != ERROR_OK)
 377                         return retval;
 378         }
 379
 380         return ERROR_OK;
 381 }
 382
 383 static int mips_m4k_deassert_reset(struct target *target)
 384 {
 385         LOG_DEBUG("target->state: %s", target_state_name(target));
 386
 387         /* deassert reset lines */
 388         jtag_add_reset(0, 0);
 389
 390         return ERROR_OK;
 391 }
 392
 393 static int mips_m4k_single_step_core(struct target *target)
 394 {
 395         struct mips32_common *mips32 = target_to_mips32(target);
 396         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 397
 398         /* configure single step mode */
 399         mips_ejtag_config_step(ejtag_info, 1);
 400
 401         /* disable interrupts while stepping */
 402         mips32_enable_interrupts(target, 0);
 403
 404         /* exit debug mode */
 405         mips_ejtag_exit_debug(ejtag_info);
 406
 407         mips_m4k_debug_entry(target);
 408
 409         return ERROR_OK;
 410 }
 411
 412 static int mips_m4k_restore_smp(struct target *target, uint32_t address, int handle_breakpoints)
 413 {
 414         int retval = ERROR_OK;
 415         struct target_list *head;
 416         struct target *curr;
 417
 418         head = target->head;
 419         while (head != (struct target_list *)NULL) {
 420                 int ret = ERROR_OK;
 421                 curr = head->target;
 422                 if ((curr != target) && (curr->state != TARGET_RUNNING)) {
 423                         /*  resume current address , not in step mode */
 424                         ret = mips_m4k_internal_restore(curr, 1, address,
 425                                                    handle_breakpoints, 0);
 426
 427                         if (ret != ERROR_OK) {
 428                                 LOG_ERROR("target->coreid :%" PRId32 " failed to resume at address :0x%" PRIx32,
 429                                                   curr->coreid, address);
 430                                 retval = ret;
 431                         }
 432                 }
 433                 head = head->next;
 434         }
 435         return retval;
 436 }
 437
 438 static int mips_m4k_internal_restore(struct target *target, int current,
 439                 target_addr_t address, int handle_breakpoints, int debug_execution)
 440 {
 441         struct mips32_common *mips32 = target_to_mips32(target);
 442         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 443         struct breakpoint *breakpoint = NULL;
 444         uint32_t resume_pc;
 445
 446         if (target->state != TARGET_HALTED) {
 447                 LOG_WARNING("target not halted");
 448                 return ERROR_TARGET_NOT_HALTED;
 449         }
 450
 451         if (!debug_execution) {
 452                 target_free_all_working_areas(target);
 453                 mips_m4k_enable_breakpoints(target);
 454                 mips_m4k_enable_watchpoints(target);
 455         }
 456
 457         /* current = 1: continue on current pc, otherwise continue at <address> */
 458         if (!current) {
 459                 mips_m4k_isa_filter(mips32->isa_imp, &address);
 460                 buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32, address);
 461                 mips32->core_cache->reg_list[MIPS32_PC].dirty = 1;
 462                 mips32->core_cache->reg_list[MIPS32_PC].valid = 1;
 463         }
 464
 465         if ((mips32->isa_imp > 1) &&  debug_execution)  /* if more than one isa supported */
 466                 buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1, mips32->isa_mode);
 467
 468         if (!current)
 469                 resume_pc = address;
 470         else
 471                 resume_pc = buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32);
 472
 473         mips32_restore_context(target);
 474
 475         /* the front-end may request us not to handle breakpoints */
 476         if (handle_breakpoints) {
 477                 /* Single step past breakpoint at current address */
 478                 breakpoint = breakpoint_find(target, resume_pc);
 479                 if (breakpoint) {
 480                         LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT "",
 481                                           breakpoint->address);
 482                         mips_m4k_unset_breakpoint(target, breakpoint);
 483                         mips_m4k_single_step_core(target);
 484                         mips_m4k_set_breakpoint(target, breakpoint);
 485                 }
 486         }
 487
 488         /* enable interrupts if we are running */
 489         mips32_enable_interrupts(target, !debug_execution);
 490
 491         /* exit debug mode */
 492         mips_ejtag_exit_debug(ejtag_info);
 493         target->debug_reason = DBG_REASON_NOTHALTED;
 494
 495         /* registers are now invalid */
 496         register_cache_invalidate(mips32->core_cache);
 497
 498         if (!debug_execution) {
 499                 target->state = TARGET_RUNNING;
 500                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 501                 LOG_DEBUG("target resumed at 0x%" PRIx32 "", resume_pc);
 502         } else {
 503                 target->state = TARGET_DEBUG_RUNNING;
 504                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
 505                 LOG_DEBUG("target debug resumed at 0x%" PRIx32 "", resume_pc);
 506         }
 507
 508         return ERROR_OK;
 509 }
 510
 511 static int mips_m4k_resume(struct target *target, int current,
 512                 target_addr_t address, int handle_breakpoints, int debug_execution)
 513 {
 514         int retval = ERROR_OK;
 515
 516         /* dummy resume for smp toggle in order to reduce gdb impact  */
 517         if ((target->smp) && (target->gdb_service->core[1] != -1)) {
 518                 /*   simulate a start and halt of target */
 519                 target->gdb_service->target = NULL;
 520                 target->gdb_service->core[0] = target->gdb_service->core[1];
 521                 /*  fake resume at next poll we play the  target core[1], see poll*/
 522                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 523                 return retval;
 524         }
 525
 526         retval = mips_m4k_internal_restore(target, current, address,
 527                                 handle_breakpoints,
 528                                 debug_execution);
 529
 530         if (retval == ERROR_OK && target->smp) {
 531                 target->gdb_service->core[0] = -1;
 532                 retval = mips_m4k_restore_smp(target, address, handle_breakpoints);
 533         }
 534
 535         return retval;
 536 }
 537
 538 static int mips_m4k_step(struct target *target, int current,
 539                 target_addr_t address, int handle_breakpoints)
 540 {
 541         /* get pointers to arch-specific information */
 542         struct mips32_common *mips32 = target_to_mips32(target);
 543         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 544         struct breakpoint *breakpoint = NULL;
 545
 546         if (target->state != TARGET_HALTED) {
 547                 LOG_WARNING("target not halted");
 548                 return ERROR_TARGET_NOT_HALTED;
 549         }
 550
 551         /* current = 1: continue on current pc, otherwise continue at <address> */
 552         if (!current) {
 553                 mips_m4k_isa_filter(mips32->isa_imp, &address);
 554                 buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32, address);
 555                 mips32->core_cache->reg_list[MIPS32_PC].dirty = 1;
 556                 mips32->core_cache->reg_list[MIPS32_PC].valid = 1;
 557         }
 558
 559         /* the front-end may request us not to handle breakpoints */
 560         if (handle_breakpoints) {
 561                 breakpoint = breakpoint_find(target,
 562                                 buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32));
 563                 if (breakpoint)
 564                         mips_m4k_unset_breakpoint(target, breakpoint);
 565         }
 566
 567         /* restore context */
 568         mips32_restore_context(target);
 569
 570         /* configure single step mode */
 571         mips_ejtag_config_step(ejtag_info, 1);
 572
 573         target->debug_reason = DBG_REASON_SINGLESTEP;
 574
 575         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 576
 577         /* disable interrupts while stepping */
 578         mips32_enable_interrupts(target, 0);
 579
 580         /* exit debug mode */
 581         mips_ejtag_exit_debug(ejtag_info);
 582
 583         /* registers are now invalid */
 584         register_cache_invalidate(mips32->core_cache);
 585
 586         LOG_DEBUG("target stepped ");
 587         mips_m4k_debug_entry(target);
 588
 589         if (breakpoint)
 590                 mips_m4k_set_breakpoint(target, breakpoint);
 591
 592         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 593
 594         return ERROR_OK;
 595 }
 596
 597 static void mips_m4k_enable_breakpoints(struct target *target)
 598 {
 599         struct breakpoint *breakpoint = target->breakpoints;
 600
 601         /* set any pending breakpoints */
 602         while (breakpoint) {
 603                 if (breakpoint->set == 0)
 604                         mips_m4k_set_breakpoint(target, breakpoint);
 605                 breakpoint = breakpoint->next;
 606         }
 607 }
 608
 609 static int mips_m4k_set_breakpoint(struct target *target,
 610                 struct breakpoint *breakpoint)
 611 {
 612         struct mips32_common *mips32 = target_to_mips32(target);
 613         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 614         struct mips32_comparator *comparator_list = mips32->inst_break_list;
 615         int retval;
 616
 617         if (breakpoint->set) {
 618                 LOG_WARNING("breakpoint already set");
 619                 return ERROR_OK;
 620         }
 621
 622         if (breakpoint->type == BKPT_HARD) {
 623                 int bp_num = 0;
 624
 625                 while (comparator_list[bp_num].used && (bp_num < mips32->num_inst_bpoints))
 626                         bp_num++;
 627                 if (bp_num >= mips32->num_inst_bpoints) {
 628                         LOG_ERROR("Can not find free FP Comparator(bpid: %" PRIu32 ")",
 629                                         breakpoint->unique_id);
 630                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 631                 }
 632                 breakpoint->set = bp_num + 1;
 633                 comparator_list[bp_num].used = 1;
 634                 comparator_list[bp_num].bp_value = breakpoint->address;
 635
 636                 if (breakpoint->length != 4)                    /* make sure isa bit set */
 637                         comparator_list[bp_num].bp_value |= 1;
 638                 else                                            /* make sure isa bit cleared */
 639                         comparator_list[bp_num].bp_value &= ~1;
 640
 641                 /* EJTAG 2.0 uses 30bit IBA. First 2 bits are reserved.
 642                  * Warning: there is no IB ASID registers in 2.0.
 643                  * Do not set it! :) */
 644                 if (ejtag_info->ejtag_version == EJTAG_VERSION_20)
 645                         comparator_list[bp_num].bp_value &= 0xFFFFFFFC;
 646
 647                 target_write_u32(target, comparator_list[bp_num].reg_address,
 648                                 comparator_list[bp_num].bp_value);
 649                 target_write_u32(target, comparator_list[bp_num].reg_address +
 650                                  ejtag_info->ejtag_ibm_offs, 0x00000000);
 651                 target_write_u32(target, comparator_list[bp_num].reg_address +
 652                                  ejtag_info->ejtag_ibc_offs, 1);
 653                 LOG_DEBUG("bpid: %" PRIu32 ", bp_num %i bp_value 0x%" PRIx32 "",
 654                                   breakpoint->unique_id,
 655                                   bp_num, comparator_list[bp_num].bp_value);
 656         } else if (breakpoint->type == BKPT_SOFT) {
 657                 LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
 658
 659                 uint32_t isa_req = breakpoint->length & 1;      /* micro mips request bit */
 660                 uint32_t bplength = breakpoint->length & ~1;    /* drop micro mips request bit for length */
 661                 uint32_t bpaddr = breakpoint->address & ~1;     /* drop isa bit from address, if set */
 662
 663                 if (bplength == 4) {
 664                         uint32_t verify = 0xffffffff;
 665                         uint32_t sdbbp32_instr = MIPS32_SDBBP(isa_req);
 666                         if (ejtag_info->endianness && isa_req)
 667                                 sdbbp32_instr = SWAP16(sdbbp32_instr);
 668
 669                         if ((breakpoint->address & 3) == 0) {   /* word alligned */
 670
 671                                 retval = target_read_memory(target, bpaddr, bplength, 1, breakpoint->orig_instr);
 672                                 if (retval != ERROR_OK)
 673                                         return retval;
 674
 675                                 retval = target_write_u32(target, bpaddr, sdbbp32_instr);
 676                                 if (retval != ERROR_OK)
 677                                         return retval;
 678
 679                                 retval = target_read_u32(target, bpaddr, &verify);
 680                                 if (retval != ERROR_OK)
 681                                         return retval;
 682
 683                                 if (verify != sdbbp32_instr)
 684                                         verify = 0;
 685
 686                         } else {        /* 16 bit aligned */
 687                                 retval = target_read_memory(target, bpaddr, 2, 2, breakpoint->orig_instr);
 688                                 if (retval != ERROR_OK)
 689                                         return retval;
 690
 691                                 uint8_t sdbbp_buf[4];
 692                                 target_buffer_set_u32(target, sdbbp_buf, sdbbp32_instr);
 693
 694                                 retval = target_write_memory(target, bpaddr, 2, 2, sdbbp_buf);
 695                                 if (retval != ERROR_OK)
 696                                         return retval;
 697
 698                                 retval = target_read_memory(target, bpaddr, 2, 2, sdbbp_buf);
 699                                 if (retval != ERROR_OK)
 700                                         return retval;
 701
 702                                 if (target_buffer_get_u32(target, sdbbp_buf) != sdbbp32_instr)
 703                                         verify = 0;
 704                         }
 705
 706                         if (verify == 0) {
 707                                 LOG_ERROR("Unable to set 32bit breakpoint at address %08" TARGET_PRIxADDR
 708                                         " - check that memory is read/writable", breakpoint->address);
 709                                 return ERROR_OK;
 710                         }
 711
 712                 } else {
 713                         uint16_t verify = 0xffff;
 714
 715                         retval = target_read_memory(target, bpaddr, bplength, 1, breakpoint->orig_instr);
 716                         if (retval != ERROR_OK)
 717                                 return retval;
 718
 719                         retval = target_write_u16(target, bpaddr, MIPS16_SDBBP(isa_req));
 720                         if (retval != ERROR_OK)
 721                                 return retval;
 722
 723                         retval = target_read_u16(target, bpaddr, &verify);
 724                         if (retval != ERROR_OK)
 725                                 return retval;
 726
 727                         if (verify != MIPS16_SDBBP(isa_req)) {
 728                                 LOG_ERROR("Unable to set 16bit breakpoint at address %08" TARGET_PRIxADDR
 729                                                 " - check that memory is read/writable", breakpoint->address);
 730                                 return ERROR_OK;
 731                         }
 732                 }
 733
 734                 breakpoint->set = 20; /* Any nice value but 0 */
 735         }
 736
 737         return ERROR_OK;
 738 }
 739
 740 static int mips_m4k_unset_breakpoint(struct target *target,
 741                 struct breakpoint *breakpoint)
 742 {
 743         /* get pointers to arch-specific information */
 744         struct mips32_common *mips32 = target_to_mips32(target);
 745         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 746         struct mips32_comparator *comparator_list = mips32->inst_break_list;
 747         int retval;
 748
 749         if (!breakpoint->set) {
 750                 LOG_WARNING("breakpoint not set");
 751                 return ERROR_OK;
 752         }
 753
 754         if (breakpoint->type == BKPT_HARD) {
 755                 int bp_num = breakpoint->set - 1;
 756                 if ((bp_num < 0) || (bp_num >= mips32->num_inst_bpoints)) {
 757                         LOG_DEBUG("Invalid FP Comparator number in breakpoint (bpid: %" PRIu32 ")",
 758                                           breakpoint->unique_id);
 759                         return ERROR_OK;
 760                 }
 761                 LOG_DEBUG("bpid: %" PRIu32 " - releasing hw: %d",
 762                                 breakpoint->unique_id,
 763                                 bp_num);
 764                 comparator_list[bp_num].used = 0;
 765                 comparator_list[bp_num].bp_value = 0;
 766                 target_write_u32(target, comparator_list[bp_num].reg_address +
 767                                  ejtag_info->ejtag_ibc_offs, 0);
 768
 769         } else {
 770                 /* restore original instruction (kept in target endianness) */
 771                 uint32_t isa_req = breakpoint->length & 1;
 772                 uint32_t bplength = breakpoint->length & ~1;
 773                 uint8_t current_instr[4];
 774                 LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
 775                 if (bplength == 4) {
 776                         uint32_t sdbbp32_instr =  MIPS32_SDBBP(isa_req);
 777                         if (ejtag_info->endianness && isa_req)
 778                                 sdbbp32_instr = SWAP16(sdbbp32_instr);
 779
 780                         if ((breakpoint->address & 3) == 0) {           /* 32bit aligned */
 781                                 /* check that user program has not modified breakpoint instruction */
 782                                 retval = target_read_memory(target, breakpoint->address, 4, 1, current_instr);
 783                                 if (retval != ERROR_OK)
 784                                         return retval;
 785                                 /**
 786                                 * target_read_memory() gets us data in _target_ endianess.
 787                                 * If we want to use this data on the host for comparisons with some macros
 788                                 * we must first transform it to _host_ endianess using target_buffer_get_u16().
 789                                 */
 790                                 if (sdbbp32_instr == target_buffer_get_u32(target, current_instr)) {
 791                                         retval = target_write_memory(target, breakpoint->address, 4, 1,
 792                                                                                 breakpoint->orig_instr);
 793                                         if (retval != ERROR_OK)
 794                                                 return retval;
 795                                 }
 796                         } else {        /* 16bit alligned */
 797                                 retval = target_read_memory(target, breakpoint->address, 2, 2, current_instr);
 798                                 if (retval != ERROR_OK)
 799                                         return retval;
 800
 801                                 if (sdbbp32_instr == target_buffer_get_u32(target, current_instr)) {
 802                                         retval = target_write_memory(target, breakpoint->address, 2, 2,
 803                                                                                 breakpoint->orig_instr);
 804                                         if (retval != ERROR_OK)
 805                                                 return retval;
 806                                 }
 807                         }
 808                 } else {
 809                         /* check that user program has not modified breakpoint instruction */
 810                         retval = target_read_memory(target, breakpoint->address, 2, 1, current_instr);
 811                         if (retval != ERROR_OK)
 812                                 return retval;
 813
 814                         if (target_buffer_get_u16(target, current_instr) == MIPS16_SDBBP(isa_req)) {
 815                                 retval = target_write_memory(target, breakpoint->address, 2, 1,
 816                                                                         breakpoint->orig_instr);
 817                                 if (retval != ERROR_OK)
 818                                         return retval;
 819                         }
 820                 }
 821         }
 822
 823         breakpoint->set = 0;
 824
 825         return ERROR_OK;
 826 }
 827
 828 static int mips_m4k_add_breakpoint(struct target *target, struct breakpoint *breakpoint)
 829 {
 830         struct mips32_common *mips32 = target_to_mips32(target);
 831
 832         if ((breakpoint->length > 5 || breakpoint->length < 2) ||               /* out of range */
 833                 (breakpoint->length == 4 && (breakpoint->address & 2)) ||       /* mips32 unaligned */
 834                 (mips32->isa_imp == MIPS32_ONLY && breakpoint->length != 4) ||  /* misp32 specific */
 835                 ((mips32->isa_imp & 1) != (breakpoint->length & 1)))            /* isa not implemented */
 836                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 837
 838         if (breakpoint->type == BKPT_HARD) {
 839                 if (mips32->num_inst_bpoints_avail < 1) {
 840                         LOG_INFO("no hardware breakpoint available");
 841                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 842                 }
 843
 844                 mips32->num_inst_bpoints_avail--;
 845         }
 846
 847         return mips_m4k_set_breakpoint(target, breakpoint);
 848 }
 849
 850 static int mips_m4k_remove_breakpoint(struct target *target,
 851                 struct breakpoint *breakpoint)
 852 {
 853         /* get pointers to arch-specific information */
 854         struct mips32_common *mips32 = target_to_mips32(target);
 855
 856         if (target->state != TARGET_HALTED) {
 857                 LOG_WARNING("target not halted");
 858                 return ERROR_TARGET_NOT_HALTED;
 859         }
 860
 861         if (breakpoint->set)
 862                 mips_m4k_unset_breakpoint(target, breakpoint);
 863
 864         if (breakpoint->type == BKPT_HARD)
 865                 mips32->num_inst_bpoints_avail++;
 866
 867         return ERROR_OK;
 868 }
 869
 870 static int mips_m4k_set_watchpoint(struct target *target,
 871                 struct watchpoint *watchpoint)
 872 {
 873         struct mips32_common *mips32 = target_to_mips32(target);
 874         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 875         struct mips32_comparator *comparator_list = mips32->data_break_list;
 876         int wp_num = 0;
 877         /*
 878          * watchpoint enabled, ignore all byte lanes in value register
 879          * and exclude both load and store accesses from  watchpoint
 880          * condition evaluation
 881         */
 882         int enable = EJTAG_DBCn_NOSB | EJTAG_DBCn_NOLB | EJTAG_DBCn_BE |
 883                         (0xff << EJTAG_DBCn_BLM_SHIFT);
 884
 885         if (watchpoint->set) {
 886                 LOG_WARNING("watchpoint already set");
 887                 return ERROR_OK;
 888         }
 889
 890         while (comparator_list[wp_num].used && (wp_num < mips32->num_data_bpoints))
 891                 wp_num++;
 892         if (wp_num >= mips32->num_data_bpoints) {
 893                 LOG_ERROR("Can not find free FP Comparator");
 894                 return ERROR_FAIL;
 895         }
 896
 897         if (watchpoint->length != 4) {
 898                 LOG_ERROR("Only watchpoints of length 4 are supported");
 899                 return ERROR_TARGET_UNALIGNED_ACCESS;
 900         }
 901
 902         if (watchpoint->address % 4) {
 903                 LOG_ERROR("Watchpoints address should be word aligned");
 904                 return ERROR_TARGET_UNALIGNED_ACCESS;
 905         }
 906
 907         switch (watchpoint->rw) {
 908                 case WPT_READ:
 909                         enable &= ~EJTAG_DBCn_NOLB;
 910                         break;
 911                 case WPT_WRITE:
 912                         enable &= ~EJTAG_DBCn_NOSB;
 913                         break;
 914                 case WPT_ACCESS:
 915                         enable &= ~(EJTAG_DBCn_NOLB | EJTAG_DBCn_NOSB);
 916                         break;
 917                 default:
 918                         LOG_ERROR("BUG: watchpoint->rw neither read, write nor access");
 919         }
 920
 921         watchpoint->set = wp_num + 1;
 922         comparator_list[wp_num].used = 1;
 923         comparator_list[wp_num].bp_value = watchpoint->address;
 924
 925         /* EJTAG 2.0 uses 29bit DBA. First 3 bits are reserved.
 926          * There is as well no ASID register support. */
 927         if (ejtag_info->ejtag_version == EJTAG_VERSION_20)
 928                 comparator_list[wp_num].bp_value &= 0xFFFFFFF8;
 929         else
 930                 target_write_u32(target, comparator_list[wp_num].reg_address +
 931                          ejtag_info->ejtag_dbasid_offs, 0x00000000);
 932
 933         target_write_u32(target, comparator_list[wp_num].reg_address,
 934                          comparator_list[wp_num].bp_value);
 935         target_write_u32(target, comparator_list[wp_num].reg_address +
 936                          ejtag_info->ejtag_dbm_offs, 0x00000000);
 937
 938         target_write_u32(target, comparator_list[wp_num].reg_address +
 939                          ejtag_info->ejtag_dbc_offs, enable);
 940         /* TODO: probably this value is ignored on 2.0 */
 941         target_write_u32(target, comparator_list[wp_num].reg_address +
 942                          ejtag_info->ejtag_dbv_offs, 0);
 943         LOG_DEBUG("wp_num %i bp_value 0x%" PRIx32 "", wp_num, comparator_list[wp_num].bp_value);
 944
 945         return ERROR_OK;
 946 }
 947
 948 static int mips_m4k_unset_watchpoint(struct target *target,
 949                 struct watchpoint *watchpoint)
 950 {
 951         /* get pointers to arch-specific information */
 952         struct mips32_common *mips32 = target_to_mips32(target);
 953         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 954         struct mips32_comparator *comparator_list = mips32->data_break_list;
 955
 956         if (!watchpoint->set) {
 957                 LOG_WARNING("watchpoint not set");
 958                 return ERROR_OK;
 959         }
 960
 961         int wp_num = watchpoint->set - 1;
 962         if ((wp_num < 0) || (wp_num >= mips32->num_data_bpoints)) {
 963                 LOG_DEBUG("Invalid FP Comparator number in watchpoint");
 964                 return ERROR_OK;
 965         }
 966         comparator_list[wp_num].used = 0;
 967         comparator_list[wp_num].bp_value = 0;
 968         target_write_u32(target, comparator_list[wp_num].reg_address +
 969                          ejtag_info->ejtag_dbc_offs, 0);
 970         watchpoint->set = 0;
 971
 972         return ERROR_OK;
 973 }
 974
 975 static int mips_m4k_add_watchpoint(struct target *target, struct watchpoint *watchpoint)
 976 {
 977         struct mips32_common *mips32 = target_to_mips32(target);
 978
 979         if (mips32->num_data_bpoints_avail < 1) {
 980                 LOG_INFO("no hardware watchpoints available");
 981                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 982         }
 983
 984         mips32->num_data_bpoints_avail--;
 985
 986         mips_m4k_set_watchpoint(target, watchpoint);
 987         return ERROR_OK;
 988 }
 989
 990 static int mips_m4k_remove_watchpoint(struct target *target,
 991                 struct watchpoint *watchpoint)
 992 {
 993         /* get pointers to arch-specific information */
 994         struct mips32_common *mips32 = target_to_mips32(target);
 995
 996         if (target->state != TARGET_HALTED) {
 997                 LOG_WARNING("target not halted");
 998                 return ERROR_TARGET_NOT_HALTED;
 999         }
1000
1001         if (watchpoint->set)
1002                 mips_m4k_unset_watchpoint(target, watchpoint);
1003
1004         mips32->num_data_bpoints_avail++;
1005
1006         return ERROR_OK;
1007 }
1008
1009 static void mips_m4k_enable_watchpoints(struct target *target)
1010 {
1011         struct watchpoint *watchpoint = target->watchpoints;
1012
1013         /* set any pending watchpoints */
1014         while (watchpoint) {
1015                 if (watchpoint->set == 0)
1016                         mips_m4k_set_watchpoint(target, watchpoint);
1017                 watchpoint = watchpoint->next;
1018         }
1019 }
1020
1021 static int mips_m4k_read_memory(struct target *target, target_addr_t address,
1022                 uint32_t size, uint32_t count, uint8_t *buffer)
1023 {
1024         struct mips32_common *mips32 = target_to_mips32(target);
1025         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
1026
1027         LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
1028                         address, size, count);
1029
1030         if (target->state != TARGET_HALTED) {
1031                 LOG_WARNING("target not halted");
1032                 return ERROR_TARGET_NOT_HALTED;
1033         }
1034
1035         /* sanitize arguments */
1036         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1037                 return ERROR_COMMAND_SYNTAX_ERROR;
1038
1039         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1040                 return ERROR_TARGET_UNALIGNED_ACCESS;
1041
1042         /* since we don't know if buffer is aligned, we allocate new mem that is always aligned */
1043         void *t = NULL;
1044
1045         if (size > 1) {
1046                 t = malloc(count * size * sizeof(uint8_t));
1047                 if (t == NULL) {
1048                         LOG_ERROR("Out of memory");
1049                         return ERROR_FAIL;
1050                 }
1051         } else
1052                 t = buffer;
1053
1054         /* if noDMA off, use DMAACC mode for memory read */
1055         int retval;
1056         if (ejtag_info->impcode & EJTAG_IMP_NODMA)
1057                 retval = mips32_pracc_read_mem(ejtag_info, address, size, count, t);
1058         else
1059                 retval = mips32_dmaacc_read_mem(ejtag_info, address, size, count, t);
1060
1061         /* mips32_..._read_mem with size 4/2 returns uint32_t/uint16_t in host */
1062         /* endianness, but byte array should represent target endianness       */
1063         if (ERROR_OK == retval) {
1064                 switch (size) {
1065                 case 4:
1066                         target_buffer_set_u32_array(target, buffer, count, t);
1067                         break;
1068                 case 2:
1069                         target_buffer_set_u16_array(target, buffer, count, t);
1070                         break;
1071                 }
1072         }
1073
1074         if ((size > 1) && (t != NULL))
1075                 free(t);
1076
1077         return retval;
1078 }
1079
1080 static int mips_m4k_write_memory(struct target *target, target_addr_t address,
1081                 uint32_t size, uint32_t count, const uint8_t *buffer)
1082 {
1083         struct mips32_common *mips32 = target_to_mips32(target);
1084         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
1085
1086         LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
1087                         address, size, count);
1088
1089         if (target->state != TARGET_HALTED) {
1090                 LOG_WARNING("target not halted");
1091                 return ERROR_TARGET_NOT_HALTED;
1092         }
1093
1094         if (size == 4 && count > 32) {
1095                 int retval = mips_m4k_bulk_write_memory(target, address, count, buffer);
1096                 if (retval == ERROR_OK)
1097                         return ERROR_OK;
1098                 LOG_WARNING("Falling back to non-bulk write");
1099         }
1100
1101         /* sanitize arguments */
1102         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1103                 return ERROR_COMMAND_SYNTAX_ERROR;
1104
1105         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1106                 return ERROR_TARGET_UNALIGNED_ACCESS;
1107
1108         /** correct endianess if we have word or hword access */
1109         void *t = NULL;
1110         if (size > 1) {
1111                 /* mips32_..._write_mem with size 4/2 requires uint32_t/uint16_t in host */
1112                 /* endianness, but byte array represents target endianness               */
1113                 t = malloc(count * size * sizeof(uint8_t));
1114                 if (t == NULL) {
1115                         LOG_ERROR("Out of memory");
1116                         return ERROR_FAIL;
1117                 }
1118
1119                 switch (size) {
1120                 case 4:
1121                         target_buffer_get_u32_array(target, buffer, count, (uint32_t *)t);
1122                         break;
1123                 case 2:
1124                         target_buffer_get_u16_array(target, buffer, count, (uint16_t *)t);
1125                         break;
1126                 }
1127                 buffer = t;
1128         }
1129
1130         /* if noDMA off, use DMAACC mode for memory write */
1131         int retval;
1132         if (ejtag_info->impcode & EJTAG_IMP_NODMA)
1133                 retval = mips32_pracc_write_mem(ejtag_info, address, size, count, buffer);
1134         else
1135                 retval = mips32_dmaacc_write_mem(ejtag_info, address, size, count, buffer);
1136
1137         if (t != NULL)
1138                 free(t);
1139
1140         if (ERROR_OK != retval)
1141                 return retval;
1142
1143         return ERROR_OK;
1144 }
1145
1146 static int mips_m4k_init_target(struct command_context *cmd_ctx,
1147                 struct target *target)
1148 {
1149         mips32_build_reg_cache(target);
1150
1151         return ERROR_OK;
1152 }
1153
1154 static int mips_m4k_init_arch_info(struct target *target,
1155                 struct mips_m4k_common *mips_m4k, struct jtag_tap *tap)
1156 {
1157         struct mips32_common *mips32 = &mips_m4k->mips32;
1158
1159         mips_m4k->common_magic = MIPSM4K_COMMON_MAGIC;
1160
1161         /* initialize mips4k specific info */
1162         mips32_init_arch_info(target, mips32, tap);
1163         mips32->arch_info = mips_m4k;
1164
1165         return ERROR_OK;
1166 }
1167
1168 static int mips_m4k_target_create(struct target *target, Jim_Interp *interp)
1169 {
1170         struct mips_m4k_common *mips_m4k = calloc(1, sizeof(struct mips_m4k_common));
1171
1172         mips_m4k_init_arch_info(target, mips_m4k, target->tap);
1173
1174         return ERROR_OK;
1175 }
1176
1177 static int mips_m4k_examine(struct target *target)
1178 {
1179         struct mips_m4k_common *mips_m4k = target_to_m4k(target);
1180         struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info;
1181
1182         if (!target_was_examined(target)) {
1183                 int retval = mips_ejtag_get_idcode(ejtag_info);
1184                 if (retval != ERROR_OK) {
1185                         LOG_ERROR("idcode read failed");
1186                         return retval;
1187                 }
1188                 if (((ejtag_info->idcode >> 1) & 0x7FF) == 0x29) {
1189                         /* we are using a pic32mx so select ejtag port
1190                          * as it is not selected by default */
1191                         mips_ejtag_set_instr(ejtag_info, MTAP_SW_ETAP);
1192                         LOG_DEBUG("PIC32 Detected - using EJTAG Interface");
1193                         mips_m4k->is_pic32mx = true;
1194                 }
1195         }
1196
1197         /* init rest of ejtag interface */
1198         int retval = mips_ejtag_init(ejtag_info);
1199         if (retval != ERROR_OK)
1200                 return retval;
1201
1202         return mips32_examine(target);
1203 }
1204
1205 static int mips_m4k_bulk_write_memory(struct target *target, target_addr_t address,
1206                 uint32_t count, const uint8_t *buffer)
1207 {
1208         struct mips32_common *mips32 = target_to_mips32(target);
1209         struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
1210         struct working_area *fast_data_area;
1211         int retval;
1212         int write_t = 1;
1213
1214         LOG_DEBUG("address: " TARGET_ADDR_FMT ", count: 0x%8.8" PRIx32 "",
1215                           address, count);
1216
1217         /* check alignment */
1218         if (address & 0x3u)
1219                 return ERROR_TARGET_UNALIGNED_ACCESS;
1220
1221         if (mips32->fast_data_area == NULL) {
1222                 /* Get memory for block write handler
1223                  * we preserve this area between calls and gain a speed increase
1224                  * of about 3kb/sec when writing flash
1225                  * this will be released/nulled by the system when the target is resumed or reset */
1226                 retval = target_alloc_working_area(target,
1227                                 MIPS32_FASTDATA_HANDLER_SIZE,
1228                                 &mips32->fast_data_area);
1229                 if (retval != ERROR_OK) {
1230                         LOG_ERROR("No working area available");
1231                         return retval;
1232                 }
1233
1234                 /* reset fastadata state so the algo get reloaded */
1235                 ejtag_info->fast_access_save = -1;
1236         }
1237
1238         fast_data_area = mips32->fast_data_area;
1239
1240         if (address <= fast_data_area->address + fast_data_area->size &&
1241                         fast_data_area->address <= address + count) {
1242                 LOG_ERROR("fast_data (" TARGET_ADDR_FMT ") is within write area "
1243                           "(" TARGET_ADDR_FMT "-" TARGET_ADDR_FMT ").",
1244                           fast_data_area->address, address, address + count);
1245                 LOG_ERROR("Change work-area-phys or load_image address!");
1246                 return ERROR_FAIL;
1247         }
1248
1249         /* mips32_pracc_fastdata_xfer requires uint32_t in host endianness, */
1250         /* but byte array represents target endianness                      */
1251         uint32_t *t = NULL;
1252         t = malloc(count * sizeof(uint32_t));
1253         if (t == NULL) {
1254                 LOG_ERROR("Out of memory");
1255                 return ERROR_FAIL;
1256         }
1257
1258         target_buffer_get_u32_array(target, buffer, count, t);
1259
1260         retval = mips32_pracc_fastdata_xfer(ejtag_info, mips32->fast_data_area, write_t, address,
1261                         count, t);
1262
1263         if (t != NULL)
1264                 free(t);
1265
1266         if (retval != ERROR_OK)
1267                 LOG_ERROR("Fastdata access Failed");
1268
1269         return retval;
1270 }
1271
1272 static int mips_m4k_verify_pointer(struct command_context *cmd_ctx,
1273                 struct mips_m4k_common *mips_m4k)
1274 {
1275         if (mips_m4k->common_magic != MIPSM4K_COMMON_MAGIC) {
1276                 command_print(cmd_ctx, "target is not an MIPS_M4K");
1277                 return ERROR_TARGET_INVALID;
1278         }
1279         return ERROR_OK;
1280 }
1281
1282 COMMAND_HANDLER(mips_m4k_handle_cp0_command)
1283 {
1284         int retval;
1285         struct target *target = get_current_target(CMD_CTX);
1286         struct mips_m4k_common *mips_m4k = target_to_m4k(target);
1287         struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info;
1288
1289         retval = mips_m4k_verify_pointer(CMD_CTX, mips_m4k);
1290         if (retval != ERROR_OK)
1291                 return retval;
1292
1293         if (target->state != TARGET_HALTED) {
1294                 command_print(CMD_CTX, "target must be stopped for \"%s\" command", CMD_NAME);
1295                 return ERROR_OK;
1296         }
1297
1298         /* two or more argument, access a single register/select (write if third argument is given) */
1299         if (CMD_ARGC < 2)
1300                 return ERROR_COMMAND_SYNTAX_ERROR;
1301         else {
1302                 uint32_t cp0_reg, cp0_sel;
1303                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], cp0_reg);
1304                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], cp0_sel);
1305
1306                 if (CMD_ARGC == 2) {
1307                         uint32_t value;
1308                         retval = mips32_cp0_read(ejtag_info, &value, cp0_reg, cp0_sel);
1309                         if (retval != ERROR_OK) {
1310                                 command_print(CMD_CTX,
1311                                                 "couldn't access reg %" PRIi32,
1312                                                 cp0_reg);
1313                                 return ERROR_OK;
1314                         }
1315                         command_print(CMD_CTX, "cp0 reg %" PRIi32 ", select %" PRIi32 ": %8.8" PRIx32,
1316                                         cp0_reg, cp0_sel, value);
1317
1318                 } else if (CMD_ARGC == 3) {
1319                         uint32_t value;
1320                         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], value);
1321                         retval = mips32_cp0_write(ejtag_info, value, cp0_reg, cp0_sel);
1322                         if (retval != ERROR_OK) {
1323                                 command_print(CMD_CTX,
1324                                                 "couldn't access cp0 reg %" PRIi32 ", select %" PRIi32,
1325                                                 cp0_reg,  cp0_sel);
1326                                 return ERROR_OK;
1327                         }
1328                         command_print(CMD_CTX, "cp0 reg %" PRIi32 ", select %" PRIi32 ": %8.8" PRIx32,
1329                                         cp0_reg, cp0_sel, value);
1330                 }
1331         }
1332
1333         return ERROR_OK;
1334 }
1335
1336 COMMAND_HANDLER(mips_m4k_handle_smp_off_command)
1337 {
1338         struct target *target = get_current_target(CMD_CTX);
1339         /* check target is an smp target */
1340         struct target_list *head;
1341         struct target *curr;
1342         head = target->head;
1343         target->smp = 0;
1344         if (head != (struct target_list *)NULL) {
1345                 while (head != (struct target_list *)NULL) {
1346                         curr = head->target;
1347                         curr->smp = 0;
1348                         head = head->next;
1349                 }
1350                 /*  fixes the target display to the debugger */
1351                 target->gdb_service->target = target;
1352         }
1353         return ERROR_OK;
1354 }
1355
1356 COMMAND_HANDLER(mips_m4k_handle_smp_on_command)
1357 {
1358         struct target *target = get_current_target(CMD_CTX);
1359         struct target_list *head;
1360         struct target *curr;
1361         head = target->head;
1362         if (head != (struct target_list *)NULL) {
1363                 target->smp = 1;
1364                 while (head != (struct target_list *)NULL) {
1365                         curr = head->target;
1366                         curr->smp = 1;
1367                         head = head->next;
1368                 }
1369         }
1370         return ERROR_OK;
1371 }
1372
1373 COMMAND_HANDLER(mips_m4k_handle_smp_gdb_command)
1374 {
1375         struct target *target = get_current_target(CMD_CTX);
1376         int retval = ERROR_OK;
1377         struct target_list *head;
1378         head = target->head;
1379         if (head != (struct target_list *)NULL) {
1380                 if (CMD_ARGC == 1) {
1381                         int coreid = 0;
1382                         COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], coreid);
1383                         if (ERROR_OK != retval)
1384                                 return retval;
1385                         target->gdb_service->core[1] = coreid;
1386
1387                 }
1388                 command_print(CMD_CTX, "gdb coreid  %" PRId32 " -> %" PRId32, target->gdb_service->core[0]
1389                         , target->gdb_service->core[1]);
1390         }
1391         return ERROR_OK;
1392 }
1393
1394 COMMAND_HANDLER(mips_m4k_handle_scan_delay_command)
1395 {
1396         struct target *target = get_current_target(CMD_CTX);
1397         struct mips_m4k_common *mips_m4k = target_to_m4k(target);
1398         struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info;
1399
1400         if (CMD_ARGC == 1)
1401                 COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], ejtag_info->scan_delay);
1402         else if (CMD_ARGC > 1)
1403                         return ERROR_COMMAND_SYNTAX_ERROR;
1404
1405         command_print(CMD_CTX, "scan delay: %d nsec", ejtag_info->scan_delay);
1406         if (ejtag_info->scan_delay >= MIPS32_SCAN_DELAY_LEGACY_MODE) {
1407                 ejtag_info->mode = 0;
1408                 command_print(CMD_CTX, "running in legacy mode");
1409         } else {
1410                 ejtag_info->mode = 1;
1411                 command_print(CMD_CTX, "running in fast queued mode");
1412         }
1413
1414         return ERROR_OK;
1415 }
1416
1417 static const struct command_registration mips_m4k_exec_command_handlers[] = {
1418         {
1419                 .name = "cp0",
1420                 .handler = mips_m4k_handle_cp0_command,
1421                 .mode = COMMAND_EXEC,
1422                 .usage = "regnum [value]",
1423                 .help = "display/modify cp0 register",
1424         },
1425         {
1426                 .name = "smp_off",
1427                 .handler = mips_m4k_handle_smp_off_command,
1428                 .mode = COMMAND_EXEC,
1429                 .help = "Stop smp handling",
1430                 .usage = "",},
1431
1432         {
1433                 .name = "smp_on",
1434                 .handler = mips_m4k_handle_smp_on_command,
1435                 .mode = COMMAND_EXEC,
1436                 .help = "Restart smp handling",
1437                 .usage = "",
1438         },
1439         {
1440                 .name = "smp_gdb",
1441                 .handler = mips_m4k_handle_smp_gdb_command,
1442                 .mode = COMMAND_EXEC,
1443                 .help = "display/fix current core played to gdb",
1444                 .usage = "",
1445         },
1446         {
1447                 .name = "scan_delay",
1448                 .handler = mips_m4k_handle_scan_delay_command,
1449                 .mode = COMMAND_ANY,
1450                 .help = "display/set scan delay in nano seconds",
1451                 .usage = "[value]",
1452         },
1453         COMMAND_REGISTRATION_DONE
1454 };
1455
1456 const struct command_registration mips_m4k_command_handlers[] = {
1457         {
1458                 .chain = mips32_command_handlers,
1459         },
1460         {
1461                 .name = "mips_m4k",
1462                 .mode = COMMAND_ANY,
1463                 .help = "mips_m4k command group",
1464                 .usage = "",
1465                 .chain = mips_m4k_exec_command_handlers,
1466         },
1467         COMMAND_REGISTRATION_DONE
1468 };
1469
1470 struct target_type mips_m4k_target = {
1471         .name = "mips_m4k",
1472
1473         .poll = mips_m4k_poll,
1474         .arch_state = mips32_arch_state,
1475
1476         .halt = mips_m4k_halt,
1477         .resume = mips_m4k_resume,
1478         .step = mips_m4k_step,
1479
1480         .assert_reset = mips_m4k_assert_reset,
1481         .deassert_reset = mips_m4k_deassert_reset,
1482
1483         .get_gdb_reg_list = mips32_get_gdb_reg_list,
1484
1485         .read_memory = mips_m4k_read_memory,
1486         .write_memory = mips_m4k_write_memory,
1487         .checksum_memory = mips32_checksum_memory,
1488         .blank_check_memory = mips32_blank_check_memory,
1489
1490         .run_algorithm = mips32_run_algorithm,
1491
1492         .add_breakpoint = mips_m4k_add_breakpoint,
1493         .remove_breakpoint = mips_m4k_remove_breakpoint,
1494         .add_watchpoint = mips_m4k_add_watchpoint,
1495         .remove_watchpoint = mips_m4k_remove_watchpoint,
1496
1497         .commands = mips_m4k_command_handlers,
1498         .target_create = mips_m4k_target_create,
1499         .init_target = mips_m4k_init_target,
1500         .examine = mips_m4k_examine,
1501 };