diff options
Diffstat (limited to 'acsr/v7_c.c')
| -rw-r--r-- | acsr/v7_c.c | 332 |
1 files changed, 326 insertions, 6 deletions
diff --git a/acsr/v7_c.c b/acsr/v7_c.c index 1c23ac0..115b851 100644 --- a/acsr/v7_c.c +++ b/acsr/v7_c.c @@ -28,6 +28,7 @@ #include "bakery.h" #include "helpers.h" +#include "debug_helpers.h" #include "context.h" #define DIDR_VERSION_SHIFT 16 @@ -51,6 +52,12 @@ #define OSDLR_UNLOCKED 0x00000000 #define OSDLR_LOCKED 0x00000001 +#define DBGREG_BP_VAL 0x0 +#define DBGREG_WP_VAL 0x1 +#define DBGREG_BP_CTRL 0x2 +#define DBGREG_WP_CTRL 0x3 +#define DBGREG_BP_XVAL 0x4 + typedef volatile struct { /* Registers Save? */ unsigned const didr; /* 0 Read only */ unsigned dscr_i; /* 1 ignore - use dscr_e instead */ @@ -105,12 +112,287 @@ typedef volatile struct { /* Registers Save? unsigned const cid[4]; /* 1020-1023 Read only */ } debug_registers_t; +typedef struct { + unsigned (*read)(void); + void (*write)(unsigned); +} rw_ops; + +typedef struct { + rw_ops bvr; + rw_ops bcr; + rw_ops wvr; + rw_ops wcr; + rw_ops bxvr; +} dbgreg_rw_ops; + +dbgreg_rw_ops dbgreg_rw_handlers[] = { + { + {read_dbg_bvr0, write_dbg_bvr0,}, + {read_dbg_bcr0, write_dbg_bcr0,}, + {read_dbg_wvr0, write_dbg_wvr0,}, + {read_dbg_wcr0, write_dbg_wcr0,}, + {read_dbg_bxvr0, write_dbg_bxvr0,}, + }, + { + {read_dbg_bvr1, write_dbg_bvr1,}, + {read_dbg_bcr1, write_dbg_bcr1,}, + {read_dbg_wvr1, write_dbg_wvr1,}, + {read_dbg_wcr1, write_dbg_wcr1,}, + {read_dbg_bxvr1, write_dbg_bxvr1,}, + }, + { + {read_dbg_bvr2, write_dbg_bvr2,}, + {read_dbg_bcr2, write_dbg_bcr2,}, + {read_dbg_wvr2, write_dbg_wvr2,}, + {read_dbg_wcr2, write_dbg_wcr2,}, + {read_dbg_bxvr2, write_dbg_bxvr2,}, + }, + { + {read_dbg_bvr3, write_dbg_bvr3,}, + {read_dbg_bcr3, write_dbg_bcr3,}, + {read_dbg_wvr3, write_dbg_wvr3,}, + {read_dbg_wcr3, write_dbg_wcr3,}, + {read_dbg_bxvr3, write_dbg_bxvr3,}, + }, + { + {read_dbg_bvr4, write_dbg_bvr4,}, + {read_dbg_bcr4, write_dbg_bcr4,}, + {read_dbg_wvr4, write_dbg_wvr4,}, + {read_dbg_wcr4, write_dbg_wcr4,}, + {read_dbg_bxvr4, write_dbg_bxvr4,}, + }, + { + {read_dbg_bvr5, write_dbg_bvr5,}, + {read_dbg_bcr5, write_dbg_bcr5,}, + {read_dbg_wvr5, write_dbg_wvr5,}, + {read_dbg_wcr5, write_dbg_wcr5,}, + {read_dbg_bxvr5, write_dbg_bxvr5,}, + }, + { + {read_dbg_bvr6, write_dbg_bvr6,}, + {read_dbg_bcr6, write_dbg_bcr6,}, + {read_dbg_wvr6, write_dbg_wvr6,}, + {read_dbg_wcr6, write_dbg_wcr6,}, + {read_dbg_bxvr6, write_dbg_bxvr6,}, + }, + { + {read_dbg_bvr7, write_dbg_bvr7,}, + {read_dbg_bcr7, write_dbg_bcr7,}, + {read_dbg_wvr7, write_dbg_wvr7,}, + {read_dbg_wcr7, write_dbg_wcr7,}, + {read_dbg_bxvr7, write_dbg_bxvr7,}, + }, + { + {read_dbg_bvr8, write_dbg_bvr8,}, + {read_dbg_bcr8, write_dbg_bcr8,}, + {read_dbg_wvr8, write_dbg_wvr8,}, + {read_dbg_wcr8, write_dbg_wcr8,}, + {read_dbg_bxvr8, write_dbg_bxvr8,}, + }, + { + {read_dbg_bvr9, write_dbg_bvr9,}, + {read_dbg_bcr9, write_dbg_bcr9,}, + {read_dbg_wvr9, write_dbg_wvr9,}, + {read_dbg_wcr9, write_dbg_wcr9,}, + {read_dbg_bxvr9, write_dbg_bxvr9,}, + }, + { + {read_dbg_bvr10, write_dbg_bvr10,}, + {read_dbg_bcr10, write_dbg_bcr10,}, + {read_dbg_wvr10, write_dbg_wvr10,}, + {read_dbg_wcr10, write_dbg_wcr10,}, + {read_dbg_bxvr10, write_dbg_bxvr10,}, + }, + { + {read_dbg_bvr11, write_dbg_bvr11,}, + {read_dbg_bcr11, write_dbg_bcr11,}, + {read_dbg_wvr11, write_dbg_wvr11,}, + {read_dbg_wcr11, write_dbg_wcr11,}, + {read_dbg_bxvr11, write_dbg_bxvr11,}, + }, + { + {read_dbg_bvr12, write_dbg_bvr12,}, + {read_dbg_bcr12, write_dbg_bcr12,}, + {read_dbg_wvr12, write_dbg_wvr12,}, + {read_dbg_wcr12, write_dbg_wcr12,}, + {read_dbg_bxvr12, write_dbg_bxvr12,}, + }, + { + {read_dbg_bvr13, write_dbg_bvr13,}, + {read_dbg_bcr13, write_dbg_bcr13,}, + {read_dbg_wvr13, write_dbg_wvr13,}, + {read_dbg_wcr13, write_dbg_wcr13,}, + {read_dbg_bxvr13, write_dbg_bxvr13,}, + }, + { + {read_dbg_bvr14, write_dbg_bvr14,}, + {read_dbg_bcr14, write_dbg_bcr14,}, + {read_dbg_wvr14, write_dbg_wvr14,}, + {read_dbg_wcr14, write_dbg_wcr14,}, + {read_dbg_bxvr14, write_dbg_bxvr14,}, + }, + { + {read_dbg_bvr15, write_dbg_bvr15,}, + {read_dbg_bcr15, write_dbg_bcr15,}, + {read_dbg_wvr15, write_dbg_wvr15,}, + {read_dbg_wcr15, write_dbg_wcr15,}, + {read_dbg_bxvr15, write_dbg_bxvr15,}, + }, +}; + +static void restore_bp_reg(debug_registers_t *dbg, unsigned index, unsigned type) +{ + switch (type) { + case DBGREG_WP_VAL: + dbgreg_rw_handlers[index].wvr.write(dbg->wvr[index]); + break; + case DBGREG_WP_CTRL: + dbgreg_rw_handlers[index].wcr.write(dbg->wcr[index]); + break; + case DBGREG_BP_XVAL: + dbgreg_rw_handlers[index].bxvr.write(dbg->bxvr[index]); + break; + case DBGREG_BP_VAL: + dbgreg_rw_handlers[index].bvr.write(dbg->bvr[index]); + break; + case DBGREG_BP_CTRL: + dbgreg_rw_handlers[index].bcr.write(dbg->bcr[index]); + break; + default: + break; + } + + return; +} + +static void save_bp_reg(debug_registers_t *dbg, unsigned index, unsigned type) +{ + switch (type) { + case DBGREG_WP_VAL: + dbg->wvr[index] = dbgreg_rw_handlers[index].wvr.read(); + break; + case DBGREG_WP_CTRL: + dbg->wcr[index] = dbgreg_rw_handlers[index].wcr.read(); + break; + case DBGREG_BP_XVAL: + dbg->bxvr[index] = dbgreg_rw_handlers[index].bxvr.read(); + break; + case DBGREG_BP_VAL: + dbg->bvr[index] = dbgreg_rw_handlers[index].bvr.read(); + break; + case DBGREG_BP_CTRL: + dbg->bcr[index] = dbgreg_rw_handlers[index].bcr.read(); + break; + default: + break; + } + + return; +} + +static void sr_bp_context(debug_registers_t *dbg, unsigned bp_type, unsigned op) +{ + unsigned num_bps, num_ctx_cmps, num_wps, didr; + unsigned index = 0, max_index = 0; + + didr = read_dbg_didr(); + num_bps = (didr >> 24) & 0xf; + num_ctx_cmps = (didr >> 20) & 0xf; + num_wps = (didr >> 28) & 0xf; + + switch (bp_type) { + case DBGREG_WP_VAL: + case DBGREG_WP_CTRL: + max_index = num_wps; + break; + case DBGREG_BP_XVAL: + index = num_bps - num_ctx_cmps; + case DBGREG_BP_VAL: + case DBGREG_BP_CTRL: + max_index = num_bps; + break; + default: + break; + } + + for (; index <= max_index; index++) + if (op) + save_bp_reg(dbg, index, bp_type); + else + restore_bp_reg(dbg, index, bp_type); + return; +} + +static void save_v7_debug_cp14(unsigned * context) +{ + debug_registers_t *dbg = (void *) context; + unsigned virtext_present = (read_dbg_devid() >> 16) & 0xf; + + /* + * Prevent updates to the debug registers during a S&R operation + */ + write_dbg_oslar(OSLAR_LOCKED); + + dbg->dtrrx_e = read_dbg_dtrrxext(); + dbg->dtrtx_e = read_dbg_dtrtxext(); + dbg->dscr_e = read_dbg_dscrext(); + dbg->wfar = read_dbg_wfar(); + dbg->vcr = read_dbg_vcr(); + dbg->claimclr = read_dbg_claimclr(); + + if (virtext_present) + sr_bp_context(dbg, DBGREG_BP_XVAL, 1); + + sr_bp_context(dbg, DBGREG_BP_VAL, 1); + sr_bp_context(dbg, DBGREG_BP_CTRL, 1); + sr_bp_context(dbg, DBGREG_WP_VAL, 1); + sr_bp_context(dbg, DBGREG_WP_CTRL, 1); + + write_dbg_osdlr(OSDLR_LOCKED); + + return; +} + +static void restore_v7_debug_cp14(unsigned * context) +{ + debug_registers_t *dbg = (void *) context; + unsigned virtext_present = (read_dbg_devid() >> 16) & 0xf; + + /* + * Prevent updates to the debug registers during a S&R operation + */ + write_dbg_oslar(OSLAR_LOCKED); + + write_dbg_dtrrxext(dbg->dtrrx_e); + write_dbg_dtrtxext(dbg->dtrtx_e); + write_dbg_dscrext(dbg->dscr_e); + write_dbg_wfar(dbg->wfar); + write_dbg_vcr(dbg->vcr); + write_dbg_claimset(dbg->claimclr); + + if (virtext_present) + sr_bp_context(dbg, DBGREG_BP_XVAL, 0); + + sr_bp_context(dbg, DBGREG_BP_VAL, 0); + sr_bp_context(dbg, DBGREG_BP_CTRL, 0); + sr_bp_context(dbg, DBGREG_WP_VAL, 0); + sr_bp_context(dbg, DBGREG_WP_CTRL, 0); + isb(); + + /* + * Unlock access to the debug registers + */ + write_dbg_oslar(OSLAR_UNLOCKED); + + return; +} + debug_registers_t *read_debug_address(void) { unsigned drar, dsar; - drar = read_drar(); - dsar = read_dsar(); + drar = read_dbg_drar(); + dsar = read_dbg_dsar(); if (!(drar & DRAR_VALID_MASK) || !(dsar & DSAR_VALID_MASK)) { @@ -128,7 +410,7 @@ debug_registers_t *read_debug_address(void) * - OSDLR is NOT locked, or the debugger would not work properly */ -void save_v7_debug(unsigned * context) +static void save_v7_debug_mmapped(unsigned * context) { debug_registers_t *dbg = (void *)read_debug_address(); debug_context_t *ctx = (void *)context; @@ -177,11 +459,11 @@ void save_v7_debug(unsigned * context) * Once we have done this, debug becomes inaccessible. */ if (v71) { - write_osdlr(OSDLR_LOCKED); + write_dbg_osdlr(OSDLR_LOCKED); } } -void restore_v7_debug(unsigned * context) +static void restore_v7_debug_mmapped(unsigned * context) { debug_registers_t *dbg = (void *)read_debug_address(); debug_context_t *ctx = (void *)context; @@ -207,7 +489,7 @@ void restore_v7_debug(unsigned * context) * (If the CPU has not actually power-cycled, osdlr may not be reset). */ if (v71) { - write_osdlr(OSDLR_UNLOCKED); + write_dbg_osdlr(OSDLR_UNLOCKED); } /* @@ -246,3 +528,41 @@ void restore_v7_debug(unsigned * context) /* Disable write access to registers */ dbg->lar = LAR_LOCKED; } + +void save_v7_debug(unsigned * context) +{ + unsigned v71 = 0, didr = read_dbg_didr(); + + v71 =(((didr >> DIDR_VERSION_SHIFT) & DIDR_VERSION_MASK) == + DIDR_VERSION_7_1); + + /* + * TODO: Code for saving the v7.0 Debug context through the + * cp14 interface has not been implemented as yet. + */ + if(v71) + save_v7_debug_cp14(context); + else + save_v7_debug_mmapped(context); + + return; +} + +void restore_v7_debug(unsigned * context) +{ + unsigned v71 = 0, didr = read_dbg_didr(); + + v71 =(((didr >> DIDR_VERSION_SHIFT) & DIDR_VERSION_MASK) == + DIDR_VERSION_7_1); + + /* + * TODO: Code for restoring the v7.0 Debug context through the + * cp14 interface has not been implemented as yet. + */ + if(v71) + restore_v7_debug_cp14(context); + else + restore_v7_debug_mmapped(context); + + return; +} |