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