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