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