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

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program; if not, write to the                         *
  17  *   Free Software Foundation, Inc.,                                       *
  18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  19  ***************************************************************************/
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include "replacements.h"
  25
  26 #include "embeddedice.h"
  27 #include "target.h"
  28 #include "target_request.h"
  29 #include "armv4_5.h"
  30 #include "arm_jtag.h"
  31 #include "jtag.h"
  32 #include "log.h"
  33 #include "arm7_9_common.h"
  34 #include "breakpoints.h"
  35
  36 #include <stdlib.h>
  37 #include <string.h>
  38 #include <unistd.h>
  39
  40 #include <sys/types.h>
  41 #include <sys/stat.h>
  42 #include <sys/time.h>
  43 #include <errno.h>
  44
  45 int arm7_9_debug_entry(target_t *target);
  46 int arm7_9_enable_sw_bkpts(struct target_s *target);
  47
  48 /* command handler forward declarations */
  49 int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  50 int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  51 int handle_arm7_9_read_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  52 int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  53 int handle_arm7_9_sw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  54 int handle_arm7_9_force_hw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  55 int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  56 int handle_arm7_9_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  57 int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  58 int handle_arm7_9_etm_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  59
  60 int arm7_9_reinit_embeddedice(target_t *target)
  61 {
  62         armv4_5_common_t *armv4_5 = target->arch_info;
  63         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
  64
  65         breakpoint_t *breakpoint = target->breakpoints;
  66
  67         arm7_9->wp_available = 2;
  68         arm7_9->wp0_used = 0;
  69         arm7_9->wp1_used = 0;
  70
  71         /* mark all hardware breakpoints as unset */
  72         while (breakpoint)
  73         {
  74                 if (breakpoint->type == BKPT_HARD)
  75                 {
  76                         breakpoint->set = 0;
  77                 }
  78                 breakpoint = breakpoint->next;
  79         }
  80
  81         if (arm7_9->sw_bkpts_enabled && arm7_9->sw_bkpts_use_wp)
  82         {
  83                 arm7_9->sw_bkpts_enabled = 0;
  84                 arm7_9_enable_sw_bkpts(target);
  85         }
  86
  87         arm7_9->reinit_embeddedice = 0;
  88
  89         return ERROR_OK;
  90 }
  91
  92 int arm7_9_jtag_callback(enum jtag_event event, void *priv)
  93 {
  94         target_t *target = priv;
  95         armv4_5_common_t *armv4_5 = target->arch_info;
  96         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
  97
  98         /* a test-logic reset occured
  99          * the EmbeddedICE registers have been reset
 100          * hardware breakpoints have been cleared
 101          */
 102         if (event == JTAG_TRST_ASSERTED)
 103         {
 104                 arm7_9->reinit_embeddedice = 1;
 105         }
 106
 107         return ERROR_OK;
 108 }
 109
 110 int arm7_9_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p)
 111 {
 112         armv4_5_common_t *armv4_5 = target->arch_info;
 113         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 114
 115         if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
 116         {
 117                 return -1;
 118         }
 119
 120         if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
 121         {
 122                 return -1;
 123         }
 124
 125         *armv4_5_p = armv4_5;
 126         *arm7_9_p = arm7_9;
 127
 128         return ERROR_OK;
 129 }
 130
 131 int arm7_9_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 132 {
 133         armv4_5_common_t *armv4_5 = target->arch_info;
 134         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 135
 136         if (target->state != TARGET_HALTED)
 137         {
 138                 WARNING("target not halted");
 139                 return ERROR_TARGET_NOT_HALTED;
 140         }
 141
 142         if (arm7_9->force_hw_bkpts)
 143                 breakpoint->type = BKPT_HARD;
 144
 145         if (breakpoint->set)
 146         {
 147                 WARNING("breakpoint already set");
 148                 return ERROR_OK;
 149         }
 150
 151         if (breakpoint->type == BKPT_HARD)
 152         {
 153                 /* either an ARM (4 byte) or Thumb (2 byte) breakpoint */
 154                 u32 mask = (breakpoint->length == 4) ? 0x3u : 0x1u;
 155                 if (!arm7_9->wp0_used)
 156                 {
 157                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], breakpoint->address);
 158                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
 159                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffffu);
 160                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 161                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 162
 163                         jtag_execute_queue();
 164                         arm7_9->wp0_used = 1;
 165                         breakpoint->set = 1;
 166                 }
 167                 else if (!arm7_9->wp1_used)
 168                 {
 169                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], breakpoint->address);
 170                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
 171                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffffu);
 172                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 173                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 174
 175                         jtag_execute_queue();
 176                         arm7_9->wp1_used = 1;
 177                         breakpoint->set = 2;
 178                 }
 179                 else
 180                 {
 181                         ERROR("BUG: no hardware comparator available");
 182                         return ERROR_OK;
 183                 }
 184         }
 185         else if (breakpoint->type == BKPT_SOFT)
 186         {
 187                 if (breakpoint->length == 4)
 188                 {
 189                         /* keep the original instruction in target endianness */
 190                         target->type->read_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
 191                         /* write the breakpoint instruction in target endianness (arm7_9->arm_bkpt is host endian) */
 192                         target_write_u32(target, breakpoint->address, arm7_9->arm_bkpt);
 193                 }
 194                 else
 195                 {
 196                         /* keep the original instruction in target endianness */
 197                         target->type->read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
 198                         /* write the breakpoint instruction in target endianness (arm7_9->thumb_bkpt is host endian) */
 199                         target_write_u16(target, breakpoint->address, arm7_9->thumb_bkpt);
 200                 }
 201                 breakpoint->set = 1;
 202         }
 203
 204         return ERROR_OK;
 205
 206 }
 207
 208 int arm7_9_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 209 {
 210         armv4_5_common_t *armv4_5 = target->arch_info;
 211         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 212
 213         if (target->state != TARGET_HALTED)
 214         {
 215                 WARNING("target not halted");
 216                 return ERROR_TARGET_NOT_HALTED;
 217         }
 218
 219         if (!breakpoint->set)
 220         {
 221                 WARNING("breakpoint not set");
 222                 return ERROR_OK;
 223         }
 224
 225         if (breakpoint->type == BKPT_HARD)
 226         {
 227                 if (breakpoint->set == 1)
 228                 {
 229                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
 230                         jtag_execute_queue();
 231                         arm7_9->wp0_used = 0;
 232                 }
 233                 else if (breakpoint->set == 2)
 234                 {
 235                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
 236                         jtag_execute_queue();
 237                         arm7_9->wp1_used = 0;
 238                 }
 239                 breakpoint->set = 0;
 240         }
 241         else
 242         {
 243                 /* restore original instruction (kept in target endianness) */
 244                 if (breakpoint->length == 4)
 245                 {
 246                         u32 current_instr;
 247                         /* check that user program as not modified breakpoint instruction */
 248                         target->type->read_memory(target, breakpoint->address, 4, 1, (u8*)&current_instr);
 249                         if (current_instr==arm7_9->arm_bkpt)
 250                                 target->type->write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
 251                 }
 252                 else
 253                 {
 254                         u16 current_instr;
 255                         /* check that user program as not modified breakpoint instruction */
 256                         target->type->read_memory(target, breakpoint->address, 2, 1, (u8*)&current_instr);
 257                         if (current_instr==arm7_9->thumb_bkpt)
 258                                 target->type->write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
 259                 }
 260                 breakpoint->set = 0;
 261         }
 262
 263         return ERROR_OK;
 264 }
 265
 266 int arm7_9_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 267 {
 268         armv4_5_common_t *armv4_5 = target->arch_info;
 269         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 270
 271         if (target->state != TARGET_HALTED)
 272         {
 273                 WARNING("target not halted");
 274                 return ERROR_TARGET_NOT_HALTED;
 275         }
 276
 277         if (arm7_9->force_hw_bkpts)
 278         {
 279                 DEBUG("forcing use of hardware breakpoint at address 0x%8.8x", breakpoint->address);
 280                 breakpoint->type = BKPT_HARD;
 281         }
 282
 283         if ((breakpoint->type == BKPT_SOFT) && (arm7_9->sw_bkpts_enabled == 0))
 284         {
 285                 INFO("sw breakpoint requested, but software breakpoints not enabled");
 286                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 287         }
 288
 289         if ((breakpoint->type == BKPT_HARD) && (arm7_9->wp_available < 1))
 290         {
 291                 INFO("no watchpoint unit available for hardware breakpoint");
 292                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 293         }
 294
 295         if ((breakpoint->length != 2) && (breakpoint->length != 4))
 296         {
 297                 INFO("only breakpoints of two (Thumb) or four (ARM) bytes length supported");
 298                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 299         }
 300
 301         if (breakpoint->type == BKPT_HARD)
 302                 arm7_9->wp_available--;
 303
 304         return ERROR_OK;
 305 }
 306
 307 int arm7_9_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
 308 {
 309         armv4_5_common_t *armv4_5 = target->arch_info;
 310         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 311
 312         if (target->state != TARGET_HALTED)
 313         {
 314                 WARNING("target not halted");
 315                 return ERROR_TARGET_NOT_HALTED;
 316         }
 317
 318         if (breakpoint->set)
 319         {
 320                 arm7_9_unset_breakpoint(target, breakpoint);
 321         }
 322
 323         if (breakpoint->type == BKPT_HARD)
 324                 arm7_9->wp_available++;
 325
 326         return ERROR_OK;
 327 }
 328
 329 int arm7_9_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 330 {
 331         armv4_5_common_t *armv4_5 = target->arch_info;
 332         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 333         int rw_mask = 1;
 334         u32 mask;
 335
 336         mask = watchpoint->length - 1;
 337
 338         if (target->state != TARGET_HALTED)
 339         {
 340                 WARNING("target not halted");
 341                 return ERROR_TARGET_NOT_HALTED;
 342         }
 343
 344         if (watchpoint->rw == WPT_ACCESS)
 345                 rw_mask = 0;
 346         else
 347                 rw_mask = 1;
 348
 349         if (!arm7_9->wp0_used)
 350         {
 351                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], watchpoint->address);
 352                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], mask);
 353                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], watchpoint->mask);
 354                 if( watchpoint->mask != 0xffffffffu )
 355                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE], watchpoint->value);
 356                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xff & ~EICE_W_CTRL_nOPC & ~rw_mask);
 357                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE | EICE_W_CTRL_nOPC | (watchpoint->rw & 1));
 358
 359                 jtag_execute_queue();
 360                 watchpoint->set = 1;
 361                 arm7_9->wp0_used = 2;
 362         }
 363         else if (!arm7_9->wp1_used)
 364         {
 365                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], watchpoint->address);
 366                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], mask);
 367                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], watchpoint->mask);
 368                 if( watchpoint->mask != 0xffffffffu )
 369                         embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE], watchpoint->value);
 370                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], 0xff & ~EICE_W_CTRL_nOPC & ~rw_mask);
 371                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE | EICE_W_CTRL_nOPC | (watchpoint->rw & 1));
 372
 373                 jtag_execute_queue();
 374                 watchpoint->set = 2;
 375                 arm7_9->wp1_used = 2;
 376         }
 377         else
 378         {
 379                 ERROR("BUG: no hardware comparator available");
 380                 return ERROR_OK;
 381         }
 382
 383         return ERROR_OK;
 384 }
 385
 386 int arm7_9_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 387 {
 388         armv4_5_common_t *armv4_5 = target->arch_info;
 389         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 390
 391         if (target->state != TARGET_HALTED)
 392         {
 393                 WARNING("target not halted");
 394                 return ERROR_TARGET_NOT_HALTED;
 395         }
 396
 397         if (!watchpoint->set)
 398         {
 399                 WARNING("breakpoint not set");
 400                 return ERROR_OK;
 401         }
 402
 403         if (watchpoint->set == 1)
 404         {
 405                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
 406                 jtag_execute_queue();
 407                 arm7_9->wp0_used = 0;
 408         }
 409         else if (watchpoint->set == 2)
 410         {
 411                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
 412                 jtag_execute_queue();
 413                 arm7_9->wp1_used = 0;
 414         }
 415         watchpoint->set = 0;
 416
 417         return ERROR_OK;
 418 }
 419
 420 int arm7_9_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 421 {
 422         armv4_5_common_t *armv4_5 = target->arch_info;
 423         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 424
 425         if (target->state != TARGET_HALTED)
 426         {
 427                 WARNING("target not halted");
 428                 return ERROR_TARGET_NOT_HALTED;
 429         }
 430
 431         if (arm7_9->wp_available < 1)
 432         {
 433                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 434         }
 435
 436         if ((watchpoint->length != 1) && (watchpoint->length != 2) && (watchpoint->length != 4))
 437         {
 438                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 439         }
 440
 441         arm7_9->wp_available--;
 442
 443         return ERROR_OK;
 444 }
 445
 446 int arm7_9_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
 447 {
 448         armv4_5_common_t *armv4_5 = target->arch_info;
 449         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 450
 451         if (target->state != TARGET_HALTED)
 452         {
 453                 WARNING("target not halted");
 454                 return ERROR_TARGET_NOT_HALTED;
 455         }
 456
 457         if (watchpoint->set)
 458         {
 459                 arm7_9_unset_watchpoint(target, watchpoint);
 460         }
 461
 462         arm7_9->wp_available++;
 463
 464         return ERROR_OK;
 465 }
 466
 467 int arm7_9_enable_sw_bkpts(struct target_s *target)
 468 {
 469         armv4_5_common_t *armv4_5 = target->arch_info;
 470         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 471         int retval;
 472
 473         if (arm7_9->sw_bkpts_enabled)
 474                 return ERROR_OK;
 475
 476         if (arm7_9->wp_available < 1)
 477         {
 478                 WARNING("can't enable sw breakpoints with no watchpoint unit available");
 479                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 480         }
 481         arm7_9->wp_available--;
 482
 483         if (!arm7_9->wp0_used)
 484         {
 485                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_VALUE], arm7_9->arm_bkpt);
 486                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0x0);
 487                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffffu);
 488                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 489                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 490                 arm7_9->sw_bkpts_enabled = 1;
 491                 arm7_9->wp0_used = 3;
 492         }
 493         else if (!arm7_9->wp1_used)
 494         {
 495                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_VALUE], arm7_9->arm_bkpt);
 496                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0x0);
 497                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0xffffffffu);
 498                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 499                 embeddedice_set_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 500                 arm7_9->sw_bkpts_enabled = 2;
 501                 arm7_9->wp1_used = 3;
 502         }
 503         else
 504         {
 505                 ERROR("BUG: both watchpoints used, but wp_available >= 1");
 506                 exit(-1);
 507         }
 508
 509         if ((retval = jtag_execute_queue()) != ERROR_OK)
 510         {
 511                 ERROR("error writing EmbeddedICE registers to enable sw breakpoints");
 512                 exit(-1);
 513         };
 514
 515         return ERROR_OK;
 516 }
 517
 518 int arm7_9_disable_sw_bkpts(struct target_s *target)
 519 {
 520         armv4_5_common_t *armv4_5 = target->arch_info;
 521         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 522
 523         if (!arm7_9->sw_bkpts_enabled)
 524                 return ERROR_OK;
 525
 526         if (arm7_9->sw_bkpts_enabled == 1)
 527         {
 528                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
 529                 arm7_9->sw_bkpts_enabled = 0;
 530                 arm7_9->wp0_used = 0;
 531                 arm7_9->wp_available++;
 532         }
 533         else if (arm7_9->sw_bkpts_enabled == 2)
 534         {
 535                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
 536                 arm7_9->sw_bkpts_enabled = 0;
 537                 arm7_9->wp1_used = 0;
 538                 arm7_9->wp_available++;
 539         }
 540
 541         return ERROR_OK;
 542 }
 543
 544 int arm7_9_execute_sys_speed(struct target_s *target)
 545 {
 546         int timeout;
 547         int retval;
 548
 549         armv4_5_common_t *armv4_5 = target->arch_info;
 550         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 551         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 552         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 553
 554         /* set RESTART instruction */
 555         jtag_add_end_state(TAP_RTI);
 556         arm_jtag_set_instr(jtag_info, 0x4, NULL);
 557
 558         for (timeout=0; timeout<50; timeout++)
 559         {
 560                 /* read debug status register */
 561                 embeddedice_read_reg(dbg_stat);
 562                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 563                         return retval;
 564                 if ((buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
 565                                    && (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_SYSCOMP, 1)))
 566                         break;
 567                 usleep(100000);
 568         }
 569         if (timeout == 50)
 570         {
 571                 ERROR("timeout waiting for SYSCOMP & DBGACK, last DBG_STATUS: %x", buf_get_u32(dbg_stat->value, 0, dbg_stat->size));
 572                 return ERROR_TARGET_TIMEOUT;
 573         }
 574
 575         return ERROR_OK;
 576 }
 577
 578 int arm7_9_execute_fast_sys_speed(struct target_s *target)
 579 {
 580         u8 check_value[4], check_mask[4];
 581
 582         armv4_5_common_t *armv4_5 = target->arch_info;
 583         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 584         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 585         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 586
 587         /* set RESTART instruction */
 588         jtag_add_end_state(TAP_RTI);
 589         arm_jtag_set_instr(jtag_info, 0x4, NULL);
 590
 591         /* check for DBGACK and SYSCOMP set (others don't care) */
 592         buf_set_u32(check_value, 0, 32, 0x9);
 593         buf_set_u32(check_mask, 0, 32, 0x9);
 594
 595         /* read debug status register */
 596         embeddedice_read_reg_w_check(dbg_stat, check_value, check_value);
 597
 598         return ERROR_OK;
 599 }
 600
 601 int arm7_9_target_request_data(target_t *target, u32 size, u8 *buffer)
 602 {
 603         armv4_5_common_t *armv4_5 = target->arch_info;
 604         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 605         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 606         u32 *data;
 607         int i;
 608
 609         data = malloc(size * (sizeof(u32)));
 610
 611         embeddedice_receive(jtag_info, data, size);
 612
 613         for (i = 0; i < size; i++)
 614         {
 615                 h_u32_to_le(buffer + (i * 4), data[i]);
 616         }
 617
 618         free(data);
 619
 620         return ERROR_OK;
 621 }
 622
 623 int arm7_9_handle_target_request(void *priv)
 624 {
 625         target_t *target = priv;
 626         armv4_5_common_t *armv4_5 = target->arch_info;
 627         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 628         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 629         reg_t *dcc_control = &arm7_9->eice_cache->reg_list[EICE_COMMS_CTRL];
 630
 631         if (target->state == TARGET_RUNNING)
 632         {
 633                 /* read DCC control register */
 634                 embeddedice_read_reg(dcc_control);
 635                 jtag_execute_queue();
 636
 637                 /* check W bit */
 638                 if (buf_get_u32(dcc_control->value, 1, 1) == 1)
 639                 {
 640                         u32 request;
 641
 642                         embeddedice_receive(jtag_info, &request, 1);
 643                         target_request(target, request);
 644                 }
 645         }
 646
 647         return ERROR_OK;
 648 }
 649
 650 enum target_state arm7_9_poll(target_t *target)
 651 {
 652         int retval;
 653         armv4_5_common_t *armv4_5 = target->arch_info;
 654         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 655         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 656
 657         if (arm7_9->reinit_embeddedice)
 658         {
 659                 arm7_9_reinit_embeddedice(target);
 660         }
 661
 662         /* read debug status register */
 663         embeddedice_read_reg(dbg_stat);
 664         if ((retval = jtag_execute_queue()) != ERROR_OK)
 665         {
 666                 switch (retval)
 667                 {
 668                         case ERROR_JTAG_QUEUE_FAILED:
 669                                 ERROR("JTAG queue failed while reading EmbeddedICE status register");
 670                                 exit(-1);
 671                                 break;
 672                         default:
 673                                 break;
 674                 }
 675         }
 676
 677         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
 678         {
 679                 DEBUG("DBGACK set, dbg_state->value: 0x%x", buf_get_u32(dbg_stat->value, 0, 32));
 680                 if ((target->state == TARGET_UNKNOWN))
 681                 {
 682                         WARNING("DBGACK set while target was in unknown state. Reset or initialize target before resuming");
 683                         target->state = TARGET_RUNNING;
 684                 }
 685                 if ((target->state == TARGET_RUNNING) || (target->state == TARGET_RESET))
 686                 {
 687                         target->state = TARGET_HALTED;
 688                         if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
 689                                 return retval;
 690
 691                         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 692                 }
 693                 if (target->state == TARGET_DEBUG_RUNNING)
 694                 {
 695                         target->state = TARGET_HALTED;
 696                         if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
 697                                 return retval;
 698
 699                         target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
 700                 }
 701         }
 702         else
 703         {
 704                 if (target->state != TARGET_DEBUG_RUNNING)
 705                         target->state = TARGET_RUNNING;
 706         }
 707
 708         return target->state;
 709 }
 710
 711 int arm7_9_assert_reset(target_t *target)
 712 {
 713         int retval;
 714
 715         DEBUG("target->state: %s", target_state_strings[target->state]);
 716
 717         if (target->state == TARGET_HALTED || target->state == TARGET_UNKNOWN)
 718         {
 719                 /* if the target wasn't running, there might be working areas allocated */
 720                 target_free_all_working_areas(target);
 721
 722                 /* assert SRST and TRST */
 723                 /* system would get ouf sync if we didn't reset test-logic, too */
 724                 if ((retval = jtag_add_reset(1, 1)) != ERROR_OK)
 725                 {
 726                         if (retval == ERROR_JTAG_RESET_CANT_SRST)
 727                         {
 728                                 WARNING("can't assert srst");
 729                                 return retval;
 730                         }
 731                         else
 732                         {
 733                                 ERROR("unknown error");
 734                                 exit(-1);
 735                         }
 736                 }
 737                 jtag_add_sleep(5000);
 738                 if ((retval = jtag_add_reset(0, 1)) != ERROR_OK)
 739                 {
 740                         if (retval == ERROR_JTAG_RESET_WOULD_ASSERT_TRST)
 741                         {
 742                                 WARNING("srst resets test logic, too");
 743                                 retval = jtag_add_reset(1, 1);
 744                         }
 745                 }
 746         }
 747         else
 748         {
 749                 if ((retval = jtag_add_reset(0, 1)) != ERROR_OK)
 750                 {
 751                         if (retval == ERROR_JTAG_RESET_WOULD_ASSERT_TRST)
 752                         {
 753                                 WARNING("srst resets test logic, too");
 754                                 retval = jtag_add_reset(1, 1);
 755                         }
 756
 757                         if (retval == ERROR_JTAG_RESET_CANT_SRST)
 758                         {
 759                                 WARNING("can't assert srst");
 760                                 return retval;
 761                         }
 762                         else if (retval != ERROR_OK)
 763                         {
 764                                 ERROR("unknown error");
 765                                 exit(-1);
 766                         }
 767                 }
 768         }
 769
 770         target->state = TARGET_RESET;
 771         jtag_add_sleep(50000);
 772
 773         armv4_5_invalidate_core_regs(target);
 774
 775         return ERROR_OK;
 776
 777 }
 778
 779 int arm7_9_deassert_reset(target_t *target)
 780 {
 781         DEBUG("target->state: %s", target_state_strings[target->state]);
 782
 783         /* deassert reset lines */
 784         jtag_add_reset(0, 0);
 785
 786         return ERROR_OK;
 787 }
 788
 789 int arm7_9_clear_halt(target_t *target)
 790 {
 791         armv4_5_common_t *armv4_5 = target->arch_info;
 792         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 793         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 794
 795         /* we used DBGRQ only if we didn't come out of reset */
 796         if (!arm7_9->debug_entry_from_reset && arm7_9->use_dbgrq)
 797         {
 798                 /* program EmbeddedICE Debug Control Register to deassert DBGRQ
 799                  */
 800                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
 801                 embeddedice_store_reg(dbg_ctrl);
 802         }
 803         else
 804         {
 805                 if (arm7_9->debug_entry_from_reset && arm7_9->has_vector_catch)
 806                 {
 807                         /* if we came out of reset, and vector catch is supported, we used
 808                          * vector catch to enter debug state
 809                          * restore the register in that case
 810                          */
 811                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH]);
 812                 }
 813                 else
 814                 {
 815                         /* restore registers if watchpoint unit 0 was in use
 816                          */
 817                         if (arm7_9->wp0_used)
 818                         {
 819                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
 820                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
 821                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
 822                         }
 823                         /* control value always has to be restored, as it was either disabled,
 824                          * or enabled with possibly different bits
 825                          */
 826                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
 827                 }
 828         }
 829
 830         return ERROR_OK;
 831 }
 832
 833 int arm7_9_soft_reset_halt(struct target_s *target)
 834 {
 835         armv4_5_common_t *armv4_5 = target->arch_info;
 836         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 837         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 838         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 839         int i;
 840
 841         if (target->state == TARGET_RUNNING)
 842         {
 843                 target->type->halt(target);
 844         }
 845
 846         while (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)
 847         {
 848                 embeddedice_read_reg(dbg_stat);
 849                 jtag_execute_queue();
 850         }
 851         target->state = TARGET_HALTED;
 852
 853         /* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
 854          * ensure that DBGRQ is cleared
 855          */
 856         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
 857         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
 858         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
 859         embeddedice_store_reg(dbg_ctrl);
 860
 861         arm7_9_clear_halt(target);
 862
 863         /* if the target is in Thumb state, change to ARM state */
 864         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
 865         {
 866                 u32 r0_thumb, pc_thumb;
 867                 DEBUG("target entered debug from Thumb state, changing to ARM");
 868                 /* Entered debug from Thumb mode */
 869                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
 870                 arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
 871         }
 872
 873         /* all register content is now invalid */
 874         armv4_5_invalidate_core_regs(target);
 875
 876         /* SVC, ARM state, IRQ and FIQ disabled */
 877         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
 878         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
 879         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
 880
 881         /* start fetching from 0x0 */
 882         buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
 883         armv4_5->core_cache->reg_list[15].dirty = 1;
 884         armv4_5->core_cache->reg_list[15].valid = 1;
 885
 886         armv4_5->core_mode = ARMV4_5_MODE_SVC;
 887         armv4_5->core_state = ARMV4_5_STATE_ARM;
 888
 889         /* reset registers */
 890         for (i = 0; i <= 14; i++)
 891         {
 892                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, 0xffffffff);
 893                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
 894                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
 895         }
 896
 897         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 898
 899         return ERROR_OK;
 900 }
 901
 902 int arm7_9_prepare_reset_halt(target_t *target)
 903 {
 904         armv4_5_common_t *armv4_5 = target->arch_info;
 905         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 906
 907         /* poll the target, and resume if it was currently halted */
 908         arm7_9_poll(target);
 909         if (target->state == TARGET_HALTED)
 910         {
 911                 arm7_9_resume(target, 1, 0x0, 0, 1);
 912         }
 913
 914         if (arm7_9->has_vector_catch)
 915         {
 916                 /* program vector catch register to catch reset vector */
 917                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH], 0x1);
 918         }
 919         else
 920         {
 921                 /* program watchpoint unit to match on reset vector address */
 922                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0x3);
 923                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0x0);
 924                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x100);
 925                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xf7);
 926         }
 927
 928         return ERROR_OK;
 929 }
 930
 931 int arm7_9_halt(target_t *target)
 932 {
 933         armv4_5_common_t *armv4_5 = target->arch_info;
 934         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 935         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 936
 937         DEBUG("target->state: %s", target_state_strings[target->state]);
 938
 939         if (target->state == TARGET_HALTED)
 940         {
 941                 WARNING("target was already halted");
 942                 return ERROR_TARGET_ALREADY_HALTED;
 943         }
 944
 945         if (target->state == TARGET_UNKNOWN)
 946         {
 947                 WARNING("target was in unknown state when halt was requested");
 948         }
 949
 950         if (target->state == TARGET_RESET)
 951         {
 952                 if ((jtag_reset_config & RESET_SRST_PULLS_TRST) && jtag_srst)
 953                 {
 954                         ERROR("can't request a halt while in reset if nSRST pulls nTRST");
 955                         return ERROR_TARGET_FAILURE;
 956                 }
 957                 else
 958                 {
 959                         /* we came here in a reset_halt or reset_init sequence
 960                          * debug entry was already prepared in arm7_9_prepare_reset_halt()
 961                          */
 962                         target->debug_reason = DBG_REASON_DBGRQ;
 963
 964                         return ERROR_OK;
 965                 }
 966         }
 967
 968         if (arm7_9->use_dbgrq)
 969         {
 970                 /* program EmbeddedICE Debug Control Register to assert DBGRQ
 971                  */
 972                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 1);
 973                 embeddedice_store_reg(dbg_ctrl);
 974         }
 975         else
 976         {
 977                 /* program watchpoint unit to match on any address
 978                  */
 979                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
 980                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
 981                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x100);
 982                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xf7);
 983         }
 984
 985         target->debug_reason = DBG_REASON_DBGRQ;
 986
 987         return ERROR_OK;
 988 }
 989
 990 int arm7_9_debug_entry(target_t *target)
 991 {
 992         int i;
 993         u32 context[16];
 994         u32* context_p[16];
 995         u32 r0_thumb, pc_thumb;
 996         u32 cpsr;
 997         int retval;
 998         /* get pointers to arch-specific information */
 999         armv4_5_common_t *armv4_5 = target->arch_info;
1000         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1001         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
1002         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1003
1004 #ifdef _DEBUG_ARM7_9_
1005         DEBUG("-");
1006 #endif
1007
1008         if (arm7_9->pre_debug_entry)
1009                 arm7_9->pre_debug_entry(target);
1010
1011         /* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
1012          * ensure that DBGRQ is cleared
1013          */
1014         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
1015         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
1016         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
1017         embeddedice_store_reg(dbg_ctrl);
1018
1019         arm7_9_clear_halt(target);
1020
1021         if ((retval = jtag_execute_queue()) != ERROR_OK)
1022         {
1023                 switch (retval)
1024                 {
1025                         case ERROR_JTAG_QUEUE_FAILED:
1026                                 ERROR("JTAG queue failed while writing EmbeddedICE control register");
1027                                 exit(-1);
1028                                 break;
1029                         default:
1030                                 break;
1031                 }
1032         }
1033
1034         if ((retval = arm7_9->examine_debug_reason(target)) != ERROR_OK)
1035                 return retval;
1036
1037
1038         if (target->state != TARGET_HALTED)
1039         {
1040                 WARNING("target not halted");
1041                 return ERROR_TARGET_NOT_HALTED;
1042         }
1043
1044         /* if the target is in Thumb state, change to ARM state */
1045         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
1046         {
1047                 DEBUG("target entered debug from Thumb state");
1048                 /* Entered debug from Thumb mode */
1049                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
1050                 arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
1051                 DEBUG("r0_thumb: 0x%8.8x, pc_thumb: 0x%8.8x", r0_thumb, pc_thumb);
1052         }
1053         else
1054         {
1055                 DEBUG("target entered debug from ARM state");
1056                 /* Entered debug from ARM mode */
1057                 armv4_5->core_state = ARMV4_5_STATE_ARM;
1058         }
1059
1060         for (i = 0; i < 16; i++)
1061                 context_p[i] = &context[i];
1062         /* save core registers (r0 - r15 of current core mode) */
1063         arm7_9->read_core_regs(target, 0xffff, context_p);
1064
1065         arm7_9->read_xpsr(target, &cpsr, 0);
1066
1067         if ((retval = jtag_execute_queue()) != ERROR_OK)
1068                 return retval;
1069
1070         /* if the core has been executing in Thumb state, set the T bit */
1071         if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1072                 cpsr |= 0x20;
1073
1074         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, cpsr);
1075         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
1076         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1077
1078         armv4_5->core_mode = cpsr & 0x1f;
1079
1080         if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
1081         {
1082                 target->state = TARGET_UNKNOWN;
1083                 ERROR("cpsr contains invalid mode value - communication failure");
1084                 return ERROR_TARGET_FAILURE;
1085         }
1086
1087         DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);
1088
1089         if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1090         {
1091                 DEBUG("thumb state, applying fixups");
1092                 context[0] = r0_thumb;
1093                 context[15] = pc_thumb;
1094         } else if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1095         {
1096                 /* adjust value stored by STM */
1097                 context[15] -= 3 * 4;
1098         }
1099
1100         if ((target->debug_reason == DBG_REASON_BREAKPOINT)
1101                         || (target->debug_reason == DBG_REASON_SINGLESTEP)
1102                         || (target->debug_reason == DBG_REASON_WATCHPOINT)
1103                         || (target->debug_reason == DBG_REASON_WPTANDBKPT)
1104                         || ((target->debug_reason == DBG_REASON_DBGRQ) && (arm7_9->use_dbgrq == 0)))
1105                 context[15] -= 3 * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
1106         else if (target->debug_reason == DBG_REASON_DBGRQ)
1107                 context[15] -= arm7_9->dbgreq_adjust_pc * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
1108         else
1109         {
1110                 ERROR("unknown debug reason: %i", target->debug_reason);
1111         }
1112
1113
1114         for (i=0; i<=15; i++)
1115         {
1116                 DEBUG("r%i: 0x%8.8x", i, context[i]);
1117                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, context[i]);
1118                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;
1119                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
1120         }
1121
1122         DEBUG("entered debug state at PC 0x%x", context[15]);
1123
1124         /* exceptions other than USR & SYS have a saved program status register */
1125         if ((armv4_5_mode_to_number(armv4_5->core_mode) != ARMV4_5_MODE_USR) && (armv4_5_mode_to_number(armv4_5->core_mode) != ARMV4_5_MODE_SYS))
1126         {
1127                 u32 spsr;
1128                 arm7_9->read_xpsr(target, &spsr, 1);
1129                 jtag_execute_queue();
1130                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, spsr);
1131                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;
1132                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;
1133         }
1134
1135         /* r0 and r15 (pc) have to be restored later */
1136         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = 1;
1137         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 15).dirty = 1;
1138
1139         if ((retval = jtag->execute_queue()) != ERROR_OK)
1140                 return retval;
1141
1142         if (arm7_9->post_debug_entry)
1143                 arm7_9->post_debug_entry(target);
1144
1145         return ERROR_OK;
1146 }
1147
1148 int arm7_9_full_context(target_t *target)
1149 {
1150         int i;
1151         int retval;
1152         armv4_5_common_t *armv4_5 = target->arch_info;
1153         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1154
1155         DEBUG("-");
1156
1157         if (target->state != TARGET_HALTED)
1158         {
1159                 WARNING("target not halted");
1160                 return ERROR_TARGET_NOT_HALTED;
1161         }
1162
1163         /* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
1164          * SYS shares registers with User, so we don't touch SYS
1165          */
1166         for(i = 0; i < 6; i++)
1167         {
1168                 u32 mask = 0;
1169                 u32* reg_p[16];
1170                 int j;
1171                 int valid = 1;
1172
1173                 /* check if there are invalid registers in the current mode
1174                  */
1175                 for (j = 0; j <= 16; j++)
1176                 {
1177                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1178                                 valid = 0;
1179                 }
1180
1181                 if (!valid)
1182                 {
1183                         u32 tmp_cpsr;
1184
1185                         /* change processor mode (and mask T bit) */
1186                         tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1187                         tmp_cpsr |= armv4_5_number_to_mode(i);
1188                         tmp_cpsr &= ~0x20;
1189                         arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1190
1191                         for (j = 0; j < 15; j++)
1192                         {
1193                                 if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1194                                 {
1195                                         reg_p[j] = (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value;
1196                                         mask |= 1 << j;
1197                                         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;
1198                                         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
1199                                 }
1200                         }
1201
1202                         /* if only the PSR is invalid, mask is all zeroes */
1203                         if (mask)
1204                                 arm7_9->read_core_regs(target, mask, reg_p);
1205
1206                         /* check if the PSR has to be read */
1207                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid == 0)
1208                         {
1209                                 arm7_9->read_xpsr(target, (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).value, 1);
1210                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;
1211                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
1212                         }
1213                 }
1214         }
1215
1216         /* restore processor mode (mask T bit) */
1217         arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1218
1219         if ((retval = jtag_execute_queue()) != ERROR_OK)
1220         {
1221                 ERROR("JTAG failure");
1222                 exit(-1);
1223         }
1224         return ERROR_OK;
1225 }
1226
1227 int arm7_9_restore_context(target_t *target)
1228 {
1229         armv4_5_common_t *armv4_5 = target->arch_info;
1230         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1231         reg_t *reg;
1232         armv4_5_core_reg_t *reg_arch_info;
1233         enum armv4_5_mode current_mode = armv4_5->core_mode;
1234         int i, j;
1235         int dirty;
1236         int mode_change;
1237
1238         DEBUG("-");
1239
1240         if (target->state != TARGET_HALTED)
1241         {
1242                 WARNING("target not halted");
1243                 return ERROR_TARGET_NOT_HALTED;
1244         }
1245
1246         if (arm7_9->pre_restore_context)
1247                 arm7_9->pre_restore_context(target);
1248
1249         /* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
1250          * SYS shares registers with User, so we don't touch SYS
1251          */
1252         for (i = 0; i < 6; i++)
1253         {
1254                 DEBUG("examining %s mode", armv4_5_mode_strings[i]);
1255                 dirty = 0;
1256                 mode_change = 0;
1257                 /* check if there are dirty registers in the current mode
1258                 */
1259                 for (j = 0; j <= 16; j++)
1260                 {
1261                         reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
1262                         reg_arch_info = reg->arch_info;
1263                         if (reg->dirty == 1)
1264                         {
1265                                 if (reg->valid == 1)
1266                                 {
1267                                         dirty = 1;
1268                                         DEBUG("examining dirty reg: %s", reg->name);
1269                                         if ((reg_arch_info->mode != ARMV4_5_MODE_ANY)
1270                                                 && (reg_arch_info->mode != current_mode)
1271                                                 && !((reg_arch_info->mode == ARMV4_5_MODE_USR) && (armv4_5->core_mode == ARMV4_5_MODE_SYS))
1272                                                 && !((reg_arch_info->mode == ARMV4_5_MODE_SYS) && (armv4_5->core_mode == ARMV4_5_MODE_USR)))
1273                                         {
1274                                                 mode_change = 1;
1275                                                 DEBUG("require mode change");
1276                                         }
1277                                 }
1278                                 else
1279                                 {
1280                                         ERROR("BUG: dirty register '%s', but no valid data", reg->name);
1281                                         exit(-1);
1282                                 }
1283                         }
1284                 }
1285
1286                 if (dirty)
1287                 {
1288                         u32 mask = 0x0;
1289                         int num_regs = 0;
1290                         u32 regs[16];
1291
1292                         if (mode_change)
1293                         {
1294                                 u32 tmp_cpsr;
1295
1296                                 /* change processor mode (mask T bit) */
1297                                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1298                                 tmp_cpsr |= armv4_5_number_to_mode(i);
1299                                 tmp_cpsr &= ~0x20;
1300                                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1301                                 current_mode = armv4_5_number_to_mode(i);
1302                         }
1303
1304                         for (j = 0; j <= 14; j++)
1305                         {
1306                                 reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
1307                                 reg_arch_info = reg->arch_info;
1308
1309
1310                                 if (reg->dirty == 1)
1311                                 {
1312                                         regs[j] = buf_get_u32(reg->value, 0, 32);
1313                                         mask |= 1 << j;
1314                                         num_regs++;
1315                                         reg->dirty = 0;
1316                                         reg->valid = 1;
1317                                         DEBUG("writing register %i of mode %s with value 0x%8.8x", j, armv4_5_mode_strings[i], regs[j]);
1318                                 }
1319                         }
1320
1321                         if (mask)
1322                         {
1323                                 arm7_9->write_core_regs(target, mask, regs);
1324                         }
1325
1326                         reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16);
1327                         reg_arch_info = reg->arch_info;
1328                         if ((reg->dirty) && (reg_arch_info->mode != ARMV4_5_MODE_ANY))
1329                         {
1330                                 DEBUG("writing SPSR of mode %i with value 0x%8.8x", i, buf_get_u32(reg->value, 0, 32));
1331                                 arm7_9->write_xpsr(target, buf_get_u32(reg->value, 0, 32), 1);
1332                         }
1333                 }
1334         }
1335
1336         if ((armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 0) && (armv4_5->core_mode != current_mode))
1337         {
1338                 /* restore processor mode (mask T bit) */
1339                 u32 tmp_cpsr;
1340
1341                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1342                 tmp_cpsr |= armv4_5_number_to_mode(i);
1343                 tmp_cpsr &= ~0x20;
1344                 DEBUG("writing lower 8 bit of cpsr with value 0x%2.2x", tmp_cpsr);
1345                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1346         }
1347         else if (armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 1)
1348         {
1349                 /* CPSR has been changed, full restore necessary (mask T bit) */
1350                 DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1351                 arm7_9->write_xpsr(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32) & ~0x20, 0);
1352                 armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
1353                 armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1354         }
1355
1356         /* restore PC */
1357         DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1358         arm7_9->write_pc(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1359         armv4_5->core_cache->reg_list[15].dirty = 0;
1360
1361         if (arm7_9->post_restore_context)
1362                 arm7_9->post_restore_context(target);
1363
1364         return ERROR_OK;
1365 }
1366
1367 int arm7_9_restart_core(struct target_s *target)
1368 {
1369         armv4_5_common_t *armv4_5 = target->arch_info;
1370         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1371         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
1372
1373         /* set RESTART instruction */
1374         jtag_add_end_state(TAP_RTI);
1375         arm_jtag_set_instr(jtag_info, 0x4, NULL);
1376
1377         jtag_add_runtest(1, TAP_RTI);
1378         if ((jtag_execute_queue()) != ERROR_OK)
1379         {
1380                 exit(-1);
1381         }
1382
1383         return ERROR_OK;
1384 }
1385
1386 void arm7_9_enable_watchpoints(struct target_s *target)
1387 {
1388         watchpoint_t *watchpoint = target->watchpoints;
1389
1390         while (watchpoint)
1391         {
1392                 if (watchpoint->set == 0)
1393                         arm7_9_set_watchpoint(target, watchpoint);
1394                 watchpoint = watchpoint->next;
1395         }
1396 }
1397
1398 void arm7_9_enable_breakpoints(struct target_s *target)
1399 {
1400         breakpoint_t *breakpoint = target->breakpoints;
1401
1402         /* set any pending breakpoints */
1403         while (breakpoint)
1404         {
1405                 if (breakpoint->set == 0)
1406                         arm7_9_set_breakpoint(target, breakpoint);
1407                 breakpoint = breakpoint->next;
1408         }
1409 }
1410
1411 void arm7_9_disable_bkpts_and_wpts(struct target_s *target)
1412 {
1413         breakpoint_t *breakpoint = target->breakpoints;
1414         watchpoint_t *watchpoint = target->watchpoints;
1415
1416         /* set any pending breakpoints */
1417         while (breakpoint)
1418         {
1419                 if (breakpoint->set != 0)
1420                         arm7_9_unset_breakpoint(target, breakpoint);
1421                 breakpoint = breakpoint->next;
1422         }
1423
1424         while (watchpoint)
1425         {
1426                 if (watchpoint->set != 0)
1427                         arm7_9_unset_watchpoint(target, watchpoint);
1428                 watchpoint = watchpoint->next;
1429         }
1430 }
1431
1432 int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
1433 {
1434         armv4_5_common_t *armv4_5 = target->arch_info;
1435         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1436         breakpoint_t *breakpoint = target->breakpoints;
1437         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1438
1439         DEBUG("-");
1440
1441         if (target->state != TARGET_HALTED)
1442         {
1443                 WARNING("target not halted");
1444                 return ERROR_TARGET_NOT_HALTED;
1445         }
1446
1447         if (!debug_execution)
1448         {
1449                 target_free_all_working_areas(target);
1450         }
1451
1452         /* current = 1: continue on current pc, otherwise continue at <address> */
1453         if (!current)
1454                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1455
1456         /* the front-end may request us not to handle breakpoints */
1457         if (handle_breakpoints)
1458         {
1459                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1460                 {
1461                         DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
1462                         arm7_9_unset_breakpoint(target, breakpoint);
1463
1464                         DEBUG("enable single-step");
1465                         arm7_9->enable_single_step(target);
1466
1467                         target->debug_reason = DBG_REASON_SINGLESTEP;
1468
1469                         arm7_9_restore_context(target);
1470
1471                         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1472                                 arm7_9->branch_resume(target);
1473                         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1474                         {
1475                                 arm7_9->branch_resume_thumb(target);
1476                         }
1477                         else
1478                         {
1479                                 ERROR("unhandled core state");
1480                                 exit(-1);
1481                         }
1482
1483                         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1484                         embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1485                         arm7_9_execute_sys_speed(target);
1486
1487                         DEBUG("disable single-step");
1488                         arm7_9->disable_single_step(target);
1489
1490                         arm7_9_debug_entry(target);
1491                         DEBUG("new PC after step: 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1492
1493                         DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
1494                         arm7_9_set_breakpoint(target, breakpoint);
1495                 }
1496         }
1497
1498         /* enable any pending breakpoints and watchpoints */
1499         arm7_9_enable_breakpoints(target);
1500         arm7_9_enable_watchpoints(target);
1501
1502         arm7_9_restore_context(target);
1503
1504         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1505         {
1506                 arm7_9->branch_resume(target);
1507         }
1508         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1509         {
1510                 arm7_9->branch_resume_thumb(target);
1511         }
1512         else
1513         {
1514                 ERROR("unhandled core state");
1515                 exit(-1);
1516         }
1517
1518         /* deassert DBGACK and INTDIS */
1519         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1520         /* INTDIS only when we really resume, not during debug execution */
1521         if (!debug_execution)
1522                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 0);
1523         embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1524
1525         arm7_9_restart_core(target);
1526
1527         target->debug_reason = DBG_REASON_NOTHALTED;
1528
1529         if (!debug_execution)
1530         {
1531                 /* registers are now invalid */
1532                 armv4_5_invalidate_core_regs(target);
1533                 target->state = TARGET_RUNNING;
1534                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1535         }
1536         else
1537         {
1538                 target->state = TARGET_DEBUG_RUNNING;
1539                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
1540         }
1541
1542         DEBUG("target resumed");
1543
1544         return ERROR_OK;
1545 }
1546
1547 void arm7_9_enable_eice_step(target_t *target)
1548 {
1549         armv4_5_common_t *armv4_5 = target->arch_info;
1550         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1551
1552         /* setup an inverse breakpoint on the current PC
1553         * - comparator 1 matches the current address
1554         * - rangeout from comparator 1 is connected to comparator 0 rangein
1555         * - comparator 0 matches any address, as long as rangein is low */
1556         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1557         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1558         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x100);
1559         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0x77);
1560         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1561         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0);
1562         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffff);
1563         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
1564         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], 0xf7);
1565 }
1566
1567 void arm7_9_disable_eice_step(target_t *target)
1568 {
1569         armv4_5_common_t *armv4_5 = target->arch_info;
1570         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1571
1572         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
1573         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
1574         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
1575         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
1576         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE]);
1577         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK]);
1578         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK]);
1579         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK]);
1580         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE]);
1581 }
1582
1583 int arm7_9_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
1584 {
1585         armv4_5_common_t *armv4_5 = target->arch_info;
1586         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1587         breakpoint_t *breakpoint = NULL;
1588
1589         if (target->state != TARGET_HALTED)
1590         {
1591                 WARNING("target not halted");
1592                 return ERROR_TARGET_NOT_HALTED;
1593         }
1594
1595         /* current = 1: continue on current pc, otherwise continue at <address> */
1596         if (!current)
1597                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1598
1599         /* the front-end may request us not to handle breakpoints */
1600         if (handle_breakpoints)
1601                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1602                         arm7_9_unset_breakpoint(target, breakpoint);
1603
1604         target->debug_reason = DBG_REASON_SINGLESTEP;
1605
1606         arm7_9_restore_context(target);
1607
1608         arm7_9->enable_single_step(target);
1609
1610         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1611         {
1612                 arm7_9->branch_resume(target);
1613         }
1614         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1615         {
1616                 arm7_9->branch_resume_thumb(target);
1617         }
1618         else
1619         {
1620                 ERROR("unhandled core state");
1621                 exit(-1);
1622         }
1623
1624         target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
1625
1626         arm7_9_execute_sys_speed(target);
1627         arm7_9->disable_single_step(target);
1628
1629         /* registers are now invalid */
1630         armv4_5_invalidate_core_regs(target);
1631
1632         arm7_9_debug_entry(target);
1633
1634         target_call_event_callbacks(target, TARGET_EVENT_HALTED);
1635
1636         if (breakpoint)
1637                 arm7_9_set_breakpoint(target, breakpoint);
1638
1639         DEBUG("target stepped");
1640
1641         return ERROR_OK;
1642
1643 }
1644
1645 int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
1646 {
1647         u32* reg_p[16];
1648         u32 value;
1649         int retval;
1650         armv4_5_common_t *armv4_5 = target->arch_info;
1651         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1652         enum armv4_5_mode reg_mode = ((armv4_5_core_reg_t*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info)->mode;
1653
1654         if ((num < 0) || (num > 16))
1655                 return ERROR_INVALID_ARGUMENTS;
1656
1657         if ((mode != ARMV4_5_MODE_ANY)
1658                         && (mode != armv4_5->core_mode)
1659                         && (reg_mode != ARMV4_5_MODE_ANY))
1660         {
1661                 u32 tmp_cpsr;
1662
1663                 /* change processor mode (mask T bit) */
1664                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1665                 tmp_cpsr |= mode;
1666                 tmp_cpsr &= ~0x20;
1667                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1668         }
1669
1670         if ((num >= 0) && (num <= 15))
1671         {
1672                 /* read a normal core register */
1673                 reg_p[num] = &value;
1674
1675                 arm7_9->read_core_regs(target, 1 << num, reg_p);
1676         }
1677         else
1678         {
1679                 /* read a program status register
1680                  * if the register mode is MODE_ANY, we read the cpsr, otherwise a spsr
1681                  */
1682                 armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
1683                 int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1684
1685                 arm7_9->read_xpsr(target, &value, spsr);
1686         }
1687
1688         if ((retval = jtag_execute_queue()) != ERROR_OK)
1689         {
1690                 ERROR("JTAG failure");
1691                 exit(-1);
1692         }
1693
1694         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
1695         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1696         buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value, 0, 32, value);
1697
1698         if ((mode != ARMV4_5_MODE_ANY)
1699                         && (mode != armv4_5->core_mode)
1700                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1701                 /* restore processor mode (mask T bit) */
1702                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1703         }
1704
1705         return ERROR_OK;
1706
1707 }
1708
1709 int arm7_9_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)
1710 {
1711         u32 reg[16];
1712         int retval;
1713         armv4_5_common_t *armv4_5 = target->arch_info;
1714         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1715         enum armv4_5_mode reg_mode = ((armv4_5_core_reg_t*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info)->mode;
1716
1717         if ((num < 0) || (num > 16))
1718                 return ERROR_INVALID_ARGUMENTS;
1719
1720         if ((mode != ARMV4_5_MODE_ANY)
1721                         && (mode != armv4_5->core_mode)
1722                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1723                 u32 tmp_cpsr;
1724
1725                 /* change processor mode (mask T bit) */
1726                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1727                 tmp_cpsr |= mode;
1728                 tmp_cpsr &= ~0x20;
1729                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1730         }
1731
1732         if ((num >= 0) && (num <= 15))
1733         {
1734                 /* write a normal core register */
1735                 reg[num] = value;
1736
1737                 arm7_9->write_core_regs(target, 1 << num, reg);
1738         }
1739         else
1740         {
1741                 /* write a program status register
1742                 * if the register mode is MODE_ANY, we write the cpsr, otherwise a spsr
1743                 */
1744                 armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
1745                 int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1746
1747                 /* if we're writing the CPSR, mask the T bit */
1748                 if (!spsr)
1749                         value &= ~0x20;
1750
1751                 arm7_9->write_xpsr(target, value, spsr);
1752         }
1753
1754         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
1755         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1756
1757         if ((mode != ARMV4_5_MODE_ANY)
1758                         && (mode != armv4_5->core_mode)
1759                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1760                 /* restore processor mode (mask T bit) */
1761                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1762         }
1763
1764         if ((retval = jtag_execute_queue()) != ERROR_OK)
1765         {
1766                 ERROR("JTAG failure");
1767                 exit(-1);
1768         }
1769
1770         return ERROR_OK;
1771
1772 }
1773
1774 int arm7_9_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1775 {
1776         armv4_5_common_t *armv4_5 = target->arch_info;
1777         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1778
1779         u32 reg[16];
1780         int num_accesses = 0;
1781         int thisrun_accesses;
1782         int i;
1783         u32 cpsr;
1784         int retval;
1785         int last_reg = 0;
1786
1787         DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1788
1789         if (target->state != TARGET_HALTED)
1790         {
1791                 WARNING("target not halted");
1792                 return ERROR_TARGET_NOT_HALTED;
1793         }
1794
1795         /* sanitize arguments */
1796         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1797                 return ERROR_INVALID_ARGUMENTS;
1798
1799         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1800                 return ERROR_TARGET_UNALIGNED_ACCESS;
1801
1802         /* load the base register with the address of the first word */
1803         reg[0] = address;
1804         arm7_9->write_core_regs(target, 0x1, reg);
1805
1806         switch (size)
1807         {
1808                 case 4:
1809                         while (num_accesses < count)
1810                         {
1811                                 u32 reg_list;
1812                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1813                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1814
1815                                 if (last_reg <= thisrun_accesses)
1816                                         last_reg = thisrun_accesses;
1817
1818                                 arm7_9->load_word_regs(target, reg_list);
1819
1820                                 /* fast memory reads are only safe when the target is running
1821                                  * from a sufficiently high clock (32 kHz is usually too slow)
1822                                  */
1823                                 if (arm7_9->fast_memory_access)
1824                                         arm7_9_execute_fast_sys_speed(target);
1825                                 else
1826                                         arm7_9_execute_sys_speed(target);
1827
1828                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 4);
1829
1830                                 /* advance buffer, count number of accesses */
1831                                 buffer += thisrun_accesses * 4;
1832                                 num_accesses += thisrun_accesses;
1833                         }
1834                         break;
1835                 case 2:
1836                         while (num_accesses < count)
1837                         {
1838                                 u32 reg_list;
1839                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1840                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1841
1842                                 for (i = 1; i <= thisrun_accesses; i++)
1843                                 {
1844                                         if (i > last_reg)
1845                                                 last_reg = i;
1846                                         arm7_9->load_hword_reg(target, i);
1847                                         /* fast memory reads are only safe when the target is running
1848                                          * from a sufficiently high clock (32 kHz is usually too slow)
1849                                          */
1850                                         if (arm7_9->fast_memory_access)
1851                                                 arm7_9_execute_fast_sys_speed(target);
1852                                         else
1853                                                 arm7_9_execute_sys_speed(target);
1854                                 }
1855
1856                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 2);
1857
1858                                 /* advance buffer, count number of accesses */
1859                                 buffer += thisrun_accesses * 2;
1860                                 num_accesses += thisrun_accesses;
1861                         }
1862                         break;
1863                 case 1:
1864                         while (num_accesses < count)
1865                         {
1866                                 u32 reg_list;
1867                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1868                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1869
1870                                 for (i = 1; i <= thisrun_accesses; i++)
1871                                 {
1872                                         if (i > last_reg)
1873                                                 last_reg = i;
1874                                         arm7_9->load_byte_reg(target, i);
1875                                         /* fast memory reads are only safe when the target is running
1876                                          * from a sufficiently high clock (32 kHz is usually too slow)
1877                                          */
1878                                         if (arm7_9->fast_memory_access)
1879                                                 arm7_9_execute_fast_sys_speed(target);
1880                                         else
1881                                                 arm7_9_execute_sys_speed(target);
1882                                 }
1883
1884                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 1);
1885
1886                                 /* advance buffer, count number of accesses */
1887                                 buffer += thisrun_accesses * 1;
1888                                 num_accesses += thisrun_accesses;
1889                         }
1890                         break;
1891                 default:
1892                         ERROR("BUG: we shouldn't get here");
1893                         exit(-1);
1894                         break;
1895         }
1896
1897         for (i=0; i<=last_reg; i++)
1898                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
1899
1900         arm7_9->read_xpsr(target, &cpsr, 0);
1901         if ((retval = jtag_execute_queue()) != ERROR_OK)
1902         {
1903                 ERROR("JTAG error while reading cpsr");
1904                 exit(-1);
1905         }
1906
1907         if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
1908         {
1909                 WARNING("memory read caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
1910
1911                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1912
1913                 return ERROR_TARGET_DATA_ABORT;
1914         }
1915
1916         return ERROR_OK;
1917 }
1918
1919 int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1920 {
1921         armv4_5_common_t *armv4_5 = target->arch_info;
1922         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1923         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1924
1925         u32 reg[16];
1926         int num_accesses = 0;
1927         int thisrun_accesses;
1928         int i;
1929         u32 cpsr;
1930         int retval;
1931         int last_reg = 0;
1932
1933         DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1934
1935         if (target->state != TARGET_HALTED)
1936         {
1937                 WARNING("target not halted");
1938                 return ERROR_TARGET_NOT_HALTED;
1939         }
1940
1941         /* sanitize arguments */
1942         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1943                 return ERROR_INVALID_ARGUMENTS;
1944
1945         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1946                 return ERROR_TARGET_UNALIGNED_ACCESS;
1947
1948         /* load the base register with the address of the first word */
1949         reg[0] = address;
1950         arm7_9->write_core_regs(target, 0x1, reg);
1951
1952         /* Clear DBGACK, to make sure memory fetches work as expected */
1953         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1954         embeddedice_store_reg(dbg_ctrl);
1955
1956         switch (size)
1957         {
1958                 case 4:
1959                         while (num_accesses < count)
1960                         {
1961                                 u32 reg_list;
1962                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1963                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1964
1965                                 for (i = 1; i <= thisrun_accesses; i++)
1966                                 {
1967                                         if (i > last_reg)
1968                                                 last_reg = i;
1969                                         reg[i] = target_buffer_get_u32(target, buffer);
1970                                         buffer += 4;
1971                                 }
1972
1973                                 arm7_9->write_core_regs(target, reg_list, reg);
1974
1975                                 arm7_9->store_word_regs(target, reg_list);
1976
1977                                 /* fast memory writes are only safe when the target is running
1978                                  * from a sufficiently high clock (32 kHz is usually too slow)
1979                                  */
1980                                 if (arm7_9->fast_memory_access)
1981                                         arm7_9_execute_fast_sys_speed(target);
1982                                 else
1983                                         arm7_9_execute_sys_speed(target);
1984
1985                                 num_accesses += thisrun_accesses;
1986                         }
1987                         break;
1988                 case 2:
1989                         while (num_accesses < count)
1990                         {
1991                                 u32 reg_list;
1992                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1993                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1994
1995                                 for (i = 1; i <= thisrun_accesses; i++)
1996                                 {
1997                                         if (i > last_reg)
1998                                                 last_reg = i;
1999                                         reg[i] = target_buffer_get_u16(target, buffer) & 0xffff;
2000                                         buffer += 2;
2001                                 }
2002
2003                                 arm7_9->write_core_regs(target, reg_list, reg);
2004
2005                                 for (i = 1; i <= thisrun_accesses; i++)
2006                                 {
2007                                         arm7_9->store_hword_reg(target, i);
2008
2009                                         /* fast memory writes are only safe when the target is running
2010                                          * from a sufficiently high clock (32 kHz is usually too slow)
2011                                          */
2012                                         if (arm7_9->fast_memory_access)
2013                                                 arm7_9_execute_fast_sys_speed(target);
2014                                         else
2015                                                 arm7_9_execute_sys_speed(target);
2016                                 }
2017
2018                                 num_accesses += thisrun_accesses;
2019                         }
2020                         break;
2021                 case 1:
2022                         while (num_accesses < count)
2023                         {
2024                                 u32 reg_list;
2025                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2026                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2027
2028                                 for (i = 1; i <= thisrun_accesses; i++)
2029                                 {
2030                                         if (i > last_reg)
2031                                                 last_reg = i;
2032                                         reg[i] = *buffer++ & 0xff;
2033                                 }
2034
2035                                 arm7_9->write_core_regs(target, reg_list, reg);
2036
2037                                 for (i = 1; i <= thisrun_accesses; i++)
2038                                 {
2039                                         arm7_9->store_byte_reg(target, i);
2040                                         /* fast memory writes are only safe when the target is running
2041                                          * from a sufficiently high clock (32 kHz is usually too slow)
2042                                          */
2043                                         if (arm7_9->fast_memory_access)
2044                                                 arm7_9_execute_fast_sys_speed(target);
2045                                         else
2046                                                 arm7_9_execute_sys_speed(target);
2047                                 }
2048
2049                                 num_accesses += thisrun_accesses;
2050                         }
2051                         break;
2052                 default:
2053                         ERROR("BUG: we shouldn't get here");
2054                         exit(-1);
2055                         break;
2056         }
2057
2058         /* Re-Set DBGACK */
2059         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
2060         embeddedice_store_reg(dbg_ctrl);
2061
2062         for (i=0; i<=last_reg; i++)
2063                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
2064
2065         arm7_9->read_xpsr(target, &cpsr, 0);
2066         if ((retval = jtag_execute_queue()) != ERROR_OK)
2067         {
2068                 ERROR("JTAG error while reading cpsr");
2069                 exit(-1);
2070         }
2071
2072         if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
2073         {
2074                 WARNING("memory write caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
2075
2076                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
2077
2078                 return ERROR_TARGET_DATA_ABORT;
2079         }
2080
2081         return ERROR_OK;
2082 }
2083
2084 int arm7_9_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)
2085 {
2086         armv4_5_common_t *armv4_5 = target->arch_info;
2087         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
2088         enum armv4_5_state core_state = armv4_5->core_state;
2089         u32 r0 = buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32);
2090         u32 r1 = buf_get_u32(armv4_5->core_cache->reg_list[1].value, 0, 32);
2091         u32 pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
2092         int i;
2093
2094         u32 dcc_code[] =
2095         {
2096                 /* MRC      TST         BNE         MRC         STR         B */
2097                 0xee101e10, 0xe3110001, 0x0afffffc, 0xee111e10, 0xe4801004, 0xeafffff9
2098         };
2099
2100         if (!arm7_9->dcc_downloads)
2101                 return target->type->write_memory(target, address, 4, count, buffer);
2102
2103         /* regrab previously allocated working_area, or allocate a new one */
2104         if (!arm7_9->dcc_working_area)
2105         {
2106                 u8 dcc_code_buf[6 * 4];
2107
2108                 /* make sure we have a working area */
2109                 if (target_alloc_working_area(target, 24, &arm7_9->dcc_working_area) != ERROR_OK)
2110                 {
2111                         INFO("no working area available, falling back to memory writes");
2112                         return target->type->write_memory(target, address, 4, count, buffer);
2113                 }
2114
2115                 /* copy target instructions to target endianness */
2116                 for (i = 0; i < 6; i++)
2117                 {
2118                         target_buffer_set_u32(target, dcc_code_buf + i*4, dcc_code[i]);
2119                 }
2120
2121                 /* write DCC code to working area */
2122                 target->type->write_memory(target, arm7_9->dcc_working_area->address, 4, 6, dcc_code_buf);
2123         }
2124
2125         buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, address);
2126         armv4_5->core_cache->reg_list[0].valid = 1;
2127         armv4_5->core_cache->reg_list[0].dirty = 1;
2128         armv4_5->core_state = ARMV4_5_STATE_ARM;
2129
2130         arm7_9_resume(target, 0, arm7_9->dcc_working_area->address, 1, 1);
2131
2132         for (i = 0; i < count; i++)
2133         {
2134                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], target_buffer_get_u32(target, buffer));
2135                 buffer += 4;
2136         }
2137
2138         target->type->halt(target);
2139
2140         while (target->state != TARGET_HALTED)
2141                 target->type->poll(target);
2142
2143         /* restore target state */
2144         buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, r0);
2145         armv4_5->core_cache->reg_list[0].valid = 1;
2146         armv4_5->core_cache->reg_list[0].dirty = 1;
2147         buf_set_u32(armv4_5->core_cache->reg_list[1].value, 0, 32, r1);
2148         armv4_5->core_cache->reg_list[1].valid = 1;
2149         armv4_5->core_cache->reg_list[1].dirty = 1;
2150         buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, pc);
2151         armv4_5->core_cache->reg_list[15].valid = 1;
2152         armv4_5->core_cache->reg_list[15].dirty = 1;
2153         armv4_5->core_state = core_state;
2154
2155         return ERROR_OK;
2156 }
2157
2158 int arm7_9_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)
2159 {
2160         working_area_t *crc_algorithm;
2161         armv4_5_algorithm_t armv4_5_info;
2162         reg_param_t reg_params[2];
2163         int retval;
2164
2165         u32 arm7_9_crc_code[] = {
2166                 0xE1A02000,                             /* mov          r2, r0 */
2167                 0xE3E00000,                             /* mov          r0, #0xffffffff */
2168                 0xE1A03001,                             /* mov          r3, r1 */
2169                 0xE3A04000,                             /* mov          r4, #0 */
2170                 0xEA00000B,                             /* b            ncomp */
2171                                                                 /* nbyte: */
2172                 0xE7D21004,                             /* ldrb r1, [r2, r4] */
2173                 0xE59F7030,                             /* ldr          r7, CRC32XOR */
2174                 0xE0200C01,                             /* eor          r0, r0, r1, asl 24 */
2175                 0xE3A05000,                             /* mov          r5, #0 */
2176                                                                 /* loop: */
2177                 0xE3500000,                             /* cmp          r0, #0 */
2178                 0xE1A06080,                             /* mov          r6, r0, asl #1 */
2179                 0xE2855001,                             /* add          r5, r5, #1 */
2180                 0xE1A00006,                             /* mov          r0, r6 */
2181                 0xB0260007,                             /* eorlt        r0, r6, r7 */
2182                 0xE3550008,                             /* cmp          r5, #8 */
2183                 0x1AFFFFF8,                             /* bne          loop */
2184                 0xE2844001,                             /* add          r4, r4, #1 */
2185                                                                 /* ncomp: */
2186                 0xE1540003,                             /* cmp          r4, r3 */
2187                 0x1AFFFFF1,                             /* bne          nbyte */
2188                                                                 /* end: */
2189                 0xEAFFFFFE,                             /* b            end */
2190                 0x04C11DB7                              /* CRC32XOR:    .word 0x04C11DB7 */
2191         };
2192
2193         int i;
2194
2195         if (target_alloc_working_area(target, sizeof(arm7_9_crc_code), &crc_algorithm) != ERROR_OK)
2196         {
2197                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2198         }
2199
2200         /* convert flash writing code into a buffer in target endianness */
2201         for (i = 0; i < (sizeof(arm7_9_crc_code)/sizeof(u32)); i++)
2202                 target_write_u32(target, crc_algorithm->address + i*sizeof(u32), arm7_9_crc_code[i]);
2203
2204         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2205         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2206         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2207
2208         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
2209         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
2210
2211         buf_set_u32(reg_params[0].value, 0, 32, address);
2212         buf_set_u32(reg_params[1].value, 0, 32, count);
2213
2214         if ((retval = target->type->run_algorithm(target, 0, NULL, 2, reg_params,
2215                 crc_algorithm->address, crc_algorithm->address + (sizeof(arm7_9_crc_code) - 8), 20000, &armv4_5_info)) != ERROR_OK)
2216         {
2217                 ERROR("error executing arm7_9 crc algorithm");
2218                 destroy_reg_param(&reg_params[0]);
2219                 destroy_reg_param(&reg_params[1]);
2220                 target_free_working_area(target, crc_algorithm);
2221                 return retval;
2222         }
2223
2224         *checksum = buf_get_u32(reg_params[0].value, 0, 32);
2225
2226         destroy_reg_param(&reg_params[0]);
2227         destroy_reg_param(&reg_params[1]);
2228
2229         target_free_working_area(target, crc_algorithm);
2230
2231         return ERROR_OK;
2232 }
2233
2234 int arm7_9_register_commands(struct command_context_s *cmd_ctx)
2235 {
2236         command_t *arm7_9_cmd;
2237
2238         arm7_9_cmd = register_command(cmd_ctx, NULL, "arm7_9", NULL, COMMAND_ANY, "arm7/9 specific commands");
2239
2240         register_command(cmd_ctx, arm7_9_cmd, "write_xpsr", handle_arm7_9_write_xpsr_command, COMMAND_EXEC, "write program status register <value> <not cpsr|spsr>");
2241         register_command(cmd_ctx, arm7_9_cmd, "write_xpsr_im8", handle_arm7_9_write_xpsr_im8_command, COMMAND_EXEC, "write program status register <8bit immediate> <rotate> <not cpsr|spsr>");
2242
2243         register_command(cmd_ctx, arm7_9_cmd, "write_core_reg", handle_arm7_9_write_core_reg_command, COMMAND_EXEC, "write core register <num> <mode> <value>");
2244
2245         register_command(cmd_ctx, arm7_9_cmd, "sw_bkpts", handle_arm7_9_sw_bkpts_command, COMMAND_EXEC, "support for software breakpoints <enable|disable>");
2246         register_command(cmd_ctx, arm7_9_cmd, "force_hw_bkpts", handle_arm7_9_force_hw_bkpts_command, COMMAND_EXEC, "use hardware breakpoints for all breakpoints (disables sw breakpoint support) <enable|disable>");
2247         register_command(cmd_ctx, arm7_9_cmd, "dbgrq", handle_arm7_9_dbgrq_command,
2248                 COMMAND_ANY, "use EmbeddedICE dbgrq instead of breakpoint for target halt requests <enable|disable>");
2249         register_command(cmd_ctx, arm7_9_cmd, "fast_writes", handle_arm7_9_fast_memory_access_command,
2250                  COMMAND_ANY, "(deprecated, see: arm7_9 fast_memory_access)");
2251         register_command(cmd_ctx, arm7_9_cmd, "fast_memory_access", handle_arm7_9_fast_memory_access_command,
2252                  COMMAND_ANY, "use fast memory accesses instead of slower but potentially unsafe slow accesses <enable|disable>");
2253         register_command(cmd_ctx, arm7_9_cmd, "dcc_downloads", handle_arm7_9_dcc_downloads_command,
2254                 COMMAND_ANY, "use DCC downloads for larger memory writes <enable|disable>");
2255
2256         armv4_5_register_commands(cmd_ctx);
2257
2258         etm_register_commands(cmd_ctx);
2259
2260         return ERROR_OK;
2261 }
2262
2263 int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2264 {
2265         u32 value;
2266         int spsr;
2267         int retval;
2268         target_t *target = get_current_target(cmd_ctx);
2269         armv4_5_common_t *armv4_5;
2270         arm7_9_common_t *arm7_9;
2271
2272         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2273         {
2274                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2275                 return ERROR_OK;
2276         }
2277
2278         if (target->state != TARGET_HALTED)
2279         {
2280                 command_print(cmd_ctx, "can't write registers while running");
2281                 return ERROR_OK;
2282         }
2283
2284         if (argc < 2)
2285         {
2286                 command_print(cmd_ctx, "usage: write_xpsr <value> <not cpsr|spsr>");
2287                 return ERROR_OK;
2288         }
2289
2290         value = strtoul(args[0], NULL, 0);
2291         spsr = strtol(args[1], NULL, 0);
2292
2293         /* if we're writing the CPSR, mask the T bit */
2294         if (!spsr)
2295                 value &= ~0x20;
2296
2297         arm7_9->write_xpsr(target, value, spsr);
2298         if ((retval = jtag_execute_queue()) != ERROR_OK)
2299         {
2300                 ERROR("JTAG error while writing to xpsr");
2301                 exit(-1);
2302         }
2303
2304         return ERROR_OK;
2305 }
2306
2307 int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2308 {
2309         u32 value;
2310         int rotate;
2311         int spsr;
2312         int retval;
2313         target_t *target = get_current_target(cmd_ctx);
2314         armv4_5_common_t *armv4_5;
2315         arm7_9_common_t *arm7_9;
2316
2317         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2318         {
2319                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2320                 return ERROR_OK;
2321         }
2322
2323         if (target->state != TARGET_HALTED)
2324         {
2325                 command_print(cmd_ctx, "can't write registers while running");
2326                 return ERROR_OK;
2327         }
2328
2329         if (argc < 3)
2330         {
2331                 command_print(cmd_ctx, "usage: write_xpsr_im8 <im8> <rotate> <not cpsr|spsr>");
2332                 return ERROR_OK;
2333         }
2334
2335         value = strtoul(args[0], NULL, 0);
2336         rotate = strtol(args[1], NULL, 0);
2337         spsr = strtol(args[2], NULL, 0);
2338
2339         arm7_9->write_xpsr_im8(target, value, rotate, spsr);
2340         if ((retval = jtag_execute_queue()) != ERROR_OK)
2341         {
2342                 ERROR("JTAG error while writing 8-bit immediate to xpsr");
2343                 exit(-1);
2344         }
2345
2346         return ERROR_OK;
2347 }
2348
2349 int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2350 {
2351         u32 value;
2352         u32 mode;
2353         int num;
2354         target_t *target = get_current_target(cmd_ctx);
2355         armv4_5_common_t *armv4_5;
2356         arm7_9_common_t *arm7_9;
2357
2358         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2359         {
2360                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2361                 return ERROR_OK;
2362         }
2363
2364         if (target->state != TARGET_HALTED)
2365         {
2366                 command_print(cmd_ctx, "can't write registers while running");
2367                 return ERROR_OK;
2368         }
2369
2370         if (argc < 3)
2371         {
2372                 command_print(cmd_ctx, "usage: write_core_reg <num> <mode> <value>");
2373                 return ERROR_OK;
2374         }
2375
2376         num = strtol(args[0], NULL, 0);
2377         mode = strtoul(args[1], NULL, 0);
2378         value = strtoul(args[2], NULL, 0);
2379
2380         arm7_9_write_core_reg(target, num, mode, value);
2381
2382         return ERROR_OK;
2383 }
2384
2385 int handle_arm7_9_sw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2386 {
2387         target_t *target = get_current_target(cmd_ctx);
2388         armv4_5_common_t *armv4_5;
2389         arm7_9_common_t *arm7_9;
2390
2391         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2392         {
2393                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2394                 return ERROR_OK;
2395         }
2396
2397         if (argc == 0)
2398         {
2399                 command_print(cmd_ctx, "software breakpoints %s", (arm7_9->sw_bkpts_enabled) ? "enabled" : "disabled");
2400                 return ERROR_OK;
2401         }
2402
2403         if (strcmp("enable", args[0]) == 0)
2404         {
2405                 if (arm7_9->sw_bkpts_use_wp)
2406                 {
2407                         arm7_9_enable_sw_bkpts(target);
2408                 }
2409                 else
2410                 {
2411                         arm7_9->sw_bkpts_enabled = 1;
2412                 }
2413         }
2414         else if (strcmp("disable", args[0]) == 0)
2415         {
2416                 if (arm7_9->sw_bkpts_use_wp)
2417                 {
2418                         arm7_9_disable_sw_bkpts(target);
2419                 }
2420                 else
2421                 {
2422                         arm7_9->sw_bkpts_enabled = 0;
2423                 }
2424         }
2425         else
2426         {
2427                 command_print(cmd_ctx, "usage: arm7_9 sw_bkpts <enable|disable>");
2428         }
2429
2430         command_print(cmd_ctx, "software breakpoints %s", (arm7_9->sw_bkpts_enabled) ? "enabled" : "disabled");
2431
2432         return ERROR_OK;
2433 }
2434
2435 int handle_arm7_9_force_hw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2436 {
2437         target_t *target = get_current_target(cmd_ctx);
2438         armv4_5_common_t *armv4_5;
2439         arm7_9_common_t *arm7_9;
2440
2441         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2442         {
2443                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2444                 return ERROR_OK;
2445         }
2446
2447         if ((argc >= 1) && (strcmp("enable", args[0]) == 0))
2448         {
2449                 arm7_9->force_hw_bkpts = 1;
2450                 if (arm7_9->sw_bkpts_use_wp)
2451                 {
2452                         arm7_9_disable_sw_bkpts(target);
2453                 }
2454         }
2455         else if ((argc >= 1) && (strcmp("disable", args[0]) == 0))
2456         {
2457                 arm7_9->force_hw_bkpts = 0;
2458         }
2459         else
2460         {
2461                 command_print(cmd_ctx, "usage: arm7_9 force_hw_bkpts <enable|disable>");
2462         }
2463
2464         command_print(cmd_ctx, "force hardware breakpoints %s", (arm7_9->force_hw_bkpts) ? "enabled" : "disabled");
2465
2466         return ERROR_OK;
2467 }
2468
2469 int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2470 {
2471         target_t *target = get_current_target(cmd_ctx);
2472         armv4_5_common_t *armv4_5;
2473         arm7_9_common_t *arm7_9;
2474
2475         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2476         {
2477                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2478                 return ERROR_OK;
2479         }
2480
2481         if (argc > 0)
2482         {
2483                 if (strcmp("enable", args[0]) == 0)
2484                 {
2485                         arm7_9->use_dbgrq = 1;
2486                 }
2487                 else if (strcmp("disable", args[0]) == 0)
2488                 {
2489                         arm7_9->use_dbgrq = 0;
2490                 }
2491                 else
2492                 {
2493                         command_print(cmd_ctx, "usage: arm7_9 dbgrq <enable|disable>");
2494                 }
2495         }
2496
2497         command_print(cmd_ctx, "use of EmbeddedICE dbgrq instead of breakpoint for target halt %s", (arm7_9->use_dbgrq) ? "enabled" : "disabled");
2498
2499         return ERROR_OK;
2500 }
2501
2502 int handle_arm7_9_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2503 {
2504         target_t *target = get_current_target(cmd_ctx);
2505         armv4_5_common_t *armv4_5;
2506         arm7_9_common_t *arm7_9;
2507
2508         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2509         {
2510                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2511                 return ERROR_OK;
2512         }
2513
2514         if (argc > 0)
2515         {
2516                 if (strcmp("enable", args[0]) == 0)
2517                 {
2518                         arm7_9->fast_memory_access = 1;
2519                 }
2520                 else if (strcmp("disable", args[0]) == 0)
2521                 {
2522                         arm7_9->fast_memory_access = 0;
2523                 }
2524                 else
2525                 {
2526                         command_print(cmd_ctx, "usage: arm7_9 fast_memory_access <enable|disable>");
2527                 }
2528         }
2529
2530         command_print(cmd_ctx, "fast memory access is %s", (arm7_9->fast_memory_access) ? "enabled" : "disabled");
2531
2532         return ERROR_OK;
2533 }
2534
2535 int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2536 {
2537         target_t *target = get_current_target(cmd_ctx);
2538         armv4_5_common_t *armv4_5;
2539         arm7_9_common_t *arm7_9;
2540
2541         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2542         {
2543                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2544                 return ERROR_OK;
2545         }
2546
2547         if (argc > 0)
2548         {
2549                 if (strcmp("enable", args[0]) == 0)
2550                 {
2551                         arm7_9->dcc_downloads = 1;
2552                 }
2553                 else if (strcmp("disable", args[0]) == 0)
2554                 {
2555                         arm7_9->dcc_downloads = 0;
2556                 }
2557                 else
2558                 {
2559                         command_print(cmd_ctx, "usage: arm7_9 dcc_downloads <enable|disable>");
2560                 }
2561         }
2562
2563         command_print(cmd_ctx, "dcc downloads are %s", (arm7_9->dcc_downloads) ? "enabled" : "disabled");
2564
2565         return ERROR_OK;
2566 }
2567
2568 int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9)
2569 {
2570         armv4_5_common_t *armv4_5 = &arm7_9->armv4_5_common;
2571
2572         arm7_9->common_magic = ARM7_9_COMMON_MAGIC;
2573
2574         arm_jtag_setup_connection(&arm7_9->jtag_info);
2575         arm7_9->wp_available = 2;
2576         arm7_9->wp0_used = 0;
2577         arm7_9->wp1_used = 0;
2578         arm7_9->force_hw_bkpts = 0;
2579         arm7_9->use_dbgrq = 0;
2580
2581         arm7_9->etm_ctx = NULL;
2582         arm7_9->has_single_step = 0;
2583         arm7_9->has_monitor_mode = 0;
2584         arm7_9->has_vector_catch = 0;
2585
2586         arm7_9->reinit_embeddedice = 0;
2587
2588         arm7_9->debug_entry_from_reset = 0;
2589
2590         arm7_9->dcc_working_area = NULL;
2591
2592         arm7_9->fast_memory_access = 0;
2593         arm7_9->dcc_downloads = 0;
2594
2595         jtag_register_event_callback(arm7_9_jtag_callback, target);
2596
2597         armv4_5->arch_info = arm7_9;
2598         armv4_5->read_core_reg = arm7_9_read_core_reg;
2599         armv4_5->write_core_reg = arm7_9_write_core_reg;
2600         armv4_5->full_context = arm7_9_full_context;
2601
2602         armv4_5_init_arch_info(target, armv4_5);
2603
2604         target_register_timer_callback(arm7_9_handle_target_request, 1, 1, target);
2605
2606         return ERROR_OK;
2607 }