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 retval = ERROR_OK;
 659         u32 i;
 660
 661         data = malloc(size * (sizeof(u32)));
 662
 663         retval = embeddedice_receive(jtag_info, data, size);
 664
 665         for (i = 0; i < size; i++)
 666         {
 667                 h_u32_to_le(buffer + (i * 4), data[i]);
 668         }
 669
 670         free(data);
 671
 672         return retval;
 673 }
 674
 675 int arm7_9_handle_target_request(void *priv)
 676 {
 677         int retval = ERROR_OK;
 678         target_t *target = priv;
 679         if (!target->type->examined)
 680                 return ERROR_OK;
 681         armv4_5_common_t *armv4_5 = target->arch_info;
 682         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 683         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
 684         reg_t *dcc_control = &arm7_9->eice_cache->reg_list[EICE_COMMS_CTRL];
 685
 686         if (!target->dbg_msg_enabled)
 687                 return ERROR_OK;
 688
 689         if (target->state == TARGET_RUNNING)
 690         {
 691                 /* read DCC control register */
 692                 embeddedice_read_reg(dcc_control);
 693                 if ((retval = jtag_execute_queue()) != ERROR_OK)
 694                 {
 695                         return retval;
 696                 }
 697
 698                 /* check W bit */
 699                 if (buf_get_u32(dcc_control->value, 1, 1) == 1)
 700                 {
 701                         u32 request;
 702
 703                         if ((retval = embeddedice_receive(jtag_info, &request, 1)) != ERROR_OK)
 704                         {
 705                                 return retval;
 706                         }
 707                         if ((retval = target_request(target, request)) != ERROR_OK)
 708                         {
 709                                 return retval;
 710                         }
 711                 }
 712         }
 713
 714         return ERROR_OK;
 715 }
 716
 717 int arm7_9_poll(target_t *target)
 718 {
 719         int retval;
 720         armv4_5_common_t *armv4_5 = target->arch_info;
 721         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 722         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 723
 724         /* read debug status register */
 725         embeddedice_read_reg(dbg_stat);
 726         if ((retval = jtag_execute_queue()) != ERROR_OK)
 727         {
 728                 return retval;
 729         }
 730
 731         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
 732         {
 733 /*              LOG_DEBUG("DBGACK set, dbg_state->value: 0x%x", buf_get_u32(dbg_stat->value, 0, 32));*/
 734                 if (target->state == TARGET_UNKNOWN)
 735                 {
 736                         target->state = TARGET_RUNNING;
 737                         LOG_WARNING("DBGACK set while target was in unknown state. Reset or initialize target.");
 738                 }
 739                 if ((target->state == TARGET_RUNNING) || (target->state == TARGET_RESET))
 740                 {
 741                         int check_pc=0;
 742                         if (target->state == TARGET_RESET)
 743                         {
 744                                 if (target->reset_halt)
 745                                 {
 746                                         if ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0)
 747                                         {
 748                                                 check_pc = 1;
 749                                         }
 750                                 }
 751                         }
 752
 753                         target->state = TARGET_HALTED;
 754
 755                         if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
 756                                 return retval;
 757
 758                         if (check_pc)
 759                         {
 760                                 reg_t *reg = register_get_by_name(target->reg_cache, "pc", 1);
 761                                 u32 t=*((u32 *)reg->value);
 762                                 if (t!=0)
 763                                 {
 764                                         LOG_ERROR("PC was not 0. Does this target need srst_pulls_trst?");
 765                                 }
 766                         }
 767
 768                         if ((retval = target_call_event_callbacks(target, TARGET_EVENT_HALTED)) != ERROR_OK)
 769                         {
 770                                 return retval;
 771                         }
 772                 }
 773                 if (target->state == TARGET_DEBUG_RUNNING)
 774                 {
 775                         target->state = TARGET_HALTED;
 776                         if ((retval = arm7_9_debug_entry(target)) != ERROR_OK)
 777                                 return retval;
 778
 779                         if ((retval = target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED)) != ERROR_OK)
 780                         {
 781                                 return retval;
 782                         }
 783                 }
 784                 if (target->state != TARGET_HALTED)
 785                 {
 786                         LOG_WARNING("DBGACK set, but the target did not end up in the halted stated %d", target->state);
 787                 }
 788         }
 789         else
 790         {
 791                 if (target->state != TARGET_DEBUG_RUNNING)
 792                         target->state = TARGET_RUNNING;
 793         }
 794
 795         return ERROR_OK;
 796 }
 797
 798 /*
 799   Some -S targets (ARM966E-S in the STR912 isn't affected, ARM926EJ-S
 800   in the LPC3180 and AT91SAM9260 is affected) completely stop the JTAG clock
 801   while the core is held in reset(SRST). It isn't possible to program the halt
 802   condition once reset was asserted, hence a hook that allows the target to set
 803   up its reset-halt condition prior to asserting reset.
 804 */
 805
 806 int arm7_9_assert_reset(target_t *target)
 807 {
 808         armv4_5_common_t *armv4_5 = target->arch_info;
 809         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 810         LOG_DEBUG("target->state: %s",
 811                   Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
 812
 813         if (!(jtag_reset_config & RESET_HAS_SRST))
 814         {
 815                 LOG_ERROR("Can't assert SRST");
 816                 return ERROR_FAIL;
 817         }
 818
 819         if (target->reset_halt)
 820         {
 821                 /*
 822                  * Some targets do not support communication while SRST is asserted. We need to
 823                  * set up the reset vector catch here.
 824                  *
 825                  * If TRST is asserted, then these settings will be reset anyway, so setting them
 826                  * here is harmless.
 827                  */
 828                 if (arm7_9->has_vector_catch)
 829                 {
 830                         /* program vector catch register to catch reset vector */
 831                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH], 0x1);
 832                 }
 833                 else
 834                 {
 835                         /* program watchpoint unit to match on reset vector address */
 836                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE], 0x0);
 837                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0x3);
 838                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
 839                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
 840                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
 841                 }
 842         }
 843
 844         /* here we should issue a srst only, but we may have to assert trst as well */
 845         if (jtag_reset_config & RESET_SRST_PULLS_TRST)
 846         {
 847                 jtag_add_reset(1, 1);
 848         } else
 849         {
 850                 jtag_add_reset(0, 1);
 851         }
 852
 853         target->state = TARGET_RESET;
 854         jtag_add_sleep(50000);
 855
 856         armv4_5_invalidate_core_regs(target);
 857
 858         if ((target->reset_halt)&&((jtag_reset_config & RESET_SRST_PULLS_TRST)==0))
 859         {
 860                 /* debug entry was already prepared in arm7_9_assert_reset() */
 861                 target->debug_reason = DBG_REASON_DBGRQ;
 862         }
 863
 864         return ERROR_OK;
 865 }
 866
 867 int arm7_9_deassert_reset(target_t *target)
 868 {
 869         int retval=ERROR_OK;
 870         LOG_DEBUG("target->state: %s",
 871                 Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
 872
 873         /* deassert reset lines */
 874         jtag_add_reset(0, 0);
 875
 876         if (target->reset_halt&&(jtag_reset_config & RESET_SRST_PULLS_TRST)!=0)
 877         {
 878                 LOG_WARNING("srst pulls trst - can not reset into halted mode. Issuing halt after reset.");
 879                 /* set up embedded ice registers again */
 880                 if ((retval=target->type->examine(target))!=ERROR_OK)
 881                         return retval;
 882
 883                 if ((retval=target_poll(target))!=ERROR_OK)
 884                 {
 885                         return retval;
 886                 }
 887
 888                 if ((retval=target_halt(target))!=ERROR_OK)
 889                 {
 890                         return retval;
 891                 }
 892
 893         }
 894         return retval;
 895 }
 896
 897 int arm7_9_clear_halt(target_t *target)
 898 {
 899         armv4_5_common_t *armv4_5 = target->arch_info;
 900         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 901         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 902
 903         /* we used DBGRQ only if we didn't come out of reset */
 904         if (!arm7_9->debug_entry_from_reset && arm7_9->use_dbgrq)
 905         {
 906                 /* program EmbeddedICE Debug Control Register to deassert DBGRQ
 907                  */
 908                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
 909                 embeddedice_store_reg(dbg_ctrl);
 910         }
 911         else
 912         {
 913                 if (arm7_9->debug_entry_from_reset && arm7_9->has_vector_catch)
 914                 {
 915                         /* if we came out of reset, and vector catch is supported, we used
 916                          * vector catch to enter debug state
 917                          * restore the register in that case
 918                          */
 919                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_VEC_CATCH]);
 920                 }
 921                 else
 922                 {
 923                         /* restore registers if watchpoint unit 0 was in use
 924                          */
 925                         if (arm7_9->wp0_used)
 926                         {
 927                                 if (arm7_9->debug_entry_from_reset)
 928                                 {
 929                                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_VALUE]);
 930                                 }
 931                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
 932                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
 933                                 embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
 934                         }
 935                         /* control value always has to be restored, as it was either disabled,
 936                          * or enabled with possibly different bits
 937                          */
 938                         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
 939                 }
 940         }
 941
 942         return ERROR_OK;
 943 }
 944
 945 int arm7_9_soft_reset_halt(struct target_s *target)
 946 {
 947         armv4_5_common_t *armv4_5 = target->arch_info;
 948         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
 949         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
 950         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
 951         int i;
 952         int retval;
 953
 954         if ((retval=target_halt(target))!=ERROR_OK)
 955                 return retval;
 956
 957         long long then=timeval_ms();
 958         int timeout;
 959         while (!(timeout=((timeval_ms()-then)>1000)))
 960         {
 961                 if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) != 0)
 962                         break;
 963                 embeddedice_read_reg(dbg_stat);
 964                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 965                         return retval;
 966                 if (debug_level>=3)
 967                 {
 968                         alive_sleep(100);
 969                 } else
 970                 {
 971                         keep_alive();
 972                 }
 973         }
 974         if (timeout)
 975         {
 976                 LOG_ERROR("Failed to halt CPU after 1 sec");
 977                 return ERROR_TARGET_TIMEOUT;
 978         }
 979         target->state = TARGET_HALTED;
 980
 981         /* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
 982          * ensure that DBGRQ is cleared
 983          */
 984         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
 985         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
 986         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
 987         embeddedice_store_reg(dbg_ctrl);
 988
 989         if ((retval = arm7_9_clear_halt(target)) != ERROR_OK)
 990         {
 991                 return retval;
 992         }
 993
 994         /* if the target is in Thumb state, change to ARM state */
 995         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
 996         {
 997                 u32 r0_thumb, pc_thumb;
 998                 LOG_DEBUG("target entered debug from Thumb state, changing to ARM");
 999                 /* Entered debug from Thumb mode */
1000                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
1001                 arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
1002         }
1003
1004         /* all register content is now invalid */
1005         if ((retval = armv4_5_invalidate_core_regs(target)) != ERROR_OK)
1006         {
1007                 return retval;
1008         }
1009
1010         /* SVC, ARM state, IRQ and FIQ disabled */
1011         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
1012         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
1013         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1014
1015         /* start fetching from 0x0 */
1016         buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
1017         armv4_5->core_cache->reg_list[15].dirty = 1;
1018         armv4_5->core_cache->reg_list[15].valid = 1;
1019
1020         armv4_5->core_mode = ARMV4_5_MODE_SVC;
1021         armv4_5->core_state = ARMV4_5_STATE_ARM;
1022
1023         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1024                 return ERROR_FAIL;
1025
1026         /* reset registers */
1027         for (i = 0; i <= 14; i++)
1028         {
1029                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, 0xffffffff);
1030                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 1;
1031                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
1032         }
1033
1034         if ((retval = target_call_event_callbacks(target, TARGET_EVENT_HALTED)) != ERROR_OK)
1035         {
1036                 return retval;
1037         }
1038
1039         return ERROR_OK;
1040 }
1041
1042 int arm7_9_halt(target_t *target)
1043 {
1044         if (target->state==TARGET_RESET)
1045         {
1046                 LOG_ERROR("BUG: arm7/9 does not support halt during reset. This is handled in arm7_9_assert_reset()");
1047                 return ERROR_OK;
1048         }
1049
1050         armv4_5_common_t *armv4_5 = target->arch_info;
1051         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1052         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1053
1054         LOG_DEBUG("target->state: %s",
1055                   Jim_Nvp_value2name_simple( nvp_target_state,target->state)->name);
1056
1057         if (target->state == TARGET_HALTED)
1058         {
1059                 LOG_DEBUG("target was already halted");
1060                 return ERROR_OK;
1061         }
1062
1063         if (target->state == TARGET_UNKNOWN)
1064         {
1065                 LOG_WARNING("target was in unknown state when halt was requested");
1066         }
1067
1068         if (arm7_9->use_dbgrq)
1069         {
1070                 /* program EmbeddedICE Debug Control Register to assert DBGRQ
1071                  */
1072                 if (arm7_9->set_special_dbgrq) {
1073                         arm7_9->set_special_dbgrq(target);
1074                 } else {
1075                         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 1);
1076                         embeddedice_store_reg(dbg_ctrl);
1077                 }
1078         }
1079         else
1080         {
1081                 /* program watchpoint unit to match on any address
1082                  */
1083                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1084                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1085                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
1086                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1087         }
1088
1089         target->debug_reason = DBG_REASON_DBGRQ;
1090
1091         return ERROR_OK;
1092 }
1093
1094 int arm7_9_debug_entry(target_t *target)
1095 {
1096         int i;
1097         u32 context[16];
1098         u32* context_p[16];
1099         u32 r0_thumb, pc_thumb;
1100         u32 cpsr;
1101         int retval;
1102         /* get pointers to arch-specific information */
1103         armv4_5_common_t *armv4_5 = target->arch_info;
1104         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1105         reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
1106         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1107
1108 #ifdef _DEBUG_ARM7_9_
1109         LOG_DEBUG("-");
1110 #endif
1111
1112         if (arm7_9->pre_debug_entry)
1113                 arm7_9->pre_debug_entry(target);
1114
1115         /* program EmbeddedICE Debug Control Register to assert DBGACK and INTDIS
1116          * ensure that DBGRQ is cleared
1117          */
1118         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
1119         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGRQ, 1, 0);
1120         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 1);
1121         embeddedice_store_reg(dbg_ctrl);
1122
1123         if ((retval = arm7_9_clear_halt(target)) != ERROR_OK)
1124         {
1125                 return retval;
1126         }
1127
1128         if ((retval = jtag_execute_queue()) != ERROR_OK)
1129         {
1130                 return retval;
1131         }
1132
1133         if ((retval = arm7_9->examine_debug_reason(target)) != ERROR_OK)
1134                 return retval;
1135
1136
1137         if (target->state != TARGET_HALTED)
1138         {
1139                 LOG_WARNING("target not halted");
1140                 return ERROR_TARGET_NOT_HALTED;
1141         }
1142
1143         /* if the target is in Thumb state, change to ARM state */
1144         if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_ITBIT, 1))
1145         {
1146                 LOG_DEBUG("target entered debug from Thumb state");
1147                 /* Entered debug from Thumb mode */
1148                 armv4_5->core_state = ARMV4_5_STATE_THUMB;
1149                 arm7_9->change_to_arm(target, &r0_thumb, &pc_thumb);
1150                 LOG_DEBUG("r0_thumb: 0x%8.8x, pc_thumb: 0x%8.8x", r0_thumb, pc_thumb);
1151         }
1152         else
1153         {
1154                 LOG_DEBUG("target entered debug from ARM state");
1155                 /* Entered debug from ARM mode */
1156                 armv4_5->core_state = ARMV4_5_STATE_ARM;
1157         }
1158
1159         for (i = 0; i < 16; i++)
1160                 context_p[i] = &context[i];
1161         /* save core registers (r0 - r15 of current core mode) */
1162         arm7_9->read_core_regs(target, 0xffff, context_p);
1163
1164         arm7_9->read_xpsr(target, &cpsr, 0);
1165
1166         if ((retval = jtag_execute_queue()) != ERROR_OK)
1167                 return retval;
1168
1169         /* if the core has been executing in Thumb state, set the T bit */
1170         if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1171                 cpsr |= 0x20;
1172
1173         buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, cpsr);
1174         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
1175         armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1176
1177         armv4_5->core_mode = cpsr & 0x1f;
1178
1179         if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
1180         {
1181                 target->state = TARGET_UNKNOWN;
1182                 LOG_ERROR("cpsr contains invalid mode value - communication failure");
1183                 return ERROR_TARGET_FAILURE;
1184         }
1185
1186         LOG_DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);
1187
1188         if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1189         {
1190                 LOG_DEBUG("thumb state, applying fixups");
1191                 context[0] = r0_thumb;
1192                 context[15] = pc_thumb;
1193         } else if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1194         {
1195                 /* adjust value stored by STM */
1196                 context[15] -= 3 * 4;
1197         }
1198
1199         if ((target->debug_reason == DBG_REASON_BREAKPOINT)
1200                         || (target->debug_reason == DBG_REASON_SINGLESTEP)
1201                         || (target->debug_reason == DBG_REASON_WATCHPOINT)
1202                         || (target->debug_reason == DBG_REASON_WPTANDBKPT)
1203                         || ((target->debug_reason == DBG_REASON_DBGRQ) && (arm7_9->use_dbgrq == 0)))
1204                 context[15] -= 3 * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
1205         else if (target->debug_reason == DBG_REASON_DBGRQ)
1206                 context[15] -= arm7_9->dbgreq_adjust_pc * ((armv4_5->core_state == ARMV4_5_STATE_ARM) ? 4 : 2);
1207         else
1208         {
1209                 LOG_ERROR("unknown debug reason: %i", target->debug_reason);
1210         }
1211
1212         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1213                 return ERROR_FAIL;
1214
1215         for (i=0; i<=15; i++)
1216         {
1217                 LOG_DEBUG("r%i: 0x%8.8x", i, context[i]);
1218                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, context[i]);
1219                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;
1220                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
1221         }
1222
1223         LOG_DEBUG("entered debug state at PC 0x%x", context[15]);
1224
1225         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1226                 return ERROR_FAIL;
1227
1228         /* exceptions other than USR & SYS have a saved program status register */
1229         if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))
1230         {
1231                 u32 spsr;
1232                 arm7_9->read_xpsr(target, &spsr, 1);
1233                 if ((retval = jtag_execute_queue()) != ERROR_OK)
1234                 {
1235                         return retval;
1236                 }
1237                 buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, spsr);
1238                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;
1239                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;
1240         }
1241
1242         /* r0 and r15 (pc) have to be restored later */
1243         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;
1244         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;
1245
1246         if ((retval = jtag_execute_queue()) != ERROR_OK)
1247                 return retval;
1248
1249         if (arm7_9->post_debug_entry)
1250                 arm7_9->post_debug_entry(target);
1251
1252         return ERROR_OK;
1253 }
1254
1255 int arm7_9_full_context(target_t *target)
1256 {
1257         int i;
1258         int retval;
1259         armv4_5_common_t *armv4_5 = target->arch_info;
1260         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1261
1262         LOG_DEBUG("-");
1263
1264         if (target->state != TARGET_HALTED)
1265         {
1266                 LOG_WARNING("target not halted");
1267                 return ERROR_TARGET_NOT_HALTED;
1268         }
1269
1270         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1271                 return ERROR_FAIL;
1272
1273         /* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
1274          * SYS shares registers with User, so we don't touch SYS
1275          */
1276         for(i = 0; i < 6; i++)
1277         {
1278                 u32 mask = 0;
1279                 u32* reg_p[16];
1280                 int j;
1281                 int valid = 1;
1282
1283                 /* check if there are invalid registers in the current mode
1284                  */
1285                 for (j = 0; j <= 16; j++)
1286                 {
1287                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1288                                 valid = 0;
1289                 }
1290
1291                 if (!valid)
1292                 {
1293                         u32 tmp_cpsr;
1294
1295                         /* change processor mode (and mask T bit) */
1296                         tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1297                         tmp_cpsr |= armv4_5_number_to_mode(i);
1298                         tmp_cpsr &= ~0x20;
1299                         arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1300
1301                         for (j = 0; j < 15; j++)
1302                         {
1303                                 if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
1304                                 {
1305                                         reg_p[j] = (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value;
1306                                         mask |= 1 << j;
1307                                         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;
1308                                         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
1309                                 }
1310                         }
1311
1312                         /* if only the PSR is invalid, mask is all zeroes */
1313                         if (mask)
1314                                 arm7_9->read_core_regs(target, mask, reg_p);
1315
1316                         /* check if the PSR has to be read */
1317                         if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid == 0)
1318                         {
1319                                 arm7_9->read_xpsr(target, (u32*)ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).value, 1);
1320                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;
1321                                 ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
1322                         }
1323                 }
1324         }
1325
1326         /* restore processor mode (mask T bit) */
1327         arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1328
1329         if ((retval = jtag_execute_queue()) != ERROR_OK)
1330         {
1331                 return retval;
1332         }
1333         return ERROR_OK;
1334 }
1335
1336 int arm7_9_restore_context(target_t *target)
1337 {
1338         armv4_5_common_t *armv4_5 = target->arch_info;
1339         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1340         reg_t *reg;
1341         armv4_5_core_reg_t *reg_arch_info;
1342         enum armv4_5_mode current_mode = armv4_5->core_mode;
1343         int i, j;
1344         int dirty;
1345         int mode_change;
1346
1347         LOG_DEBUG("-");
1348
1349         if (target->state != TARGET_HALTED)
1350         {
1351                 LOG_WARNING("target not halted");
1352                 return ERROR_TARGET_NOT_HALTED;
1353         }
1354
1355         if (arm7_9->pre_restore_context)
1356                 arm7_9->pre_restore_context(target);
1357
1358         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1359                 return ERROR_FAIL;
1360
1361         /* iterate through processor modes (User, FIQ, IRQ, SVC, ABT, UND)
1362          * SYS shares registers with User, so we don't touch SYS
1363          */
1364         for (i = 0; i < 6; i++)
1365         {
1366                 LOG_DEBUG("examining %s mode", armv4_5_mode_strings[i]);
1367                 dirty = 0;
1368                 mode_change = 0;
1369                 /* check if there are dirty registers in the current mode
1370                 */
1371                 for (j = 0; j <= 16; j++)
1372                 {
1373                         reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
1374                         reg_arch_info = reg->arch_info;
1375                         if (reg->dirty == 1)
1376                         {
1377                                 if (reg->valid == 1)
1378                                 {
1379                                         dirty = 1;
1380                                         LOG_DEBUG("examining dirty reg: %s", reg->name);
1381                                         if ((reg_arch_info->mode != ARMV4_5_MODE_ANY)
1382                                                 && (reg_arch_info->mode != current_mode)
1383                                                 && !((reg_arch_info->mode == ARMV4_5_MODE_USR) && (armv4_5->core_mode == ARMV4_5_MODE_SYS))
1384                                                 && !((reg_arch_info->mode == ARMV4_5_MODE_SYS) && (armv4_5->core_mode == ARMV4_5_MODE_USR)))
1385                                         {
1386                                                 mode_change = 1;
1387                                                 LOG_DEBUG("require mode change");
1388                                         }
1389                                 }
1390                                 else
1391                                 {
1392                                         LOG_ERROR("BUG: dirty register '%s', but no valid data", reg->name);
1393                                 }
1394                         }
1395                 }
1396
1397                 if (dirty)
1398                 {
1399                         u32 mask = 0x0;
1400                         int num_regs = 0;
1401                         u32 regs[16];
1402
1403                         if (mode_change)
1404                         {
1405                                 u32 tmp_cpsr;
1406
1407                                 /* change processor mode (mask T bit) */
1408                                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1409                                 tmp_cpsr |= armv4_5_number_to_mode(i);
1410                                 tmp_cpsr &= ~0x20;
1411                                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1412                                 current_mode = armv4_5_number_to_mode(i);
1413                         }
1414
1415                         for (j = 0; j <= 14; j++)
1416                         {
1417                                 reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j);
1418                                 reg_arch_info = reg->arch_info;
1419
1420
1421                                 if (reg->dirty == 1)
1422                                 {
1423                                         regs[j] = buf_get_u32(reg->value, 0, 32);
1424                                         mask |= 1 << j;
1425                                         num_regs++;
1426                                         reg->dirty = 0;
1427                                         reg->valid = 1;
1428                                         LOG_DEBUG("writing register %i of mode %s with value 0x%8.8x", j, armv4_5_mode_strings[i], regs[j]);
1429                                 }
1430                         }
1431
1432                         if (mask)
1433                         {
1434                                 arm7_9->write_core_regs(target, mask, regs);
1435                         }
1436
1437                         reg = &ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16);
1438                         reg_arch_info = reg->arch_info;
1439                         if ((reg->dirty) && (reg_arch_info->mode != ARMV4_5_MODE_ANY))
1440                         {
1441                                 LOG_DEBUG("writing SPSR of mode %i with value 0x%8.8x", i, buf_get_u32(reg->value, 0, 32));
1442                                 arm7_9->write_xpsr(target, buf_get_u32(reg->value, 0, 32), 1);
1443                         }
1444                 }
1445         }
1446
1447         if ((armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 0) && (armv4_5->core_mode != current_mode))
1448         {
1449                 /* restore processor mode (mask T bit) */
1450                 u32 tmp_cpsr;
1451
1452                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1453                 tmp_cpsr |= armv4_5_number_to_mode(i);
1454                 tmp_cpsr &= ~0x20;
1455                 LOG_DEBUG("writing lower 8 bit of cpsr with value 0x%2.2x", tmp_cpsr);
1456                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1457         }
1458         else if (armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty == 1)
1459         {
1460                 /* CPSR has been changed, full restore necessary (mask T bit) */
1461                 LOG_DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
1462                 arm7_9->write_xpsr(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32) & ~0x20, 0);
1463                 armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 0;
1464                 armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
1465         }
1466
1467         /* restore PC */
1468         LOG_DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1469         arm7_9->write_pc(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1470         armv4_5->core_cache->reg_list[15].dirty = 0;
1471
1472         if (arm7_9->post_restore_context)
1473                 arm7_9->post_restore_context(target);
1474
1475         return ERROR_OK;
1476 }
1477
1478 int arm7_9_restart_core(struct target_s *target)
1479 {
1480         armv4_5_common_t *armv4_5 = target->arch_info;
1481         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1482         arm_jtag_t *jtag_info = &arm7_9->jtag_info;
1483
1484         /* set RESTART instruction */
1485         jtag_add_end_state(TAP_IDLE);
1486         if (arm7_9->need_bypass_before_restart) {
1487                 arm7_9->need_bypass_before_restart = 0;
1488                 arm_jtag_set_instr(jtag_info, 0xf, NULL);
1489         }
1490         arm_jtag_set_instr(jtag_info, 0x4, NULL);
1491
1492         jtag_add_runtest(1, TAP_IDLE);
1493         return jtag_execute_queue();
1494 }
1495
1496 void arm7_9_enable_watchpoints(struct target_s *target)
1497 {
1498         watchpoint_t *watchpoint = target->watchpoints;
1499
1500         while (watchpoint)
1501         {
1502                 if (watchpoint->set == 0)
1503                         arm7_9_set_watchpoint(target, watchpoint);
1504                 watchpoint = watchpoint->next;
1505         }
1506 }
1507
1508 void arm7_9_enable_breakpoints(struct target_s *target)
1509 {
1510         breakpoint_t *breakpoint = target->breakpoints;
1511
1512         /* set any pending breakpoints */
1513         while (breakpoint)
1514         {
1515                 arm7_9_set_breakpoint(target, breakpoint);
1516                 breakpoint = breakpoint->next;
1517         }
1518 }
1519
1520 int arm7_9_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
1521 {
1522         armv4_5_common_t *armv4_5 = target->arch_info;
1523         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1524         breakpoint_t *breakpoint = target->breakpoints;
1525         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
1526         int err, retval = ERROR_OK;
1527
1528         LOG_DEBUG("-");
1529
1530         if (target->state != TARGET_HALTED)
1531         {
1532                 LOG_WARNING("target not halted");
1533                 return ERROR_TARGET_NOT_HALTED;
1534         }
1535
1536         if (!debug_execution)
1537         {
1538                 target_free_all_working_areas(target);
1539         }
1540
1541         /* current = 1: continue on current pc, otherwise continue at <address> */
1542         if (!current)
1543                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1544
1545         u32 current_pc;
1546         current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1547
1548         /* the front-end may request us not to handle breakpoints */
1549         if (handle_breakpoints)
1550         {
1551                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1552                 {
1553                         LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
1554                         if ((retval = arm7_9_unset_breakpoint(target, breakpoint)) != ERROR_OK)
1555                         {
1556                                 return retval;
1557                         }
1558
1559                         /* calculate PC of next instruction */
1560                         u32 next_pc;
1561                         if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)
1562                         {
1563                                 u32 current_opcode;
1564                                 target_read_u32(target, current_pc, &current_opcode);
1565                                 LOG_ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);
1566                                 return retval;
1567                         }
1568
1569                         LOG_DEBUG("enable single-step");
1570                         arm7_9->enable_single_step(target, next_pc);
1571
1572                         target->debug_reason = DBG_REASON_SINGLESTEP;
1573
1574                         if ((retval = arm7_9_restore_context(target)) != ERROR_OK)
1575                         {
1576                                 return retval;
1577                         }
1578
1579                         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1580                                 arm7_9->branch_resume(target);
1581                         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1582                         {
1583                                 arm7_9->branch_resume_thumb(target);
1584                         }
1585                         else
1586                         {
1587                                 LOG_ERROR("unhandled core state");
1588                                 return ERROR_FAIL;
1589                         }
1590
1591                         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1592                         embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1593                         err = arm7_9_execute_sys_speed(target);
1594
1595                         LOG_DEBUG("disable single-step");
1596                         arm7_9->disable_single_step(target);
1597
1598                         if (err != ERROR_OK)
1599                         {
1600                                 if ((retval = arm7_9_set_breakpoint(target, breakpoint)) != ERROR_OK)
1601                                 {
1602                                         return retval;
1603                                 }
1604                                 target->state = TARGET_UNKNOWN;
1605                                 return err;
1606                         }
1607
1608                         arm7_9_debug_entry(target);
1609                         LOG_DEBUG("new PC after step: 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
1610
1611                         LOG_DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
1612                         if ((retval = arm7_9_set_breakpoint(target, breakpoint)) != ERROR_OK)
1613                         {
1614                                 return retval;
1615                         }
1616                 }
1617         }
1618
1619         /* enable any pending breakpoints and watchpoints */
1620         arm7_9_enable_breakpoints(target);
1621         arm7_9_enable_watchpoints(target);
1622
1623         if ((retval = arm7_9_restore_context(target)) != ERROR_OK)
1624         {
1625                 return retval;
1626         }
1627
1628         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1629         {
1630                 arm7_9->branch_resume(target);
1631         }
1632         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1633         {
1634                 arm7_9->branch_resume_thumb(target);
1635         }
1636         else
1637         {
1638                 LOG_ERROR("unhandled core state");
1639                 return ERROR_FAIL;
1640         }
1641
1642         /* deassert DBGACK and INTDIS */
1643         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
1644         /* INTDIS only when we really resume, not during debug execution */
1645         if (!debug_execution)
1646                 buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_INTDIS, 1, 0);
1647         embeddedice_write_reg(dbg_ctrl, buf_get_u32(dbg_ctrl->value, 0, dbg_ctrl->size));
1648
1649         if ((retval = arm7_9_restart_core(target)) != ERROR_OK)
1650         {
1651                 return retval;
1652         }
1653
1654         target->debug_reason = DBG_REASON_NOTHALTED;
1655
1656         if (!debug_execution)
1657         {
1658                 /* registers are now invalid */
1659                 armv4_5_invalidate_core_regs(target);
1660                 target->state = TARGET_RUNNING;
1661                 if ((retval = target_call_event_callbacks(target, TARGET_EVENT_RESUMED)) != ERROR_OK)
1662                 {
1663                         return retval;
1664                 }
1665         }
1666         else
1667         {
1668                 target->state = TARGET_DEBUG_RUNNING;
1669                 if ((retval = target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED)) != ERROR_OK)
1670                 {
1671                         return retval;
1672                 }
1673         }
1674
1675         LOG_DEBUG("target resumed");
1676
1677         return ERROR_OK;
1678 }
1679
1680 void arm7_9_enable_eice_step(target_t *target, u32 next_pc)
1681 {
1682         armv4_5_common_t *armv4_5 = target->arch_info;
1683         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1684
1685         u32 current_pc;
1686         current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1687
1688         if(next_pc != current_pc)
1689         {
1690                 /* setup an inverse breakpoint on the current PC
1691                 * - comparator 1 matches the current address
1692                 * - rangeout from comparator 1 is connected to comparator 0 rangein
1693                 * - comparator 0 matches any address, as long as rangein is low */
1694                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1695                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1696                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
1697                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], ~(EICE_W_CTRL_RANGE|EICE_W_CTRL_nOPC) & 0xff);
1698                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], current_pc);
1699                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0);
1700                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffff);
1701                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], 0x0);
1702                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1703         }
1704         else
1705         {
1706                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK], 0xffffffff);
1707                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK], 0xffffffff);
1708                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE], 0x0);
1709                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK], 0xff);
1710                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE], next_pc);
1711                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK], 0);
1712                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK], 0xffffffff);
1713                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE], EICE_W_CTRL_ENABLE);
1714                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK], ~EICE_W_CTRL_nOPC & 0xff);
1715         }
1716 }
1717
1718 void arm7_9_disable_eice_step(target_t *target)
1719 {
1720         armv4_5_common_t *armv4_5 = target->arch_info;
1721         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1722
1723         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_ADDR_MASK]);
1724         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_DATA_MASK]);
1725         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_VALUE]);
1726         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W0_CONTROL_MASK]);
1727         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_VALUE]);
1728         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_ADDR_MASK]);
1729         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_DATA_MASK]);
1730         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_MASK]);
1731         embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE]);
1732 }
1733
1734 int arm7_9_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
1735 {
1736         armv4_5_common_t *armv4_5 = target->arch_info;
1737         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1738         breakpoint_t *breakpoint = NULL;
1739         int err, retval;
1740
1741         if (target->state != TARGET_HALTED)
1742         {
1743                 LOG_WARNING("target not halted");
1744                 return ERROR_TARGET_NOT_HALTED;
1745         }
1746
1747         /* current = 1: continue on current pc, otherwise continue at <address> */
1748         if (!current)
1749                 buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
1750
1751         u32 current_pc;
1752         current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
1753
1754         /* the front-end may request us not to handle breakpoints */
1755         if (handle_breakpoints)
1756                 if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
1757                         if ((retval = arm7_9_unset_breakpoint(target, breakpoint)) != ERROR_OK)
1758                         {
1759                                 return retval;
1760                         }
1761
1762         target->debug_reason = DBG_REASON_SINGLESTEP;
1763
1764         /* calculate PC of next instruction */
1765         u32 next_pc;
1766         if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)
1767         {
1768                 u32 current_opcode;
1769                 target_read_u32(target, current_pc, &current_opcode);
1770                 LOG_ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);
1771                 return retval;
1772         }
1773
1774         if ((retval = arm7_9_restore_context(target)) != ERROR_OK)
1775         {
1776                 return retval;
1777         }
1778
1779         arm7_9->enable_single_step(target, next_pc);
1780
1781         if (armv4_5->core_state == ARMV4_5_STATE_ARM)
1782         {
1783                 arm7_9->branch_resume(target);
1784         }
1785         else if (armv4_5->core_state == ARMV4_5_STATE_THUMB)
1786         {
1787                 arm7_9->branch_resume_thumb(target);
1788         }
1789         else
1790         {
1791                 LOG_ERROR("unhandled core state");
1792                 return ERROR_FAIL;
1793         }
1794
1795         if ((retval = target_call_event_callbacks(target, TARGET_EVENT_RESUMED)) != ERROR_OK)
1796         {
1797                 return retval;
1798         }
1799
1800         err = arm7_9_execute_sys_speed(target);
1801         arm7_9->disable_single_step(target);
1802
1803         /* registers are now invalid */
1804         armv4_5_invalidate_core_regs(target);
1805
1806         if (err != ERROR_OK)
1807         {
1808                 target->state = TARGET_UNKNOWN;
1809         } else {
1810                 arm7_9_debug_entry(target);
1811                 if ((retval = target_call_event_callbacks(target, TARGET_EVENT_HALTED)) != ERROR_OK)
1812                 {
1813                         return retval;
1814                 }
1815                 LOG_DEBUG("target stepped");
1816         }
1817
1818         if (breakpoint)
1819                 if ((retval = arm7_9_set_breakpoint(target, breakpoint)) != ERROR_OK)
1820                 {
1821                         return retval;
1822                 }
1823
1824         return err;
1825 }
1826
1827 int arm7_9_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
1828 {
1829         u32* reg_p[16];
1830         u32 value;
1831         int retval;
1832         armv4_5_common_t *armv4_5 = target->arch_info;
1833         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1834
1835         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1836                 return ERROR_FAIL;
1837
1838         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;
1839
1840         if ((num < 0) || (num > 16))
1841                 return ERROR_INVALID_ARGUMENTS;
1842
1843         if ((mode != ARMV4_5_MODE_ANY)
1844                         && (mode != armv4_5->core_mode)
1845                         && (reg_mode != ARMV4_5_MODE_ANY))
1846         {
1847                 u32 tmp_cpsr;
1848
1849                 /* change processor mode (mask T bit) */
1850                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1851                 tmp_cpsr |= mode;
1852                 tmp_cpsr &= ~0x20;
1853                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1854         }
1855
1856         if ((num >= 0) && (num <= 15))
1857         {
1858                 /* read a normal core register */
1859                 reg_p[num] = &value;
1860
1861                 arm7_9->read_core_regs(target, 1 << num, reg_p);
1862         }
1863         else
1864         {
1865                 /* read a program status register
1866                  * if the register mode is MODE_ANY, we read the cpsr, otherwise a spsr
1867                  */
1868                 armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
1869                 int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1870
1871                 arm7_9->read_xpsr(target, &value, spsr);
1872         }
1873
1874         if ((retval = jtag_execute_queue()) != ERROR_OK)
1875         {
1876                 return retval;
1877         }
1878
1879         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
1880         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1881         buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).value, 0, 32, value);
1882
1883         if ((mode != ARMV4_5_MODE_ANY)
1884                         && (mode != armv4_5->core_mode)
1885                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1886                 /* restore processor mode (mask T bit) */
1887                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1888         }
1889
1890         return ERROR_OK;
1891 }
1892
1893 int arm7_9_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)
1894 {
1895         u32 reg[16];
1896         armv4_5_common_t *armv4_5 = target->arch_info;
1897         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1898
1899         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
1900                 return ERROR_FAIL;
1901
1902         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;
1903
1904         if ((num < 0) || (num > 16))
1905                 return ERROR_INVALID_ARGUMENTS;
1906
1907         if ((mode != ARMV4_5_MODE_ANY)
1908                         && (mode != armv4_5->core_mode)
1909                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1910                 u32 tmp_cpsr;
1911
1912                 /* change processor mode (mask T bit) */
1913                 tmp_cpsr = buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & 0xE0;
1914                 tmp_cpsr |= mode;
1915                 tmp_cpsr &= ~0x20;
1916                 arm7_9->write_xpsr_im8(target, tmp_cpsr & 0xff, 0, 0);
1917         }
1918
1919         if ((num >= 0) && (num <= 15))
1920         {
1921                 /* write a normal core register */
1922                 reg[num] = value;
1923
1924                 arm7_9->write_core_regs(target, 1 << num, reg);
1925         }
1926         else
1927         {
1928                 /* write a program status register
1929                 * if the register mode is MODE_ANY, we write the cpsr, otherwise a spsr
1930                 */
1931                 armv4_5_core_reg_t *arch_info = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).arch_info;
1932                 int spsr = (arch_info->mode == ARMV4_5_MODE_ANY) ? 0 : 1;
1933
1934                 /* if we're writing the CPSR, mask the T bit */
1935                 if (!spsr)
1936                         value &= ~0x20;
1937
1938                 arm7_9->write_xpsr(target, value, spsr);
1939         }
1940
1941         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).valid = 1;
1942         ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, mode, num).dirty = 0;
1943
1944         if ((mode != ARMV4_5_MODE_ANY)
1945                         && (mode != armv4_5->core_mode)
1946                         && (reg_mode != ARMV4_5_MODE_ANY))      {
1947                 /* restore processor mode (mask T bit) */
1948                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
1949         }
1950
1951         return jtag_execute_queue();
1952 }
1953
1954 int arm7_9_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
1955 {
1956         armv4_5_common_t *armv4_5 = target->arch_info;
1957         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
1958
1959         u32 reg[16];
1960         u32 num_accesses = 0;
1961         int thisrun_accesses;
1962         int i;
1963         u32 cpsr;
1964         int retval;
1965         int last_reg = 0;
1966
1967         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
1968
1969         if (target->state != TARGET_HALTED)
1970         {
1971                 LOG_WARNING("target not halted");
1972                 return ERROR_TARGET_NOT_HALTED;
1973         }
1974
1975         /* sanitize arguments */
1976         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
1977                 return ERROR_INVALID_ARGUMENTS;
1978
1979         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
1980                 return ERROR_TARGET_UNALIGNED_ACCESS;
1981
1982         /* load the base register with the address of the first word */
1983         reg[0] = address;
1984         arm7_9->write_core_regs(target, 0x1, reg);
1985
1986         int j=0;
1987
1988         switch (size)
1989         {
1990                 case 4:
1991                         while (num_accesses < count)
1992                         {
1993                                 u32 reg_list;
1994                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
1995                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
1996
1997                                 if (last_reg <= thisrun_accesses)
1998                                         last_reg = thisrun_accesses;
1999
2000                                 arm7_9->load_word_regs(target, reg_list);
2001
2002                                 /* fast memory reads are only safe when the target is running
2003                                  * from a sufficiently high clock (32 kHz is usually too slow)
2004                                  */
2005                                 if (arm7_9->fast_memory_access)
2006                                         retval = arm7_9_execute_fast_sys_speed(target);
2007                                 else
2008                                         retval = arm7_9_execute_sys_speed(target);
2009                                 if (retval != ERROR_OK)
2010                                         return retval;
2011
2012                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 4);
2013
2014                                 /* advance buffer, count number of accesses */
2015                                 buffer += thisrun_accesses * 4;
2016                                 num_accesses += thisrun_accesses;
2017
2018                                 if ((j++%1024)==0)
2019                                 {
2020                                         keep_alive();
2021                                 }
2022                         }
2023                         break;
2024                 case 2:
2025                         while (num_accesses < count)
2026                         {
2027                                 u32 reg_list;
2028                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2029                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2030
2031                                 for (i = 1; i <= thisrun_accesses; i++)
2032                                 {
2033                                         if (i > last_reg)
2034                                                 last_reg = i;
2035                                         arm7_9->load_hword_reg(target, i);
2036                                         /* fast memory reads are only safe when the target is running
2037                                          * from a sufficiently high clock (32 kHz is usually too slow)
2038                                          */
2039                                         if (arm7_9->fast_memory_access)
2040                                                 retval = arm7_9_execute_fast_sys_speed(target);
2041                                         else
2042                                                 retval = arm7_9_execute_sys_speed(target);
2043                                         if(retval != ERROR_OK)
2044                                         {
2045                                                 return retval;
2046                                         }
2047
2048                                 }
2049
2050                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 2);
2051
2052                                 /* advance buffer, count number of accesses */
2053                                 buffer += thisrun_accesses * 2;
2054                                 num_accesses += thisrun_accesses;
2055
2056                                 if ((j++%1024)==0)
2057                                 {
2058                                         keep_alive();
2059                                 }
2060                         }
2061                         break;
2062                 case 1:
2063                         while (num_accesses < count)
2064                         {
2065                                 u32 reg_list;
2066                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2067                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2068
2069                                 for (i = 1; i <= thisrun_accesses; i++)
2070                                 {
2071                                         if (i > last_reg)
2072                                                 last_reg = i;
2073                                         arm7_9->load_byte_reg(target, i);
2074                                         /* fast memory reads are only safe when the target is running
2075                                          * from a sufficiently high clock (32 kHz is usually too slow)
2076                                          */
2077                                         if (arm7_9->fast_memory_access)
2078                                                 retval = arm7_9_execute_fast_sys_speed(target);
2079                                         else
2080                                                 retval = arm7_9_execute_sys_speed(target);
2081                                         if(retval != ERROR_OK)
2082                                         {
2083                                                 return retval;
2084                                         }
2085                                 }
2086
2087                                 arm7_9->read_core_regs_target_buffer(target, reg_list, buffer, 1);
2088
2089                                 /* advance buffer, count number of accesses */
2090                                 buffer += thisrun_accesses * 1;
2091                                 num_accesses += thisrun_accesses;
2092
2093                                 if ((j++%1024)==0)
2094                                 {
2095                                         keep_alive();
2096                                 }
2097                         }
2098                         break;
2099                 default:
2100                         LOG_ERROR("BUG: we shouldn't get here");
2101                         exit(-1);
2102                         break;
2103         }
2104
2105         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
2106                 return ERROR_FAIL;
2107
2108         for (i=0; i<=last_reg; i++)
2109                 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;
2110
2111         arm7_9->read_xpsr(target, &cpsr, 0);
2112         if ((retval = jtag_execute_queue()) != ERROR_OK)
2113         {
2114                 LOG_ERROR("JTAG error while reading cpsr");
2115                 return ERROR_TARGET_DATA_ABORT;
2116         }
2117
2118         if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
2119         {
2120                 LOG_WARNING("memory read caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
2121
2122                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
2123
2124                 return ERROR_TARGET_DATA_ABORT;
2125         }
2126
2127         return ERROR_OK;
2128 }
2129
2130 int arm7_9_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
2131 {
2132         armv4_5_common_t *armv4_5 = target->arch_info;
2133         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
2134         reg_t *dbg_ctrl = &arm7_9->eice_cache->reg_list[EICE_DBG_CTRL];
2135
2136         u32 reg[16];
2137         u32 num_accesses = 0;
2138         int thisrun_accesses;
2139         int i;
2140         u32 cpsr;
2141         int retval;
2142         int last_reg = 0;
2143
2144 #ifdef _DEBUG_ARM7_9_
2145         LOG_DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
2146 #endif
2147
2148         if (target->state != TARGET_HALTED)
2149         {
2150                 LOG_WARNING("target not halted");
2151                 return ERROR_TARGET_NOT_HALTED;
2152         }
2153
2154         /* sanitize arguments */
2155         if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
2156                 return ERROR_INVALID_ARGUMENTS;
2157
2158         if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
2159                 return ERROR_TARGET_UNALIGNED_ACCESS;
2160
2161         /* load the base register with the address of the first word */
2162         reg[0] = address;
2163         arm7_9->write_core_regs(target, 0x1, reg);
2164
2165         /* Clear DBGACK, to make sure memory fetches work as expected */
2166         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 0);
2167         embeddedice_store_reg(dbg_ctrl);
2168
2169         switch (size)
2170         {
2171                 case 4:
2172                         while (num_accesses < count)
2173                         {
2174                                 u32 reg_list;
2175                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2176                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2177
2178                                 for (i = 1; i <= thisrun_accesses; i++)
2179                                 {
2180                                         if (i > last_reg)
2181                                                 last_reg = i;
2182                                         reg[i] = target_buffer_get_u32(target, buffer);
2183                                         buffer += 4;
2184                                 }
2185
2186                                 arm7_9->write_core_regs(target, reg_list, reg);
2187
2188                                 arm7_9->store_word_regs(target, reg_list);
2189
2190                                 /* fast memory writes are only safe when the target is running
2191                                  * from a sufficiently high clock (32 kHz is usually too slow)
2192                                  */
2193                                 if (arm7_9->fast_memory_access)
2194                                         retval = arm7_9_execute_fast_sys_speed(target);
2195                                 else
2196                                         retval = arm7_9_execute_sys_speed(target);
2197                                 if(retval != ERROR_OK)
2198                                 {
2199                                         return retval;
2200                                 }
2201
2202                                 num_accesses += thisrun_accesses;
2203                         }
2204                         break;
2205                 case 2:
2206                         while (num_accesses < count)
2207                         {
2208                                 u32 reg_list;
2209                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2210                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2211
2212                                 for (i = 1; i <= thisrun_accesses; i++)
2213                                 {
2214                                         if (i > last_reg)
2215                                                 last_reg = i;
2216                                         reg[i] = target_buffer_get_u16(target, buffer) & 0xffff;
2217                                         buffer += 2;
2218                                 }
2219
2220                                 arm7_9->write_core_regs(target, reg_list, reg);
2221
2222                                 for (i = 1; i <= thisrun_accesses; i++)
2223                                 {
2224                                         arm7_9->store_hword_reg(target, i);
2225
2226                                         /* fast memory writes are only safe when the target is running
2227                                          * from a sufficiently high clock (32 kHz is usually too slow)
2228                                          */
2229                                         if (arm7_9->fast_memory_access)
2230                                                 retval = arm7_9_execute_fast_sys_speed(target);
2231                                         else
2232                                                 retval = arm7_9_execute_sys_speed(target);
2233                                         if(retval != ERROR_OK)
2234                                         {
2235                                                 return retval;
2236                                         }
2237                                 }
2238
2239                                 num_accesses += thisrun_accesses;
2240                         }
2241                         break;
2242                 case 1:
2243                         while (num_accesses < count)
2244                         {
2245                                 u32 reg_list;
2246                                 thisrun_accesses = ((count - num_accesses) >= 14) ? 14 : (count - num_accesses);
2247                                 reg_list = (0xffff >> (15 - thisrun_accesses)) & 0xfffe;
2248
2249                                 for (i = 1; i <= thisrun_accesses; i++)
2250                                 {
2251                                         if (i > last_reg)
2252                                                 last_reg = i;
2253                                         reg[i] = *buffer++ & 0xff;
2254                                 }
2255
2256                                 arm7_9->write_core_regs(target, reg_list, reg);
2257
2258                                 for (i = 1; i <= thisrun_accesses; i++)
2259                                 {
2260                                         arm7_9->store_byte_reg(target, i);
2261                                         /* fast memory writes are only safe when the target is running
2262                                          * from a sufficiently high clock (32 kHz is usually too slow)
2263                                          */
2264                                         if (arm7_9->fast_memory_access)
2265                                                 retval = arm7_9_execute_fast_sys_speed(target);
2266                                         else
2267                                                 retval = arm7_9_execute_sys_speed(target);
2268                                         if(retval != ERROR_OK)
2269                                         {
2270                                                 return retval;
2271                                         }
2272
2273                                 }
2274
2275                                 num_accesses += thisrun_accesses;
2276                         }
2277                         break;
2278                 default:
2279                         LOG_ERROR("BUG: we shouldn't get here");
2280                         exit(-1);
2281                         break;
2282         }
2283
2284         /* Re-Set DBGACK */
2285         buf_set_u32(dbg_ctrl->value, EICE_DBG_CONTROL_DBGACK, 1, 1);
2286         embeddedice_store_reg(dbg_ctrl);
2287
2288         if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
2289                 return ERROR_FAIL;
2290
2291         for (i=0; i<=last_reg; i++)
2292                 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;
2293
2294         arm7_9->read_xpsr(target, &cpsr, 0);
2295         if ((retval = jtag_execute_queue()) != ERROR_OK)
2296         {
2297                 LOG_ERROR("JTAG error while reading cpsr");
2298                 return ERROR_TARGET_DATA_ABORT;
2299         }
2300
2301         if (((cpsr & 0x1f) == ARMV4_5_MODE_ABT) && (armv4_5->core_mode != ARMV4_5_MODE_ABT))
2302         {
2303                 LOG_WARNING("memory write caused data abort (address: 0x%8.8x, size: 0x%x, count: 0x%x)", address, size, count);
2304
2305                 arm7_9->write_xpsr_im8(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8) & ~0x20, 0, 0);
2306
2307                 return ERROR_TARGET_DATA_ABORT;
2308         }
2309
2310         return ERROR_OK;
2311 }
2312
2313 static int dcc_count;
2314 static u8 *dcc_buffer;
2315
2316 static int arm7_9_dcc_completion(struct target_s *target, u32 exit_point, int timeout_ms, void *arch_info)
2317 {
2318         int retval = ERROR_OK;
2319         armv4_5_common_t *armv4_5 = target->arch_info;
2320         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
2321
2322         if ((retval=target_wait_state(target, TARGET_DEBUG_RUNNING, 500))!=ERROR_OK)
2323                 return retval;
2324
2325         int little=target->endianness==TARGET_LITTLE_ENDIAN;
2326         int count=dcc_count;
2327         u8 *buffer=dcc_buffer;
2328         if (count>2)
2329         {
2330                 /* Handle first & last using standard embeddedice_write_reg and the middle ones w/the
2331                  * core function repeated. */
2332                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2333                 buffer+=4;
2334
2335                 embeddedice_reg_t *ice_reg = arm7_9->eice_cache->reg_list[EICE_COMMS_DATA].arch_info;
2336                 u8 reg_addr = ice_reg->addr & 0x1f;
2337                 jtag_tap_t *tap;
2338                 tap = ice_reg->jtag_info->tap;
2339
2340                 embeddedice_write_dcc(tap, reg_addr, buffer, little, count-2);
2341                 buffer += (count-2)*4;
2342
2343                 embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2344         } else
2345         {
2346                 int i;
2347                 for (i = 0; i < count; i++)
2348                 {
2349                         embeddedice_write_reg(&arm7_9->eice_cache->reg_list[EICE_COMMS_DATA], fast_target_buffer_get_u32(buffer, little));
2350                         buffer += 4;
2351                 }
2352         }
2353
2354         if((retval = target_halt(target))!= ERROR_OK)
2355         {
2356                 return retval;
2357         }
2358         return target_wait_state(target, TARGET_HALTED, 500);
2359 }
2360
2361 static const u32 dcc_code[] =
2362 {
2363         /* MRC      TST         BNE         MRC         STR         B */
2364         0xee101e10, 0xe3110001, 0x0afffffc, 0xee111e10, 0xe4801004, 0xeafffff9
2365 };
2366
2367 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));
2368
2369 int arm7_9_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)
2370 {
2371         int retval;
2372         armv4_5_common_t *armv4_5 = target->arch_info;
2373         arm7_9_common_t *arm7_9 = armv4_5->arch_info;
2374         int i;
2375
2376         if (!arm7_9->dcc_downloads)
2377                 return target->type->write_memory(target, address, 4, count, buffer);
2378
2379         /* regrab previously allocated working_area, or allocate a new one */
2380         if (!arm7_9->dcc_working_area)
2381         {
2382                 u8 dcc_code_buf[6 * 4];
2383
2384                 /* make sure we have a working area */
2385                 if (target_alloc_working_area(target, 24, &arm7_9->dcc_working_area) != ERROR_OK)
2386                 {
2387                         LOG_INFO("no working area available, falling back to memory writes");
2388                         return target->type->write_memory(target, address, 4, count, buffer);
2389                 }
2390
2391                 /* copy target instructions to target endianness */
2392                 for (i = 0; i < 6; i++)
2393                 {
2394                         target_buffer_set_u32(target, dcc_code_buf + i*4, dcc_code[i]);
2395                 }
2396
2397                 /* write DCC code to working area */
2398                 if ((retval = target->type->write_memory(target, arm7_9->dcc_working_area->address, 4, 6, dcc_code_buf)) != ERROR_OK)
2399                 {
2400                         return retval;
2401                 }
2402         }
2403
2404         armv4_5_algorithm_t armv4_5_info;
2405         reg_param_t reg_params[1];
2406
2407         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2408         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2409         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2410
2411         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
2412
2413         buf_set_u32(reg_params[0].value, 0, 32, address);
2414
2415         dcc_count=count;
2416         dcc_buffer=buffer;
2417         retval = armv4_5_run_algorithm_inner(target, 0, NULL, 1, reg_params,
2418                         arm7_9->dcc_working_area->address, arm7_9->dcc_working_area->address+6*4, 20*1000, &armv4_5_info, arm7_9_dcc_completion);
2419
2420         if (retval==ERROR_OK)
2421         {
2422                 u32 endaddress=buf_get_u32(reg_params[0].value, 0, 32);
2423                 if (endaddress!=(address+count*4))
2424                 {
2425                         LOG_ERROR("DCC write failed, expected end address 0x%08x got 0x%0x", (address+count*4), endaddress);
2426                         retval=ERROR_FAIL;
2427                 }
2428         }
2429
2430         destroy_reg_param(&reg_params[0]);
2431
2432         return retval;
2433 }
2434
2435 int arm7_9_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)
2436 {
2437         working_area_t *crc_algorithm;
2438         armv4_5_algorithm_t armv4_5_info;
2439         reg_param_t reg_params[2];
2440         int retval;
2441
2442         u32 arm7_9_crc_code[] = {
2443                 0xE1A02000,                             /* mov          r2, r0 */
2444                 0xE3E00000,                             /* mov          r0, #0xffffffff */
2445                 0xE1A03001,                             /* mov          r3, r1 */
2446                 0xE3A04000,                             /* mov          r4, #0 */
2447                 0xEA00000B,                             /* b            ncomp */
2448                                                                 /* nbyte: */
2449                 0xE7D21004,                             /* ldrb r1, [r2, r4] */
2450                 0xE59F7030,                             /* ldr          r7, CRC32XOR */
2451                 0xE0200C01,                             /* eor          r0, r0, r1, asl 24 */
2452                 0xE3A05000,                             /* mov          r5, #0 */
2453                                                                 /* loop: */
2454                 0xE3500000,                             /* cmp          r0, #0 */
2455                 0xE1A06080,                             /* mov          r6, r0, asl #1 */
2456                 0xE2855001,                             /* add          r5, r5, #1 */
2457                 0xE1A00006,                             /* mov          r0, r6 */
2458                 0xB0260007,                             /* eorlt        r0, r6, r7 */
2459                 0xE3550008,                             /* cmp          r5, #8 */
2460                 0x1AFFFFF8,                             /* bne          loop */
2461                 0xE2844001,                             /* add          r4, r4, #1 */
2462                                                                 /* ncomp: */
2463                 0xE1540003,                             /* cmp          r4, r3 */
2464                 0x1AFFFFF1,                             /* bne          nbyte */
2465                                                                 /* end: */
2466                 0xEAFFFFFE,                             /* b            end */
2467                 0x04C11DB7                              /* CRC32XOR:    .word 0x04C11DB7 */
2468         };
2469
2470         u32 i;
2471
2472         if (target_alloc_working_area(target, sizeof(arm7_9_crc_code), &crc_algorithm) != ERROR_OK)
2473         {
2474                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2475         }
2476
2477         /* convert flash writing code into a buffer in target endianness */
2478         for (i = 0; i < (sizeof(arm7_9_crc_code)/sizeof(u32)); i++)
2479         {
2480                 if ((retval=target_write_u32(target, crc_algorithm->address + i*sizeof(u32), arm7_9_crc_code[i]))!=ERROR_OK)
2481                 {
2482                         return retval;
2483                 }
2484         }
2485
2486         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2487         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2488         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2489
2490         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);
2491         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
2492
2493         buf_set_u32(reg_params[0].value, 0, 32, address);
2494         buf_set_u32(reg_params[1].value, 0, 32, count);
2495
2496         if ((retval = target->type->run_algorithm(target, 0, NULL, 2, reg_params,
2497                 crc_algorithm->address, crc_algorithm->address + (sizeof(arm7_9_crc_code) - 8), 20000, &armv4_5_info)) != ERROR_OK)
2498         {
2499                 LOG_ERROR("error executing arm7_9 crc algorithm");
2500                 destroy_reg_param(&reg_params[0]);
2501                 destroy_reg_param(&reg_params[1]);
2502                 target_free_working_area(target, crc_algorithm);
2503                 return retval;
2504         }
2505
2506         *checksum = buf_get_u32(reg_params[0].value, 0, 32);
2507
2508         destroy_reg_param(&reg_params[0]);
2509         destroy_reg_param(&reg_params[1]);
2510
2511         target_free_working_area(target, crc_algorithm);
2512
2513         return ERROR_OK;
2514 }
2515
2516 int arm7_9_blank_check_memory(struct target_s *target, u32 address, u32 count, u32* blank)
2517 {
2518         working_area_t *erase_check_algorithm;
2519         reg_param_t reg_params[3];
2520         armv4_5_algorithm_t armv4_5_info;
2521         int retval;
2522         u32 i;
2523
2524         u32 erase_check_code[] =
2525         {
2526                                                 /* loop: */
2527                 0xe4d03001,             /* ldrb r3, [r0], #1    */
2528                 0xe0022003,             /* and r2, r2, r3               */
2529                 0xe2511001,     /* subs r1, r1, #1              */
2530                 0x1afffffb,             /* bne loop                             */
2531                                                 /* end: */
2532                 0xeafffffe              /* b end                                */
2533         };
2534
2535         /* make sure we have a working area */
2536         if (target_alloc_working_area(target, sizeof(erase_check_code), &erase_check_algorithm) != ERROR_OK)
2537         {
2538                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
2539         }
2540
2541         /* convert flash writing code into a buffer in target endianness */
2542         for (i = 0; i < (sizeof(erase_check_code)/sizeof(u32)); i++)
2543                 if ((retval = target_write_u32(target, erase_check_algorithm->address + i*sizeof(u32), erase_check_code[i])) != ERROR_OK)
2544                 {
2545                         return retval;
2546                 }
2547
2548         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
2549         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
2550         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
2551
2552         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
2553         buf_set_u32(reg_params[0].value, 0, 32, address);
2554
2555         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
2556         buf_set_u32(reg_params[1].value, 0, 32, count);
2557
2558         init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
2559         buf_set_u32(reg_params[2].value, 0, 32, 0xff);
2560
2561         if ((retval = target->type->run_algorithm(target, 0, NULL, 3, reg_params,
2562                         erase_check_algorithm->address, erase_check_algorithm->address + (sizeof(erase_check_code) - 4), 10000, &armv4_5_info)) != ERROR_OK)
2563         {
2564                 destroy_reg_param(&reg_params[0]);
2565                 destroy_reg_param(&reg_params[1]);
2566                 destroy_reg_param(&reg_params[2]);
2567                 target_free_working_area(target, erase_check_algorithm);
2568                 return 0;
2569         }
2570
2571         *blank = buf_get_u32(reg_params[2].value, 0, 32);
2572
2573         destroy_reg_param(&reg_params[0]);
2574         destroy_reg_param(&reg_params[1]);
2575         destroy_reg_param(&reg_params[2]);
2576
2577         target_free_working_area(target, erase_check_algorithm);
2578
2579         return ERROR_OK;
2580 }
2581
2582 int arm7_9_register_commands(struct command_context_s *cmd_ctx)
2583 {
2584         command_t *arm7_9_cmd;
2585
2586         arm7_9_cmd = register_command(cmd_ctx, NULL, "arm7_9", NULL, COMMAND_ANY, "arm7/9 specific commands");
2587
2588         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>");
2589         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>");
2590
2591         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>");
2592
2593         register_command(cmd_ctx, arm7_9_cmd, "dbgrq", handle_arm7_9_dbgrq_command,
2594                 COMMAND_ANY, "use EmbeddedICE dbgrq instead of breakpoint for target halt requests <enable|disable>");
2595         register_command(cmd_ctx, arm7_9_cmd, "fast_memory_access", handle_arm7_9_fast_memory_access_command,
2596                  COMMAND_ANY, "use fast memory accesses instead of slower but potentially unsafe slow accesses <enable|disable>");
2597         register_command(cmd_ctx, arm7_9_cmd, "dcc_downloads", handle_arm7_9_dcc_downloads_command,
2598                 COMMAND_ANY, "use DCC downloads for larger memory writes <enable|disable>");
2599
2600         armv4_5_register_commands(cmd_ctx);
2601
2602         etm_register_commands(cmd_ctx);
2603
2604         return ERROR_OK;
2605 }
2606
2607 int handle_arm7_9_write_xpsr_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2608 {
2609         u32 value;
2610         int spsr;
2611         int retval;
2612         target_t *target = get_current_target(cmd_ctx);
2613         armv4_5_common_t *armv4_5;
2614         arm7_9_common_t *arm7_9;
2615
2616         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2617         {
2618                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2619                 return ERROR_OK;
2620         }
2621
2622         if (target->state != TARGET_HALTED)
2623         {
2624                 command_print(cmd_ctx, "can't write registers while running");
2625                 return ERROR_OK;
2626         }
2627
2628         if (argc < 2)
2629         {
2630                 command_print(cmd_ctx, "usage: write_xpsr <value> <not cpsr|spsr>");
2631                 return ERROR_OK;
2632         }
2633
2634         value = strtoul(args[0], NULL, 0);
2635         spsr = strtol(args[1], NULL, 0);
2636
2637         /* if we're writing the CPSR, mask the T bit */
2638         if (!spsr)
2639                 value &= ~0x20;
2640
2641         arm7_9->write_xpsr(target, value, spsr);
2642         if ((retval = jtag_execute_queue()) != ERROR_OK)
2643         {
2644                 LOG_ERROR("JTAG error while writing to xpsr");
2645                 return retval;
2646         }
2647
2648         return ERROR_OK;
2649 }
2650
2651 int handle_arm7_9_write_xpsr_im8_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2652 {
2653         u32 value;
2654         int rotate;
2655         int spsr;
2656         int retval;
2657         target_t *target = get_current_target(cmd_ctx);
2658         armv4_5_common_t *armv4_5;
2659         arm7_9_common_t *arm7_9;
2660
2661         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2662         {
2663                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2664                 return ERROR_OK;
2665         }
2666
2667         if (target->state != TARGET_HALTED)
2668         {
2669                 command_print(cmd_ctx, "can't write registers while running");
2670                 return ERROR_OK;
2671         }
2672
2673         if (argc < 3)
2674         {
2675                 command_print(cmd_ctx, "usage: write_xpsr_im8 <im8> <rotate> <not cpsr|spsr>");
2676                 return ERROR_OK;
2677         }
2678
2679         value = strtoul(args[0], NULL, 0);
2680         rotate = strtol(args[1], NULL, 0);
2681         spsr = strtol(args[2], NULL, 0);
2682
2683         arm7_9->write_xpsr_im8(target, value, rotate, spsr);
2684         if ((retval = jtag_execute_queue()) != ERROR_OK)
2685         {
2686                 LOG_ERROR("JTAG error while writing 8-bit immediate to xpsr");
2687                 return retval;
2688         }
2689
2690         return ERROR_OK;
2691 }
2692
2693 int handle_arm7_9_write_core_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2694 {
2695         u32 value;
2696         u32 mode;
2697         int num;
2698         target_t *target = get_current_target(cmd_ctx);
2699         armv4_5_common_t *armv4_5;
2700         arm7_9_common_t *arm7_9;
2701
2702         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2703         {
2704                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2705                 return ERROR_OK;
2706         }
2707
2708         if (target->state != TARGET_HALTED)
2709         {
2710                 command_print(cmd_ctx, "can't write registers while running");
2711                 return ERROR_OK;
2712         }
2713
2714         if (argc < 3)
2715         {
2716                 command_print(cmd_ctx, "usage: write_core_reg <num> <mode> <value>");
2717                 return ERROR_OK;
2718         }
2719
2720         num = strtol(args[0], NULL, 0);
2721         mode = strtoul(args[1], NULL, 0);
2722         value = strtoul(args[2], NULL, 0);
2723
2724         return arm7_9_write_core_reg(target, num, mode, value);
2725 }
2726
2727 int handle_arm7_9_dbgrq_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2728 {
2729         target_t *target = get_current_target(cmd_ctx);
2730         armv4_5_common_t *armv4_5;
2731         arm7_9_common_t *arm7_9;
2732
2733         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2734         {
2735                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2736                 return ERROR_OK;
2737         }
2738
2739         if (argc > 0)
2740         {
2741                 if (strcmp("enable", args[0]) == 0)
2742                 {
2743                         arm7_9->use_dbgrq = 1;
2744                 }
2745                 else if (strcmp("disable", args[0]) == 0)
2746                 {
2747                         arm7_9->use_dbgrq = 0;
2748                 }
2749                 else
2750                 {
2751                         command_print(cmd_ctx, "usage: arm7_9 dbgrq <enable|disable>");
2752                 }
2753         }
2754
2755         command_print(cmd_ctx, "use of EmbeddedICE dbgrq instead of breakpoint for target halt %s", (arm7_9->use_dbgrq) ? "enabled" : "disabled");
2756
2757         return ERROR_OK;
2758 }
2759
2760 int handle_arm7_9_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2761 {
2762         target_t *target = get_current_target(cmd_ctx);
2763         armv4_5_common_t *armv4_5;
2764         arm7_9_common_t *arm7_9;
2765
2766         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2767         {
2768                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2769                 return ERROR_OK;
2770         }
2771
2772         if (argc > 0)
2773         {
2774                 if (strcmp("enable", args[0]) == 0)
2775                 {
2776                         arm7_9->fast_memory_access = 1;
2777                 }
2778                 else if (strcmp("disable", args[0]) == 0)
2779                 {
2780                         arm7_9->fast_memory_access = 0;
2781                 }
2782                 else
2783                 {
2784                         command_print(cmd_ctx, "usage: arm7_9 fast_memory_access <enable|disable>");
2785                 }
2786         }
2787
2788         command_print(cmd_ctx, "fast memory access is %s", (arm7_9->fast_memory_access) ? "enabled" : "disabled");
2789
2790         return ERROR_OK;
2791 }
2792
2793 int handle_arm7_9_dcc_downloads_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2794 {
2795         target_t *target = get_current_target(cmd_ctx);
2796         armv4_5_common_t *armv4_5;
2797         arm7_9_common_t *arm7_9;
2798
2799         if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
2800         {
2801                 command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
2802                 return ERROR_OK;
2803         }
2804
2805         if (argc > 0)
2806         {
2807                 if (strcmp("enable", args[0]) == 0)
2808                 {
2809                         arm7_9->dcc_downloads = 1;
2810                 }
2811                 else if (strcmp("disable", args[0]) == 0)
2812                 {
2813                         arm7_9->dcc_downloads = 0;
2814                 }
2815                 else
2816                 {
2817                         command_print(cmd_ctx, "usage: arm7_9 dcc_downloads <enable|disable>");
2818                 }
2819         }
2820
2821         command_print(cmd_ctx, "dcc downloads are %s", (arm7_9->dcc_downloads) ? "enabled" : "disabled");
2822
2823         return ERROR_OK;
2824 }
2825
2826 int arm7_9_init_arch_info(target_t *target, arm7_9_common_t *arm7_9)
2827 {
2828         int retval = ERROR_OK;
2829         armv4_5_common_t *armv4_5 = &arm7_9->armv4_5_common;
2830
2831         arm7_9->common_magic = ARM7_9_COMMON_MAGIC;
2832
2833         if((retval = arm_jtag_setup_connection(&arm7_9->jtag_info)) != ERROR_OK)
2834         {
2835                 return retval;
2836         }
2837
2838         arm7_9->wp_available = 0; /* this is set up in arm7_9_clear_watchpoints() */
2839         arm7_9->wp_available_max = 2;
2840         arm7_9->sw_breakpoints_added = 0;
2841         arm7_9->breakpoint_count = 0;
2842         arm7_9->wp0_used = 0;
2843         arm7_9->wp1_used = 0;
2844         arm7_9->wp1_used_default = 0;
2845         arm7_9->use_dbgrq = 0;
2846
2847         arm7_9->etm_ctx = NULL;
2848         arm7_9->has_single_step = 0;
2849         arm7_9->has_monitor_mode = 0;
2850         arm7_9->has_vector_catch = 0;
2851
2852         arm7_9->debug_entry_from_reset = 0;
2853
2854         arm7_9->dcc_working_area = NULL;
2855
2856         arm7_9->fast_memory_access = fast_and_dangerous;
2857         arm7_9->dcc_downloads = fast_and_dangerous;
2858
2859         arm7_9->need_bypass_before_restart = 0;
2860
2861         armv4_5->arch_info = arm7_9;
2862         armv4_5->read_core_reg = arm7_9_read_core_reg;
2863         armv4_5->write_core_reg = arm7_9_write_core_reg;
2864         armv4_5->full_context = arm7_9_full_context;
2865
2866         if((retval = armv4_5_init_arch_info(target, armv4_5)) != ERROR_OK)
2867         {
2868                 return retval;
2869         }
2870
2871         if((retval = target_register_timer_callback(arm7_9_handle_target_request, 1, 1, target)) != ERROR_OK)
2872         {
2873                 return retval;
2874         }
2875
2876         return ERROR_OK;
2877 }