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

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