ioutil: make the file compile on MacOS
[fw/openocd] / src / target / arm_dpm.c
index 3c18e6334b26326aa8d8974423d339e91a5de427..b83d17a1fcbf4ccfb09b2d9ece4a9e3c5e7d30cc 100644 (file)
@@ -51,8 +51,8 @@
 
 /* Read coprocessor */
 static int dpm_mrc(struct target *target, int cpnum,
-               uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
-               uint32_t *value)
+       uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+       uint32_t *value)
 {
        struct arm *arm = target_to_arm(target);
        struct arm_dpm *dpm = arm->dpm;
@@ -63,8 +63,8 @@ static int dpm_mrc(struct target *target, int cpnum,
                return retval;
 
        LOG_DEBUG("MRC p%d, %d, r0, c%d, c%d, %d", cpnum,
-                       (int) op1, (int) CRn,
-                       (int) CRm, (int) op2);
+               (int) op1, (int) CRn,
+               (int) CRm, (int) op2);
 
        /* read coprocessor register into R0; return via DCC */
        retval = dpm->instr_read_data_r0(dpm,
@@ -76,8 +76,8 @@ static int dpm_mrc(struct target *target, int cpnum,
 }
 
 static int dpm_mcr(struct target *target, int cpnum,
-               uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
-               uint32_t value)
+       uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+       uint32_t value)
 {
        struct arm *arm = target_to_arm(target);
        struct arm_dpm *dpm = arm->dpm;
@@ -88,8 +88,8 @@ static int dpm_mcr(struct target *target, int cpnum,
                return retval;
 
        LOG_DEBUG("MCR p%d, %d, r0, c%d, c%d, %d", cpnum,
-                       (int) op1, (int) CRn,
-                       (int) CRm, (int) op2);
+               (int) op1, (int) CRn,
+               (int) CRm, (int) op2);
 
        /* read DCC into r0; then write coprocessor register from R0 */
        retval = dpm->instr_write_data_r0(dpm,
@@ -109,7 +109,7 @@ static int dpm_mcr(struct target *target, int cpnum,
 /* Toggles between recorded core mode (USR, SVC, etc) and a temporary one.
  * Routines *must* restore the original mode before returning!!
  */
-static int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
+int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 {
        int retval;
        uint32_t cpsr;
@@ -123,6 +123,8 @@ static int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
                cpsr = mode;
 
        retval = dpm->instr_write_data_r0(dpm, ARMV4_5_MSR_GP(0, 0xf, 0), cpsr);
+       if (retval != ERROR_OK)
+               return retval;
 
        if (dpm->instr_cpsr_sync)
                retval = dpm->instr_cpsr_sync(dpm);
@@ -137,44 +139,44 @@ static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
        int retval;
 
        switch (regnum) {
-       case 0 ... 14:
-               /* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
-               retval = dpm->instr_read_data_dcc(dpm,
+               case 0 ... 14:
+                       /* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
+                       retval = dpm->instr_read_data_dcc(dpm,
                                ARMV4_5_MCR(14, 0, regnum, 0, 5, 0),
                                &value);
-               break;
-       case 15:        /* PC */
-               /* "MOV r0, pc"; then return via DCC */
-               retval = dpm->instr_read_data_r0(dpm, 0xe1a0000f, &value);
-
-               /* NOTE: this seems like a slightly awkward place to update
-                * this value ... but if the PC gets written (the only way
-                * to change what we compute), the arch spec says subsequent
-                * reads return values which are "unpredictable".  So this
-                * is always right except in those broken-by-intent cases.
-                */
-               switch (dpm->arm->core_state) {
-               case ARM_STATE_ARM:
-                       value -= 8;
                        break;
-               case ARM_STATE_THUMB:
-               case ARM_STATE_THUMB_EE:
-                       value -= 4;
-                       break;
-               case ARM_STATE_JAZELLE:
-                       /* core-specific ... ? */
-                       LOG_WARNING("Jazelle PC adjustment unknown");
+               case 15:/* PC
+                        * "MOV r0, pc"; then return via DCC */
+                       retval = dpm->instr_read_data_r0(dpm, 0xe1a0000f, &value);
+
+                       /* NOTE: this seems like a slightly awkward place to update
+                        * this value ... but if the PC gets written (the only way
+                        * to change what we compute), the arch spec says subsequent
+                        * reads return values which are "unpredictable".  So this
+                        * is always right except in those broken-by-intent cases.
+                        */
+                       switch (dpm->arm->core_state) {
+                               case ARM_STATE_ARM:
+                                       value -= 8;
+                                       break;
+                               case ARM_STATE_THUMB:
+                               case ARM_STATE_THUMB_EE:
+                                       value -= 4;
+                                       break;
+                               case ARM_STATE_JAZELLE:
+                                       /* core-specific ... ? */
+                                       LOG_WARNING("Jazelle PC adjustment unknown");
+                                       break;
+                       }
                        break;
-               }
-               break;
-       default:
-               /* 16: "MRS r0, CPSR"; then return via DCC
-                * 17: "MRS r0, SPSR"; then return via DCC
-                */
-               retval = dpm->instr_read_data_r0(dpm,
+               default:
+                       /* 16: "MRS r0, CPSR"; then return via DCC
+                        * 17: "MRS r0, SPSR"; then return via DCC
+                        */
+                       retval = dpm->instr_read_data_r0(dpm,
                                ARMV4_5_MRS(0, regnum & 1),
                                &value);
-               break;
+                       break;
        }
 
        if (retval == ERROR_OK) {
@@ -194,28 +196,30 @@ static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
        uint32_t value = buf_get_u32(r->value, 0, 32);
 
        switch (regnum) {
-       case 0 ... 14:
-               /* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
-               retval = dpm->instr_write_data_dcc(dpm,
+               case 0 ... 14:
+                       /* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
+                       retval = dpm->instr_write_data_dcc(dpm,
                                ARMV4_5_MRC(14, 0, regnum, 0, 5, 0),
                                value);
-               break;
-       case 15:        /* PC */
-               /* read r0 from DCC; then "MOV pc, r0" */
-               retval = dpm->instr_write_data_r0(dpm, 0xe1a0f000, value);
-               break;
-       default:
-               /* 16: read r0 from DCC, then "MSR r0, CPSR_cxsf"
-                * 17: read r0 from DCC, then "MSR r0, SPSR_cxsf"
-                */
-               retval = dpm->instr_write_data_r0(dpm,
+                       break;
+               case 15:/* PC
+                        * read r0 from DCC; then "MOV pc, r0" */
+                       retval = dpm->instr_write_data_r0(dpm, 0xe1a0f000, value);
+                       break;
+               default:
+                       /* 16: read r0 from DCC, then "MSR r0, CPSR_cxsf"
+                        * 17: read r0 from DCC, then "MSR r0, SPSR_cxsf"
+                        */
+                       retval = dpm->instr_write_data_r0(dpm,
                                ARMV4_5_MSR_GP(0, 0xf, regnum & 1),
                                value);
+                       if (retval != ERROR_OK)
+                               return retval;
 
-               if (regnum == 16 && dpm->instr_cpsr_sync)
-                       retval = dpm->instr_cpsr_sync(dpm);
+                       if (regnum == 16 && dpm->instr_cpsr_sync)
+                               retval = dpm->instr_cpsr_sync(dpm);
 
-               break;
+                       break;
        }
 
        if (retval == ERROR_OK) {
@@ -288,7 +292,7 @@ fail:
  * or running debugger code.
  */
 static int dpm_maybe_update_bpwp(struct arm_dpm *dpm, bool bpwp,
-               struct dpm_bpwp *xp, int *set_p)
+       struct dpm_bpwp *xp, int *set_p)
 {
        int retval = ERROR_OK;
        bool disable;
@@ -320,14 +324,17 @@ static int dpm_maybe_update_bpwp(struct arm_dpm *dpm, bool bpwp,
                                xp->address, xp->control);
 
        if (retval != ERROR_OK)
-               LOG_ERROR("%s: can't %s HW bp/wp %d",
-                               disable ? "disable" : "enable",
-                               target_name(dpm->arm->target),
-                               xp->number);
+               LOG_ERROR("%s: can't %s HW %spoint %d",
+                       disable ? "disable" : "enable",
+                       target_name(dpm->arm->target),
+                       (xp->number < 16) ? "break" : "watch",
+                       xp->number & 0xf);
 done:
        return retval;
 }
 
+static int dpm_add_breakpoint(struct target *target, struct breakpoint *bp);
+
 /**
  * Writes all modified core registers for all processor modes.  In normal
  * operation this is called on exit from halting debug state.
@@ -354,13 +361,15 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
         * we should be able to assume we handle them; but until then,
         * cope with the hand-crafted breakpoint code.
         */
-       if (0) {
+       if (arm->target->type->add_breakpoint == dpm_add_breakpoint) {
                for (unsigned i = 0; i < dpm->nbp; i++) {
                        struct dpm_bp *dbp = dpm->dbp + i;
                        struct breakpoint *bp = dbp->bp;
 
                        retval = dpm_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
                                        bp ? &bp->set : NULL);
+                       if (retval != ERROR_OK)
+                               goto done;
                }
        }
 
@@ -371,6 +380,8 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 
                retval = dpm_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
                                wp ? &wp->set : NULL);
+               if (retval != ERROR_OK)
+                       goto done;
        }
 
        /* NOTE:  writes to breakpoint and watchpoint registers might
@@ -412,25 +423,28 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 
                                /* cope with special cases */
                                switch (regnum) {
-                               case 8 ... 12:
-                                       /* r8..r12 "anything but FIQ" case;
-                                        * we "know" core mode is accurate
-                                        * since we haven't changed it yet
-                                        */
-                                       if (arm->core_mode == ARM_MODE_FIQ
+                                       case 8 ... 12:
+                                               /* r8..r12 "anything but FIQ" case;
+                                                * we "know" core mode is accurate
+                                                * since we haven't changed it yet
+                                                */
+                                               if (arm->core_mode == ARM_MODE_FIQ
                                                        && ARM_MODE_ANY
-                                                               != mode)
-                                               tmode = ARM_MODE_USR;
-                                       break;
-                               case 16:
-                                       /* SPSR */
-                                       regnum++;
-                                       break;
+                                                       != mode)
+                                                       tmode = ARM_MODE_USR;
+                                               break;
+                                       case 16:
+                                               /* SPSR */
+                                               regnum++;
+                                               break;
                                }
 
                                /* REVISIT error checks */
-                               if (tmode != ARM_MODE_ANY)
+                               if (tmode != ARM_MODE_ANY) {
                                        retval = dpm_modeswitch(dpm, tmode);
+                                       if (retval != ERROR_OK)
+                                               goto done;
+                               }
                        }
                        if (r->mode != mode)
                                continue;
@@ -438,7 +452,8 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
                        retval = dpm_write_reg(dpm,
                                        &cache->reg_list[i],
                                        regnum);
-
+                       if (retval != ERROR_OK)
+                               goto done;
                }
 
        } while (did_write);
@@ -448,13 +463,19 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
         * defined, and must not write it before CPSR.
         */
        retval = dpm_modeswitch(dpm, ARM_MODE_ANY);
+       if (retval != ERROR_OK)
+               goto done;
        arm->cpsr->dirty = false;
 
-       retval = dpm_write_reg(dpm, &cache->reg_list[15], 15);
-       cache->reg_list[15].dirty = false;
+       retval = dpm_write_reg(dpm, arm->pc, 15);
+       if (retval != ERROR_OK)
+               goto done;
+       arm->pc->dirty = false;
 
        /* flush R0 -- it's *very* dirty by now */
        retval = dpm_write_reg(dpm, &cache->reg_list[0], 0);
+       if (retval != ERROR_OK)
+               goto done;
        cache->reg_list[0].dirty = false;
 
        /* (void) */ dpm->finish(dpm);
@@ -468,34 +489,34 @@ done:
  * or MODE_ANY.
  */
 static enum arm_mode dpm_mapmode(struct arm *arm,
-               unsigned num, enum arm_mode mode)
+       unsigned num, enum arm_mode mode)
 {
        enum arm_mode amode = arm->core_mode;
 
        /* don't switch if the mode is already correct */
        if (amode == ARM_MODE_SYS)
-                amode = ARM_MODE_USR;
+               amode = ARM_MODE_USR;
        if (mode == amode)
                return ARM_MODE_ANY;
 
        switch (num) {
-       /* don't switch for non-shadowed registers (r0..r7, r15/pc, cpsr) */
-       case 0 ... 7:
-       case 15:
-       case 16:
-               break;
-       /* r8..r12 aren't shadowed for anything except FIQ */
-       case 8 ... 12:
-               if (mode == ARM_MODE_FIQ)
+               /* don't switch for non-shadowed registers (r0..r7, r15/pc, cpsr) */
+               case 0 ... 7:
+               case 15:
+               case 16:
+                       break;
+               /* r8..r12 aren't shadowed for anything except FIQ */
+               case 8 ... 12:
+                       if (mode == ARM_MODE_FIQ)
+                               return mode;
+                       break;
+               /* r13/sp, and r14/lr are always shadowed */
+               case 13:
+               case 14:
                        return mode;
-               break;
-       /* r13/sp, and r14/lr are always shadowed */
-       case 13:
-       case 14:
-               return mode;
-       default:
-               LOG_WARNING("invalid register #%u", num);
-               break;
+               default:
+                       LOG_WARNING("invalid register #%u", num);
+                       break;
        }
        return ARM_MODE_ANY;
 }
@@ -508,13 +529,13 @@ static enum arm_mode dpm_mapmode(struct arm *arm,
  */
 
 static int arm_dpm_read_core_reg(struct target *target, struct reg *r,
-               int regnum, enum arm_mode mode)
+       int regnum, enum arm_mode mode)
 {
        struct arm_dpm *dpm = target_to_arm(target)->dpm;
        int retval;
 
        if (regnum < 0 || regnum > 16)
-               return ERROR_INVALID_ARGUMENTS;
+               return ERROR_COMMAND_SYNTAX_ERROR;
 
        if (regnum == 16) {
                if (mode != ARM_MODE_ANY)
@@ -537,6 +558,8 @@ static int arm_dpm_read_core_reg(struct target *target, struct reg *r,
        }
 
        retval = dpm_read_reg(dpm, r, regnum);
+       if (retval != ERROR_OK)
+               goto fail;
        /* always clean up, regardless of error */
 
        if (mode != ARM_MODE_ANY)
@@ -548,14 +571,14 @@ fail:
 }
 
 static int arm_dpm_write_core_reg(struct target *target, struct reg *r,
-               int regnum, enum arm_mode mode, uint32_t value)
+       int regnum, enum arm_mode mode, uint32_t value)
 {
        struct arm_dpm *dpm = target_to_arm(target)->dpm;
        int retval;
 
 
        if (regnum < 0 || regnum > 16)
-               return ERROR_INVALID_ARGUMENTS;
+               return ERROR_COMMAND_SYNTAX_ERROR;
 
        if (regnum == 16) {
                if (mode != ARM_MODE_ANY)
@@ -633,6 +656,8 @@ static int arm_dpm_full_context(struct target *target)
 
                                /* REVISIT error checks */
                                retval = dpm_modeswitch(dpm, mode);
+                               if (retval != ERROR_OK)
+                                       goto done;
                        }
                        if (r->mode != mode)
                                continue;
@@ -641,7 +666,8 @@ static int arm_dpm_full_context(struct target *target)
                        retval = dpm_read_reg(dpm,
                                        &cache->reg_list[i],
                                        (r->num == 16) ? 17 : r->num);
-
+                       if (retval != ERROR_OK)
+                               goto done;
                }
 
        } while (did_read);
@@ -665,79 +691,146 @@ done:
  * fact isn't currently leveraged.
  */
 
-static int dpm_watchpoint_setup(struct arm_dpm *dpm, unsigned index,
-               struct watchpoint *wp)
+static int dpm_bpwp_setup(struct arm_dpm *dpm, struct dpm_bpwp *xp,
+       uint32_t addr, uint32_t length)
 {
-       uint32_t addr = wp->address;
        uint32_t control;
 
-       /* this hardware doesn't support data value matching or masking */
-       if (wp->value || wp->mask != ~(uint32_t)0) {
-               LOG_DEBUG("watchpoint values and masking not supported");
-               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-       }
-
        control = (1 << 0)      /* enable */
                | (3 << 1);     /* both user and privileged access */
 
-       switch (wp->rw) {
-       case WPT_READ:
-               control |= 1 << 3;
-               break;
-       case WPT_WRITE:
-               control |= 2 << 3;
-               break;
-       case WPT_ACCESS:
-               control |= 3 << 3;
-               break;
-       }
-
        /* Match 1, 2, or all 4 byte addresses in this word.
         *
-        * FIXME:  v7 hardware allows lengths up to 2 GB, and has eight
-        * byte address select bits.  Support larger wp->length, if addr
-        * is suitably aligned.
+        * FIXME:  v7 hardware allows lengths up to 2 GB for BP and WP.
+        * Support larger length, when addr is suitably aligned.  In
+        * particular, allow watchpoints on 8 byte "double" values.
+        *
+        * REVISIT allow watchpoints on unaligned 2-bit values; and on
+        * v7 hardware, unaligned 4-byte ones too.
         */
-       switch (wp->length) {
-       case 1:
-               control |= (1 << (addr & 3)) << 5;
-               addr &= ~3;
-               break;
-       case 2:
-               /* require 2-byte alignment */
-               if (!(addr & 1)) {
-                       control |= (3 << (addr & 2)) << 5;
+       switch (length) {
+               case 1:
+                       control |= (1 << (addr & 3)) << 5;
                        break;
-               }
+               case 2:
+                       /* require 2-byte alignment */
+                       if (!(addr & 1)) {
+                               control |= (3 << (addr & 2)) << 5;
+                               break;
+                       }
                /* FALL THROUGH */
-       case 4:
-               /* require 4-byte alignment */
-               if (!(addr & 3)) {
-                       control |= 0xf << 5;
-                       break;
-               }
+               case 4:
+                       /* require 4-byte alignment */
+                       if (!(addr & 3)) {
+                               control |= 0xf << 5;
+                               break;
+                       }
                /* FALL THROUGH */
-       default:
-               LOG_DEBUG("bad watchpoint length or alignment");
-               return ERROR_INVALID_ARGUMENTS;
+               default:
+                       LOG_ERROR("unsupported {break,watch}point length/alignment");
+                       return ERROR_COMMAND_SYNTAX_ERROR;
        }
 
-       /* other control bits:
-        * bits 9:12 == 0 ... only checking up to four byte addresses (v7 only)
+       /* other shared control bits:
         * bits 15:14 == 0 ... both secure and nonsecure states (v6.1+ only)
         * bit 20 == 0 ... not linked to a context ID
         * bit 28:24 == 0 ... not ignoring N LSBs (v7 only)
         */
 
-       dpm->dwp[index].wp = wp;
-       dpm->dwp[index].bpwp.address = addr & ~3;
-       dpm->dwp[index].bpwp.control = control;
-       dpm->dwp[index].bpwp.dirty = true;
+       xp->address = addr & ~3;
+       xp->control = control;
+       xp->dirty = true;
+
+       LOG_DEBUG("BPWP: addr %8.8" PRIx32 ", control %" PRIx32 ", number %d",
+               xp->address, control, xp->number);
 
        /* hardware is updated in write_dirty_registers() */
        return ERROR_OK;
 }
 
+static int dpm_add_breakpoint(struct target *target, struct breakpoint *bp)
+{
+       struct arm *arm = target_to_arm(target);
+       struct arm_dpm *dpm = arm->dpm;
+       int retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
+       if (bp->length < 2)
+               return ERROR_COMMAND_SYNTAX_ERROR;
+       if (!dpm->bpwp_enable)
+               return retval;
+
+       /* FIXME we need a generic solution for software breakpoints. */
+       if (bp->type == BKPT_SOFT)
+               LOG_DEBUG("using HW bkpt, not SW...");
+
+       for (unsigned i = 0; i < dpm->nbp; i++) {
+               if (!dpm->dbp[i].bp) {
+                       retval = dpm_bpwp_setup(dpm, &dpm->dbp[i].bpwp,
+                                       bp->address, bp->length);
+                       if (retval == ERROR_OK)
+                               dpm->dbp[i].bp = bp;
+                       break;
+               }
+       }
+
+       return retval;
+}
+
+static int dpm_remove_breakpoint(struct target *target, struct breakpoint *bp)
+{
+       struct arm *arm = target_to_arm(target);
+       struct arm_dpm *dpm = arm->dpm;
+       int retval = ERROR_COMMAND_SYNTAX_ERROR;
+
+       for (unsigned i = 0; i < dpm->nbp; i++) {
+               if (dpm->dbp[i].bp == bp) {
+                       dpm->dbp[i].bp = NULL;
+                       dpm->dbp[i].bpwp.dirty = true;
+
+                       /* hardware is updated in write_dirty_registers() */
+                       retval = ERROR_OK;
+                       break;
+               }
+       }
+
+       return retval;
+}
+
+static int dpm_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
+       struct watchpoint *wp)
+{
+       int retval;
+       struct dpm_wp *dwp = dpm->dwp + index_t;
+       uint32_t control;
+
+       /* this hardware doesn't support data value matching or masking */
+       if (wp->value || wp->mask != ~(uint32_t)0) {
+               LOG_DEBUG("watchpoint values and masking not supported");
+               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+       }
+
+       retval = dpm_bpwp_setup(dpm, &dwp->bpwp, wp->address, wp->length);
+       if (retval != ERROR_OK)
+               return retval;
+
+       control = dwp->bpwp.control;
+       switch (wp->rw) {
+               case WPT_READ:
+                       control |= 1 << 3;
+                       break;
+               case WPT_WRITE:
+                       control |= 2 << 3;
+                       break;
+               case WPT_ACCESS:
+                       control |= 3 << 3;
+                       break;
+       }
+       dwp->bpwp.control = control;
+
+       dpm->dwp[index_t].wp = wp;
+
+       return retval;
+}
 
 static int dpm_add_watchpoint(struct target *target, struct watchpoint *wp)
 {
@@ -761,7 +854,7 @@ static int dpm_remove_watchpoint(struct target *target, struct watchpoint *wp)
 {
        struct arm *arm = target_to_arm(target);
        struct arm_dpm *dpm = arm->dpm;
-       int retval = ERROR_INVALID_ARGUMENTS;
+       int retval = ERROR_COMMAND_SYNTAX_ERROR;
 
        for (unsigned i = 0; i < dpm->nwp; i++) {
                if (dpm->dwp[i].wp == wp) {
@@ -780,16 +873,16 @@ static int dpm_remove_watchpoint(struct target *target, struct watchpoint *wp)
 void arm_dpm_report_wfar(struct arm_dpm *dpm, uint32_t addr)
 {
        switch (dpm->arm->core_state) {
-       case ARM_STATE_ARM:
-               addr -= 8;
-               break;
-       case ARM_STATE_THUMB:
-       case ARM_STATE_THUMB_EE:
-               addr -= 4;
-               break;
-       case ARM_STATE_JAZELLE:
-               /* ?? */
-               break;
+               case ARM_STATE_ARM:
+                       addr -= 8;
+                       break;
+               case ARM_STATE_THUMB:
+               case ARM_STATE_THUMB_EE:
+                       addr -= 4;
+                       break;
+               case ARM_STATE_JAZELLE:
+                       /* ?? */
+                       break;
        }
        dpm->wp_pc = addr;
 }
@@ -808,25 +901,25 @@ void arm_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
 
        /* Examine debug reason */
        switch (DSCR_ENTRY(dscr)) {
-       case 6:         /* Data abort (v6 only) */
-       case 7:         /* Prefetch abort (v6 only) */
+               case 6: /* Data abort (v6 only) */
+               case 7: /* Prefetch abort (v6 only) */
                /* FALL THROUGH -- assume a v6 core in abort mode */
-       case 0:         /* HALT request from debugger */
-       case 4:         /* EDBGRQ */
-               target->debug_reason = DBG_REASON_DBGRQ;
-               break;
-       case 1:         /* HW breakpoint */
-       case 3:         /* SW BKPT */
-       case 5:         /* vector catch */
-               target->debug_reason = DBG_REASON_BREAKPOINT;
-               break;
-       case 2:         /* asynch watchpoint */
-       case 10:        /* precise watchpoint */
-               target->debug_reason = DBG_REASON_WATCHPOINT;
-               break;
-       default:
-               target->debug_reason = DBG_REASON_UNDEFINED;
-               break;
+               case 0: /* HALT request from debugger */
+               case 4: /* EDBGRQ */
+                       target->debug_reason = DBG_REASON_DBGRQ;
+                       break;
+               case 1: /* HW breakpoint */
+               case 3: /* SW BKPT */
+               case 5: /* vector catch */
+                       target->debug_reason = DBG_REASON_BREAKPOINT;
+                       break;
+               case 2: /* asynch watchpoint */
+               case 10:/* precise watchpoint */
+                       target->debug_reason = DBG_REASON_WATCHPOINT;
+                       break;
+               default:
+                       target->debug_reason = DBG_REASON_UNDEFINED;
+                       break;
        }
 }
 
@@ -865,11 +958,16 @@ int arm_dpm_setup(struct arm_dpm *dpm)
        arm->mrc = dpm_mrc;
        arm->mcr = dpm_mcr;
 
-       /* breakpoint and watchpoint setup */
+       /* breakpoint setup -- optional until it works everywhere */
+       if (!target->type->add_breakpoint) {
+               target->type->add_breakpoint = dpm_add_breakpoint;
+               target->type->remove_breakpoint = dpm_remove_breakpoint;
+       }
+
+       /* watchpoint setup */
        target->type->add_watchpoint = dpm_add_watchpoint;
        target->type->remove_watchpoint = dpm_remove_watchpoint;
 
-       /* FIXME add breakpoint support */
        /* FIXME add vector catch support */
 
        dpm->nbp = 1 + ((dpm->didr >> 24) & 0xf);
@@ -885,7 +983,7 @@ int arm_dpm_setup(struct arm_dpm *dpm)
        }
 
        LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
-                       target_name(target), dpm->nbp, dpm->nwp);
+               target_name(target), dpm->nbp, dpm->nwp);
 
        /* REVISIT ... and some of those breakpoints could match
         * execution context IDs...