git.sur5r.net Git - openocd/blob - src/target/arm7_9_common.c

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