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