git.gag.com Git - fw/openocd/blob - src/target/arm7_9_common.c

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