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

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