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