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

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