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

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   Copyright (C) 2006 by Magnus Lundin                                   *
   6  *   lundin@mlu.mine.nu                                                    *
   7  *                                                                         *
   8  *   Copyright (C) 2008 by Spencer Oliver                                  *
   9  *   spen@spen-soft.co.uk                                                  *
  10  *                                                                         *
  11  *   Copyright (C) 2009 by Dirk Behme                                      *
  12  *   dirk.behme@gmail.com - copy from cortex_m3                            *
  13  *                                                                         *
  14  *   This program is free software; you can redistribute it and/or modify  *
  15  *   it under the terms of the GNU General Public License as published by  *
  16  *   the Free Software Foundation; either version 2 of the License, or     *
  17  *   (at your option) any later version.                                   *
  18  *                                                                         *
  19  *   This program is distributed in the hope that it will be useful,       *
  20  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  21  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  22  *   GNU General Public License for more details.                          *
  23  *                                                                         *
  24  *   You should have received a copy of the GNU General Public License     *
  25  *   along with this program; if not, write to the                         *
  26  *   Free Software Foundation, Inc.,                                       *
  27  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  28  *                                                                         *
  29  *   Cortex-A8(tm) TRM, ARM DDI 0344H                                      *
  30  *                                                                         *
  31  ***************************************************************************/
  32 #ifdef HAVE_CONFIG_H
  33 #include "config.h"
  34 #endif
  35
  36 #include "breakpoints.h"
  37 #include "cortex_a8.h"
  38 #include "register.h"
  39 #include "target_request.h"
  40 #include "target_type.h"
  41
  42 static int cortex_a8_poll(struct target *target);
  43 static int cortex_a8_debug_entry(struct target *target);
  44 static int cortex_a8_restore_context(struct target *target, bool bpwp);
  45 static int cortex_a8_set_breakpoint(struct target *target,
  46                 struct breakpoint *breakpoint, uint8_t matchmode);
  47 static int cortex_a8_unset_breakpoint(struct target *target,
  48                 struct breakpoint *breakpoint);
  49 static int cortex_a8_dap_read_coreregister_u32(struct target *target,
  50                 uint32_t *value, int regnum);
  51 static int cortex_a8_dap_write_coreregister_u32(struct target *target,
  52                 uint32_t value, int regnum);
  53 /*
  54  * FIXME do topology discovery using the ROM; don't
  55  * assume this is an OMAP3.
  56  */
  57 #define swjdp_memoryap 0
  58 #define swjdp_debugap 1
  59 #define OMAP3530_DEBUG_BASE 0x54011000
  60
  61 /*
  62  * Cortex-A8 Basic debug access, very low level assumes state is saved
  63  */
  64 static int cortex_a8_init_debug_access(struct target *target)
  65 {
  66         struct armv7a_common *armv7a = target_to_armv7a(target);
  67         struct swjdp_common *swjdp = &armv7a->swjdp_info;
  68
  69         int retval;
  70         uint32_t dummy;
  71
  72         LOG_DEBUG(" ");
  73
  74         /* Unlocking the debug registers for modification */
  75         /* The debugport might be uninitialised so try twice */
  76         retval = mem_ap_write_atomic_u32(swjdp, armv7a->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
  77         if (retval != ERROR_OK)
  78                 mem_ap_write_atomic_u32(swjdp, armv7a->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
  79         /* Clear Sticky Power Down status Bit in PRSR to enable access to
  80            the registers in the Core Power Domain */
  81         retval = mem_ap_read_atomic_u32(swjdp, armv7a->debug_base + CPUDBG_PRSR, &dummy);
  82         /* Enabling of instruction execution in debug mode is done in debug_entry code */
  83
  84         /* Resync breakpoint registers */
  85
  86         /* Since this is likley called from init or reset, update targtet state information*/
  87         cortex_a8_poll(target);
  88
  89         return retval;
  90 }
  91
  92 /* To reduce needless round-trips, pass in a pointer to the current
  93  * DSCR value.  Initialize it to zero if you just need to know the
  94  * value on return from this function; or (1 << DSCR_INSTR_COMP) if
  95  * you happen to know that no instruction is pending.
  96  */
  97 static int cortex_a8_exec_opcode(struct target *target,
  98                 uint32_t opcode, uint32_t *dscr_p)
  99 {
 100         uint32_t dscr;
 101         int retval;
 102         struct armv7a_common *armv7a = target_to_armv7a(target);
 103         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 104
 105         dscr = dscr_p ? *dscr_p : 0;
 106
 107         LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
 108
 109         /* Wait for InstrCompl bit to be set */
 110         while ((dscr & (1 << DSCR_INSTR_COMP)) == 0)
 111         {
 112                 retval = mem_ap_read_atomic_u32(swjdp,
 113                                 armv7a->debug_base + CPUDBG_DSCR, &dscr);
 114                 if (retval != ERROR_OK)
 115                 {
 116                         LOG_ERROR("Could not read DSCR register, opcode = 0x%08" PRIx32, opcode);
 117                         return retval;
 118                 }
 119         }
 120
 121         mem_ap_write_u32(swjdp, armv7a->debug_base + CPUDBG_ITR, opcode);
 122
 123         do
 124         {
 125                 retval = mem_ap_read_atomic_u32(swjdp,
 126                                 armv7a->debug_base + CPUDBG_DSCR, &dscr);
 127                 if (retval != ERROR_OK)
 128                 {
 129                         LOG_ERROR("Could not read DSCR register");
 130                         return retval;
 131                 }
 132         }
 133         while ((dscr & (1 << DSCR_INSTR_COMP)) == 0); /* Wait for InstrCompl bit to be set */
 134
 135         if (dscr_p)
 136                 *dscr_p = dscr;
 137
 138         return retval;
 139 }
 140
 141 /**************************************************************************
 142 Read core register with very few exec_opcode, fast but needs work_area.
 143 This can cause problems with MMU active.
 144 **************************************************************************/
 145 static int cortex_a8_read_regs_through_mem(struct target *target, uint32_t address,
 146                 uint32_t * regfile)
 147 {
 148         int retval = ERROR_OK;
 149         struct armv7a_common *armv7a = target_to_armv7a(target);
 150         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 151
 152         cortex_a8_dap_read_coreregister_u32(target, regfile, 0);
 153         cortex_a8_dap_write_coreregister_u32(target, address, 0);
 154         cortex_a8_exec_opcode(target, ARMV4_5_STMIA(0, 0xFFFE, 0, 0), NULL);
 155         dap_ap_select(swjdp, swjdp_memoryap);
 156         mem_ap_read_buf_u32(swjdp, (uint8_t *)(&regfile[1]), 4*15, address);
 157         dap_ap_select(swjdp, swjdp_debugap);
 158
 159         return retval;
 160 }
 161
 162 static int cortex_a8_dap_read_coreregister_u32(struct target *target,
 163                 uint32_t *value, int regnum)
 164 {
 165         int retval = ERROR_OK;
 166         uint8_t reg = regnum&0xFF;
 167         uint32_t dscr = 0;
 168         struct armv7a_common *armv7a = target_to_armv7a(target);
 169         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 170
 171         if (reg > 17)
 172                 return retval;
 173
 174         if (reg < 15)
 175         {
 176                 /* Rn to DCCTX, "MCR p14, 0, Rn, c0, c5, 0"  0xEE00nE15 */
 177                 cortex_a8_exec_opcode(target,
 178                                 ARMV4_5_MCR(14, 0, reg, 0, 5, 0),
 179                                 &dscr);
 180         }
 181         else if (reg == 15)
 182         {
 183                 /* "MOV r0, r15"; then move r0 to DCCTX */
 184                 cortex_a8_exec_opcode(target, 0xE1A0000F, &dscr);
 185                 cortex_a8_exec_opcode(target,
 186                                 ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
 187                                 &dscr);
 188         }
 189         else
 190         {
 191                 /* "MRS r0, CPSR" or "MRS r0, SPSR"
 192                  * then move r0 to DCCTX
 193                  */
 194                 cortex_a8_exec_opcode(target, ARMV4_5_MRS(0, reg & 1), &dscr);
 195                 cortex_a8_exec_opcode(target,
 196                                 ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
 197                                 &dscr);
 198         }
 199
 200         /* Wait for DTRRXfull then read DTRRTX */
 201         while ((dscr & (1 << DSCR_DTR_TX_FULL)) == 0)
 202         {
 203                 retval = mem_ap_read_atomic_u32(swjdp,
 204                                 armv7a->debug_base + CPUDBG_DSCR, &dscr);
 205         }
 206
 207         retval = mem_ap_read_atomic_u32(swjdp,
 208                         armv7a->debug_base + CPUDBG_DTRTX, value);
 209         LOG_DEBUG("read DCC 0x%08" PRIx32, *value);
 210
 211         return retval;
 212 }
 213
 214 static int cortex_a8_dap_write_coreregister_u32(struct target *target,
 215                 uint32_t value, int regnum)
 216 {
 217         int retval = ERROR_OK;
 218         uint8_t Rd = regnum&0xFF;
 219         uint32_t dscr;
 220         struct armv7a_common *armv7a = target_to_armv7a(target);
 221         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 222
 223         LOG_DEBUG("register %i, value 0x%08" PRIx32, regnum, value);
 224
 225         /* Check that DCCRX is not full */
 226         retval = mem_ap_read_atomic_u32(swjdp,
 227                                 armv7a->debug_base + CPUDBG_DSCR, &dscr);
 228         if (dscr & (1 << DSCR_DTR_RX_FULL))
 229         {
 230                 LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
 231                 /* Clear DCCRX with MCR(p14, 0, Rd, c0, c5, 0), opcode  0xEE000E15 */
 232                 cortex_a8_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
 233                                 &dscr);
 234         }
 235
 236         if (Rd > 17)
 237                 return retval;
 238
 239         /* Write DTRRX ... sets DSCR.DTRRXfull but exec_opcode() won't care */
 240         LOG_DEBUG("write DCC 0x%08" PRIx32, value);
 241         retval = mem_ap_write_u32(swjdp,
 242                         armv7a->debug_base + CPUDBG_DTRRX, value);
 243
 244         if (Rd < 15)
 245         {
 246                 /* DCCRX to Rn, "MCR p14, 0, Rn, c0, c5, 0", 0xEE00nE15 */
 247                 cortex_a8_exec_opcode(target, ARMV4_5_MRC(14, 0, Rd, 0, 5, 0),
 248                                 &dscr);
 249         }
 250         else if (Rd == 15)
 251         {
 252                 /* DCCRX to R0, "MCR p14, 0, R0, c0, c5, 0", 0xEE000E15
 253                  * then "mov r15, r0"
 254                  */
 255                 cortex_a8_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
 256                                 &dscr);
 257                 cortex_a8_exec_opcode(target, 0xE1A0F000, &dscr);
 258         }
 259         else
 260         {
 261                 /* DCCRX to R0, "MCR p14, 0, R0, c0, c5, 0", 0xEE000E15
 262                  * then "MSR CPSR_cxsf, r0" or "MSR SPSR_cxsf, r0" (all fields)
 263                  */
 264                 cortex_a8_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
 265                                 &dscr);
 266                 cortex_a8_exec_opcode(target, ARMV4_5_MSR_GP(0, 0xF, Rd & 1),
 267                                 &dscr);
 268
 269                 /* "Prefetch flush" after modifying execution status in CPSR */
 270                 if (Rd == 16)
 271                         cortex_a8_exec_opcode(target,
 272                                         ARMV4_5_MCR(15, 0, 0, 7, 5, 4),
 273                                         &dscr);
 274         }
 275
 276         return retval;
 277 }
 278
 279 /* Write to memory mapped registers directly with no cache or mmu handling */
 280 static int cortex_a8_dap_write_memap_register_u32(struct target *target, uint32_t address, uint32_t value)
 281 {
 282         int retval;
 283         struct armv7a_common *armv7a = target_to_armv7a(target);
 284         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 285
 286         retval = mem_ap_write_atomic_u32(swjdp, address, value);
 287
 288         return retval;
 289 }
 290
 291 /*
 292  * Cortex-A8 implementation of Debug Programmer's Model
 293  *
 294  * NOTE the invariant:  these routines return with DSCR_INSTR_COMP set,
 295  * so there's no need to poll for it before executing an instruction.
 296  *
 297  * NOTE that in several of these cases the "stall" mode might be useful.
 298  * It'd let us queue a few operations together... prepare/finish might
 299  * be the places to enable/disable that mode.
 300  */
 301
 302 static inline struct cortex_a8_common *dpm_to_a8(struct arm_dpm *dpm)
 303 {
 304         return container_of(dpm, struct cortex_a8_common, armv7a_common.dpm);
 305 }
 306
 307 static int cortex_a8_write_dcc(struct cortex_a8_common *a8, uint32_t data)
 308 {
 309         LOG_DEBUG("write DCC 0x%08" PRIx32, data);
 310         return mem_ap_write_u32(&a8->armv7a_common.swjdp_info,
 311                         a8->armv7a_common.debug_base + CPUDBG_DTRRX, data);
 312 }
 313
 314 static int cortex_a8_read_dcc(struct cortex_a8_common *a8, uint32_t *data,
 315                 uint32_t *dscr_p)
 316 {
 317         struct swjdp_common *swjdp = &a8->armv7a_common.swjdp_info;
 318         uint32_t dscr = 1 << DSCR_INSTR_COMP;
 319         int retval;
 320
 321         if (dscr_p)
 322                 dscr = *dscr_p;
 323
 324         /* Wait for DTRRXfull */
 325         while ((dscr & (1 << DSCR_DTR_TX_FULL)) == 0) {
 326                 retval = mem_ap_read_atomic_u32(swjdp,
 327                                 a8->armv7a_common.debug_base + CPUDBG_DSCR,
 328                                 &dscr);
 329         }
 330
 331         retval = mem_ap_read_atomic_u32(swjdp,
 332                         a8->armv7a_common.debug_base + CPUDBG_DTRTX, data);
 333         //LOG_DEBUG("read DCC 0x%08" PRIx32, *data);
 334
 335         if (dscr_p)
 336                 *dscr_p = dscr;
 337
 338         return retval;
 339 }
 340
 341 static int cortex_a8_dpm_prepare(struct arm_dpm *dpm)
 342 {
 343         struct cortex_a8_common *a8 = dpm_to_a8(dpm);
 344         struct swjdp_common *swjdp = &a8->armv7a_common.swjdp_info;
 345         uint32_t dscr;
 346         int retval;
 347
 348         /* set up invariant:  INSTR_COMP is set after ever DPM operation */
 349         do {
 350                 retval = mem_ap_read_atomic_u32(swjdp,
 351                                 a8->armv7a_common.debug_base + CPUDBG_DSCR,
 352                                 &dscr);
 353         } while ((dscr & (1 << DSCR_INSTR_COMP)) == 0);
 354
 355         /* this "should never happen" ... */
 356         if (dscr & (1 << DSCR_DTR_RX_FULL)) {
 357                 LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
 358                 /* Clear DCCRX */
 359                 retval = cortex_a8_exec_opcode(
 360                                 a8->armv7a_common.armv4_5_common.target,
 361                                 ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
 362                                 &dscr);
 363         }
 364
 365         return retval;
 366 }
 367
 368 static int cortex_a8_dpm_finish(struct arm_dpm *dpm)
 369 {
 370         /* REVISIT what could be done here? */
 371         return ERROR_OK;
 372 }
 373
 374 static int cortex_a8_instr_write_data_dcc(struct arm_dpm *dpm,
 375                 uint32_t opcode, uint32_t data)
 376 {
 377         struct cortex_a8_common *a8 = dpm_to_a8(dpm);
 378         int retval;
 379         uint32_t dscr = 1 << DSCR_INSTR_COMP;
 380
 381         retval = cortex_a8_write_dcc(a8, data);
 382
 383         return cortex_a8_exec_opcode(
 384                         a8->armv7a_common.armv4_5_common.target,
 385                         opcode,
 386                         &dscr);
 387 }
 388
 389 static int cortex_a8_instr_write_data_r0(struct arm_dpm *dpm,
 390                 uint32_t opcode, uint32_t data)
 391 {
 392         struct cortex_a8_common *a8 = dpm_to_a8(dpm);
 393         uint32_t dscr = 1 << DSCR_INSTR_COMP;
 394         int retval;
 395
 396         retval = cortex_a8_write_dcc(a8, data);
 397
 398         /* DCCRX to R0, "MCR p14, 0, R0, c0, c5, 0", 0xEE000E15 */
 399         retval = cortex_a8_exec_opcode(
 400                         a8->armv7a_common.armv4_5_common.target,
 401                         ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
 402                         &dscr);
 403
 404         /* then the opcode, taking data from R0 */
 405         retval = cortex_a8_exec_opcode(
 406                         a8->armv7a_common.armv4_5_common.target,
 407                         opcode,
 408                         &dscr);
 409
 410         return retval;
 411 }
 412
 413 static int cortex_a8_instr_cpsr_sync(struct arm_dpm *dpm)
 414 {
 415         struct target *target = dpm->arm->target;
 416         uint32_t dscr = 1 << DSCR_INSTR_COMP;
 417
 418         /* "Prefetch flush" after modifying execution status in CPSR */
 419         return cortex_a8_exec_opcode(target,
 420                         ARMV4_5_MCR(15, 0, 0, 7, 5, 4),
 421                         &dscr);
 422 }
 423
 424 static int cortex_a8_instr_read_data_dcc(struct arm_dpm *dpm,
 425                 uint32_t opcode, uint32_t *data)
 426 {
 427         struct cortex_a8_common *a8 = dpm_to_a8(dpm);
 428         int retval;
 429         uint32_t dscr = 1 << DSCR_INSTR_COMP;
 430
 431         /* the opcode, writing data to DCC */
 432         retval = cortex_a8_exec_opcode(
 433                         a8->armv7a_common.armv4_5_common.target,
 434                         opcode,
 435                         &dscr);
 436
 437         return cortex_a8_read_dcc(a8, data, &dscr);
 438 }
 439
 440
 441 static int cortex_a8_instr_read_data_r0(struct arm_dpm *dpm,
 442                 uint32_t opcode, uint32_t *data)
 443 {
 444         struct cortex_a8_common *a8 = dpm_to_a8(dpm);
 445         uint32_t dscr = 1 << DSCR_INSTR_COMP;
 446         int retval;
 447
 448         /* the opcode, writing data to R0 */
 449         retval = cortex_a8_exec_opcode(
 450                         a8->armv7a_common.armv4_5_common.target,
 451                         opcode,
 452                         &dscr);
 453
 454         /* write R0 to DCC */
 455         retval = cortex_a8_exec_opcode(
 456                         a8->armv7a_common.armv4_5_common.target,
 457                         ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
 458                         &dscr);
 459
 460         return cortex_a8_read_dcc(a8, data, &dscr);
 461 }
 462
 463 static int cortex_a8_bpwp_enable(struct arm_dpm *dpm, unsigned index,
 464                 uint32_t addr, uint32_t control)
 465 {
 466         struct cortex_a8_common *a8 = dpm_to_a8(dpm);
 467         uint32_t vr = a8->armv7a_common.debug_base;
 468         uint32_t cr = a8->armv7a_common.debug_base;
 469         int retval;
 470
 471         switch (index) {
 472         case 0 ... 15:          /* breakpoints */
 473                 vr += CPUDBG_BVR_BASE;
 474                 cr += CPUDBG_BCR_BASE;
 475                 break;
 476         case 16 ... 31:         /* watchpoints */
 477                 vr += CPUDBG_WVR_BASE;
 478                 cr += CPUDBG_WCR_BASE;
 479                 index -= 16;
 480                 break;
 481         default:
 482                 return ERROR_FAIL;
 483         }
 484         vr += 4 * index;
 485         cr += 4 * index;
 486
 487         LOG_DEBUG("A8: bpwp enable, vr %08x cr %08x",
 488                         (unsigned) vr, (unsigned) cr);
 489
 490         retval = cortex_a8_dap_write_memap_register_u32(dpm->arm->target,
 491                         vr, addr);
 492         if (retval != ERROR_OK)
 493                 return retval;
 494         retval = cortex_a8_dap_write_memap_register_u32(dpm->arm->target,
 495                         cr, control);
 496         return retval;
 497 }
 498
 499 static int cortex_a8_bpwp_disable(struct arm_dpm *dpm, unsigned index)
 500 {
 501         struct cortex_a8_common *a8 = dpm_to_a8(dpm);
 502         uint32_t cr;
 503
 504         switch (index) {
 505         case 0 ... 15:
 506                 cr = a8->armv7a_common.debug_base + CPUDBG_BCR_BASE;
 507                 break;
 508         case 16 ... 31:
 509                 cr = a8->armv7a_common.debug_base + CPUDBG_WCR_BASE;
 510                 index -= 16;
 511                 break;
 512         default:
 513                 return ERROR_FAIL;
 514         }
 515         cr += 4 * index;
 516
 517         LOG_DEBUG("A8: bpwp disable, cr %08x", (unsigned) cr);
 518
 519         /* clear control register */
 520         return cortex_a8_dap_write_memap_register_u32(dpm->arm->target, cr, 0);
 521 }
 522
 523 static int cortex_a8_dpm_setup(struct cortex_a8_common *a8, uint32_t didr)
 524 {
 525         struct arm_dpm *dpm = &a8->armv7a_common.dpm;
 526
 527         dpm->arm = &a8->armv7a_common.armv4_5_common;
 528         dpm->didr = didr;
 529
 530         dpm->prepare = cortex_a8_dpm_prepare;
 531         dpm->finish = cortex_a8_dpm_finish;
 532
 533         dpm->instr_write_data_dcc = cortex_a8_instr_write_data_dcc;
 534         dpm->instr_write_data_r0 = cortex_a8_instr_write_data_r0;
 535         dpm->instr_cpsr_sync = cortex_a8_instr_cpsr_sync;
 536
 537         dpm->instr_read_data_dcc = cortex_a8_instr_read_data_dcc;
 538         dpm->instr_read_data_r0 = cortex_a8_instr_read_data_r0;
 539
 540         dpm->bpwp_enable = cortex_a8_bpwp_enable;
 541         dpm->bpwp_disable = cortex_a8_bpwp_disable;
 542
 543         return arm_dpm_setup(dpm);
 544 }
 545
 546
 547 /*
 548  * Cortex-A8 Run control
 549  */
 550
 551 static int cortex_a8_poll(struct target *target)
 552 {
 553         int retval = ERROR_OK;
 554         uint32_t dscr;
 555         struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
 556         struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
 557         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 558         enum target_state prev_target_state = target->state;
 559         uint8_t saved_apsel = dap_ap_get_select(swjdp);
 560
 561         dap_ap_select(swjdp, swjdp_debugap);
 562         retval = mem_ap_read_atomic_u32(swjdp,
 563                         armv7a->debug_base + CPUDBG_DSCR, &dscr);
 564         if (retval != ERROR_OK)
 565         {
 566                 dap_ap_select(swjdp, saved_apsel);
 567                 return retval;
 568         }
 569         cortex_a8->cpudbg_dscr = dscr;
 570
 571         if ((dscr & 0x3) == 0x3)
 572         {
 573                 if (prev_target_state != TARGET_HALTED)
 574                 {
 575                         /* We have a halting debug event */
 576                         LOG_DEBUG("Target halted");
 577                         target->state = TARGET_HALTED;
 578                         if ((prev_target_state == TARGET_RUNNING)
 579                                         || (prev_target_state == TARGET_RESET))
 580                         {
 581                                 retval = cortex_a8_debug_entry(target);
 582                                 if (retval != ERROR_OK)
 583                                         return retval;
 584
 585                                 target_call_event_callbacks(target,
 586                                                 TARGET_EVENT_HALTED);
 587                         }
 588                         if (prev_target_state == TARGET_DEBUG_RUNNING)
 589                         {
 590                                 LOG_DEBUG(" ");
 591
 592                                 retval = cortex_a8_debug_entry(target);
 593                                 if (retval != ERROR_OK)
 594                                         return retval;
 595
 596                                 target_call_event_callbacks(target,
 597                                                 TARGET_EVENT_DEBUG_HALTED);
 598                         }
 599                 }
 600         }
 601         else if ((dscr & 0x3) == 0x2)
 602         {
 603                 target->state = TARGET_RUNNING;
 604         }
 605         else
 606         {
 607                 LOG_DEBUG("Unknown target state dscr = 0x%08" PRIx32, dscr);
 608                 target->state = TARGET_UNKNOWN;
 609         }
 610
 611         dap_ap_select(swjdp, saved_apsel);
 612
 613         return retval;
 614 }
 615
 616 static int cortex_a8_halt(struct target *target)
 617 {
 618         int retval = ERROR_OK;
 619         uint32_t dscr;
 620         struct armv7a_common *armv7a = target_to_armv7a(target);
 621         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 622         uint8_t saved_apsel = dap_ap_get_select(swjdp);
 623         dap_ap_select(swjdp, swjdp_debugap);
 624
 625         /*
 626          * Tell the core to be halted by writing DRCR with 0x1
 627          * and then wait for the core to be halted.
 628          */
 629         retval = mem_ap_write_atomic_u32(swjdp,
 630                         armv7a->debug_base + CPUDBG_DRCR, 0x1);
 631
 632         /*
 633          * enter halting debug mode
 634          */
 635         mem_ap_read_atomic_u32(swjdp, armv7a->debug_base + CPUDBG_DSCR, &dscr);
 636         retval = mem_ap_write_atomic_u32(swjdp,
 637                 armv7a->debug_base + CPUDBG_DSCR, dscr | (1 << DSCR_HALT_DBG_MODE));
 638
 639         if (retval != ERROR_OK)
 640                 goto out;
 641
 642         do {
 643                 mem_ap_read_atomic_u32(swjdp,
 644                         armv7a->debug_base + CPUDBG_DSCR, &dscr);
 645         } while ((dscr & (1 << DSCR_CORE_HALTED)) == 0);
 646
 647         target->debug_reason = DBG_REASON_DBGRQ;
 648
 649 out:
 650         dap_ap_select(swjdp, saved_apsel);
 651         return retval;
 652 }
 653
 654 static int cortex_a8_resume(struct target *target, int current,
 655                 uint32_t address, int handle_breakpoints, int debug_execution)
 656 {
 657         struct armv7a_common *armv7a = target_to_armv7a(target);
 658         struct arm *armv4_5 = &armv7a->armv4_5_common;
 659         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 660
 661 //      struct breakpoint *breakpoint = NULL;
 662         uint32_t resume_pc, dscr;
 663
 664         uint8_t saved_apsel = dap_ap_get_select(swjdp);
 665         dap_ap_select(swjdp, swjdp_debugap);
 666
 667         if (!debug_execution)
 668                 target_free_all_working_areas(target);
 669
 670 #if 0
 671         if (debug_execution)
 672         {
 673                 /* Disable interrupts */
 674                 /* We disable interrupts in the PRIMASK register instead of
 675                  * masking with C_MASKINTS,
 676                  * This is probably the same issue as Cortex-M3 Errata 377493:
 677                  * C_MASKINTS in parallel with disabled interrupts can cause
 678                  * local faults to not be taken. */
 679                 buf_set_u32(armv7m->core_cache->reg_list[ARMV7M_PRIMASK].value, 0, 32, 1);
 680                 armv7m->core_cache->reg_list[ARMV7M_PRIMASK].dirty = 1;
 681                 armv7m->core_cache->reg_list[ARMV7M_PRIMASK].valid = 1;
 682
 683                 /* Make sure we are in Thumb mode */
 684                 buf_set_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32,
 685                         buf_get_u32(armv7m->core_cache->reg_list[ARMV7M_xPSR].value, 0, 32) | (1 << 24));
 686                 armv7m->core_cache->reg_list[ARMV7M_xPSR].dirty = 1;
 687                 armv7m->core_cache->reg_list[ARMV7M_xPSR].valid = 1;
 688         }
 689 #endif
 690
 691         /* current = 1: continue on current pc, otherwise continue at <address> */
 692         resume_pc = buf_get_u32(
 693                         armv4_5->core_cache->reg_list[15].value,
 694                         0, 32);
 695         if (!current)
 696                 resume_pc = address;
 697
 698         /* Make sure that the Armv7 gdb thumb fixups does not
 699          * kill the return address
 700          */
 701         switch (armv4_5->core_state)
 702         {
 703         case ARMV4_5_STATE_ARM:
 704                 resume_pc &= 0xFFFFFFFC;
 705                 break;
 706         case ARMV4_5_STATE_THUMB:
 707         case ARM_STATE_THUMB_EE:
 708                 /* When the return address is loaded into PC
 709                  * bit 0 must be 1 to stay in Thumb state
 710                  */
 711                 resume_pc |= 0x1;
 712                 break;
 713         case ARMV4_5_STATE_JAZELLE:
 714                 LOG_ERROR("How do I resume into Jazelle state??");
 715                 return ERROR_FAIL;
 716         }
 717         LOG_DEBUG("resume pc = 0x%08" PRIx32, resume_pc);
 718         buf_set_u32(armv4_5->core_cache->reg_list[15].value,
 719                         0, 32, resume_pc);
 720         armv4_5->core_cache->reg_list[15].dirty = 1;
 721         armv4_5->core_cache->reg_list[15].valid = 1;
 722
 723         cortex_a8_restore_context(target, handle_breakpoints);
 724
 725 #if 0
 726         /* the front-end may request us not to handle breakpoints */
 727         if (handle_breakpoints)
 728         {
 729                 /* Single step past breakpoint at current address */
 730                 if ((breakpoint = breakpoint_find(target, resume_pc)))
 731                 {
 732                         LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
 733                         cortex_m3_unset_breakpoint(target, breakpoint);
 734                         cortex_m3_single_step_core(target);
 735                         cortex_m3_set_breakpoint(target, breakpoint);
 736                 }
 737         }
 738
 739 #endif
 740         /* Restart core and wait for it to be started */
 741         mem_ap_write_atomic_u32(swjdp, armv7a->debug_base + CPUDBG_DRCR, 0x2);
 742
 743         do {
 744                 mem_ap_read_atomic_u32(swjdp,
 745                         armv7a->debug_base + CPUDBG_DSCR, &dscr);
 746         } while ((dscr & (1 << DSCR_CORE_RESTARTED)) == 0);
 747
 748         target->debug_reason = DBG_REASON_NOTHALTED;
 749         target->state = TARGET_RUNNING;
 750
 751         /* registers are now invalid */
 752         register_cache_invalidate(armv4_5->core_cache);
 753
 754         if (!debug_execution)
 755         {
 756                 target->state = TARGET_RUNNING;
 757                 target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 758                 LOG_DEBUG("target resumed at 0x%" PRIx32, resume_pc);
 759         }
 760         else
 761         {
 762                 target->state = TARGET_DEBUG_RUNNING;
 763                 target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
 764                 LOG_DEBUG("target debug resumed at 0x%" PRIx32, resume_pc);
 765         }
 766
 767         dap_ap_select(swjdp, saved_apsel);
 768
 769         return ERROR_OK;
 770 }
 771
 772 static int cortex_a8_debug_entry(struct target *target)
 773 {
 774         int i;
 775         uint32_t regfile[16], pc, cpsr, dscr;
 776         int retval = ERROR_OK;
 777         struct working_area *regfile_working_area = NULL;
 778         struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
 779         struct armv7a_common *armv7a = target_to_armv7a(target);
 780         struct arm *armv4_5 = &armv7a->armv4_5_common;
 781         struct swjdp_common *swjdp = &armv7a->swjdp_info;
 782         struct reg *reg;
 783
 784         LOG_DEBUG("dscr = 0x%08" PRIx32, cortex_a8->cpudbg_dscr);
 785
 786         /* Enable the ITR execution once we are in debug mode */
 787         mem_ap_read_atomic_u32(swjdp,
 788                                 armv7a->debug_base + CPUDBG_DSCR, &dscr);
 789
 790         /* REVISIT see A8 TRM 12.11.4 steps 2..3 -- make sure that any
 791          * imprecise data aborts get discarded by issuing a Data
 792          * Synchronization Barrier:  ARMV4_5_MCR(15, 0, 0, 7, 10, 4).
 793          */
 794
 795         dscr |= (1 << DSCR_EXT_INT_EN);
 796         retval = mem_ap_write_atomic_u32(swjdp,
 797                         armv7a->debug_base + CPUDBG_DSCR, dscr);
 798
 799         /* Examine debug reason */
 800         switch ((cortex_a8->cpudbg_dscr >> 2)&0xF)
 801         {
 802                 case 0:         /* DRCR[0] write */
 803                 case 4:         /* EDBGRQ */
 804                         target->debug_reason = DBG_REASON_DBGRQ;
 805                         break;
 806                 case 1:         /* HW breakpoint */
 807                 case 3:         /* SW BKPT */
 808                 case 5:         /* vector catch */
 809                         target->debug_reason = DBG_REASON_BREAKPOINT;
 810                         break;
 811                 case 2:         /* asynch watchpoint */
 812                 case 10:        /* precise watchpoint */
 813                         target->debug_reason = DBG_REASON_WATCHPOINT;
 814                         /* REVISIT could collect WFAR later, to see just
 815                          * which instruction triggered the watchpoint.
 816                          */
 817                         break;
 818                 default:
 819                         target->debug_reason = DBG_REASON_UNDEFINED;
 820                         break;
 821         }
 822
 823         /* REVISIT fast_reg_read is never set ... */
 824
 825         /* Examine target state and mode */
 826         if (cortex_a8->fast_reg_read)
 827                 target_alloc_working_area(target, 64, &regfile_working_area);
 828
 829         /* First load register acessible through core debug port*/
 830         if (!regfile_working_area)
 831         {
 832                 retval = arm_dpm_read_current_registers(&armv7a->dpm);
 833         }
 834         else
 835         {
 836                 dap_ap_select(swjdp, swjdp_memoryap);
 837                 cortex_a8_read_regs_through_mem(target,
 838                                 regfile_working_area->address, regfile);
 839                 dap_ap_select(swjdp, swjdp_memoryap);
 840                 target_free_working_area(target, regfile_working_area);
 841
 842                 /* read Current PSR */
 843                 cortex_a8_dap_read_coreregister_u32(target, &cpsr, 16);
 844                 pc = regfile[15];
 845                 dap_ap_select(swjdp, swjdp_debugap);
 846                 LOG_DEBUG("cpsr: %8.8" PRIx32, cpsr);
 847
 848                 arm_set_cpsr(armv4_5, cpsr);
 849
 850                 /* update cache */
 851                 for (i = 0; i <= ARM_PC; i++)
 852                 {
 853                         reg = arm_reg_current(armv4_5, i);
 854
 855                         buf_set_u32(reg->value, 0, 32, regfile[i]);
 856                         reg->valid = 1;
 857                         reg->dirty = 0;
 858                 }
 859
 860                 /* Fixup PC Resume Address */
 861                 if (cpsr & (1 << 5))
 862                 {
 863                         // T bit set for Thumb or ThumbEE state
 864                         regfile[ARM_PC] -= 4;
 865                 }
 866                 else
 867                 {
 868                         // ARM state
 869                         regfile[ARM_PC] -= 8;
 870                 }
 871
 872                 reg = armv4_5->core_cache->reg_list + 15;
 873                 buf_set_u32(reg->value, 0, 32, regfile[ARM_PC]);
 874                 reg->dirty = reg->valid;
 875         }
 876
 877 #if 0
 878 /* TODO, Move this */
 879         uint32_t cp15_control_register, cp15_cacr, cp15_nacr;
 880         cortex_a8_read_cp(target, &cp15_control_register, 15, 0, 1, 0, 0);
 881         LOG_DEBUG("cp15_control_register = 0x%08x", cp15_control_register);
 882
 883         cortex_a8_read_cp(target, &cp15_cacr, 15, 0, 1, 0, 2);
 884         LOG_DEBUG("cp15 Coprocessor Access Control Register = 0x%08x", cp15_cacr);
 885
 886         cortex_a8_read_cp(target, &cp15_nacr, 15, 0, 1, 1, 2);
 887         LOG_DEBUG("cp15 Nonsecure Access Control Register = 0x%08x", cp15_nacr);
 888 #endif
 889
 890         /* Are we in an exception handler */
 891 //      armv4_5->exception_number = 0;
 892         if (armv7a->post_debug_entry)
 893                 armv7a->post_debug_entry(target);
 894
 895
 896
 897         return retval;
 898
 899 }
 900
 901 static void cortex_a8_post_debug_entry(struct target *target)
 902 {
 903         struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
 904         struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
 905         int retval;
 906
 907         /* MRC p15,0,<Rt>,c1,c0,0 ; Read CP15 System Control Register */
 908         retval = armv7a->armv4_5_common.mrc(target, 15,
 909                         0, 0,   /* op1, op2 */
 910                         1, 0,   /* CRn, CRm */
 911                         &cortex_a8->cp15_control_reg);
 912         LOG_DEBUG("cp15_control_reg: %8.8" PRIx32, cortex_a8->cp15_control_reg);
 913
 914         if (armv7a->armv4_5_mmu.armv4_5_cache.ctype == -1)
 915         {
 916                 uint32_t cache_type_reg;
 917
 918                 /* MRC p15,0,<Rt>,c0,c0,1 ; Read CP15 Cache Type Register */
 919                 retval = armv7a->armv4_5_common.mrc(target, 15,
 920                                 0, 1,   /* op1, op2 */
 921                                 0, 0,   /* CRn, CRm */
 922                                 &cache_type_reg);
 923                 LOG_DEBUG("cp15 cache type: %8.8x", (unsigned) cache_type_reg);
 924
 925                 /* FIXME the armv4_4 cache info DOES NOT APPLY to Cortex-A8 */
 926                 armv4_5_identify_cache(cache_type_reg,
 927                                 &armv7a->armv4_5_mmu.armv4_5_cache);
 928         }
 929
 930         armv7a->armv4_5_mmu.mmu_enabled =
 931                         (cortex_a8->cp15_control_reg & 0x1U) ? 1 : 0;
 932         armv7a->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled =
 933                         (cortex_a8->cp15_control_reg & 0x4U) ? 1 : 0;
 934         armv7a->armv4_5_mmu.armv4_5_cache.i_cache_enabled =
 935                         (cortex_a8->cp15_control_reg & 0x1000U) ? 1 : 0;
 936
 937
 938 }
 939
 940 static int cortex_a8_step(struct target *target, int current, uint32_t address,
 941                 int handle_breakpoints)
 942 {
 943         struct armv7a_common *armv7a = target_to_armv7a(target);
 944         struct arm *armv4_5 = &armv7a->armv4_5_common;
 945         struct breakpoint *breakpoint = NULL;
 946         struct breakpoint stepbreakpoint;
 947         struct reg *r;
 948
 949         int timeout = 100;
 950
 951         if (target->state != TARGET_HALTED)
 952         {
 953                 LOG_WARNING("target not halted");
 954                 return ERROR_TARGET_NOT_HALTED;
 955         }
 956
 957         /* current = 1: continue on current pc, otherwise continue at <address> */
 958         r = armv4_5->core_cache->reg_list + 15;
 959         if (!current)
 960         {
 961                 buf_set_u32(r->value, 0, 32, address);
 962         }
 963         else
 964         {
 965                 address = buf_get_u32(r->value, 0, 32);
 966         }
 967
 968         /* The front-end may request us not to handle breakpoints.
 969          * But since Cortex-A8 uses breakpoint for single step,
 970          * we MUST handle breakpoints.
 971          */
 972         handle_breakpoints = 1;
 973         if (handle_breakpoints) {
 974                 breakpoint = breakpoint_find(target, address);
 975                 if (breakpoint)
 976                         cortex_a8_unset_breakpoint(target, breakpoint);
 977         }
 978
 979         /* Setup single step breakpoint */
 980         stepbreakpoint.address = address;
 981         stepbreakpoint.length = (armv4_5->core_state == ARMV4_5_STATE_THUMB)
 982                         ? 2 : 4;
 983         stepbreakpoint.type = BKPT_HARD;
 984         stepbreakpoint.set = 0;
 985
 986         /* Break on IVA mismatch */
 987         cortex_a8_set_breakpoint(target, &stepbreakpoint, 0x04);
 988
 989         target->debug_reason = DBG_REASON_SINGLESTEP;
 990
 991         cortex_a8_resume(target, 1, address, 0, 0);
 992
 993         while (target->state != TARGET_HALTED)
 994         {
 995                 cortex_a8_poll(target);
 996                 if (--timeout == 0)
 997                 {
 998                         LOG_WARNING("timeout waiting for target halt");
 999                         break;
1000                 }
1001         }
1002
1003         cortex_a8_unset_breakpoint(target, &stepbreakpoint);
1004         if (timeout > 0) target->debug_reason = DBG_REASON_BREAKPOINT;
1005
1006         if (breakpoint)
1007                 cortex_a8_set_breakpoint(target, breakpoint, 0);
1008
1009         if (target->state != TARGET_HALTED)
1010                 LOG_DEBUG("target stepped");
1011
1012         return ERROR_OK;
1013 }
1014
1015 static int cortex_a8_restore_context(struct target *target, bool bpwp)
1016 {
1017         struct armv7a_common *armv7a = target_to_armv7a(target);
1018
1019         LOG_DEBUG(" ");
1020
1021         if (armv7a->pre_restore_context)
1022                 armv7a->pre_restore_context(target);
1023
1024         arm_dpm_write_dirty_registers(&armv7a->dpm, bpwp);
1025
1026         if (armv7a->post_restore_context)
1027                 armv7a->post_restore_context(target);
1028
1029         return ERROR_OK;
1030 }
1031
1032
1033 /*
1034  * Cortex-A8 Breakpoint and watchpoint fuctions
1035  */
1036
1037 /* Setup hardware Breakpoint Register Pair */
1038 static int cortex_a8_set_breakpoint(struct target *target,
1039                 struct breakpoint *breakpoint, uint8_t matchmode)
1040 {
1041         int retval;
1042         int brp_i=0;
1043         uint32_t control;
1044         uint8_t byte_addr_select = 0x0F;
1045         struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
1046         struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
1047         struct cortex_a8_brp * brp_list = cortex_a8->brp_list;
1048
1049         if (breakpoint->set)
1050         {
1051                 LOG_WARNING("breakpoint already set");
1052                 return ERROR_OK;
1053         }
1054
1055         if (breakpoint->type == BKPT_HARD)
1056         {
1057                 while (brp_list[brp_i].used && (brp_i < cortex_a8->brp_num))
1058                         brp_i++ ;
1059                 if (brp_i >= cortex_a8->brp_num)
1060                 {
1061                         LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
1062                         return ERROR_FAIL;
1063                 }
1064                 breakpoint->set = brp_i + 1;
1065                 if (breakpoint->length == 2)
1066                 {
1067                         byte_addr_select = (3 << (breakpoint->address & 0x02));
1068                 }
1069                 control = ((matchmode & 0x7) << 20)
1070                                 | (byte_addr_select << 5)
1071                                 | (3 << 1) | 1;
1072                 brp_list[brp_i].used = 1;
1073                 brp_list[brp_i].value = (breakpoint->address & 0xFFFFFFFC);
1074                 brp_list[brp_i].control = control;
1075                 cortex_a8_dap_write_memap_register_u32(target, armv7a->debug_base
1076                                 + CPUDBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
1077                                 brp_list[brp_i].value);
1078                 cortex_a8_dap_write_memap_register_u32(target, armv7a->debug_base
1079                                 + CPUDBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
1080                                 brp_list[brp_i].control);
1081                 LOG_DEBUG("brp %i control 0x%0" PRIx32 " value 0x%0" PRIx32, brp_i,
1082                                 brp_list[brp_i].control,
1083                                 brp_list[brp_i].value);
1084         }
1085         else if (breakpoint->type == BKPT_SOFT)
1086         {
1087                 uint8_t code[4];
1088                 if (breakpoint->length == 2)
1089                 {
1090                         buf_set_u32(code, 0, 32, ARMV5_T_BKPT(0x11));
1091                 }
1092                 else
1093                 {
1094                         buf_set_u32(code, 0, 32, ARMV5_BKPT(0x11));
1095                 }
1096                 retval = target->type->read_memory(target,
1097                                 breakpoint->address & 0xFFFFFFFE,
1098                                 breakpoint->length, 1,
1099                                 breakpoint->orig_instr);
1100                 if (retval != ERROR_OK)
1101                         return retval;
1102                 retval = target->type->write_memory(target,
1103                                 breakpoint->address & 0xFFFFFFFE,
1104                                 breakpoint->length, 1, code);
1105                 if (retval != ERROR_OK)
1106                         return retval;
1107                 breakpoint->set = 0x11; /* Any nice value but 0 */
1108         }
1109
1110         return ERROR_OK;
1111 }
1112
1113 static int cortex_a8_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
1114 {
1115         int retval;
1116         struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
1117         struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
1118         struct cortex_a8_brp * brp_list = cortex_a8->brp_list;
1119
1120         if (!breakpoint->set)
1121         {
1122                 LOG_WARNING("breakpoint not set");
1123                 return ERROR_OK;
1124         }
1125
1126         if (breakpoint->type == BKPT_HARD)
1127         {
1128                 int brp_i = breakpoint->set - 1;
1129                 if ((brp_i < 0) || (brp_i >= cortex_a8->brp_num))
1130                 {
1131                         LOG_DEBUG("Invalid BRP number in breakpoint");
1132                         return ERROR_OK;
1133                 }
1134                 LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%0" PRIx32, brp_i,
1135                                 brp_list[brp_i].control, brp_list[brp_i].value);
1136                 brp_list[brp_i].used = 0;
1137                 brp_list[brp_i].value = 0;
1138                 brp_list[brp_i].control = 0;
1139                 cortex_a8_dap_write_memap_register_u32(target, armv7a->debug_base
1140                                 + CPUDBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
1141                                 brp_list[brp_i].control);
1142                 cortex_a8_dap_write_memap_register_u32(target, armv7a->debug_base
1143                                 + CPUDBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
1144                                 brp_list[brp_i].value);
1145         }
1146         else
1147         {
1148                 /* restore original instruction (kept in target endianness) */
1149                 if (breakpoint->length == 4)
1150                 {
1151                         retval = target->type->write_memory(target,
1152                                         breakpoint->address & 0xFFFFFFFE,
1153                                         4, 1, breakpoint->orig_instr);
1154                         if (retval != ERROR_OK)
1155                                 return retval;
1156                 }
1157                 else
1158                 {
1159                         retval = target->type->write_memory(target,
1160                                         breakpoint->address & 0xFFFFFFFE,
1161                                         2, 1, breakpoint->orig_instr);
1162                         if (retval != ERROR_OK)
1163                                 return retval;
1164                 }
1165         }
1166         breakpoint->set = 0;
1167
1168         return ERROR_OK;
1169 }
1170
1171 static int cortex_a8_add_breakpoint(struct target *target,
1172                 struct breakpoint *breakpoint)
1173 {
1174         struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
1175
1176         if ((breakpoint->type == BKPT_HARD) && (cortex_a8->brp_num_available < 1))
1177         {
1178                 LOG_INFO("no hardware breakpoint available");
1179                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1180         }
1181
1182         if (breakpoint->type == BKPT_HARD)
1183                 cortex_a8->brp_num_available--;
1184         cortex_a8_set_breakpoint(target, breakpoint, 0x00); /* Exact match */
1185
1186         return ERROR_OK;
1187 }
1188
1189 static int cortex_a8_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
1190 {
1191         struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
1192
1193 #if 0
1194 /* It is perfectly possible to remove brakpoints while the taget is running */
1195         if (target->state != TARGET_HALTED)
1196         {
1197                 LOG_WARNING("target not halted");
1198                 return ERROR_TARGET_NOT_HALTED;
1199         }
1200 #endif
1201
1202         if (breakpoint->set)
1203         {
1204                 cortex_a8_unset_breakpoint(target, breakpoint);
1205                 if (breakpoint->type == BKPT_HARD)
1206                         cortex_a8->brp_num_available++ ;
1207         }
1208
1209
1210         return ERROR_OK;
1211 }
1212
1213
1214
1215 /*
1216  * Cortex-A8 Reset fuctions
1217  */
1218
1219 static int cortex_a8_assert_reset(struct target *target)
1220 {
1221         struct armv7a_common *armv7a = target_to_armv7a(target);
1222
1223         LOG_DEBUG(" ");
1224
1225         /* FIXME when halt is requested, make it work somehow... */
1226
1227         /* Issue some kind of warm reset. */
1228         if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT)) {
1229                 target_handle_event(target, TARGET_EVENT_RESET_ASSERT);
1230         } else if (jtag_get_reset_config() & RESET_HAS_SRST) {
1231                 /* REVISIT handle "pulls" cases, if there's
1232                  * hardware that needs them to work.
1233                  */
1234                 jtag_add_reset(0, 1);
1235         } else {
1236                 LOG_ERROR("%s: how to reset?", target_name(target));
1237                 return ERROR_FAIL;
1238         }
1239
1240         /* registers are now invalid */
1241         register_cache_invalidate(armv7a->armv4_5_common.core_cache);
1242
1243         target->state = TARGET_RESET;
1244
1245         return ERROR_OK;
1246 }
1247
1248 static int cortex_a8_deassert_reset(struct target *target)
1249 {
1250         int retval;
1251
1252         LOG_DEBUG(" ");
1253
1254         /* be certain SRST is off */
1255         jtag_add_reset(0, 0);
1256
1257         retval = cortex_a8_poll(target);
1258
1259         if (target->reset_halt) {
1260                 if (target->state != TARGET_HALTED) {
1261                         LOG_WARNING("%s: ran after reset and before halt ...",
1262                                         target_name(target));
1263                         if ((retval = target_halt(target)) != ERROR_OK)
1264                                 return retval;
1265                 }
1266         }
1267
1268         return ERROR_OK;
1269 }
1270
1271 /*
1272  * Cortex-A8 Memory access
1273  *
1274  * This is same Cortex M3 but we must also use the correct
1275  * ap number for every access.
1276  */
1277
1278 static int cortex_a8_read_memory(struct target *target, uint32_t address,
1279                 uint32_t size, uint32_t count, uint8_t *buffer)
1280 {
1281         struct armv7a_common *armv7a = target_to_armv7a(target);
1282         struct swjdp_common *swjdp = &armv7a->swjdp_info;
1283         int retval = ERROR_INVALID_ARGUMENTS;
1284
1285         /* cortex_a8 handles unaligned memory access */
1286
1287 // ???  dap_ap_select(swjdp, swjdp_memoryap);
1288
1289         if (count && buffer) {
1290                 switch (size) {
1291                 case 4:
1292                         retval = mem_ap_read_buf_u32(swjdp, buffer, 4 * count, address);
1293                         break;
1294                 case 2:
1295                         retval = mem_ap_read_buf_u16(swjdp, buffer, 2 * count, address);
1296                         break;
1297                 case 1:
1298                         retval = mem_ap_read_buf_u8(swjdp, buffer, count, address);
1299                         break;
1300                 }
1301         }
1302
1303         return retval;
1304 }
1305
1306 static int cortex_a8_write_memory(struct target *target, uint32_t address,
1307                 uint32_t size, uint32_t count, uint8_t *buffer)
1308 {
1309         struct armv7a_common *armv7a = target_to_armv7a(target);
1310         struct swjdp_common *swjdp = &armv7a->swjdp_info;
1311         int retval = ERROR_INVALID_ARGUMENTS;
1312
1313 // ???  dap_ap_select(swjdp, swjdp_memoryap);
1314
1315         if (count && buffer) {
1316                 switch (size) {
1317                 case 4:
1318                         retval = mem_ap_write_buf_u32(swjdp, buffer, 4 * count, address);
1319                         break;
1320                 case 2:
1321                         retval = mem_ap_write_buf_u16(swjdp, buffer, 2 * count, address);
1322                         break;
1323                 case 1:
1324                         retval = mem_ap_write_buf_u8(swjdp, buffer, count, address);
1325                         break;
1326                 }
1327         }
1328
1329         /* REVISIT this op is generic ARMv7-A/R stuff */
1330         if (retval == ERROR_OK && target->state == TARGET_HALTED)
1331         {
1332                 struct arm_dpm *dpm = armv7a->armv4_5_common.dpm;
1333
1334                 retval = dpm->prepare(dpm);
1335                 if (retval != ERROR_OK)
1336                         return retval;
1337
1338                 /* The Cache handling will NOT work with MMU active, the
1339                  * wrong addresses will be invalidated!
1340                  *
1341                  * For both ICache and DCache, walk all cache lines in the
1342                  * address range. Cortex-A8 has fixed 64 byte line length.
1343                  *
1344                  * REVISIT per ARMv7, these may trigger watchpoints ...
1345                  */
1346
1347                 /* invalidate I-Cache */
1348                 if (armv7a->armv4_5_mmu.armv4_5_cache.i_cache_enabled)
1349                 {
1350                         /* ICIMVAU - Invalidate Cache single entry
1351                          * with MVA to PoU
1352                          *      MCR p15, 0, r0, c7, c5, 1
1353                          */
1354                         for (uint32_t cacheline = address;
1355                                         cacheline < address + size * count;
1356                                         cacheline += 64) {
1357                                 retval = dpm->instr_write_data_r0(dpm,
1358                                         ARMV4_5_MCR(15, 0, 0, 7, 5, 1),
1359                                         cacheline);
1360                         }
1361                 }
1362
1363                 /* invalidate D-Cache */
1364                 if (armv7a->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
1365                 {
1366                         /* DCIMVAC - Invalidate data Cache line
1367                          * with MVA to PoC
1368                          *      MCR p15, 0, r0, c7, c6, 1
1369                          */
1370                         for (uint32_t cacheline = address;
1371                                         cacheline < address + size * count;
1372                                         cacheline += 64) {
1373                                 retval = dpm->instr_write_data_r0(dpm,
1374                                         ARMV4_5_MCR(15, 0, 0, 7, 6, 1),
1375                                         cacheline);
1376                         }
1377                 }
1378
1379                 /* (void) */ dpm->finish(dpm);
1380         }
1381
1382         return retval;
1383 }
1384
1385 static int cortex_a8_bulk_write_memory(struct target *target, uint32_t address,
1386                 uint32_t count, uint8_t *buffer)
1387 {
1388         return cortex_a8_write_memory(target, address, 4, count, buffer);
1389 }
1390
1391
1392 static int cortex_a8_dcc_read(struct swjdp_common *swjdp, uint8_t *value, uint8_t *ctrl)
1393 {
1394 #if 0
1395         u16 dcrdr;
1396
1397         mem_ap_read_buf_u16(swjdp, (uint8_t*)&dcrdr, 1, DCB_DCRDR);
1398         *ctrl = (uint8_t)dcrdr;
1399         *value = (uint8_t)(dcrdr >> 8);
1400
1401         LOG_DEBUG("data 0x%x ctrl 0x%x", *value, *ctrl);
1402
1403         /* write ack back to software dcc register
1404          * signify we have read data */
1405         if (dcrdr & (1 << 0))
1406         {
1407                 dcrdr = 0;
1408                 mem_ap_write_buf_u16(swjdp, (uint8_t*)&dcrdr, 1, DCB_DCRDR);
1409         }
1410 #endif
1411         return ERROR_OK;
1412 }
1413
1414
1415 static int cortex_a8_handle_target_request(void *priv)
1416 {
1417         struct target *target = priv;
1418         struct armv7a_common *armv7a = target_to_armv7a(target);
1419         struct swjdp_common *swjdp = &armv7a->swjdp_info;
1420
1421         if (!target_was_examined(target))
1422                 return ERROR_OK;
1423         if (!target->dbg_msg_enabled)
1424                 return ERROR_OK;
1425
1426         if (target->state == TARGET_RUNNING)
1427         {
1428                 uint8_t data = 0;
1429                 uint8_t ctrl = 0;
1430
1431                 cortex_a8_dcc_read(swjdp, &data, &ctrl);
1432
1433                 /* check if we have data */
1434                 if (ctrl & (1 << 0))
1435                 {
1436                         uint32_t request;
1437
1438                         /* we assume target is quick enough */
1439                         request = data;
1440                         cortex_a8_dcc_read(swjdp, &data, &ctrl);
1441                         request |= (data << 8);
1442                         cortex_a8_dcc_read(swjdp, &data, &ctrl);
1443                         request |= (data << 16);
1444                         cortex_a8_dcc_read(swjdp, &data, &ctrl);
1445                         request |= (data << 24);
1446                         target_request(target, request);
1447                 }
1448         }
1449
1450         return ERROR_OK;
1451 }
1452
1453 /*
1454  * Cortex-A8 target information and configuration
1455  */
1456
1457 static int cortex_a8_examine_first(struct target *target)
1458 {
1459         struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
1460         struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
1461         struct swjdp_common *swjdp = &armv7a->swjdp_info;
1462         int i;
1463         int retval = ERROR_OK;
1464         uint32_t didr, ctypr, ttypr, cpuid;
1465
1466         LOG_DEBUG("TODO");
1467
1468         /* Here we shall insert a proper ROM Table scan */
1469         armv7a->debug_base = OMAP3530_DEBUG_BASE;
1470
1471         /* We do one extra read to ensure DAP is configured,
1472          * we call ahbap_debugport_init(swjdp) instead
1473          */
1474         ahbap_debugport_init(swjdp);
1475         mem_ap_read_atomic_u32(swjdp, armv7a->debug_base + CPUDBG_CPUID, &cpuid);
1476         if ((retval = mem_ap_read_atomic_u32(swjdp,
1477                         armv7a->debug_base + CPUDBG_CPUID, &cpuid)) != ERROR_OK)
1478         {
1479                 LOG_DEBUG("Examine failed");
1480                 return retval;
1481         }
1482
1483         if ((retval = mem_ap_read_atomic_u32(swjdp,
1484                         armv7a->debug_base + CPUDBG_CTYPR, &ctypr)) != ERROR_OK)
1485         {
1486                 LOG_DEBUG("Examine failed");
1487                 return retval;
1488         }
1489
1490         if ((retval = mem_ap_read_atomic_u32(swjdp,
1491                         armv7a->debug_base + CPUDBG_TTYPR, &ttypr)) != ERROR_OK)
1492         {
1493                 LOG_DEBUG("Examine failed");
1494                 return retval;
1495         }
1496
1497         if ((retval = mem_ap_read_atomic_u32(swjdp,
1498                         armv7a->debug_base + CPUDBG_DIDR, &didr)) != ERROR_OK)
1499         {
1500                 LOG_DEBUG("Examine failed");
1501                 return retval;
1502         }
1503
1504         LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
1505         LOG_DEBUG("ctypr = 0x%08" PRIx32, ctypr);
1506         LOG_DEBUG("ttypr = 0x%08" PRIx32, ttypr);
1507         LOG_DEBUG("didr = 0x%08" PRIx32, didr);
1508
1509         armv7a->armv4_5_common.core_type = ARM_MODE_MON;
1510         cortex_a8_dpm_setup(cortex_a8, didr);
1511
1512         /* Setup Breakpoint Register Pairs */
1513         cortex_a8->brp_num = ((didr >> 24) & 0x0F) + 1;
1514         cortex_a8->brp_num_context = ((didr >> 20) & 0x0F) + 1;
1515         cortex_a8->brp_num_available = cortex_a8->brp_num;
1516         cortex_a8->brp_list = calloc(cortex_a8->brp_num, sizeof(struct cortex_a8_brp));
1517 //      cortex_a8->brb_enabled = ????;
1518         for (i = 0; i < cortex_a8->brp_num; i++)
1519         {
1520                 cortex_a8->brp_list[i].used = 0;
1521                 if (i < (cortex_a8->brp_num-cortex_a8->brp_num_context))
1522                         cortex_a8->brp_list[i].type = BRP_NORMAL;
1523                 else
1524                         cortex_a8->brp_list[i].type = BRP_CONTEXT;
1525                 cortex_a8->brp_list[i].value = 0;
1526                 cortex_a8->brp_list[i].control = 0;
1527                 cortex_a8->brp_list[i].BRPn = i;
1528         }
1529
1530         /* Setup Watchpoint Register Pairs */
1531         cortex_a8->wrp_num = ((didr >> 28) & 0x0F) + 1;
1532         cortex_a8->wrp_num_available = cortex_a8->wrp_num;
1533         cortex_a8->wrp_list = calloc(cortex_a8->wrp_num, sizeof(struct cortex_a8_wrp));
1534         for (i = 0; i < cortex_a8->wrp_num; i++)
1535         {
1536                 cortex_a8->wrp_list[i].used = 0;
1537                 cortex_a8->wrp_list[i].type = 0;
1538                 cortex_a8->wrp_list[i].value = 0;
1539                 cortex_a8->wrp_list[i].control = 0;
1540                 cortex_a8->wrp_list[i].WRPn = i;
1541         }
1542         LOG_DEBUG("Configured %i hw breakpoint pairs and %i hw watchpoint pairs",
1543                         cortex_a8->brp_num , cortex_a8->wrp_num);
1544
1545         target_set_examined(target);
1546         return ERROR_OK;
1547 }
1548
1549 static int cortex_a8_examine(struct target *target)
1550 {
1551         int retval = ERROR_OK;
1552
1553         /* don't re-probe hardware after each reset */
1554         if (!target_was_examined(target))
1555                 retval = cortex_a8_examine_first(target);
1556
1557         /* Configure core debug access */
1558         if (retval == ERROR_OK)
1559                 retval = cortex_a8_init_debug_access(target);
1560
1561         return retval;
1562 }
1563
1564 /*
1565  *      Cortex-A8 target creation and initialization
1566  */
1567
1568 static int cortex_a8_init_target(struct command_context *cmd_ctx,
1569                 struct target *target)
1570 {
1571         /* examine_first() does a bunch of this */
1572         return ERROR_OK;
1573 }
1574
1575 static int cortex_a8_init_arch_info(struct target *target,
1576                 struct cortex_a8_common *cortex_a8, struct jtag_tap *tap)
1577 {
1578         struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
1579         struct arm *armv4_5 = &armv7a->armv4_5_common;
1580         struct swjdp_common *swjdp = &armv7a->swjdp_info;
1581
1582         /* Setup struct cortex_a8_common */
1583         cortex_a8->common_magic = CORTEX_A8_COMMON_MAGIC;
1584         armv4_5->arch_info = armv7a;
1585
1586         /* prepare JTAG information for the new target */
1587         cortex_a8->jtag_info.tap = tap;
1588         cortex_a8->jtag_info.scann_size = 4;
1589
1590         swjdp->dp_select_value = -1;
1591         swjdp->ap_csw_value = -1;
1592         swjdp->ap_tar_value = -1;
1593         swjdp->jtag_info = &cortex_a8->jtag_info;
1594         swjdp->memaccess_tck = 80;
1595
1596         /* Number of bits for tar autoincrement, impl. dep. at least 10 */
1597         swjdp->tar_autoincr_block = (1 << 10);
1598
1599         cortex_a8->fast_reg_read = 0;
1600
1601         /* register arch-specific functions */
1602         armv7a->examine_debug_reason = NULL;
1603
1604         armv7a->post_debug_entry = cortex_a8_post_debug_entry;
1605
1606         armv7a->pre_restore_context = NULL;
1607         armv7a->post_restore_context = NULL;
1608         armv7a->armv4_5_mmu.armv4_5_cache.ctype = -1;
1609 //      armv7a->armv4_5_mmu.get_ttb = armv7a_get_ttb;
1610         armv7a->armv4_5_mmu.read_memory = cortex_a8_read_memory;
1611         armv7a->armv4_5_mmu.write_memory = cortex_a8_write_memory;
1612 //      armv7a->armv4_5_mmu.disable_mmu_caches = armv7a_disable_mmu_caches;
1613 //      armv7a->armv4_5_mmu.enable_mmu_caches = armv7a_enable_mmu_caches;
1614         armv7a->armv4_5_mmu.has_tiny_pages = 1;
1615         armv7a->armv4_5_mmu.mmu_enabled = 0;
1616
1617
1618 //      arm7_9->handle_target_request = cortex_a8_handle_target_request;
1619
1620         /* REVISIT v7a setup should be in a v7a-specific routine */
1621         armv4_5_init_arch_info(target, armv4_5);
1622         armv7a->common_magic = ARMV7_COMMON_MAGIC;
1623
1624         target_register_timer_callback(cortex_a8_handle_target_request, 1, 1, target);
1625
1626         return ERROR_OK;
1627 }
1628
1629 static int cortex_a8_target_create(struct target *target, Jim_Interp *interp)
1630 {
1631         struct cortex_a8_common *cortex_a8 = calloc(1, sizeof(struct cortex_a8_common));
1632
1633         cortex_a8_init_arch_info(target, cortex_a8, target->tap);
1634
1635         return ERROR_OK;
1636 }
1637
1638 COMMAND_HANDLER(cortex_a8_handle_cache_info_command)
1639 {
1640         struct target *target = get_current_target(CMD_CTX);
1641         struct armv7a_common *armv7a = target_to_armv7a(target);
1642
1643         return armv4_5_handle_cache_info_command(CMD_CTX,
1644                         &armv7a->armv4_5_mmu.armv4_5_cache);
1645 }
1646
1647
1648 COMMAND_HANDLER(cortex_a8_handle_dbginit_command)
1649 {
1650         struct target *target = get_current_target(CMD_CTX);
1651
1652         cortex_a8_init_debug_access(target);
1653
1654         return ERROR_OK;
1655 }
1656
1657 static const struct command_registration cortex_a8_exec_command_handlers[] = {
1658         {
1659                 .name = "cache_info",
1660                 .handler = &cortex_a8_handle_cache_info_command,
1661                 .mode = COMMAND_EXEC,
1662                 .help = "display information about target caches",
1663         },
1664         {
1665                 .name = "dbginit",
1666                 .handler = &cortex_a8_handle_dbginit_command,
1667                 .mode = COMMAND_EXEC,
1668                 .help = "Initialize core debug",
1669         },
1670         COMMAND_REGISTRATION_DONE
1671 };
1672 static const struct command_registration cortex_a8_command_handlers[] = {
1673         {
1674                 .chain = arm_command_handlers,
1675         },
1676         {
1677                 .chain = armv7a_command_handlers,
1678         },
1679         {
1680                 .name = "cortex_a8",
1681                 .mode = COMMAND_ANY,
1682                 .help = "Cortex-A8 command group",
1683                 .chain = cortex_a8_exec_command_handlers,
1684         },
1685         COMMAND_REGISTRATION_DONE
1686 };
1687
1688 struct target_type cortexa8_target = {
1689         .name = "cortex_a8",
1690
1691         .poll = cortex_a8_poll,
1692         .arch_state = armv7a_arch_state,
1693
1694         .target_request_data = NULL,
1695
1696         .halt = cortex_a8_halt,
1697         .resume = cortex_a8_resume,
1698         .step = cortex_a8_step,
1699
1700         .assert_reset = cortex_a8_assert_reset,
1701         .deassert_reset = cortex_a8_deassert_reset,
1702         .soft_reset_halt = NULL,
1703
1704         .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
1705
1706         .read_memory = cortex_a8_read_memory,
1707         .write_memory = cortex_a8_write_memory,
1708         .bulk_write_memory = cortex_a8_bulk_write_memory,
1709
1710         .checksum_memory = arm_checksum_memory,
1711         .blank_check_memory = arm_blank_check_memory,
1712
1713         .run_algorithm = armv4_5_run_algorithm,
1714
1715         .add_breakpoint = cortex_a8_add_breakpoint,
1716         .remove_breakpoint = cortex_a8_remove_breakpoint,
1717         .add_watchpoint = NULL,
1718         .remove_watchpoint = NULL,
1719
1720         .commands = cortex_a8_command_handlers,
1721         .target_create = cortex_a8_target_create,
1722         .init_target = cortex_a8_init_target,
1723         .examine = cortex_a8_examine,
1724 };