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