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