git.gag.com Git - fw/openocd/blob - src/target/xscale.c

   1 /***************************************************************************
   2  *   Copyright (C) 2006, 2007 by Dominic Rath                              *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
   6  *   oyvind.harboe@zylin.com                                               *
   7  *                                                                         *
   8  *   This program is free software; you can redistribute it and/or modify  *
   9  *   it under the terms of the GNU General Public License as published by  *
  10  *   the Free Software Foundation; either version 2 of the License, or     *
  11  *   (at your option) any later version.                                   *
  12  *                                                                         *
  13  *   This program is distributed in the hope that it will be useful,       *
  14  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  15  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  16  *   GNU General Public License for more details.                          *
  17  *                                                                         *
  18  *   You should have received a copy of the GNU General Public License     *
  19  *   along with this program; if not, write to the                         *
  20  *   Free Software Foundation, Inc.,                                       *
  21  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  22  ***************************************************************************/
  23 #ifdef HAVE_CONFIG_H
  24 #include "config.h"
  25 #endif
  26
  27 #include "xscale.h"
  28 #include "arm7_9_common.h"
  29 #include "arm_simulator.h"
  30 #include "arm_disassembler.h"
  31 #include "time_support.h"
  32 #include "image.h"
  33
  34 /* cli handling */
  35 int xscale_register_commands(struct command_context_s *cmd_ctx);
  36
  37 /* forward declarations */
  38 int xscale_target_create(struct target_s *target, Jim_Interp *interp);
  39 int xscale_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
  40 int xscale_quit(void);
  41
  42 int xscale_arch_state(struct target_s *target);
  43 int xscale_poll(target_t *target);
  44 int xscale_halt(target_t *target);
  45 int xscale_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution);
  46 int xscale_step(struct target_s *target, int current, u32 address, int handle_breakpoints);
  47 int xscale_debug_entry(target_t *target);
  48 int xscale_restore_context(target_t *target);
  49
  50 int xscale_assert_reset(target_t *target);
  51 int xscale_deassert_reset(target_t *target);
  52 int xscale_soft_reset_halt(struct target_s *target);
  53
  54 int xscale_set_reg_u32(reg_t *reg, u32 value);
  55
  56 int xscale_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode);
  57 int xscale_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value);
  58
  59 int xscale_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
  60 int xscale_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
  61 int xscale_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer);
  62
  63 int xscale_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
  64 int xscale_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
  65 int xscale_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
  66 int xscale_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
  67 int xscale_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint);
  68 int xscale_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint);
  69 void xscale_enable_watchpoints(struct target_s *target);
  70 void xscale_enable_breakpoints(struct target_s *target);
  71 static int xscale_virt2phys(struct target_s *target, u32 virtual, u32 *physical);
  72 static int xscale_mmu(struct target_s *target, int *enabled);
  73
  74 int xscale_read_trace(target_t *target);
  75
  76 target_type_t xscale_target =
  77 {
  78         .name = "xscale",
  79
  80         .poll = xscale_poll,
  81         .arch_state = xscale_arch_state,
  82
  83         .target_request_data = NULL,
  84
  85         .halt = xscale_halt,
  86         .resume = xscale_resume,
  87         .step = xscale_step,
  88
  89         .assert_reset = xscale_assert_reset,
  90         .deassert_reset = xscale_deassert_reset,
  91         .soft_reset_halt = xscale_soft_reset_halt,
  92
  93         .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
  94
  95         .read_memory = xscale_read_memory,
  96         .write_memory = xscale_write_memory,
  97         .bulk_write_memory = xscale_bulk_write_memory,
  98         .checksum_memory = arm7_9_checksum_memory,
  99         .blank_check_memory = arm7_9_blank_check_memory,
 100
 101         .run_algorithm = armv4_5_run_algorithm,
 102
 103         .add_breakpoint = xscale_add_breakpoint,
 104         .remove_breakpoint = xscale_remove_breakpoint,
 105         .add_watchpoint = xscale_add_watchpoint,
 106         .remove_watchpoint = xscale_remove_watchpoint,
 107
 108         .register_commands = xscale_register_commands,
 109         .target_create = xscale_target_create,
 110         .init_target = xscale_init_target,
 111         .quit = xscale_quit,
 112
 113         .virt2phys = xscale_virt2phys,
 114         .mmu = xscale_mmu
 115 };
 116
 117 char* xscale_reg_list[] =
 118 {
 119         "XSCALE_MAINID",                /* 0 */
 120         "XSCALE_CACHETYPE",
 121         "XSCALE_CTRL",
 122         "XSCALE_AUXCTRL",
 123         "XSCALE_TTB",
 124         "XSCALE_DAC",
 125         "XSCALE_FSR",
 126         "XSCALE_FAR",
 127         "XSCALE_PID",
 128         "XSCALE_CPACCESS",
 129         "XSCALE_IBCR0",                 /* 10 */
 130         "XSCALE_IBCR1",
 131         "XSCALE_DBR0",
 132         "XSCALE_DBR1",
 133         "XSCALE_DBCON",
 134         "XSCALE_TBREG",
 135         "XSCALE_CHKPT0",
 136         "XSCALE_CHKPT1",
 137         "XSCALE_DCSR",
 138         "XSCALE_TX",
 139         "XSCALE_RX",                    /* 20 */
 140         "XSCALE_TXRXCTRL",
 141 };
 142
 143 xscale_reg_t xscale_reg_arch_info[] =
 144 {
 145         {XSCALE_MAINID, NULL},
 146         {XSCALE_CACHETYPE, NULL},
 147         {XSCALE_CTRL, NULL},
 148         {XSCALE_AUXCTRL, NULL},
 149         {XSCALE_TTB, NULL},
 150         {XSCALE_DAC, NULL},
 151         {XSCALE_FSR, NULL},
 152         {XSCALE_FAR, NULL},
 153         {XSCALE_PID, NULL},
 154         {XSCALE_CPACCESS, NULL},
 155         {XSCALE_IBCR0, NULL},
 156         {XSCALE_IBCR1, NULL},
 157         {XSCALE_DBR0, NULL},
 158         {XSCALE_DBR1, NULL},
 159         {XSCALE_DBCON, NULL},
 160         {XSCALE_TBREG, NULL},
 161         {XSCALE_CHKPT0, NULL},
 162         {XSCALE_CHKPT1, NULL},
 163         {XSCALE_DCSR, NULL}, /* DCSR accessed via JTAG or SW */
 164         {-1, NULL}, /* TX accessed via JTAG */
 165         {-1, NULL}, /* RX accessed via JTAG */
 166         {-1, NULL}, /* TXRXCTRL implicit access via JTAG */
 167 };
 168
 169 int xscale_reg_arch_type = -1;
 170
 171 int xscale_get_reg(reg_t *reg);
 172 int xscale_set_reg(reg_t *reg, u8 *buf);
 173
 174 int xscale_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, xscale_common_t **xscale_p)
 175 {
 176         armv4_5_common_t *armv4_5 = target->arch_info;
 177         xscale_common_t *xscale = armv4_5->arch_info;
 178
 179         if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
 180         {
 181                 LOG_ERROR("target isn't an XScale target");
 182                 return -1;
 183         }
 184
 185         if (xscale->common_magic != XSCALE_COMMON_MAGIC)
 186         {
 187                 LOG_ERROR("target isn't an XScale target");
 188                 return -1;
 189         }
 190
 191         *armv4_5_p = armv4_5;
 192         *xscale_p = xscale;
 193
 194         return ERROR_OK;
 195 }
 196
 197 int xscale_jtag_set_instr(jtag_tap_t *tap, u32 new_instr)
 198 {
 199         if (tap==NULL)
 200                 return ERROR_FAIL;
 201
 202         if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr)
 203         {
 204                 scan_field_t field;
 205
 206                 field.tap = tap;
 207                 field.num_bits = tap->ir_length;
 208                 field.out_value = calloc(CEIL(field.num_bits, 8), 1);
 209                 buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
 210
 211                 u8 tmp[4];
 212                 field.in_value = tmp;
 213
 214                 jtag_add_ir_scan(1, &field, TAP_INVALID);
 215
 216                 /* FIX!!!! isn't this check superfluous? verify_ircapture handles this? */
 217                 jtag_check_value_mask(&field, tap->expected, tap->expected_mask);
 218
 219                 free(field.out_value);
 220         }
 221
 222         return ERROR_OK;
 223 }
 224
 225 int xscale_read_dcsr(target_t *target)
 226 {
 227         armv4_5_common_t *armv4_5 = target->arch_info;
 228         xscale_common_t *xscale = armv4_5->arch_info;
 229
 230         int retval;
 231
 232         scan_field_t fields[3];
 233         u8 field0 = 0x0;
 234         u8 field0_check_value = 0x2;
 235         u8 field0_check_mask = 0x7;
 236         u8 field2 = 0x0;
 237         u8 field2_check_value = 0x0;
 238         u8 field2_check_mask = 0x1;
 239
 240         jtag_add_end_state(TAP_DRPAUSE);
 241         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.dcsr);
 242
 243         buf_set_u32(&field0, 1, 1, xscale->hold_rst);
 244         buf_set_u32(&field0, 2, 1, xscale->external_debug_break);
 245
 246         fields[0].tap = xscale->jtag_info.tap;
 247         fields[0].num_bits = 3;
 248         fields[0].out_value = &field0;
 249         u8 tmp;
 250         fields[0].in_value = &tmp;
 251
 252         fields[1].tap = xscale->jtag_info.tap;
 253         fields[1].num_bits = 32;
 254         fields[1].out_value = NULL;
 255         fields[1].in_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;
 256
 257
 258         fields[2].tap = xscale->jtag_info.tap;
 259         fields[2].num_bits = 1;
 260         fields[2].out_value = &field2;
 261         u8 tmp2;
 262         fields[2].in_value = &tmp2;
 263
 264         jtag_add_dr_scan(3, fields, TAP_INVALID);
 265
 266         jtag_check_value_mask(fields+0, &field0_check_value, &field0_check_mask);
 267         jtag_check_value_mask(fields+2, &field2_check_value, &field2_check_mask);
 268
 269         if ((retval = jtag_execute_queue()) != ERROR_OK)
 270         {
 271                 LOG_ERROR("JTAG error while reading DCSR");
 272                 return retval;
 273         }
 274
 275         xscale->reg_cache->reg_list[XSCALE_DCSR].dirty = 0;
 276         xscale->reg_cache->reg_list[XSCALE_DCSR].valid = 1;
 277
 278         /* write the register with the value we just read
 279          * on this second pass, only the first bit of field0 is guaranteed to be 0)
 280          */
 281         field0_check_mask = 0x1;
 282         fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;
 283         fields[1].in_value = NULL;
 284
 285         jtag_add_end_state(TAP_IDLE);
 286
 287         jtag_add_dr_scan(3, fields, TAP_INVALID);
 288
 289         /* DANGER!!! this must be here. It will make sure that the arguments
 290          * to jtag_set_check_value() does not go out of scope! */
 291         return jtag_execute_queue();
 292 }
 293
 294
 295 static void xscale_getbuf(u8 *in)
 296 {
 297         *((u32 *)in)=buf_get_u32(in, 0, 32);
 298 }
 299
 300 int xscale_receive(target_t *target, u32 *buffer, int num_words)
 301 {
 302         if (num_words==0)
 303                 return ERROR_INVALID_ARGUMENTS;
 304
 305         int retval=ERROR_OK;
 306         armv4_5_common_t *armv4_5 = target->arch_info;
 307         xscale_common_t *xscale = armv4_5->arch_info;
 308
 309         tap_state_t path[3];
 310         scan_field_t fields[3];
 311
 312         u8 *field0 = malloc(num_words * 1);
 313         u8 field0_check_value = 0x2;
 314         u8 field0_check_mask = 0x6;
 315         u32 *field1 = malloc(num_words * 4);
 316         u8 field2_check_value = 0x0;
 317         u8 field2_check_mask = 0x1;
 318         int words_done = 0;
 319         int words_scheduled = 0;
 320
 321         int i;
 322
 323         path[0] = TAP_DRSELECT;
 324         path[1] = TAP_DRCAPTURE;
 325         path[2] = TAP_DRSHIFT;
 326
 327         fields[0].tap = xscale->jtag_info.tap;
 328         fields[0].num_bits = 3;
 329         fields[0].out_value = NULL;
 330         fields[0].in_value = NULL;
 331         fields[0].check_value = &field0_check_value;
 332         fields[0].check_mask = &field0_check_mask;
 333
 334         fields[1].tap = xscale->jtag_info.tap;
 335         fields[1].num_bits = 32;
 336         fields[1].out_value = NULL;
 337         fields[1].check_value = NULL;
 338         fields[1].check_mask = NULL;
 339
 340         fields[2].tap = xscale->jtag_info.tap;
 341         fields[2].num_bits = 1;
 342         fields[2].out_value = NULL;
 343         fields[2].in_value = NULL;
 344         fields[2].check_value = &field2_check_value;
 345         fields[2].check_mask = &field2_check_mask;
 346
 347         jtag_add_end_state(TAP_IDLE);
 348         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.dbgtx);
 349         jtag_add_runtest(1, TAP_INVALID); /* ensures that we're in the TAP_IDLE state as the above could be a no-op */
 350
 351         /* repeat until all words have been collected */
 352         int attempts=0;
 353         while (words_done < num_words)
 354         {
 355                 /* schedule reads */
 356                 words_scheduled = 0;
 357                 for (i = words_done; i < num_words; i++)
 358                 {
 359                         fields[0].in_value = &field0[i];
 360
 361                         jtag_add_pathmove(3, path);
 362
 363                         fields[1].in_value = (u8 *)(field1+i);
 364
 365                         jtag_add_dr_scan_check(3, fields, TAP_IDLE);
 366
 367                         jtag_add_callback(xscale_getbuf, (u8 *)(field1+i));
 368
 369                         words_scheduled++;
 370                 }
 371
 372                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 373                 {
 374                         LOG_ERROR("JTAG error while receiving data from debug handler");
 375                         break;
 376                 }
 377
 378                 /* examine results */
 379                 for (i = words_done; i < num_words; i++)
 380                 {
 381                         if (!(field0[0] & 1))
 382                         {
 383                                 /* move backwards if necessary */
 384                                 int j;
 385                                 for (j = i; j < num_words - 1; j++)
 386                                 {
 387                                         field0[j] = field0[j+1];
 388                                         field1[j] = field1[j+1];
 389                                 }
 390                                 words_scheduled--;
 391                         }
 392                 }
 393                 if (words_scheduled==0)
 394                 {
 395                         if (attempts++==1000)
 396                         {
 397                                 LOG_ERROR("Failed to receiving data from debug handler after 1000 attempts");
 398                                 retval=ERROR_TARGET_TIMEOUT;
 399                                 break;
 400                         }
 401                 }
 402
 403                 words_done += words_scheduled;
 404         }
 405
 406         for (i = 0; i < num_words; i++)
 407                 *(buffer++) = buf_get_u32((u8*)&field1[i], 0, 32);
 408
 409         free(field1);
 410
 411         return retval;
 412 }
 413
 414 int xscale_read_tx(target_t *target, int consume)
 415 {
 416         armv4_5_common_t *armv4_5 = target->arch_info;
 417         xscale_common_t *xscale = armv4_5->arch_info;
 418         tap_state_t path[3];
 419         tap_state_t noconsume_path[6];
 420
 421         int retval;
 422         struct timeval timeout, now;
 423
 424         scan_field_t fields[3];
 425         u8 field0_in = 0x0;
 426         u8 field0_check_value = 0x2;
 427         u8 field0_check_mask = 0x6;
 428         u8 field2_check_value = 0x0;
 429         u8 field2_check_mask = 0x1;
 430
 431         jtag_add_end_state(TAP_IDLE);
 432
 433         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.dbgtx);
 434
 435         path[0] = TAP_DRSELECT;
 436         path[1] = TAP_DRCAPTURE;
 437         path[2] = TAP_DRSHIFT;
 438
 439         noconsume_path[0] = TAP_DRSELECT;
 440         noconsume_path[1] = TAP_DRCAPTURE;
 441         noconsume_path[2] = TAP_DREXIT1;
 442         noconsume_path[3] = TAP_DRPAUSE;
 443         noconsume_path[4] = TAP_DREXIT2;
 444         noconsume_path[5] = TAP_DRSHIFT;
 445
 446         fields[0].tap = xscale->jtag_info.tap;
 447         fields[0].num_bits = 3;
 448         fields[0].out_value = NULL;
 449         fields[0].in_value = &field0_in;
 450
 451         fields[1].tap = xscale->jtag_info.tap;
 452         fields[1].num_bits = 32;
 453         fields[1].out_value = NULL;
 454         fields[1].in_value = xscale->reg_cache->reg_list[XSCALE_TX].value;
 455
 456
 457         fields[2].tap = xscale->jtag_info.tap;
 458         fields[2].num_bits = 1;
 459         fields[2].out_value = NULL;
 460         u8 tmp;
 461         fields[2].in_value = &tmp;
 462
 463         gettimeofday(&timeout, NULL);
 464         timeval_add_time(&timeout, 1, 0);
 465
 466         for (;;)
 467         {
 468                 /* if we want to consume the register content (i.e. clear TX_READY),
 469                  * we have to go straight from Capture-DR to Shift-DR
 470                  * otherwise, we go from Capture-DR to Exit1-DR to Pause-DR
 471                 */
 472                 if (consume)
 473                         jtag_add_pathmove(3, path);
 474                 else
 475                 {
 476                         jtag_add_pathmove(sizeof(noconsume_path)/sizeof(*noconsume_path), noconsume_path);
 477                 }
 478
 479                 jtag_add_dr_scan(3, fields, TAP_IDLE);
 480
 481                 jtag_check_value_mask(fields+0, &field0_check_value, &field0_check_mask);
 482                 jtag_check_value_mask(fields+2, &field2_check_value, &field2_check_mask);
 483
 484                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 485                 {
 486                         LOG_ERROR("JTAG error while reading TX");
 487                         return ERROR_TARGET_TIMEOUT;
 488                 }
 489
 490                 gettimeofday(&now, NULL);
 491                 if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec)&& (now.tv_usec > timeout.tv_usec)))
 492                 {
 493                         LOG_ERROR("time out reading TX register");
 494                         return ERROR_TARGET_TIMEOUT;
 495                 }
 496                 if (!((!(field0_in & 1)) && consume))
 497                 {
 498                         goto done;
 499                 }
 500                 if (debug_level>=3)
 501                 {
 502                         LOG_DEBUG("waiting 100ms");
 503                         alive_sleep(100); /* avoid flooding the logs */
 504                 } else
 505                 {
 506                         keep_alive();
 507                 }
 508         }
 509         done:
 510
 511         if (!(field0_in & 1))
 512                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 513
 514         return ERROR_OK;
 515 }
 516
 517 int xscale_write_rx(target_t *target)
 518 {
 519         armv4_5_common_t *armv4_5 = target->arch_info;
 520         xscale_common_t *xscale = armv4_5->arch_info;
 521
 522         int retval;
 523         struct timeval timeout, now;
 524
 525         scan_field_t fields[3];
 526         u8 field0_out = 0x0;
 527         u8 field0_in = 0x0;
 528         u8 field0_check_value = 0x2;
 529         u8 field0_check_mask = 0x6;
 530         u8 field2 = 0x0;
 531         u8 field2_check_value = 0x0;
 532         u8 field2_check_mask = 0x1;
 533
 534         jtag_add_end_state(TAP_IDLE);
 535
 536         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.dbgrx);
 537
 538         fields[0].tap = xscale->jtag_info.tap;
 539         fields[0].num_bits = 3;
 540         fields[0].out_value = &field0_out;
 541         fields[0].in_value = &field0_in;
 542
 543         fields[1].tap = xscale->jtag_info.tap;
 544         fields[1].num_bits = 32;
 545         fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_RX].value;
 546         fields[1].in_value = NULL;
 547
 548
 549         fields[2].tap = xscale->jtag_info.tap;
 550         fields[2].num_bits = 1;
 551         fields[2].out_value = &field2;
 552         u8 tmp;
 553         fields[2].in_value = &tmp;
 554
 555         gettimeofday(&timeout, NULL);
 556         timeval_add_time(&timeout, 1, 0);
 557
 558         /* poll until rx_read is low */
 559         LOG_DEBUG("polling RX");
 560         for (;;)
 561         {
 562                 jtag_add_dr_scan(3, fields, TAP_IDLE);
 563
 564                 jtag_check_value_mask(fields+0, &field0_check_value, &field0_check_mask);
 565                 jtag_check_value_mask(fields+2, &field2_check_value, &field2_check_mask);
 566
 567                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 568                 {
 569                         LOG_ERROR("JTAG error while writing RX");
 570                         return retval;
 571                 }
 572
 573                 gettimeofday(&now, NULL);
 574                 if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec)&& (now.tv_usec > timeout.tv_usec)))
 575                 {
 576                         LOG_ERROR("time out writing RX register");
 577                         return ERROR_TARGET_TIMEOUT;
 578                 }
 579                 if (!(field0_in & 1))
 580                         goto done;
 581                 if (debug_level>=3)
 582                 {
 583                         LOG_DEBUG("waiting 100ms");
 584                         alive_sleep(100); /* avoid flooding the logs */
 585                 } else
 586                 {
 587                         keep_alive();
 588                 }
 589         }
 590         done:
 591
 592         /* set rx_valid */
 593         field2 = 0x1;
 594         jtag_add_dr_scan(3, fields, TAP_IDLE);
 595
 596         if ((retval = jtag_execute_queue()) != ERROR_OK)
 597         {
 598                 LOG_ERROR("JTAG error while writing RX");
 599                 return retval;
 600         }
 601
 602         return ERROR_OK;
 603 }
 604
 605 /* send count elements of size byte to the debug handler */
 606 int xscale_send(target_t *target, u8 *buffer, int count, int size)
 607 {
 608         armv4_5_common_t *armv4_5 = target->arch_info;
 609         xscale_common_t *xscale = armv4_5->arch_info;
 610         u32 t[3];
 611         int bits[3];
 612
 613         int retval;
 614
 615         int done_count = 0;
 616
 617         jtag_add_end_state(TAP_IDLE);
 618
 619         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.dbgrx);
 620
 621         bits[0]=3;
 622         t[0]=0;
 623         bits[1]=32;
 624         t[2]=1;
 625         bits[2]=1;
 626         int endianness = target->endianness;
 627         while (done_count++ < count)
 628         {
 629                 switch (size)
 630                 {
 631                 case 4:
 632                         if (endianness == TARGET_LITTLE_ENDIAN)
 633                         {
 634                                 t[1]=le_to_h_u32(buffer);
 635                         } else
 636                         {
 637                                 t[1]=be_to_h_u32(buffer);
 638                         }
 639                         break;
 640                 case 2:
 641                         if (endianness == TARGET_LITTLE_ENDIAN)
 642                         {
 643                                 t[1]=le_to_h_u16(buffer);
 644                         } else
 645                         {
 646                                 t[1]=be_to_h_u16(buffer);
 647                         }
 648                         break;
 649                 case 1:
 650                         t[1]=buffer[0];
 651                         break;
 652                 default:
 653                         LOG_ERROR("BUG: size neither 4, 2 nor 1");
 654                         exit(-1);
 655                 }
 656                 jtag_add_dr_out(xscale->jtag_info.tap,
 657                                 3,
 658                                 bits,
 659                                 t,
 660                                 TAP_IDLE);
 661                 buffer += size;
 662         }
 663
 664         if ((retval = jtag_execute_queue()) != ERROR_OK)
 665         {
 666                 LOG_ERROR("JTAG error while sending data to debug handler");
 667                 return retval;
 668         }
 669
 670         return ERROR_OK;
 671 }
 672
 673 int xscale_send_u32(target_t *target, u32 value)
 674 {
 675         armv4_5_common_t *armv4_5 = target->arch_info;
 676         xscale_common_t *xscale = armv4_5->arch_info;
 677
 678         buf_set_u32(xscale->reg_cache->reg_list[XSCALE_RX].value, 0, 32, value);
 679         return xscale_write_rx(target);
 680 }
 681
 682 int xscale_write_dcsr(target_t *target, int hold_rst, int ext_dbg_brk)
 683 {
 684         armv4_5_common_t *armv4_5 = target->arch_info;
 685         xscale_common_t *xscale = armv4_5->arch_info;
 686
 687         int retval;
 688
 689         scan_field_t fields[3];
 690         u8 field0 = 0x0;
 691         u8 field0_check_value = 0x2;
 692         u8 field0_check_mask = 0x7;
 693         u8 field2 = 0x0;
 694         u8 field2_check_value = 0x0;
 695         u8 field2_check_mask = 0x1;
 696
 697         if (hold_rst != -1)
 698                 xscale->hold_rst = hold_rst;
 699
 700         if (ext_dbg_brk != -1)
 701                 xscale->external_debug_break = ext_dbg_brk;
 702
 703         jtag_add_end_state(TAP_IDLE);
 704         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.dcsr);
 705
 706         buf_set_u32(&field0, 1, 1, xscale->hold_rst);
 707         buf_set_u32(&field0, 2, 1, xscale->external_debug_break);
 708
 709         fields[0].tap = xscale->jtag_info.tap;
 710         fields[0].num_bits = 3;
 711         fields[0].out_value = &field0;
 712         u8 tmp;
 713         fields[0].in_value = &tmp;
 714
 715         fields[1].tap = xscale->jtag_info.tap;
 716         fields[1].num_bits = 32;
 717         fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;
 718         fields[1].in_value = NULL;
 719
 720
 721         fields[2].tap = xscale->jtag_info.tap;
 722         fields[2].num_bits = 1;
 723         fields[2].out_value = &field2;
 724         u8 tmp2;
 725         fields[2].in_value = &tmp2;
 726
 727         jtag_add_dr_scan(3, fields, TAP_INVALID);
 728
 729         jtag_check_value_mask(fields+0, &field0_check_value, &field0_check_mask);
 730         jtag_check_value_mask(fields+2, &field2_check_value, &field2_check_mask);
 731
 732         if ((retval = jtag_execute_queue()) != ERROR_OK)
 733         {
 734                 LOG_ERROR("JTAG error while writing DCSR");
 735                 return retval;
 736         }
 737
 738         xscale->reg_cache->reg_list[XSCALE_DCSR].dirty = 0;
 739         xscale->reg_cache->reg_list[XSCALE_DCSR].valid = 1;
 740
 741         return ERROR_OK;
 742 }
 743
 744 /* parity of the number of bits 0 if even; 1 if odd. for 32 bit words */
 745 unsigned int parity (unsigned int v)
 746 {
 747         unsigned int ov = v;
 748         v ^= v >> 16;
 749         v ^= v >> 8;
 750         v ^= v >> 4;
 751         v &= 0xf;
 752         LOG_DEBUG("parity of 0x%x is %i", ov, (0x6996 >> v) & 1);
 753         return (0x6996 >> v) & 1;
 754 }
 755
 756 int xscale_load_ic(target_t *target, int mini, u32 va, u32 buffer[8])
 757 {
 758         armv4_5_common_t *armv4_5 = target->arch_info;
 759         xscale_common_t *xscale = armv4_5->arch_info;
 760         u8 packet[4];
 761         u8 cmd;
 762         int word;
 763
 764         scan_field_t fields[2];
 765
 766         LOG_DEBUG("loading miniIC at 0x%8.8x", va);
 767
 768         jtag_add_end_state(TAP_IDLE);
 769         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.ldic); /* LDIC */
 770
 771         /* CMD is b010 for Main IC and b011 for Mini IC */
 772         if (mini)
 773                 buf_set_u32(&cmd, 0, 3, 0x3);
 774         else
 775                 buf_set_u32(&cmd, 0, 3, 0x2);
 776
 777         buf_set_u32(&cmd, 3, 3, 0x0);
 778
 779         /* virtual address of desired cache line */
 780         buf_set_u32(packet, 0, 27, va >> 5);
 781
 782         fields[0].tap = xscale->jtag_info.tap;
 783         fields[0].num_bits = 6;
 784         fields[0].out_value = &cmd;
 785
 786         fields[0].in_value = NULL;
 787
 788
 789
 790
 791
 792         fields[1].tap = xscale->jtag_info.tap;
 793         fields[1].num_bits = 27;
 794         fields[1].out_value = packet;
 795
 796         fields[1].in_value = NULL;
 797
 798
 799
 800
 801
 802         jtag_add_dr_scan(2, fields, TAP_INVALID);
 803
 804         fields[0].num_bits = 32;
 805         fields[0].out_value = packet;
 806
 807         fields[1].num_bits = 1;
 808         fields[1].out_value = &cmd;
 809
 810         for (word = 0; word < 8; word++)
 811         {
 812                 buf_set_u32(packet, 0, 32, buffer[word]);
 813
 814                 u32 value;
 815                 memcpy(&value, packet, sizeof(u32));
 816                 cmd = parity(value);
 817
 818                 jtag_add_dr_scan(2, fields, TAP_INVALID);
 819         }
 820
 821         jtag_execute_queue();
 822
 823         return ERROR_OK;
 824 }
 825
 826 int xscale_invalidate_ic_line(target_t *target, u32 va)
 827 {
 828         armv4_5_common_t *armv4_5 = target->arch_info;
 829         xscale_common_t *xscale = armv4_5->arch_info;
 830         u8 packet[4];
 831         u8 cmd;
 832
 833         scan_field_t fields[2];
 834
 835         jtag_add_end_state(TAP_IDLE);
 836         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.ldic); /* LDIC */
 837
 838         /* CMD for invalidate IC line b000, bits [6:4] b000 */
 839         buf_set_u32(&cmd, 0, 6, 0x0);
 840
 841         /* virtual address of desired cache line */
 842         buf_set_u32(packet, 0, 27, va >> 5);
 843
 844         fields[0].tap = xscale->jtag_info.tap;
 845         fields[0].num_bits = 6;
 846         fields[0].out_value = &cmd;
 847
 848         fields[0].in_value = NULL;
 849
 850
 851
 852
 853
 854         fields[1].tap = xscale->jtag_info.tap;
 855         fields[1].num_bits = 27;
 856         fields[1].out_value = packet;
 857
 858         fields[1].in_value = NULL;
 859
 860
 861
 862
 863
 864         jtag_add_dr_scan(2, fields, TAP_INVALID);
 865
 866         return ERROR_OK;
 867 }
 868
 869 int xscale_update_vectors(target_t *target)
 870 {
 871         armv4_5_common_t *armv4_5 = target->arch_info;
 872         xscale_common_t *xscale = armv4_5->arch_info;
 873         int i;
 874         int retval;
 875
 876         u32 low_reset_branch, high_reset_branch;
 877
 878         for (i = 1; i < 8; i++)
 879         {
 880                 /* if there's a static vector specified for this exception, override */
 881                 if (xscale->static_high_vectors_set & (1 << i))
 882                 {
 883                         xscale->high_vectors[i] = xscale->static_high_vectors[i];
 884                 }
 885                 else
 886                 {
 887                         retval=target_read_u32(target, 0xffff0000 + 4*i, &xscale->high_vectors[i]);
 888                         if (retval == ERROR_TARGET_TIMEOUT)
 889                                 return retval;
 890                         if (retval!=ERROR_OK)
 891                         {
 892                                 /* Some of these reads will fail as part of normal execution */
 893                                 xscale->high_vectors[i] = ARMV4_5_B(0xfffffe, 0);
 894                         }
 895                 }
 896         }
 897
 898         for (i = 1; i < 8; i++)
 899         {
 900                 if (xscale->static_low_vectors_set & (1 << i))
 901                 {
 902                         xscale->low_vectors[i] = xscale->static_low_vectors[i];
 903                 }
 904                 else
 905                 {
 906                         retval=target_read_u32(target, 0x0 + 4*i, &xscale->low_vectors[i]);
 907                         if (retval == ERROR_TARGET_TIMEOUT)
 908                                 return retval;
 909                         if (retval!=ERROR_OK)
 910                         {
 911                                 /* Some of these reads will fail as part of normal execution */
 912                                 xscale->low_vectors[i] = ARMV4_5_B(0xfffffe, 0);
 913                         }
 914                 }
 915         }
 916
 917         /* calculate branches to debug handler */
 918         low_reset_branch = (xscale->handler_address + 0x20 - 0x0 - 0x8) >> 2;
 919         high_reset_branch = (xscale->handler_address + 0x20 - 0xffff0000 - 0x8) >> 2;
 920
 921         xscale->low_vectors[0] = ARMV4_5_B((low_reset_branch & 0xffffff), 0);
 922         xscale->high_vectors[0] = ARMV4_5_B((high_reset_branch & 0xffffff), 0);
 923
 924         /* invalidate and load exception vectors in mini i-cache */
 925         xscale_invalidate_ic_line(target, 0x0);
 926         xscale_invalidate_ic_line(target, 0xffff0000);
 927
 928         xscale_load_ic(target, 1, 0x0, xscale->low_vectors);
 929         xscale_load_ic(target, 1, 0xffff0000, xscale->high_vectors);
 930
 931         return ERROR_OK;
 932 }
 933
 934 int xscale_arch_state(struct target_s *target)
 935 {
 936         armv4_5_common_t *armv4_5 = target->arch_info;
 937         xscale_common_t *xscale = armv4_5->arch_info;
 938
 939         char *state[] =
 940         {
 941                 "disabled", "enabled"
 942         };
 943
 944         char *arch_dbg_reason[] =
 945         {
 946                 "", "\n(processor reset)", "\n(trace buffer full)"
 947         };
 948
 949         if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
 950         {
 951                 LOG_ERROR("BUG: called for a non-ARMv4/5 target");
 952                 exit(-1);
 953         }
 954
 955         LOG_USER("target halted in %s state due to %s, current mode: %s\n"
 956                         "cpsr: 0x%8.8x pc: 0x%8.8x\n"
 957                         "MMU: %s, D-Cache: %s, I-Cache: %s"
 958                         "%s",
 959                          armv4_5_state_strings[armv4_5->core_state],
 960                          Jim_Nvp_value2name_simple( nvp_target_debug_reason, target->debug_reason )->name ,
 961                          armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
 962                          buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
 963                          buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),
 964                          state[xscale->armv4_5_mmu.mmu_enabled],
 965                          state[xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled],
 966                          state[xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled],
 967                          arch_dbg_reason[xscale->arch_debug_reason]);
 968
 969         return ERROR_OK;
 970 }
 971
 972 int xscale_poll(target_t *target)
 973 {
 974         int retval=ERROR_OK;
 975         armv4_5_common_t *armv4_5 = target->arch_info;
 976         xscale_common_t *xscale = armv4_5->arch_info;
 977
 978         if ((target->state == TARGET_RUNNING) || (target->state == TARGET_DEBUG_RUNNING))
 979         {
 980                 enum target_state previous_state = target->state;
 981                 if ((retval = xscale_read_tx(target, 0)) == ERROR_OK)
 982                 {
 983
 984                         /* there's data to read from the tx register, we entered debug state */
 985                         xscale->handler_running = 1;
 986
 987                         target->state = TARGET_HALTED;
 988
 989                         /* process debug entry, fetching current mode regs */
 990                         retval = xscale_debug_entry(target);
 991                 }
 992                 else if (retval != ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
 993                 {
 994                         LOG_USER("error while polling TX register, reset CPU");
 995                         /* here we "lie" so GDB won't get stuck and a reset can be perfomed */
 996                         target->state = TARGET_HALTED;
 997                 }
 998
 999                 /* debug_entry could have overwritten target state (i.e. immediate resume)
1000                  * don't signal event handlers in that case
1001                  */
1002                 if (target->state != TARGET_HALTED)
1003                         return ERROR_OK;
1004
1005                 /* if target was running, signal that we halted
1006                  * otherwise we reentered from debug execution */
1007                 if (previous_state == TARGET_RUNNING)
1008                         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1009                 else
1010                         target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
1011         }
1012
1013         return retval;
1014 }
1015
1016 int xscale_debug_entry(target_t *target)
1017 {
1018         armv4_5_common_t *armv4_5 = target->arch_info;
1019         xscale_common_t *xscale = armv4_5->arch_info;
1020         u32 pc;
1021         u32 buffer[10];
1022         int i;
1023         int retval;
1024
1025         u32 moe;
1026
1027         /* clear external dbg break (will be written on next DCSR read) */
1028         xscale->external_debug_break = 0;
1029         if ((retval=xscale_read_dcsr(target))!=ERROR_OK)
1030                 return retval;
1031
1032         /* get r0, pc, r1 to r7 and cpsr */
1033         if ((retval=xscale_receive(target, buffer, 10))!=ERROR_OK)
1034                 return retval;
1035
1036         /* move r0 from buffer to register cache */
1037         buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, buffer[0]);
1038         armv4_5->core_cache->reg_list[15].dirty = 1;
1039         armv4_5->core_cache->reg_list[15].valid = 1;
1040         LOG_DEBUG("r0: 0x%8.8x", buffer[0]);
1041
1042         /* move pc from buffer to register cache */
1043         buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, buffer[1]);
1044         armv4_5->core_cache->reg_list[15].dirty = 1;
1045         armv4_5->core_cache->reg_list[15].valid = 1;
1046         LOG_DEBUG("pc: 0x%8.8x", buffer[1]);
1047
1048         /* move data from buffer to register cache */
1049         for (i = 1; i <= 7; i++)
1050         {
1051                 buf_set_u32(armv4_5->core_cache->reg_list[i].value, 0, 32, buffer[1 + i]);
1052                 armv4_5->core_cache->reg_list[i].dirty = 1;
1053                 armv4_5->core_cache->reg_list[i].valid = 1;
1054                 LOG_DEBUG("r%i: 0x%8.8x", i, buffer[i + 1]);
1055         }
1056
1057         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, buffer[9]);
1058         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
1059         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1060         LOG_DEBUG("cpsr: 0x%8.8x", buffer[9]);
1061
1062         armv4_5->core_mode = buffer[9] & 0x1f;
1063         if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
1064         {
1065                 target->state = TARGET_UNKNOWN;
1066                 LOG_ERROR("cpsr contains invalid mode value - communication failure");
1067                 return ERROR_TARGET_FAILURE;
1068         }
1069         LOG_DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);
1070
1071         if (buffer[9] & 0x20)
1072                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
1073         else
1074                 armv4_5->core_state = ARMV4_5_STATE_ARM;
1075
1076
1077         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1078                 return ERROR_FAIL;
1079
1080         /* get banked registers, r8 to r14, and spsr if not in USR/SYS mode */
1081         if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))
1082         {
1083                 xscale_receive(target, buffer, 8);
1084                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, buffer[7]);
1085                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;
1086                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;
1087         }
1088         else
1089         {
1090                 /* r8 to r14, but no spsr */
1091                 xscale_receive(target, buffer, 7);
1092         }
1093
1094         /* move data from buffer to register cache */
1095         for (i = 8; i <= 14; i++)
1096         {
1097                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, buffer[i - 8]);
1098                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;
1099                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
1100         }
1101
1102         /* examine debug reason */
1103         xscale_read_dcsr(target);
1104         moe = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 2, 3);
1105
1106         /* stored PC (for calculating fixup) */
1107         pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1108
1109         switch (moe)
1110         {
1111                 case 0x0: /* Processor reset */
1112                         target->debug_reason = DBG_REASON_DBGRQ;
1113                         xscale->arch_debug_reason = XSCALE_DBG_REASON_RESET;
1114                         pc -= 4;
1115                         break;
1116                 case 0x1: /* Instruction breakpoint hit */
1117                         target->debug_reason = DBG_REASON_BREAKPOINT;
1118                         xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
1119                         pc -= 4;
1120                         break;
1121                 case 0x2: /* Data breakpoint hit */
1122                         target->debug_reason = DBG_REASON_WATCHPOINT;
1123                         xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
1124                         pc -= 4;
1125                         break;
1126                 case 0x3: /* BKPT instruction executed */
1127                         target->debug_reason = DBG_REASON_BREAKPOINT;
1128                         xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
1129                         pc -= 4;
1130                         break;
1131                 case 0x4: /* Ext. debug event */
1132                         target->debug_reason = DBG_REASON_DBGRQ;
1133                         xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
1134                         pc -= 4;
1135                         break;
1136                 case 0x5: /* Vector trap occured */
1137                         target->debug_reason = DBG_REASON_BREAKPOINT;
1138                         xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
1139                         pc -= 4;
1140                         break;
1141                 case 0x6: /* Trace buffer full break */
1142                         target->debug_reason = DBG_REASON_DBGRQ;
1143                         xscale->arch_debug_reason = XSCALE_DBG_REASON_TB_FULL;
1144                         pc -= 4;
1145                         break;
1146                 case 0x7: /* Reserved */
1147                 default:
1148                         LOG_ERROR("Method of Entry is 'Reserved'");
1149                         exit(-1);
1150                         break;
1151         }
1152
1153         /* apply PC fixup */
1154         buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, pc);
1155
1156         /* on the first debug entry, identify cache type */
1157         if (xscale->armv4_5_mmu.armv4_5_cache.ctype == -1)
1158         {
1159                 u32 cache_type_reg;
1160
1161                 /* read cp15 cache type register */
1162                 xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CACHETYPE]);
1163                 cache_type_reg = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CACHETYPE].value, 0, 32);
1164
1165                 armv4_5_identify_cache(cache_type_reg, &xscale->armv4_5_mmu.armv4_5_cache);
1166         }
1167
1168         /* examine MMU and Cache settings */
1169         /* read cp15 control register */
1170         xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
1171         xscale->cp15_control_reg = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);
1172         xscale->armv4_5_mmu.mmu_enabled = (xscale->cp15_control_reg & 0x1U) ? 1 : 0;
1173         xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (xscale->cp15_control_reg & 0x4U) ? 1 : 0;
1174         xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = (xscale->cp15_control_reg & 0x1000U) ? 1 : 0;
1175
1176         /* tracing enabled, read collected trace data */
1177         if (xscale->trace.buffer_enabled)
1178         {
1179                 xscale_read_trace(target);
1180                 xscale->trace.buffer_fill--;
1181
1182                 /* resume if we're still collecting trace data */
1183                 if ((xscale->arch_debug_reason == XSCALE_DBG_REASON_TB_FULL)
1184                         && (xscale->trace.buffer_fill > 0))
1185                 {
1186                         xscale_resume(target, 1, 0x0, 1, 0);
1187                 }
1188                 else
1189                 {
1190                         xscale->trace.buffer_enabled = 0;
1191                 }
1192         }
1193
1194         return ERROR_OK;
1195 }
1196
1197 int xscale_halt(target_t *target)
1198 {
1199         armv4_5_common_t *armv4_5 = target->arch_info;
1200         xscale_common_t *xscale = armv4_5->arch_info;
1201
1202         LOG_DEBUG("target->state: %s",
1203                   Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name);
1204
1205         if (target->state == TARGET_HALTED)
1206         {
1207                 LOG_DEBUG("target was already halted");
1208                 return ERROR_OK;
1209         }
1210         else if (target->state == TARGET_UNKNOWN)
1211         {
1212                 /* this must not happen for a xscale target */
1213                 LOG_ERROR("target was in unknown state when halt was requested");
1214                 return ERROR_TARGET_INVALID;
1215         }
1216         else if (target->state == TARGET_RESET)
1217         {
1218                 LOG_DEBUG("target->state == TARGET_RESET");
1219         }
1220         else
1221         {
1222                 /* assert external dbg break */
1223                 xscale->external_debug_break = 1;
1224                 xscale_read_dcsr(target);
1225
1226                 target->debug_reason = DBG_REASON_DBGRQ;
1227         }
1228
1229         return ERROR_OK;
1230 }
1231
1232 int xscale_enable_single_step(struct target_s *target, u32 next_pc)
1233 {
1234         armv4_5_common_t *armv4_5 = target->arch_info;
1235         xscale_common_t *xscale= armv4_5->arch_info;
1236         reg_t *ibcr0 = &xscale->reg_cache->reg_list[XSCALE_IBCR0];
1237         int retval;
1238
1239         if (xscale->ibcr0_used)
1240         {
1241                 breakpoint_t *ibcr0_bp = breakpoint_find(target, buf_get_u32(ibcr0->value, 0, 32) & 0xfffffffe);
1242
1243                 if (ibcr0_bp)
1244                 {
1245                         xscale_unset_breakpoint(target, ibcr0_bp);
1246                 }
1247                 else
1248                 {
1249                         LOG_ERROR("BUG: xscale->ibcr0_used is set, but no breakpoint with that address found");
1250                         exit(-1);
1251                 }
1252         }
1253
1254         if ((retval=xscale_set_reg_u32(ibcr0, next_pc | 0x1))!=ERROR_OK)
1255                 return retval;
1256
1257         return ERROR_OK;
1258 }
1259
1260 int xscale_disable_single_step(struct target_s *target)
1261 {
1262         armv4_5_common_t *armv4_5 = target->arch_info;
1263         xscale_common_t *xscale= armv4_5->arch_info;
1264         reg_t *ibcr0 = &xscale->reg_cache->reg_list[XSCALE_IBCR0];
1265         int retval;
1266
1267         if ((retval=xscale_set_reg_u32(ibcr0, 0x0))!=ERROR_OK)
1268                 return retval;
1269
1270         return ERROR_OK;
1271 }
1272
1273 int xscale_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
1274 {
1275         armv4_5_common_t *armv4_5 = target->arch_info;
1276         xscale_common_t *xscale= armv4_5->arch_info;
1277         breakpoint_t *breakpoint = target->breakpoints;
1278
1279         u32 current_pc;
1280
1281         int retval;
1282         int i;
1283
1284         LOG_DEBUG("-");
1285
1286         if (target->state != TARGET_HALTED)
1287         {
1288                 LOG_WARNING("target not halted");
1289                 return ERROR_TARGET_NOT_HALTED;
1290         }
1291
1292         if (!debug_execution)
1293         {
1294                 target_free_all_working_areas(target);
1295         }
1296
1297         /* update vector tables */
1298         if ((retval=xscale_update_vectors(target))!=ERROR_OK)
1299                 return retval;
1300
1301         /* current = 1: continue on current pc, otherwise continue at <address> */
1302         if (!current)
1303                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1304
1305         current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1306
1307         /* if we're at the reset vector, we have to simulate the branch */
1308         if (current_pc == 0x0)
1309         {
1310                 arm_simulate_step(target, NULL);
1311                 current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1312         }
1313
1314         /* the front-end may request us not to handle breakpoints */
1315         if (handle_breakpoints)
1316         {
1317                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1318                 {
1319                         u32 next_pc;
1320
1321                         /* there's a breakpoint at the current PC, we have to step over it */
1322                         LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
1323                         xscale_unset_breakpoint(target, breakpoint);
1324
1325                         /* calculate PC of next instruction */
1326                         if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)
1327                         {
1328                                 u32 current_opcode;
1329                                 target_read_u32(target, current_pc, &current_opcode);
1330                                 LOG_ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);
1331                         }
1332
1333                         LOG_DEBUG("enable single-step");
1334                         xscale_enable_single_step(target, next_pc);
1335
1336                         /* restore banked registers */
1337                         xscale_restore_context(target);
1338
1339                         /* send resume request (command 0x30 or 0x31)
1340                          * clean the trace buffer if it is to be enabled (0x62) */
1341                         if (xscale->trace.buffer_enabled)
1342                         {
1343                                 xscale_send_u32(target, 0x62);
1344                                 xscale_send_u32(target, 0x31);
1345                         }
1346                         else
1347                                 xscale_send_u32(target, 0x30);
1348
1349                         /* send CPSR */
1350                         xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1351                         LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1352
1353                         for (i = 7; i >= 0; i--)
1354                         {
1355                                 /* send register */
1356                                 xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
1357                                 LOG_DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
1358                         }
1359
1360                         /* send PC */
1361                         xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1362                         LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1363
1364                         /* wait for and process debug entry */
1365                         xscale_debug_entry(target);
1366
1367                         LOG_DEBUG("disable single-step");
1368                         xscale_disable_single_step(target);
1369
1370                         LOG_DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
1371                         xscale_set_breakpoint(target, breakpoint);
1372                 }
1373         }
1374
1375         /* enable any pending breakpoints and watchpoints */
1376         xscale_enable_breakpoints(target);
1377         xscale_enable_watchpoints(target);
1378
1379         /* restore banked registers */
1380         xscale_restore_context(target);
1381
1382         /* send resume request (command 0x30 or 0x31)
1383          * clean the trace buffer if it is to be enabled (0x62) */
1384         if (xscale->trace.buffer_enabled)
1385         {
1386                 xscale_send_u32(target, 0x62);
1387                 xscale_send_u32(target, 0x31);
1388         }
1389         else
1390                 xscale_send_u32(target, 0x30);
1391
1392         /* send CPSR */
1393         xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1394         LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1395
1396         for (i = 7; i >= 0; i--)
1397         {
1398                 /* send register */
1399                 xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
1400                 LOG_DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
1401         }
1402
1403         /* send PC */
1404         xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1405         LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1406
1407         target->debug_reason = DBG_REASON_NOTHALTED;
1408
1409         if (!debug_execution)
1410         {
1411                 /* registers are now invalid */
1412                 armv4_5_invalidate_core_regs(target);
1413                 target->state = TARGET_RUNNING;
1414                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1415         }
1416         else
1417         {
1418                 target->state = TARGET_DEBUG_RUNNING;
1419                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
1420         }
1421
1422         LOG_DEBUG("target resumed");
1423
1424         xscale->handler_running = 1;
1425
1426         return ERROR_OK;
1427 }
1428
1429 static int xscale_step_inner(struct target_s *target, int current, u32 address, int handle_breakpoints)
1430 {
1431         armv4_5_common_t *armv4_5 = target->arch_info;
1432         xscale_common_t *xscale = armv4_5->arch_info;
1433
1434         u32 next_pc;
1435         int retval;
1436         int i;
1437
1438         target->debug_reason = DBG_REASON_SINGLESTEP;
1439
1440         /* calculate PC of next instruction */
1441         if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)
1442         {
1443                 u32 current_opcode, current_pc;
1444                 current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1445
1446                 target_read_u32(target, current_pc, &current_opcode);
1447                 LOG_ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);
1448                 return retval;
1449         }
1450
1451         LOG_DEBUG("enable single-step");
1452         if ((retval=xscale_enable_single_step(target, next_pc))!=ERROR_OK)
1453                 return retval;
1454
1455         /* restore banked registers */
1456         if ((retval=xscale_restore_context(target))!=ERROR_OK)
1457                 return retval;
1458
1459         /* send resume request (command 0x30 or 0x31)
1460          * clean the trace buffer if it is to be enabled (0x62) */
1461         if (xscale->trace.buffer_enabled)
1462         {
1463                 if ((retval=xscale_send_u32(target, 0x62))!=ERROR_OK)
1464                         return retval;
1465                 if ((retval=xscale_send_u32(target, 0x31))!=ERROR_OK)
1466                         return retval;
1467         }
1468         else
1469                 if ((retval=xscale_send_u32(target, 0x30))!=ERROR_OK)
1470                         return retval;
1471
1472         /* send CPSR */
1473         if ((retval=xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32)))!=ERROR_OK)
1474                 return retval;
1475         LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1476
1477         for (i = 7; i >= 0; i--)
1478         {
1479                 /* send register */
1480                 if ((retval=xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32)))!=ERROR_OK)
1481                         return retval;
1482                 LOG_DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
1483         }
1484
1485         /* send PC */
1486         if ((retval=xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32)))!=ERROR_OK)
1487                 return retval;
1488         LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1489
1490         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1491
1492         /* registers are now invalid */
1493         if ((retval=armv4_5_invalidate_core_regs(target))!=ERROR_OK)
1494                 return retval;
1495
1496         /* wait for and process debug entry */
1497         if ((retval=xscale_debug_entry(target))!=ERROR_OK)
1498                 return retval;
1499
1500         LOG_DEBUG("disable single-step");
1501         if ((retval=xscale_disable_single_step(target))!=ERROR_OK)
1502                 return retval;
1503
1504         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1505
1506         return ERROR_OK;
1507 }
1508
1509 int xscale_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
1510 {
1511         armv4_5_common_t *armv4_5 = target->arch_info;
1512         breakpoint_t *breakpoint = target->breakpoints;
1513
1514         u32 current_pc;
1515         int retval;
1516
1517         if (target->state != TARGET_HALTED)
1518         {
1519                 LOG_WARNING("target not halted");
1520                 return ERROR_TARGET_NOT_HALTED;
1521         }
1522
1523         /* current = 1: continue on current pc, otherwise continue at <address> */
1524         if (!current)
1525                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1526
1527         current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1528
1529         /* if we're at the reset vector, we have to simulate the step */
1530         if (current_pc == 0x0)
1531         {
1532                 if ((retval=arm_simulate_step(target, NULL))!=ERROR_OK)
1533                         return retval;
1534                 current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1535
1536                 target->debug_reason = DBG_REASON_SINGLESTEP;
1537                 target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1538
1539                 return ERROR_OK;
1540         }
1541
1542         /* the front-end may request us not to handle breakpoints */
1543         if (handle_breakpoints)
1544                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1545                 {
1546                         if ((retval=xscale_unset_breakpoint(target, breakpoint))!=ERROR_OK)
1547                                 return retval;
1548                 }
1549
1550         retval = xscale_step_inner(target, current, address, handle_breakpoints);
1551
1552         if (breakpoint)
1553         {
1554                 xscale_set_breakpoint(target, breakpoint);
1555         }
1556
1557         LOG_DEBUG("target stepped");
1558
1559         return ERROR_OK;
1560
1561 }
1562
1563 int xscale_assert_reset(target_t *target)
1564 {
1565         armv4_5_common_t *armv4_5 = target->arch_info;
1566         xscale_common_t *xscale = armv4_5->arch_info;
1567
1568         LOG_DEBUG("target->state: %s",
1569                   Jim_Nvp_value2name_simple( nvp_target_state, target->state )->name);
1570
1571         /* select DCSR instruction (set endstate to R-T-I to ensure we don't
1572          * end up in T-L-R, which would reset JTAG
1573          */
1574         jtag_add_end_state(TAP_IDLE);
1575         xscale_jtag_set_instr(xscale->jtag_info.tap, xscale->jtag_info.dcsr);
1576
1577         /* set Hold reset, Halt mode and Trap Reset */
1578         buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);
1579         buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);
1580         xscale_write_dcsr(target, 1, 0);
1581
1582         /* select BYPASS, because having DCSR selected caused problems on the PXA27x */
1583         xscale_jtag_set_instr(xscale->jtag_info.tap, 0x7f);
1584         jtag_execute_queue();
1585
1586         /* assert reset */
1587         jtag_add_reset(0, 1);
1588
1589         /* sleep 1ms, to be sure we fulfill any requirements */
1590         jtag_add_sleep(1000);
1591         jtag_execute_queue();
1592
1593         target->state = TARGET_RESET;
1594
1595     if (target->reset_halt)
1596     {
1597         int retval;
1598                 if ((retval = target_halt(target))!=ERROR_OK)
1599                         return retval;
1600     }
1601
1602         return ERROR_OK;
1603 }
1604
1605 int xscale_deassert_reset(target_t *target)
1606 {
1607         armv4_5_common_t *armv4_5 = target->arch_info;
1608         xscale_common_t *xscale = armv4_5->arch_info;
1609
1610         fileio_t debug_handler;
1611         u32 address;
1612         u32 binary_size;
1613
1614         u32 buf_cnt;
1615         u32 i;
1616         int retval;
1617
1618         breakpoint_t *breakpoint = target->breakpoints;
1619
1620         LOG_DEBUG("-");
1621
1622         xscale->ibcr_available = 2;
1623         xscale->ibcr0_used = 0;
1624         xscale->ibcr1_used = 0;
1625
1626         xscale->dbr_available = 2;
1627         xscale->dbr0_used = 0;
1628         xscale->dbr1_used = 0;
1629
1630         /* mark all hardware breakpoints as unset */
1631         while (breakpoint)
1632         {
1633                 if (breakpoint->type == BKPT_HARD)
1634                 {
1635                         breakpoint->set = 0;
1636                 }
1637                 breakpoint = breakpoint->next;
1638         }
1639
1640         if (!xscale->handler_installed)
1641         {
1642                 /* release SRST */
1643                 jtag_add_reset(0, 0);
1644
1645                 /* wait 300ms; 150 and 100ms were not enough */
1646                 jtag_add_sleep(300*1000);
1647
1648                 jtag_add_runtest(2030, TAP_IDLE);
1649                 jtag_execute_queue();
1650
1651                 /* set Hold reset, Halt mode and Trap Reset */
1652                 buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);
1653                 buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);
1654                 xscale_write_dcsr(target, 1, 0);
1655
1656                 /* Load debug handler */
1657                 if (fileio_open(&debug_handler, "xscale/debug_handler.bin", FILEIO_READ, FILEIO_BINARY) != ERROR_OK)
1658                 {
1659                         return ERROR_OK;
1660                 }
1661
1662                 if ((binary_size = debug_handler.size) % 4)
1663                 {
1664                         LOG_ERROR("debug_handler.bin: size not a multiple of 4");
1665                         exit(-1);
1666                 }
1667
1668                 if (binary_size > 0x800)
1669                 {
1670                         LOG_ERROR("debug_handler.bin: larger than 2kb");
1671                         exit(-1);
1672                 }
1673
1674                 binary_size = CEIL(binary_size, 32) * 32;
1675
1676                 address = xscale->handler_address;
1677                 while (binary_size > 0)
1678                 {
1679                         u32 cache_line[8];
1680                         u8 buffer[32];
1681
1682                         if ((retval = fileio_read(&debug_handler, 32, buffer, &buf_cnt)) != ERROR_OK)
1683                         {
1684
1685                         }
1686
1687                         for (i = 0; i < buf_cnt; i += 4)
1688                         {
1689                                 /* convert LE buffer to host-endian u32 */
1690                                 cache_line[i / 4] = le_to_h_u32(&buffer[i]);
1691                         }
1692
1693                         for (; buf_cnt < 32; buf_cnt += 4)
1694                         {
1695                                 cache_line[buf_cnt / 4] = 0xe1a08008;
1696                         }
1697
1698                         /* only load addresses other than the reset vectors */
1699                         if ((address % 0x400) != 0x0)
1700                         {
1701                                 xscale_load_ic(target, 1, address, cache_line);
1702                         }
1703
1704                         address += buf_cnt;
1705                         binary_size -= buf_cnt;
1706                 };
1707
1708                 xscale_load_ic(target, 1, 0x0, xscale->low_vectors);
1709                 xscale_load_ic(target, 1, 0xffff0000, xscale->high_vectors);
1710
1711                 jtag_add_runtest(30, TAP_IDLE);
1712
1713                 jtag_add_sleep(100000);
1714
1715                 /* set Hold reset, Halt mode and Trap Reset */
1716                 buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);
1717                 buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);
1718                 xscale_write_dcsr(target, 1, 0);
1719
1720                 /* clear Hold reset to let the target run (should enter debug handler) */
1721                 xscale_write_dcsr(target, 0, 1);
1722                 target->state = TARGET_RUNNING;
1723
1724                 if (!target->reset_halt)
1725                 {
1726                         jtag_add_sleep(10000);
1727
1728                         /* we should have entered debug now */
1729                         xscale_debug_entry(target);
1730                         target->state = TARGET_HALTED;
1731
1732                         /* resume the target */
1733                         xscale_resume(target, 1, 0x0, 1, 0);
1734                 }
1735
1736                 fileio_close(&debug_handler);
1737         }
1738         else
1739         {
1740                 jtag_add_reset(0, 0);
1741         }
1742
1743         return ERROR_OK;
1744 }
1745
1746 int xscale_soft_reset_halt(struct target_s *target)
1747 {
1748         return ERROR_OK;
1749 }
1750
1751 int xscale_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
1752 {
1753         return ERROR_OK;
1754 }
1755
1756 int xscale_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)
1757 {
1758
1759         return ERROR_OK;
1760 }
1761
1762 int xscale_full_context(target_t *target)
1763 {
1764         armv4_5_common_t *armv4_5 = target->arch_info;
1765
1766         u32 *buffer;
1767
1768         int i, j;
1769
1770         LOG_DEBUG("-");
1771
1772         if (target->state != TARGET_HALTED)
1773         {
1774                 LOG_WARNING("target not halted");
1775                 return ERROR_TARGET_NOT_HALTED;
1776         }
1777
1778         buffer = malloc(4 * 8);
1779
1780         /* iterate through processor modes (FIQ, IRQ, SVC, ABT, UND and SYS)
1781          * we can't enter User mode on an XScale (unpredictable),
1782          * but User shares registers with SYS
1783          */
1784         for(i = 1; i < 7; i++)
1785         {
1786                 int valid = 1;
1787
1788                 /* check if there are invalid registers in the current mode
1789                  */
1790                 for (j = 0; j <= 16; j++)
1791                 {
1792                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1793                                 valid = 0;
1794                 }
1795
1796                 if (!valid)
1797                 {
1798                         u32 tmp_cpsr;
1799
1800                         /* request banked registers */
1801                         xscale_send_u32(target, 0x0);
1802
1803                         tmp_cpsr = 0x0;
1804                         tmp_cpsr |= armv4_5_number_to_mode(i);
1805                         tmp_cpsr |= 0xc0; /* I/F bits */
1806
1807                         /* send CPSR for desired mode */
1808                         xscale_send_u32(target, tmp_cpsr);
1809
1810                         /* get banked registers, r8 to r14, and spsr if not in USR/SYS mode */
1811                         if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))
1812                         {
1813                                 xscale_receive(target, buffer, 8);
1814                                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, buffer[7]);
1815                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
1816                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;
1817                         }
1818                         else
1819                         {
1820                                 xscale_receive(target, buffer, 7);
1821                         }
1822
1823                         /* move data from buffer to register cache */
1824                         for (j = 8; j <= 14; j++)
1825                         {
1826                                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value, 0, 32, buffer[j - 8]);
1827                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
1828                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;
1829                         }
1830                 }
1831         }
1832
1833         free(buffer);
1834
1835         return ERROR_OK;
1836 }
1837
1838 int xscale_restore_context(target_t *target)
1839 {
1840         armv4_5_common_t *armv4_5 = target->arch_info;
1841
1842         int i, j;
1843
1844         LOG_DEBUG("-");
1845
1846         if (target->state != TARGET_HALTED)
1847         {
1848                 LOG_WARNING("target not halted");
1849                 return ERROR_TARGET_NOT_HALTED;
1850         }
1851
1852         /* iterate through processor modes (FIQ, IRQ, SVC, ABT, UND and SYS)
1853         * we can't enter User mode on an XScale (unpredictable),
1854         * but User shares registers with SYS
1855         */
1856         for(i = 1; i < 7; i++)
1857         {
1858                 int dirty = 0;
1859
1860                 /* check if there are invalid registers in the current mode
1861                 */
1862                 for (j = 8; j <= 14; j++)
1863                 {
1864                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty == 1)
1865                                 dirty = 1;
1866                 }
1867
1868                 /* if not USR/SYS, check if the SPSR needs to be written */
1869                 if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))
1870                 {
1871                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty == 1)
1872                                 dirty = 1;
1873                 }
1874
1875                 if (dirty)
1876                 {
1877                         u32 tmp_cpsr;
1878
1879                         /* send banked registers */
1880                         xscale_send_u32(target, 0x1);
1881
1882                         tmp_cpsr = 0x0;
1883                         tmp_cpsr |= armv4_5_number_to_mode(i);
1884                         tmp_cpsr |= 0xc0; /* I/F bits */
1885
1886                         /* send CPSR for desired mode */
1887                         xscale_send_u32(target, tmp_cpsr);
1888
1889                         /* send banked registers, r8 to r14, and spsr if not in USR/SYS mode */
1890                         for (j = 8; j <= 14; j++)
1891                         {
1892                                 xscale_send_u32(target, buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, j).value, 0, 32));
1893                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
1894                         }
1895
1896                         if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))
1897                         {
1898                                 xscale_send_u32(target, buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32));
1899                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
1900                         }
1901                 }
1902         }
1903
1904         return ERROR_OK;
1905 }
1906
1907 int xscale_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1908 {
1909         armv4_5_common_t *armv4_5 = target->arch_info;
1910         xscale_common_t *xscale = armv4_5->arch_info;
1911         u32 *buf32;
1912         u32 i;
1913         int retval;
1914
1915         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1916
1917         if (target->state != TARGET_HALTED)
1918         {
1919                 LOG_WARNING("target not halted");
1920                 return ERROR_TARGET_NOT_HALTED;
1921         }
1922
1923         /* sanitize arguments */
1924         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1925                 return ERROR_INVALID_ARGUMENTS;
1926
1927         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1928                 return ERROR_TARGET_UNALIGNED_ACCESS;
1929
1930         /* send memory read request (command 0x1n, n: access size) */
1931         if ((retval=xscale_send_u32(target, 0x10 | size))!=ERROR_OK)
1932                 return retval;
1933
1934         /* send base address for read request */
1935         if ((retval=xscale_send_u32(target, address))!=ERROR_OK)
1936                 return retval;
1937
1938         /* send number of requested data words */
1939         if ((retval=xscale_send_u32(target, count))!=ERROR_OK)
1940                 return retval;
1941
1942         /* receive data from target (count times 32-bit words in host endianness) */
1943         buf32 = malloc(4 * count);
1944         if ((retval=xscale_receive(target, buf32, count))!=ERROR_OK)
1945                 return retval;
1946
1947         /* extract data from host-endian buffer into byte stream */
1948         for (i = 0; i < count; i++)
1949         {
1950                 switch (size)
1951                 {
1952                         case 4:
1953                                 target_buffer_set_u32(target, buffer, buf32[i]);
1954                                 buffer += 4;
1955                                 break;
1956                         case 2:
1957                                 target_buffer_set_u16(target, buffer, buf32[i] & 0xffff);
1958                                 buffer += 2;
1959                                 break;
1960                         case 1:
1961                                 *buffer++ = buf32[i] & 0xff;
1962                                 break;
1963                         default:
1964                                 LOG_ERROR("should never get here");
1965                                 exit(-1);
1966                 }
1967         }
1968
1969         free(buf32);
1970
1971         /* examine DCSR, to see if Sticky Abort (SA) got set */
1972         if ((retval=xscale_read_dcsr(target))!=ERROR_OK)
1973                 return retval;
1974         if (buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 5, 1) == 1)
1975         {
1976                 /* clear SA bit */
1977                 if ((retval=xscale_send_u32(target, 0x60))!=ERROR_OK)
1978                         return retval;
1979
1980                 return ERROR_TARGET_DATA_ABORT;
1981         }
1982
1983         return ERROR_OK;
1984 }
1985
1986 int xscale_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1987 {
1988         armv4_5_common_t *armv4_5 = target->arch_info;
1989         xscale_common_t *xscale = armv4_5->arch_info;
1990         int retval;
1991
1992         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1993
1994         if (target->state != TARGET_HALTED)
1995         {
1996                 LOG_WARNING("target not halted");
1997                 return ERROR_TARGET_NOT_HALTED;
1998         }
1999
2000         /* sanitize arguments */
2001         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
2002                 return ERROR_INVALID_ARGUMENTS;
2003
2004         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
2005                 return ERROR_TARGET_UNALIGNED_ACCESS;
2006
2007         /* send memory write request (command 0x2n, n: access size) */
2008         if ((retval=xscale_send_u32(target, 0x20 | size))!=ERROR_OK)
2009                 return retval;
2010
2011         /* send base address for read request */
2012         if ((retval=xscale_send_u32(target, address))!=ERROR_OK)
2013                 return retval;
2014
2015         /* send number of requested data words to be written*/
2016         if ((retval=xscale_send_u32(target, count))!=ERROR_OK)
2017                 return retval;
2018
2019         /* extract data from host-endian buffer into byte stream */
2020 #if 0
2021         for (i = 0; i < count; i++)
2022         {
2023                 switch (size)
2024                 {
2025                         case 4:
2026                                 value = target_buffer_get_u32(target, buffer);
2027                                 xscale_send_u32(target, value);
2028                                 buffer += 4;
2029                                 break;
2030                         case 2:
2031                                 value = target_buffer_get_u16(target, buffer);
2032                                 xscale_send_u32(target, value);
2033                                 buffer += 2;
2034                                 break;
2035                         case 1:
2036                                 value = *buffer;
2037                                 xscale_send_u32(target, value);
2038                                 buffer += 1;
2039                                 break;
2040                         default:
2041                                 LOG_ERROR("should never get here");
2042                                 exit(-1);
2043                 }
2044         }
2045 #endif
2046         if ((retval=xscale_send(target, buffer, count, size))!=ERROR_OK)
2047                 return retval;
2048
2049         /* examine DCSR, to see if Sticky Abort (SA) got set */
2050         if ((retval=xscale_read_dcsr(target))!=ERROR_OK)
2051                 return retval;
2052         if (buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 5, 1) == 1)
2053         {
2054                 /* clear SA bit */
2055                 if ((retval=xscale_send_u32(target, 0x60))!=ERROR_OK)
2056                         return retval;
2057
2058                 return ERROR_TARGET_DATA_ABORT;
2059         }
2060
2061         return ERROR_OK;
2062 }
2063
2064 int xscale_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)
2065 {
2066         return xscale_write_memory(target, address, 4, count, buffer);
2067 }
2068
2069 u32 xscale_get_ttb(target_t *target)
2070 {
2071         armv4_5_common_t *armv4_5 = target->arch_info;
2072         xscale_common_t *xscale = armv4_5->arch_info;
2073         u32 ttb;
2074
2075         xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_TTB]);
2076         ttb = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_TTB].value, 0, 32);
2077
2078         return ttb;
2079 }
2080
2081 void xscale_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
2082 {
2083         armv4_5_common_t *armv4_5 = target->arch_info;
2084         xscale_common_t *xscale = armv4_5->arch_info;
2085         u32 cp15_control;
2086
2087         /* read cp15 control register */
2088         xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
2089         cp15_control = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);
2090
2091         if (mmu)
2092                 cp15_control &= ~0x1U;
2093
2094         if (d_u_cache)
2095         {
2096                 /* clean DCache */
2097                 xscale_send_u32(target, 0x50);
2098                 xscale_send_u32(target, xscale->cache_clean_address);
2099
2100                 /* invalidate DCache */
2101                 xscale_send_u32(target, 0x51);
2102
2103                 cp15_control &= ~0x4U;
2104         }
2105
2106         if (i_cache)
2107         {
2108                 /* invalidate ICache */
2109                 xscale_send_u32(target, 0x52);
2110                 cp15_control &= ~0x1000U;
2111         }
2112
2113         /* write new cp15 control register */
2114         xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);
2115
2116         /* execute cpwait to ensure outstanding operations complete */
2117         xscale_send_u32(target, 0x53);
2118 }
2119
2120 void xscale_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
2121 {
2122         armv4_5_common_t *armv4_5 = target->arch_info;
2123         xscale_common_t *xscale = armv4_5->arch_info;
2124         u32 cp15_control;
2125
2126         /* read cp15 control register */
2127         xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
2128         cp15_control = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);
2129
2130         if (mmu)
2131                 cp15_control |= 0x1U;
2132
2133         if (d_u_cache)
2134                 cp15_control |= 0x4U;
2135
2136         if (i_cache)
2137                 cp15_control |= 0x1000U;
2138
2139         /* write new cp15 control register */
2140         xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);
2141
2142         /* execute cpwait to ensure outstanding operations complete */
2143         xscale_send_u32(target, 0x53);
2144 }
2145
2146 int xscale_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
2147 {
2148         int retval;
2149         armv4_5_common_t *armv4_5 = target->arch_info;
2150         xscale_common_t *xscale = armv4_5->arch_info;
2151
2152         if (target->state != TARGET_HALTED)
2153         {
2154                 LOG_WARNING("target not halted");
2155                 return ERROR_TARGET_NOT_HALTED;
2156         }
2157
2158         if (breakpoint->set)
2159         {
2160                 LOG_WARNING("breakpoint already set");
2161                 return ERROR_OK;
2162         }
2163
2164         if (breakpoint->type == BKPT_HARD)
2165         {
2166                 u32 value = breakpoint->address | 1;
2167                 if (!xscale->ibcr0_used)
2168                 {
2169                         xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR0], value);
2170                         xscale->ibcr0_used = 1;
2171                         breakpoint->set = 1;    /* breakpoint set on first breakpoint register */
2172                 }
2173                 else if (!xscale->ibcr1_used)
2174                 {
2175                         xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR1], value);
2176                         xscale->ibcr1_used = 1;
2177                         breakpoint->set = 2;    /* breakpoint set on second breakpoint register */
2178                 }
2179                 else
2180                 {
2181                         LOG_ERROR("BUG: no hardware comparator available");
2182                         return ERROR_OK;
2183                 }
2184         }
2185         else if (breakpoint->type == BKPT_SOFT)
2186         {
2187                 if (breakpoint->length == 4)
2188                 {
2189                         /* keep the original instruction in target endianness */
2190                         if((retval = target_read_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
2191                         {
2192                                 return retval;
2193                         }
2194                         /* write the original instruction in target endianness (arm7_9->arm_bkpt is host endian) */
2195                         if((retval = target_write_u32(target, breakpoint->address, xscale->arm_bkpt)) != ERROR_OK)
2196                         {
2197                                 return retval;
2198                         }
2199                 }
2200                 else
2201                 {
2202                         /* keep the original instruction in target endianness */
2203                         if((retval = target_read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr)) != ERROR_OK)
2204                         {
2205                                 return retval;
2206                         }
2207                         /* write the original instruction in target endianness (arm7_9->arm_bkpt is host endian) */
2208                         if((retval = target_write_u32(target, breakpoint->address, xscale->thumb_bkpt)) != ERROR_OK)
2209                         {
2210                                 return retval;
2211                         }
2212                 }
2213                 breakpoint->set = 1;
2214         }
2215
2216         return ERROR_OK;
2217 }
2218
2219 int xscale_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
2220 {
2221         armv4_5_common_t *armv4_5 = target->arch_info;
2222         xscale_common_t *xscale = armv4_5->arch_info;
2223
2224         if (target->state != TARGET_HALTED)
2225         {
2226                 LOG_WARNING("target not halted");
2227                 return ERROR_TARGET_NOT_HALTED;
2228         }
2229
2230         if ((breakpoint->type == BKPT_HARD) && (xscale->ibcr_available < 1))
2231         {
2232                 LOG_INFO("no breakpoint unit available for hardware breakpoint");
2233                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2234         }
2235
2236         if ((breakpoint->length != 2) && (breakpoint->length != 4))
2237         {
2238                 LOG_INFO("only breakpoints of two (Thumb) or four (ARM) bytes length supported");
2239                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2240         }
2241
2242         if (breakpoint->type == BKPT_HARD)
2243         {
2244                 xscale->ibcr_available--;
2245         }
2246
2247         return ERROR_OK;
2248 }
2249
2250 int xscale_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
2251 {
2252         int retval;
2253         armv4_5_common_t *armv4_5 = target->arch_info;
2254         xscale_common_t *xscale = armv4_5->arch_info;
2255
2256         if (target->state != TARGET_HALTED)
2257         {
2258                 LOG_WARNING("target not halted");
2259                 return ERROR_TARGET_NOT_HALTED;
2260         }
2261
2262         if (!breakpoint->set)
2263         {
2264                 LOG_WARNING("breakpoint not set");
2265                 return ERROR_OK;
2266         }
2267
2268         if (breakpoint->type == BKPT_HARD)
2269         {
2270                 if (breakpoint->set == 1)
2271                 {
2272                         xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR0], 0x0);
2273                         xscale->ibcr0_used = 0;
2274                 }
2275                 else if (breakpoint->set == 2)
2276                 {
2277                         xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR1], 0x0);
2278                         xscale->ibcr1_used = 0;
2279                 }
2280                 breakpoint->set = 0;
2281         }
2282         else
2283         {
2284                 /* restore original instruction (kept in target endianness) */
2285                 if (breakpoint->length == 4)
2286                 {
2287                         if((retval = target_write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr)) != ERROR_OK)
2288                         {
2289                                 return retval;
2290                         }
2291                 }
2292                 else
2293                 {
2294                         if((retval = target_write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr)) != ERROR_OK)
2295                         {
2296                                 return retval;
2297                         }
2298                 }
2299                 breakpoint->set = 0;
2300         }
2301
2302         return ERROR_OK;
2303 }
2304
2305 int xscale_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
2306 {
2307         armv4_5_common_t *armv4_5 = target->arch_info;
2308         xscale_common_t *xscale = armv4_5->arch_info;
2309
2310         if (target->state != TARGET_HALTED)
2311         {
2312                 LOG_WARNING("target not halted");
2313                 return ERROR_TARGET_NOT_HALTED;
2314         }
2315
2316         if (breakpoint->set)
2317         {
2318                 xscale_unset_breakpoint(target, breakpoint);
2319         }
2320
2321         if (breakpoint->type == BKPT_HARD)
2322                 xscale->ibcr_available++;
2323
2324         return ERROR_OK;
2325 }
2326
2327 int xscale_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
2328 {
2329         armv4_5_common_t *armv4_5 = target->arch_info;
2330         xscale_common_t *xscale = armv4_5->arch_info;
2331         u8 enable=0;
2332         reg_t *dbcon = &xscale->reg_cache->reg_list[XSCALE_DBCON];
2333         u32 dbcon_value = buf_get_u32(dbcon->value, 0, 32);
2334
2335         if (target->state != TARGET_HALTED)
2336         {
2337                 LOG_WARNING("target not halted");
2338                 return ERROR_TARGET_NOT_HALTED;
2339         }
2340
2341         xscale_get_reg(dbcon);
2342
2343         switch (watchpoint->rw)
2344         {
2345                 case WPT_READ:
2346                         enable = 0x3;
2347                         break;
2348                 case WPT_ACCESS:
2349                         enable = 0x2;
2350                         break;
2351                 case WPT_WRITE:
2352                         enable = 0x1;
2353                         break;
2354                 default:
2355                         LOG_ERROR("BUG: watchpoint->rw neither read, write nor access");
2356         }
2357
2358         if (!xscale->dbr0_used)
2359         {
2360                 xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_DBR0], watchpoint->address);
2361                 dbcon_value |= enable;
2362                 xscale_set_reg_u32(dbcon, dbcon_value);
2363                 watchpoint->set = 1;
2364                 xscale->dbr0_used = 1;
2365         }
2366         else if (!xscale->dbr1_used)
2367         {
2368                 xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_DBR1], watchpoint->address);
2369                 dbcon_value |= enable << 2;
2370                 xscale_set_reg_u32(dbcon, dbcon_value);
2371                 watchpoint->set = 2;
2372                 xscale->dbr1_used = 1;
2373         }
2374         else
2375         {
2376                 LOG_ERROR("BUG: no hardware comparator available");
2377                 return ERROR_OK;
2378         }
2379
2380         return ERROR_OK;
2381 }
2382
2383 int xscale_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
2384 {
2385         armv4_5_common_t *armv4_5 = target->arch_info;
2386         xscale_common_t *xscale = armv4_5->arch_info;
2387
2388         if (target->state != TARGET_HALTED)
2389         {
2390                 LOG_WARNING("target not halted");
2391                 return ERROR_TARGET_NOT_HALTED;
2392         }
2393
2394         if (xscale->dbr_available < 1)
2395         {
2396                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2397         }
2398
2399         if ((watchpoint->length != 1) && (watchpoint->length != 2) && (watchpoint->length != 4))
2400         {
2401                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2402         }
2403
2404         xscale->dbr_available--;
2405
2406         return ERROR_OK;
2407 }
2408
2409 int xscale_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
2410 {
2411         armv4_5_common_t *armv4_5 = target->arch_info;
2412         xscale_common_t *xscale = armv4_5->arch_info;
2413         reg_t *dbcon = &xscale->reg_cache->reg_list[XSCALE_DBCON];
2414         u32 dbcon_value = buf_get_u32(dbcon->value, 0, 32);
2415
2416         if (target->state != TARGET_HALTED)
2417         {
2418                 LOG_WARNING("target not halted");
2419                 return ERROR_TARGET_NOT_HALTED;
2420         }
2421
2422         if (!watchpoint->set)
2423         {
2424                 LOG_WARNING("breakpoint not set");
2425                 return ERROR_OK;
2426         }
2427
2428         if (watchpoint->set == 1)
2429         {
2430                 dbcon_value &= ~0x3;
2431                 xscale_set_reg_u32(dbcon, dbcon_value);
2432                 xscale->dbr0_used = 0;
2433         }
2434         else if (watchpoint->set == 2)
2435         {
2436                 dbcon_value &= ~0xc;
2437                 xscale_set_reg_u32(dbcon, dbcon_value);
2438                 xscale->dbr1_used = 0;
2439         }
2440         watchpoint->set = 0;
2441
2442         return ERROR_OK;
2443 }
2444
2445 int xscale_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
2446 {
2447         armv4_5_common_t *armv4_5 = target->arch_info;
2448         xscale_common_t *xscale = armv4_5->arch_info;
2449
2450         if (target->state != TARGET_HALTED)
2451         {
2452                 LOG_WARNING("target not halted");
2453                 return ERROR_TARGET_NOT_HALTED;
2454         }
2455
2456         if (watchpoint->set)
2457         {
2458                 xscale_unset_watchpoint(target, watchpoint);
2459         }
2460
2461         xscale->dbr_available++;
2462
2463         return ERROR_OK;
2464 }
2465
2466 void xscale_enable_watchpoints(struct target_s *target)
2467 {
2468         watchpoint_t *watchpoint = target->watchpoints;
2469
2470         while (watchpoint)
2471         {
2472                 if (watchpoint->set == 0)
2473                         xscale_set_watchpoint(target, watchpoint);
2474                 watchpoint = watchpoint->next;
2475         }
2476 }
2477
2478 void xscale_enable_breakpoints(struct target_s *target)
2479 {
2480         breakpoint_t *breakpoint = target->breakpoints;
2481
2482         /* set any pending breakpoints */
2483         while (breakpoint)
2484         {
2485                 if (breakpoint->set == 0)
2486                         xscale_set_breakpoint(target, breakpoint);
2487                 breakpoint = breakpoint->next;
2488         }
2489 }
2490
2491 int xscale_get_reg(reg_t *reg)
2492 {
2493         xscale_reg_t *arch_info = reg->arch_info;
2494         target_t *target = arch_info->target;
2495         armv4_5_common_t *armv4_5 = target->arch_info;
2496         xscale_common_t *xscale = armv4_5->arch_info;
2497
2498         /* DCSR, TX and RX are accessible via JTAG */
2499         if (strcmp(reg->name, "XSCALE_DCSR") == 0)
2500         {
2501                 return xscale_read_dcsr(arch_info->target);
2502         }
2503         else if (strcmp(reg->name, "XSCALE_TX") == 0)
2504         {
2505                 /* 1 = consume register content */
2506                 return xscale_read_tx(arch_info->target, 1);
2507         }
2508         else if (strcmp(reg->name, "XSCALE_RX") == 0)
2509         {
2510                 /* can't read from RX register (host -> debug handler) */
2511                 return ERROR_OK;
2512         }
2513         else if (strcmp(reg->name, "XSCALE_TXRXCTRL") == 0)
2514         {
2515                 /* can't (explicitly) read from TXRXCTRL register */
2516                 return ERROR_OK;
2517         }
2518         else /* Other DBG registers have to be transfered by the debug handler */
2519         {
2520                 /* send CP read request (command 0x40) */
2521                 xscale_send_u32(target, 0x40);
2522
2523                 /* send CP register number */
2524                 xscale_send_u32(target, arch_info->dbg_handler_number);
2525
2526                 /* read register value */
2527                 xscale_read_tx(target, 1);
2528                 buf_cpy(xscale->reg_cache->reg_list[XSCALE_TX].value, reg->value, 32);
2529
2530                 reg->dirty = 0;
2531                 reg->valid = 1;
2532         }
2533
2534         return ERROR_OK;
2535 }
2536
2537 int xscale_set_reg(reg_t *reg, u8* buf)
2538 {
2539         xscale_reg_t *arch_info = reg->arch_info;
2540         target_t *target = arch_info->target;
2541         armv4_5_common_t *armv4_5 = target->arch_info;
2542         xscale_common_t *xscale = armv4_5->arch_info;
2543         u32 value = buf_get_u32(buf, 0, 32);
2544
2545         /* DCSR, TX and RX are accessible via JTAG */
2546         if (strcmp(reg->name, "XSCALE_DCSR") == 0)
2547         {
2548                 buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 0, 32, value);
2549                 return xscale_write_dcsr(arch_info->target, -1, -1);
2550         }
2551         else if (strcmp(reg->name, "XSCALE_RX") == 0)
2552         {
2553                 buf_set_u32(xscale->reg_cache->reg_list[XSCALE_RX].value, 0, 32, value);
2554                 return xscale_write_rx(arch_info->target);
2555         }
2556         else if (strcmp(reg->name, "XSCALE_TX") == 0)
2557         {
2558                 /* can't write to TX register (debug-handler -> host) */
2559                 return ERROR_OK;
2560         }
2561         else if (strcmp(reg->name, "XSCALE_TXRXCTRL") == 0)
2562         {
2563                 /* can't (explicitly) write to TXRXCTRL register */
2564                 return ERROR_OK;
2565         }
2566         else /* Other DBG registers have to be transfered by the debug handler */
2567         {
2568                 /* send CP write request (command 0x41) */
2569                 xscale_send_u32(target, 0x41);
2570
2571                 /* send CP register number */
2572                 xscale_send_u32(target, arch_info->dbg_handler_number);
2573
2574                 /* send CP register value */
2575                 xscale_send_u32(target, value);
2576                 buf_set_u32(reg->value, 0, 32, value);
2577         }
2578
2579         return ERROR_OK;
2580 }
2581
2582 /* convenience wrapper to access XScale specific registers */
2583 int xscale_set_reg_u32(reg_t *reg, u32 value)
2584 {
2585         u8 buf[4];
2586
2587         buf_set_u32(buf, 0, 32, value);
2588
2589         return xscale_set_reg(reg, buf);
2590 }
2591
2592 int xscale_write_dcsr_sw(target_t *target, u32 value)
2593 {
2594         /* get pointers to arch-specific information */
2595         armv4_5_common_t *armv4_5 = target->arch_info;
2596         xscale_common_t *xscale = armv4_5->arch_info;
2597         reg_t *dcsr = &xscale->reg_cache->reg_list[XSCALE_DCSR];
2598         xscale_reg_t *dcsr_arch_info = dcsr->arch_info;
2599
2600         /* send CP write request (command 0x41) */
2601         xscale_send_u32(target, 0x41);
2602
2603         /* send CP register number */
2604         xscale_send_u32(target, dcsr_arch_info->dbg_handler_number);
2605
2606         /* send CP register value */
2607         xscale_send_u32(target, value);
2608         buf_set_u32(dcsr->value, 0, 32, value);
2609
2610         return ERROR_OK;
2611 }
2612
2613 int xscale_read_trace(target_t *target)
2614 {
2615         /* get pointers to arch-specific information */
2616         armv4_5_common_t *armv4_5 = target->arch_info;
2617         xscale_common_t *xscale = armv4_5->arch_info;
2618         xscale_trace_data_t **trace_data_p;
2619
2620         /* 258 words from debug handler
2621          * 256 trace buffer entries
2622          * 2 checkpoint addresses
2623          */
2624         u32 trace_buffer[258];
2625         int is_address[256];
2626         int i, j;
2627
2628         if (target->state != TARGET_HALTED)
2629         {
2630                 LOG_WARNING("target must be stopped to read trace data");
2631                 return ERROR_TARGET_NOT_HALTED;
2632         }
2633
2634         /* send read trace buffer command (command 0x61) */
2635         xscale_send_u32(target, 0x61);
2636
2637         /* receive trace buffer content */
2638         xscale_receive(target, trace_buffer, 258);
2639
2640         /* parse buffer backwards to identify address entries */
2641         for (i = 255; i >= 0; i--)
2642         {
2643                 is_address[i] = 0;
2644                 if (((trace_buffer[i] & 0xf0) == 0x90) ||
2645                         ((trace_buffer[i] & 0xf0) == 0xd0))
2646                 {
2647                         if (i >= 3)
2648                                 is_address[--i] = 1;
2649                         if (i >= 2)
2650                                 is_address[--i] = 1;
2651                         if (i >= 1)
2652                                 is_address[--i] = 1;
2653                         if (i >= 0)
2654                                 is_address[--i] = 1;
2655                 }
2656         }
2657
2658
2659         /* search first non-zero entry */
2660         for (j = 0; (j < 256) && (trace_buffer[j] == 0) && (!is_address[j]); j++)
2661                 ;
2662
2663         if (j == 256)
2664         {
2665                 LOG_DEBUG("no trace data collected");
2666                 return ERROR_XSCALE_NO_TRACE_DATA;
2667         }
2668
2669         for (trace_data_p = &xscale->trace.data; *trace_data_p; trace_data_p = &(*trace_data_p)->next)
2670                 ;
2671
2672         *trace_data_p = malloc(sizeof(xscale_trace_data_t));
2673         (*trace_data_p)->next = NULL;
2674         (*trace_data_p)->chkpt0 = trace_buffer[256];
2675         (*trace_data_p)->chkpt1 = trace_buffer[257];
2676         (*trace_data_p)->last_instruction = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
2677         (*trace_data_p)->entries = malloc(sizeof(xscale_trace_entry_t) * (256 - j));
2678         (*trace_data_p)->depth = 256 - j;
2679
2680         for (i = j; i < 256; i++)
2681         {
2682                 (*trace_data_p)->entries[i - j].data = trace_buffer[i];
2683                 if (is_address[i])
2684                         (*trace_data_p)->entries[i - j].type = XSCALE_TRACE_ADDRESS;
2685                 else
2686                         (*trace_data_p)->entries[i - j].type = XSCALE_TRACE_MESSAGE;
2687         }
2688
2689         return ERROR_OK;
2690 }
2691
2692 int xscale_read_instruction(target_t *target, arm_instruction_t *instruction)
2693 {
2694         /* get pointers to arch-specific information */
2695         armv4_5_common_t *armv4_5 = target->arch_info;
2696         xscale_common_t *xscale = armv4_5->arch_info;
2697         int i;
2698         int section = -1;
2699         u32 size_read;
2700         u32 opcode;
2701         int retval;
2702
2703         if (!xscale->trace.image)
2704                 return ERROR_TRACE_IMAGE_UNAVAILABLE;
2705
2706         /* search for the section the current instruction belongs to */
2707         for (i = 0; i < xscale->trace.image->num_sections; i++)
2708         {
2709                 if ((xscale->trace.image->sections[i].base_address <= xscale->trace.current_pc) &&
2710                         (xscale->trace.image->sections[i].base_address + xscale->trace.image->sections[i].size > xscale->trace.current_pc))
2711                 {
2712                         section = i;
2713                         break;
2714                 }
2715         }
2716
2717         if (section == -1)
2718         {
2719                 /* current instruction couldn't be found in the image */
2720                 return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
2721         }
2722
2723         if (xscale->trace.core_state == ARMV4_5_STATE_ARM)
2724         {
2725                 u8 buf[4];
2726                 if ((retval = image_read_section(xscale->trace.image, section,
2727                         xscale->trace.current_pc - xscale->trace.image->sections[section].base_address,
2728                         4, buf, &size_read)) != ERROR_OK)
2729                 {
2730                         LOG_ERROR("error while reading instruction: %i", retval);
2731                         return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
2732                 }
2733                 opcode = target_buffer_get_u32(target, buf);
2734                 arm_evaluate_opcode(opcode, xscale->trace.current_pc, instruction);
2735         }
2736         else if (xscale->trace.core_state == ARMV4_5_STATE_THUMB)
2737         {
2738                 u8 buf[2];
2739                 if ((retval = image_read_section(xscale->trace.image, section,
2740                         xscale->trace.current_pc - xscale->trace.image->sections[section].base_address,
2741                         2, buf, &size_read)) != ERROR_OK)
2742                 {
2743                         LOG_ERROR("error while reading instruction: %i", retval);
2744                         return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
2745                 }
2746                 opcode = target_buffer_get_u16(target, buf);
2747                 thumb_evaluate_opcode(opcode, xscale->trace.current_pc, instruction);
2748         }
2749         else
2750         {
2751                 LOG_ERROR("BUG: unknown core state encountered");
2752                 exit(-1);
2753         }
2754
2755         return ERROR_OK;
2756 }
2757
2758 int xscale_branch_address(xscale_trace_data_t *trace_data, int i, u32 *target)
2759 {
2760         /* if there are less than four entries prior to the indirect branch message
2761          * we can't extract the address */
2762         if (i < 4)
2763         {
2764                 return -1;
2765         }
2766
2767         *target = (trace_data->entries[i-1].data) | (trace_data->entries[i-2].data << 8) |
2768                                 (trace_data->entries[i-3].data << 16) | (trace_data->entries[i-4].data << 24);
2769
2770         return 0;
2771 }
2772
2773 int xscale_analyze_trace(target_t *target, command_context_t *cmd_ctx)
2774 {
2775         /* get pointers to arch-specific information */
2776         armv4_5_common_t *armv4_5 = target->arch_info;
2777         xscale_common_t *xscale = armv4_5->arch_info;
2778         int next_pc_ok = 0;
2779         u32 next_pc = 0x0;
2780         xscale_trace_data_t *trace_data = xscale->trace.data;
2781         int retval;
2782
2783         while (trace_data)
2784         {
2785                 int i, chkpt;
2786                 int rollover;
2787                 int branch;
2788                 int exception;
2789                 xscale->trace.core_state = ARMV4_5_STATE_ARM;
2790
2791                 chkpt = 0;
2792                 rollover = 0;
2793
2794                 for (i = 0; i < trace_data->depth; i++)
2795                 {
2796                         next_pc_ok = 0;
2797                         branch = 0;
2798                         exception = 0;
2799
2800                         if (trace_data->entries[i].type == XSCALE_TRACE_ADDRESS)
2801                                 continue;
2802
2803                         switch ((trace_data->entries[i].data & 0xf0) >> 4)
2804                         {
2805                                 case 0:         /* Exceptions */
2806                                 case 1:
2807                                 case 2:
2808                                 case 3:
2809                                 case 4:
2810                                 case 5:
2811                                 case 6:
2812                                 case 7:
2813                                         exception = (trace_data->entries[i].data & 0x70) >> 4;
2814                                         next_pc_ok = 1;
2815                                         next_pc = (trace_data->entries[i].data & 0xf0) >> 2;
2816                                         command_print(cmd_ctx, "--- exception %i ---", (trace_data->entries[i].data & 0xf0) >> 4);
2817                                         break;
2818                                 case 8:         /* Direct Branch */
2819                                         branch = 1;
2820                                         break;
2821                                 case 9:         /* Indirect Branch */
2822                                         branch = 1;
2823                                         if (xscale_branch_address(trace_data, i, &next_pc) == 0)
2824                                         {
2825                                                 next_pc_ok = 1;
2826                                         }
2827                                         break;
2828                                 case 13:        /* Checkpointed Indirect Branch */
2829                                         if (xscale_branch_address(trace_data, i, &next_pc) == 0)
2830                                         {
2831                                                 next_pc_ok = 1;
2832                                                 if (((chkpt == 0) && (next_pc != trace_data->chkpt0))
2833                                                         || ((chkpt == 1) && (next_pc != trace_data->chkpt1)))
2834                                                         LOG_WARNING("checkpointed indirect branch target address doesn't match checkpoint");
2835                                         }
2836                                         /* explicit fall-through */
2837                                 case 12:        /* Checkpointed Direct Branch */
2838                                         branch = 1;
2839                                         if (chkpt == 0)
2840                                         {
2841                                                 next_pc_ok = 1;
2842                                                 next_pc = trace_data->chkpt0;
2843                                                 chkpt++;
2844                                         }
2845                                         else if (chkpt == 1)
2846                                         {
2847                                                 next_pc_ok = 1;
2848                                                 next_pc = trace_data->chkpt0;
2849                                                 chkpt++;
2850                                         }
2851                                         else
2852                                         {
2853                                                 LOG_WARNING("more than two checkpointed branches encountered");
2854                                         }
2855                                         break;
2856                                 case 15:        /* Roll-over */
2857                                         rollover++;
2858                                         continue;
2859                                 default:        /* Reserved */
2860                                         command_print(cmd_ctx, "--- reserved trace message ---");
2861                                         LOG_ERROR("BUG: trace message %i is reserved", (trace_data->entries[i].data & 0xf0) >> 4);
2862                                         return ERROR_OK;
2863                         }
2864
2865                         if (xscale->trace.pc_ok)
2866                         {
2867                                 int executed = (trace_data->entries[i].data & 0xf) + rollover * 16;
2868                                 arm_instruction_t instruction;
2869
2870                                 if ((exception == 6) || (exception == 7))
2871                                 {
2872                                         /* IRQ or FIQ exception, no instruction executed */
2873                                         executed -= 1;
2874                                 }
2875
2876                                 while (executed-- >= 0)
2877                                 {
2878                                         if ((retval = xscale_read_instruction(target, &instruction)) != ERROR_OK)
2879                                         {
2880                                                 /* can't continue tracing with no image available */
2881                                                 if (retval == ERROR_TRACE_IMAGE_UNAVAILABLE)
2882                                                 {
2883                                                         return retval;
2884                                                 }
2885                                                 else if (retval == ERROR_TRACE_INSTRUCTION_UNAVAILABLE)
2886                                                 {
2887                                                         /* TODO: handle incomplete images */
2888                                                 }
2889                                         }
2890
2891                                         /* a precise abort on a load to the PC is included in the incremental
2892                                          * word count, other instructions causing data aborts are not included
2893                                          */
2894                                         if ((executed == 0) && (exception == 4)
2895                                                 && ((instruction.type >= ARM_LDR) && (instruction.type <= ARM_LDM)))
2896                                         {
2897                                                 if ((instruction.type == ARM_LDM)
2898                                                         && ((instruction.info.load_store_multiple.register_list & 0x8000) == 0))
2899                                                 {
2900                                                         executed--;
2901                                                 }
2902                                                 else if (((instruction.type >= ARM_LDR) && (instruction.type <= ARM_LDRSH))
2903                                                         && (instruction.info.load_store.Rd != 15))
2904                                                 {
2905                                                         executed--;
2906                                                 }
2907                                         }
2908
2909                                         /* only the last instruction executed
2910                                          * (the one that caused the control flow change)
2911                                          * could be a taken branch
2912                                          */
2913                                         if (((executed == -1) && (branch == 1)) &&
2914                                                 (((instruction.type == ARM_B) ||
2915                                                         (instruction.type == ARM_BL) ||
2916                                                         (instruction.type == ARM_BLX)) &&
2917                                                         (instruction.info.b_bl_bx_blx.target_address != 0xffffffff)))
2918                                         {
2919                                                 xscale->trace.current_pc = instruction.info.b_bl_bx_blx.target_address;
2920                                         }
2921                                         else
2922                                         {
2923                                                 xscale->trace.current_pc += (xscale->trace.core_state == ARMV4_5_STATE_ARM) ? 4 : 2;
2924                                         }
2925                                         command_print(cmd_ctx, "%s", instruction.text);
2926                                 }
2927
2928                                 rollover = 0;
2929                         }
2930
2931                         if (next_pc_ok)
2932                         {
2933                                 xscale->trace.current_pc = next_pc;
2934                                 xscale->trace.pc_ok = 1;
2935                         }
2936                 }
2937
2938                 for (; xscale->trace.current_pc < trace_data->last_instruction; xscale->trace.current_pc += (xscale->trace.core_state == ARMV4_5_STATE_ARM) ? 4 : 2)
2939                 {
2940                         arm_instruction_t instruction;
2941                         if ((retval = xscale_read_instruction(target, &instruction)) != ERROR_OK)
2942                         {
2943                                 /* can't continue tracing with no image available */
2944                                 if (retval == ERROR_TRACE_IMAGE_UNAVAILABLE)
2945                                 {
2946                                         return retval;
2947                                 }
2948                                 else if (retval == ERROR_TRACE_INSTRUCTION_UNAVAILABLE)
2949                                 {
2950                                         /* TODO: handle incomplete images */
2951                                 }
2952                         }
2953                         command_print(cmd_ctx, "%s", instruction.text);
2954                 }
2955
2956                 trace_data = trace_data->next;
2957         }
2958
2959         return ERROR_OK;
2960 }
2961
2962 void xscale_build_reg_cache(target_t *target)
2963 {
2964         /* get pointers to arch-specific information */
2965         armv4_5_common_t *armv4_5 = target->arch_info;
2966         xscale_common_t *xscale = armv4_5->arch_info;
2967
2968         reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);
2969         xscale_reg_t *arch_info = malloc(sizeof(xscale_reg_arch_info));
2970         int i;
2971         int num_regs = sizeof(xscale_reg_arch_info) / sizeof(xscale_reg_t);
2972
2973         (*cache_p) = armv4_5_build_reg_cache(target, armv4_5);
2974         armv4_5->core_cache = (*cache_p);
2975
2976         /* register a register arch-type for XScale dbg registers only once */
2977         if (xscale_reg_arch_type == -1)
2978                 xscale_reg_arch_type = register_reg_arch_type(xscale_get_reg, xscale_set_reg);
2979
2980         (*cache_p)->next = malloc(sizeof(reg_cache_t));
2981         cache_p = &(*cache_p)->next;
2982
2983         /* fill in values for the xscale reg cache */
2984         (*cache_p)->name = "XScale registers";
2985         (*cache_p)->next = NULL;
2986         (*cache_p)->reg_list = malloc(num_regs * sizeof(reg_t));
2987         (*cache_p)->num_regs = num_regs;
2988
2989         for (i = 0; i < num_regs; i++)
2990         {
2991                 (*cache_p)->reg_list[i].name = xscale_reg_list[i];
2992                 (*cache_p)->reg_list[i].value = calloc(4, 1);
2993                 (*cache_p)->reg_list[i].dirty = 0;
2994                 (*cache_p)->reg_list[i].valid = 0;
2995                 (*cache_p)->reg_list[i].size = 32;
2996                 (*cache_p)->reg_list[i].bitfield_desc = NULL;
2997                 (*cache_p)->reg_list[i].num_bitfields = 0;
2998                 (*cache_p)->reg_list[i].arch_info = &arch_info[i];
2999                 (*cache_p)->reg_list[i].arch_type = xscale_reg_arch_type;
3000                 arch_info[i] = xscale_reg_arch_info[i];
3001                 arch_info[i].target = target;
3002         }
3003
3004         xscale->reg_cache = (*cache_p);
3005 }
3006
3007 int xscale_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
3008 {
3009         return ERROR_OK;
3010 }
3011
3012 int xscale_quit(void)
3013 {
3014         return ERROR_OK;
3015 }
3016
3017 int xscale_init_arch_info(target_t *target, xscale_common_t *xscale, jtag_tap_t *tap, const char *variant)
3018 {
3019         armv4_5_common_t *armv4_5;
3020         u32 high_reset_branch, low_reset_branch;
3021         int i;
3022
3023         armv4_5 = &xscale->armv4_5_common;
3024
3025         /* store architecture specfic data (none so far) */
3026         xscale->arch_info = NULL;
3027         xscale->common_magic = XSCALE_COMMON_MAGIC;
3028
3029         /* remember the variant (PXA25x, PXA27x, IXP42x, ...) */
3030         xscale->variant = strdup(variant);
3031
3032         /* prepare JTAG information for the new target */
3033         xscale->jtag_info.tap = tap;
3034
3035         xscale->jtag_info.dbgrx = 0x02;
3036         xscale->jtag_info.dbgtx = 0x10;
3037         xscale->jtag_info.dcsr = 0x09;
3038         xscale->jtag_info.ldic = 0x07;
3039
3040         if ((strcmp(xscale->variant, "pxa250") == 0) ||
3041                 (strcmp(xscale->variant, "pxa255") == 0) ||
3042                 (strcmp(xscale->variant, "pxa26x") == 0))
3043         {
3044                 xscale->jtag_info.ir_length = 5;
3045         }
3046         else if ((strcmp(xscale->variant, "pxa27x") == 0) ||
3047                 (strcmp(xscale->variant, "ixp42x") == 0) ||
3048                 (strcmp(xscale->variant, "ixp45x") == 0) ||
3049                 (strcmp(xscale->variant, "ixp46x") == 0))
3050         {
3051                 xscale->jtag_info.ir_length = 7;
3052         }
3053
3054         /* the debug handler isn't installed (and thus not running) at this time */
3055         xscale->handler_installed = 0;
3056         xscale->handler_running = 0;
3057         xscale->handler_address = 0xfe000800;
3058
3059         /* clear the vectors we keep locally for reference */
3060         memset(xscale->low_vectors, 0, sizeof(xscale->low_vectors));
3061         memset(xscale->high_vectors, 0, sizeof(xscale->high_vectors));
3062
3063         /* no user-specified vectors have been configured yet */
3064         xscale->static_low_vectors_set = 0x0;
3065         xscale->static_high_vectors_set = 0x0;
3066
3067         /* calculate branches to debug handler */
3068         low_reset_branch = (xscale->handler_address + 0x20 - 0x0 - 0x8) >> 2;
3069         high_reset_branch = (xscale->handler_address + 0x20 - 0xffff0000 - 0x8) >> 2;
3070
3071         xscale->low_vectors[0] = ARMV4_5_B((low_reset_branch & 0xffffff), 0);
3072         xscale->high_vectors[0] = ARMV4_5_B((high_reset_branch & 0xffffff), 0);
3073
3074         for (i = 1; i <= 7; i++)
3075         {
3076                 xscale->low_vectors[i] = ARMV4_5_B(0xfffffe, 0);
3077                 xscale->high_vectors[i] = ARMV4_5_B(0xfffffe, 0);
3078         }
3079
3080         /* 64kB aligned region used for DCache cleaning */
3081         xscale->cache_clean_address = 0xfffe0000;
3082
3083         xscale->hold_rst = 0;
3084         xscale->external_debug_break = 0;
3085
3086         xscale->ibcr_available = 2;
3087         xscale->ibcr0_used = 0;
3088         xscale->ibcr1_used = 0;
3089
3090         xscale->dbr_available = 2;
3091         xscale->dbr0_used = 0;
3092         xscale->dbr1_used = 0;
3093
3094         xscale->arm_bkpt = ARMV5_BKPT(0x0);
3095         xscale->thumb_bkpt = ARMV5_T_BKPT(0x0) & 0xffff;
3096
3097         xscale->vector_catch = 0x1;
3098
3099         xscale->trace.capture_status = TRACE_IDLE;
3100         xscale->trace.data = NULL;
3101         xscale->trace.image = NULL;
3102         xscale->trace.buffer_enabled = 0;
3103         xscale->trace.buffer_fill = 0;
3104
3105         /* prepare ARMv4/5 specific information */
3106         armv4_5->arch_info = xscale;
3107         armv4_5->read_core_reg = xscale_read_core_reg;
3108         armv4_5->write_core_reg = xscale_write_core_reg;
3109         armv4_5->full_context = xscale_full_context;
3110
3111         armv4_5_init_arch_info(target, armv4_5);
3112
3113         xscale->armv4_5_mmu.armv4_5_cache.ctype = -1;
3114         xscale->armv4_5_mmu.get_ttb = xscale_get_ttb;
3115         xscale->armv4_5_mmu.read_memory = xscale_read_memory;
3116         xscale->armv4_5_mmu.write_memory = xscale_write_memory;
3117         xscale->armv4_5_mmu.disable_mmu_caches = xscale_disable_mmu_caches;
3118         xscale->armv4_5_mmu.enable_mmu_caches = xscale_enable_mmu_caches;
3119         xscale->armv4_5_mmu.has_tiny_pages = 1;
3120         xscale->armv4_5_mmu.mmu_enabled = 0;
3121
3122         return ERROR_OK;
3123 }
3124
3125 /* target xscale <endianess> <startup_mode> <chain_pos> <variant> */
3126 int xscale_target_create(struct target_s *target, Jim_Interp *interp)
3127 {
3128         xscale_common_t *xscale = calloc(1,sizeof(xscale_common_t));
3129
3130         xscale_init_arch_info(target, xscale, target->tap, target->variant);
3131         xscale_build_reg_cache(target);
3132
3133         return ERROR_OK;
3134 }
3135
3136 int xscale_handle_debug_handler_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
3137 {
3138         target_t *target = NULL;
3139         armv4_5_common_t *armv4_5;
3140         xscale_common_t *xscale;
3141
3142         u32 handler_address;
3143
3144         if (argc < 2)
3145         {
3146                 LOG_ERROR("'xscale debug_handler <target#> <address>' command takes two required operands");
3147                 return ERROR_OK;
3148         }
3149
3150         if ((target = get_target(args[0])) == NULL)
3151         {
3152                 LOG_ERROR("target '%s' not defined", args[0]);
3153                 return ERROR_FAIL;
3154         }
3155
3156         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3157         {
3158                 return ERROR_FAIL;
3159         }
3160
3161         handler_address = strtoul(args[1], NULL, 0);
3162
3163         if (((handler_address >= 0x800) && (handler_address <= 0x1fef800)) ||
3164                 ((handler_address >= 0xfe000800) && (handler_address <= 0xfffff800)))
3165         {
3166                 xscale->handler_address = handler_address;
3167         }
3168         else
3169         {
3170                 LOG_ERROR("xscale debug_handler <address> must be between 0x800 and 0x1fef800 or between 0xfe000800 and 0xfffff800");
3171                 return ERROR_FAIL;
3172         }
3173
3174         return ERROR_OK;
3175 }
3176
3177 int xscale_handle_cache_clean_address_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
3178 {
3179         target_t *target = NULL;
3180         armv4_5_common_t *armv4_5;
3181         xscale_common_t *xscale;
3182
3183         u32 cache_clean_address;
3184
3185         if (argc < 2)
3186         {
3187                 return ERROR_COMMAND_SYNTAX_ERROR;
3188         }
3189
3190         target = get_target(args[0]);
3191         if (target == NULL)
3192         {
3193                 LOG_ERROR("target '%s' not defined", args[0]);
3194                 return ERROR_FAIL;
3195         }
3196
3197         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3198         {
3199                 return ERROR_FAIL;
3200         }
3201
3202         cache_clean_address = strtoul(args[1], NULL, 0);
3203
3204         if (cache_clean_address & 0xffff)
3205         {
3206                 LOG_ERROR("xscale cache_clean_address <address> must be 64kb aligned");
3207         }
3208         else
3209         {
3210                 xscale->cache_clean_address = cache_clean_address;
3211         }
3212
3213         return ERROR_OK;
3214 }
3215
3216 int xscale_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
3217 {
3218         target_t *target = get_current_target(cmd_ctx);
3219         armv4_5_common_t *armv4_5;
3220         xscale_common_t *xscale;
3221
3222         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3223         {
3224                 return ERROR_OK;
3225         }
3226
3227         return armv4_5_handle_cache_info_command(cmd_ctx, &xscale->armv4_5_mmu.armv4_5_cache);
3228 }
3229
3230 static int xscale_virt2phys(struct target_s *target, u32 virtual, u32 *physical)
3231 {
3232         armv4_5_common_t *armv4_5;
3233         xscale_common_t *xscale;
3234         int retval;
3235         int type;
3236         u32 cb;
3237         int domain;
3238         u32 ap;
3239
3240         if ((retval = xscale_get_arch_pointers(target, &armv4_5, &xscale)) != ERROR_OK)
3241         {
3242                 return retval;
3243         }
3244         u32 ret = armv4_5_mmu_translate_va(target, &xscale->armv4_5_mmu, virtual, &type, &cb, &domain, &ap);
3245         if (type == -1)
3246         {
3247                 return ret;
3248         }
3249         *physical = ret;
3250         return ERROR_OK;
3251 }
3252
3253 static int xscale_mmu(struct target_s *target, int *enabled)
3254 {
3255         armv4_5_common_t *armv4_5 = target->arch_info;
3256         xscale_common_t *xscale = armv4_5->arch_info;
3257
3258         if (target->state != TARGET_HALTED)
3259         {
3260                 LOG_ERROR("Target not halted");
3261                 return ERROR_TARGET_INVALID;
3262         }
3263         *enabled = xscale->armv4_5_mmu.mmu_enabled;
3264         return ERROR_OK;
3265 }
3266
3267 int xscale_handle_mmu_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
3268 {
3269         target_t *target = get_current_target(cmd_ctx);
3270         armv4_5_common_t *armv4_5;
3271         xscale_common_t *xscale;
3272
3273         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3274         {
3275                 return ERROR_OK;
3276         }
3277
3278         if (target->state != TARGET_HALTED)
3279         {
3280                 command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
3281                 return ERROR_OK;
3282         }
3283
3284         if (argc >= 1)
3285         {
3286                 if (strcmp("enable", args[0]) == 0)
3287                 {
3288                         xscale_enable_mmu_caches(target, 1, 0, 0);
3289                         xscale->armv4_5_mmu.mmu_enabled = 1;
3290                 }
3291                 else if (strcmp("disable", args[0]) == 0)
3292                 {
3293                         xscale_disable_mmu_caches(target, 1, 0, 0);
3294                         xscale->armv4_5_mmu.mmu_enabled = 0;
3295                 }
3296         }
3297
3298         command_print(cmd_ctx, "mmu %s", (xscale->armv4_5_mmu.mmu_enabled) ? "enabled" : "disabled");
3299
3300         return ERROR_OK;
3301 }
3302
3303 int xscale_handle_idcache_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
3304 {
3305         target_t *target = get_current_target(cmd_ctx);
3306         armv4_5_common_t *armv4_5;
3307         xscale_common_t *xscale;
3308         int icache = 0, dcache = 0;
3309
3310         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3311         {
3312                 return ERROR_OK;
3313         }
3314
3315         if (target->state != TARGET_HALTED)
3316         {
3317                 command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
3318                 return ERROR_OK;
3319         }
3320
3321         if (strcmp(cmd, "icache") == 0)
3322                 icache = 1;
3323         else if (strcmp(cmd, "dcache") == 0)
3324                 dcache = 1;
3325
3326         if (argc >= 1)
3327         {
3328                 if (strcmp("enable", args[0]) == 0)
3329                 {
3330                         xscale_enable_mmu_caches(target, 0, dcache, icache);
3331
3332                         if (icache)
3333                                 xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 1;
3334                         else if (dcache)
3335                                 xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 1;
3336                 }
3337                 else if (strcmp("disable", args[0]) == 0)
3338                 {
3339                         xscale_disable_mmu_caches(target, 0, dcache, icache);
3340
3341                         if (icache)
3342                                 xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
3343                         else if (dcache)
3344                                 xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
3345                 }
3346         }
3347
3348         if (icache)
3349                 command_print(cmd_ctx, "icache %s", (xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled) ? "enabled" : "disabled");
3350
3351         if (dcache)
3352                 command_print(cmd_ctx, "dcache %s", (xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled) ? "enabled" : "disabled");
3353
3354         return ERROR_OK;
3355 }
3356
3357 int xscale_handle_vector_catch_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
3358 {
3359         target_t *target = get_current_target(cmd_ctx);
3360         armv4_5_common_t *armv4_5;
3361         xscale_common_t *xscale;
3362
3363         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3364         {
3365                 return ERROR_OK;
3366         }
3367
3368         if (argc < 1)
3369         {
3370                 command_print(cmd_ctx, "usage: xscale vector_catch [mask]");
3371         }
3372         else
3373         {
3374                 xscale->vector_catch = strtoul(args[0], NULL, 0);
3375                 buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 8, xscale->vector_catch);
3376                 xscale_write_dcsr(target, -1, -1);
3377         }
3378
3379         command_print(cmd_ctx, "vector catch mask: 0x%2.2x", xscale->vector_catch);
3380
3381         return ERROR_OK;
3382 }
3383
3384
3385 int xscale_handle_trace_buffer_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
3386 {
3387         target_t *target = get_current_target(cmd_ctx);
3388         armv4_5_common_t *armv4_5;
3389         xscale_common_t *xscale;
3390         u32 dcsr_value;
3391
3392         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3393         {
3394                 return ERROR_OK;
3395         }
3396
3397         if (target->state != TARGET_HALTED)
3398         {
3399                 command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
3400                 return ERROR_OK;
3401         }
3402
3403         if ((argc >= 1) && (strcmp("enable", args[0]) == 0))
3404         {
3405                 xscale_trace_data_t *td, *next_td;
3406                 xscale->trace.buffer_enabled = 1;
3407
3408                 /* free old trace data */
3409                 td = xscale->trace.data;
3410                 while (td)
3411                 {
3412                         next_td = td->next;
3413
3414                         if (td->entries)
3415                                 free(td->entries);
3416                         free(td);
3417                         td = next_td;
3418                 }
3419                 xscale->trace.data = NULL;
3420         }
3421         else if ((argc >= 1) && (strcmp("disable", args[0]) == 0))
3422         {
3423                 xscale->trace.buffer_enabled = 0;
3424         }
3425
3426         if ((argc >= 2) && (strcmp("fill", args[1]) == 0))
3427         {
3428                 if (argc >= 3)
3429                         xscale->trace.buffer_fill = strtoul(args[2], NULL, 0);
3430                 else
3431                         xscale->trace.buffer_fill = 1;
3432         }
3433         else if ((argc >= 2) && (strcmp("wrap", args[1]) == 0))
3434         {
3435                 xscale->trace.buffer_fill = -1;
3436         }
3437
3438         if (xscale->trace.buffer_enabled)
3439         {
3440                 /* if we enable the trace buffer in fill-once
3441                  * mode we know the address of the first instruction */
3442                 xscale->trace.pc_ok = 1;
3443                 xscale->trace.current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
3444         }
3445         else
3446         {
3447                 /* otherwise the address is unknown, and we have no known good PC */
3448                 xscale->trace.pc_ok = 0;
3449         }
3450
3451         command_print(cmd_ctx, "trace buffer %s (%s)",
3452                 (xscale->trace.buffer_enabled) ? "enabled" : "disabled",
3453                 (xscale->trace.buffer_fill > 0) ? "fill" : "wrap");
3454
3455         dcsr_value = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 0, 32);
3456         if (xscale->trace.buffer_fill >= 0)
3457                 xscale_write_dcsr_sw(target, (dcsr_value & 0xfffffffc) | 2);
3458         else
3459                 xscale_write_dcsr_sw(target, dcsr_value & 0xfffffffc);
3460
3461         return ERROR_OK;
3462 }
3463
3464 int xscale_handle_trace_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
3465 {
3466         target_t *target;
3467         armv4_5_common_t *armv4_5;
3468         xscale_common_t *xscale;
3469
3470         if (argc < 1)
3471         {
3472                 command_print(cmd_ctx, "usage: xscale trace_image <file> [base address] [type]");
3473                 return ERROR_OK;
3474         }
3475
3476         target = get_current_target(cmd_ctx);
3477
3478         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3479         {
3480                 return ERROR_OK;
3481         }
3482
3483         if (xscale->trace.image)
3484         {
3485                 image_close(xscale->trace.image);
3486                 free(xscale->trace.image);
3487                 command_print(cmd_ctx, "previously loaded image found and closed");
3488         }
3489
3490         xscale->trace.image = malloc(sizeof(image_t));
3491         xscale->trace.image->base_address_set = 0;
3492         xscale->trace.image->start_address_set = 0;
3493
3494         /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
3495         if (argc >= 2)
3496         {
3497                 xscale->trace.image->base_address_set = 1;
3498                 xscale->trace.image->base_address = strtoul(args[1], NULL, 0);
3499         }
3500         else
3501         {
3502                 xscale->trace.image->base_address_set = 0;
3503         }
3504
3505         if (image_open(xscale->trace.image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)
3506         {
3507                 free(xscale->trace.image);
3508                 xscale->trace.image = NULL;
3509                 return ERROR_OK;
3510         }
3511
3512         return ERROR_OK;
3513 }
3514
3515 int xscale_handle_dump_trace_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
3516 {
3517         target_t *target = get_current_target(cmd_ctx);
3518         armv4_5_common_t *armv4_5;
3519         xscale_common_t *xscale;
3520         xscale_trace_data_t *trace_data;
3521         fileio_t file;
3522
3523         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3524         {
3525                 return ERROR_OK;
3526         }
3527
3528         if (target->state != TARGET_HALTED)
3529         {
3530                 command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
3531                 return ERROR_OK;
3532         }
3533
3534         if (argc < 1)
3535         {
3536                 command_print(cmd_ctx, "usage: xscale dump_trace <file>");
3537                 return ERROR_OK;
3538         }
3539
3540         trace_data = xscale->trace.data;
3541
3542         if (!trace_data)
3543         {
3544                 command_print(cmd_ctx, "no trace data collected");
3545                 return ERROR_OK;
3546         }
3547
3548         if (fileio_open(&file, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
3549         {
3550                 return ERROR_OK;
3551         }
3552
3553         while (trace_data)
3554         {
3555                 int i;
3556
3557                 fileio_write_u32(&file, trace_data->chkpt0);
3558                 fileio_write_u32(&file, trace_data->chkpt1);
3559                 fileio_write_u32(&file, trace_data->last_instruction);
3560                 fileio_write_u32(&file, trace_data->depth);
3561
3562                 for (i = 0; i < trace_data->depth; i++)
3563                         fileio_write_u32(&file, trace_data->entries[i].data | ((trace_data->entries[i].type & 0xffff) << 16));
3564
3565                 trace_data = trace_data->next;
3566         }
3567
3568         fileio_close(&file);
3569
3570         return ERROR_OK;
3571 }
3572
3573 int xscale_handle_analyze_trace_buffer_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
3574 {
3575         target_t *target = get_current_target(cmd_ctx);
3576         armv4_5_common_t *armv4_5;
3577         xscale_common_t *xscale;
3578
3579         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3580         {
3581                 return ERROR_OK;
3582         }
3583
3584         xscale_analyze_trace(target, cmd_ctx);
3585
3586         return ERROR_OK;
3587 }
3588
3589 int xscale_handle_cp15(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
3590 {
3591         target_t *target = get_current_target(cmd_ctx);
3592         armv4_5_common_t *armv4_5;
3593         xscale_common_t *xscale;
3594
3595         if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
3596         {
3597                 return ERROR_OK;
3598         }
3599
3600         if (target->state != TARGET_HALTED)
3601         {
3602                 command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
3603                 return ERROR_OK;
3604         }
3605         u32 reg_no = 0;
3606         reg_t *reg = NULL;
3607         if(argc > 0)
3608         {
3609                 reg_no = strtoul(args[0], NULL, 0);
3610                 /*translate from xscale cp15 register no to openocd register*/
3611                 switch(reg_no)
3612                 {
3613                 case 0:
3614                         reg_no = XSCALE_MAINID;
3615                         break;
3616                 case 1:
3617                         reg_no = XSCALE_CTRL;
3618                         break;
3619                 case 2:
3620                         reg_no = XSCALE_TTB;
3621                         break;
3622                 case 3:
3623                         reg_no = XSCALE_DAC;
3624                         break;
3625                 case 5:
3626                         reg_no = XSCALE_FSR;
3627                         break;
3628                 case 6:
3629                         reg_no = XSCALE_FAR;
3630                         break;
3631                 case 13:
3632                         reg_no = XSCALE_PID;
3633                         break;
3634                 case 15:
3635                         reg_no = XSCALE_CPACCESS;
3636                         break;
3637                 default:
3638                         command_print(cmd_ctx, "invalid register number");
3639                         return ERROR_INVALID_ARGUMENTS;
3640                 }
3641                 reg = &xscale->reg_cache->reg_list[reg_no];
3642
3643         }
3644         if(argc == 1)
3645         {
3646                 u32 value;
3647
3648                 /* read cp15 control register */
3649                 xscale_get_reg(reg);
3650                 value = buf_get_u32(reg->value, 0, 32);
3651                 command_print(cmd_ctx, "%s (/%i): 0x%x", reg->name, reg->size, value);
3652         }
3653         else if(argc == 2)
3654         {
3655
3656                 u32 value = strtoul(args[1], NULL, 0);
3657
3658                 /* send CP write request (command 0x41) */
3659                 xscale_send_u32(target, 0x41);
3660
3661                 /* send CP register number */
3662                 xscale_send_u32(target, reg_no);
3663
3664                 /* send CP register value */
3665                 xscale_send_u32(target, value);
3666
3667                 /* execute cpwait to ensure outstanding operations complete */
3668                 xscale_send_u32(target, 0x53);
3669         }
3670         else
3671         {
3672                 command_print(cmd_ctx, "usage: cp15 [register]<, [value]>");
3673         }
3674
3675         return ERROR_OK;
3676 }
3677
3678 int xscale_register_commands(struct command_context_s *cmd_ctx)
3679 {
3680         command_t *xscale_cmd;
3681
3682         xscale_cmd = register_command(cmd_ctx, NULL, "xscale", NULL, COMMAND_ANY, "xscale specific commands");
3683
3684         register_command(cmd_ctx, xscale_cmd, "debug_handler", xscale_handle_debug_handler_command, COMMAND_ANY, "'xscale debug_handler <target#> <address>' command takes two required operands");
3685         register_command(cmd_ctx, xscale_cmd, "cache_clean_address", xscale_handle_cache_clean_address_command, COMMAND_ANY, NULL);
3686
3687         register_command(cmd_ctx, xscale_cmd, "cache_info", xscale_handle_cache_info_command, COMMAND_EXEC, NULL);
3688         register_command(cmd_ctx, xscale_cmd, "mmu", xscale_handle_mmu_command, COMMAND_EXEC, "['enable'|'disable'] the MMU");
3689         register_command(cmd_ctx, xscale_cmd, "icache", xscale_handle_idcache_command, COMMAND_EXEC, "['enable'|'disable'] the ICache");
3690         register_command(cmd_ctx, xscale_cmd, "dcache", xscale_handle_idcache_command, COMMAND_EXEC, "['enable'|'disable'] the DCache");
3691
3692         register_command(cmd_ctx, xscale_cmd, "vector_catch", xscale_handle_vector_catch_command, COMMAND_EXEC, "<mask> of vectors that should be catched");
3693
3694         register_command(cmd_ctx, xscale_cmd, "trace_buffer", xscale_handle_trace_buffer_command, COMMAND_EXEC, "<enable|disable> ['fill' [n]|'wrap']");
3695
3696         register_command(cmd_ctx, xscale_cmd, "dump_trace", xscale_handle_dump_trace_command, COMMAND_EXEC, "dump content of trace buffer to <file>");
3697         register_command(cmd_ctx, xscale_cmd, "analyze_trace", xscale_handle_analyze_trace_buffer_command, COMMAND_EXEC, "analyze content of trace buffer");
3698         register_command(cmd_ctx, xscale_cmd, "trace_image", xscale_handle_trace_image_command,
3699                 COMMAND_EXEC, "load image from <file> [base address]");
3700
3701         register_command(cmd_ctx, xscale_cmd, "cp15", xscale_handle_cp15, COMMAND_EXEC, "access coproc 15 <register> [value]");
3702
3703         armv4_5_register_commands(cmd_ctx);
3704
3705         return ERROR_OK;
3706 }