diff options
| -rw-r--r-- | src/jtag/swd.h | 4 | ||||
| -rw-r--r-- | src/target/adi_v5_swd.c | 112 | ||||
| -rw-r--r-- | src/target/arm_adi_v5.c | 2 | ||||
| -rw-r--r-- | src/target/arm_adi_v5.h | 23 |
4 files changed, 121 insertions, 20 deletions
diff --git a/src/jtag/swd.h b/src/jtag/swd.h index f131ddb..fee7f91 100644 --- a/src/jtag/swd.h +++ b/src/jtag/swd.h @@ -33,7 +33,7 @@ /* followed by TRN, 3-bits of ACK, TRN */ /* pbit16 holds precomputed parity bits for each nibble */ -#define pbit(parity, nibble) (parity << nibble) +#define pbit(parity, nibble) ((parity) << (nibble)) static const uint16_t pbit16 = pbit(0, 0) | pbit(1, 1) | pbit(1, 2) | pbit(0, 3) @@ -41,7 +41,7 @@ static const uint16_t pbit16 = | pbit(1, 8) | pbit(0, 9) | pbit(0, 0xa) | pbit(1, 0xb) | pbit(0, 0xc) | pbit(1, 0xd) | pbit(1, 0xe) | pbit(0, 0xf); -#define nibble_parity(nibble) (pbit16 & pbit(1, nibble)) +#define nibble_parity(nibble) (pbit16 & pbit(1, (nibble))) /** * Construct a "cmd" byte, in lSB bit order, which swd_driver.read_reg() diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index 6ff858a..c9c3ae5 100644 --- a/src/target/adi_v5_swd.c +++ b/src/target/adi_v5_swd.c @@ -58,14 +58,56 @@ /* YUK! - but this is currently a global.... */ extern struct jtag_interface *jtag_interface; +static int (swd_queue_dp_write)(struct adiv5_dap *dap, unsigned reg, + uint32_t data); + +static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack) +{ + const struct swd_driver *swd = jtag_interface->swd; + assert(swd); + + return swd->write_reg(swd_cmd(false, false, DP_ABORT), + STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR); +} + +/** Select the DP register bank matching bits 7:4 of reg. */ +static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg) +{ + uint32_t select_dp_bank = (reg & 0x000000F0) >> 4; + + if (reg == DP_SELECT) + return ERROR_OK; + + if (select_dp_bank == dap->dp_bank_value) + return ERROR_OK; + + dap->dp_bank_value = select_dp_bank; + select_dp_bank |= dap->ap_current | dap->ap_bank_value; + + return swd_queue_dp_write(dap, DP_SELECT, select_dp_bank); +} + static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg, uint32_t *data) { + int retval; /* REVISIT status return vs ack ... */ const struct swd_driver *swd = jtag_interface->swd; assert(swd); - return swd->read_reg(swd_cmd(true, false, reg), data); + retval = swd_queue_dp_bankselect(dap, reg); + if (retval != ERROR_OK) + return retval; + + retval = swd->read_reg(swd_cmd(true, false, reg), data); + + if (retval != ERROR_OK) { + /* fault response */ + uint8_t ack = retval & 0xff; + swd_queue_ap_abort(dap, &ack); + } + + return retval; } static int swd_queue_idcode_read(struct adiv5_dap *dap, @@ -82,39 +124,82 @@ static int swd_queue_idcode_read(struct adiv5_dap *dap, static int (swd_queue_dp_write)(struct adiv5_dap *dap, unsigned reg, uint32_t data) { + int retval; /* REVISIT status return vs ack ... */ const struct swd_driver *swd = jtag_interface->swd; assert(swd); - return swd->write_reg(swd_cmd(false, false, reg), data); + retval = swd_queue_dp_bankselect(dap, reg); + if (retval != ERROR_OK) + return retval; + + retval = swd->write_reg(swd_cmd(false, false, reg), data); + + if (retval != ERROR_OK) { + /* fault response */ + uint8_t ack = retval & 0xff; + swd_queue_ap_abort(dap, &ack); + } + + return retval; } +/** Select the AP register bank matching bits 7:4 of reg. */ +static int swd_queue_ap_bankselect(struct adiv5_dap *dap, unsigned reg) +{ + uint32_t select_ap_bank = reg & 0x000000F0; + + if (select_ap_bank == dap->ap_bank_value) + return ERROR_OK; + + dap->ap_bank_value = select_ap_bank; + select_ap_bank |= dap->ap_current | dap->dp_bank_value; + + return swd_queue_dp_write(dap, DP_SELECT, select_ap_bank); +} static int (swd_queue_ap_read)(struct adiv5_dap *dap, unsigned reg, uint32_t *data) { - /* REVISIT APSEL ... */ /* REVISIT status return ... */ const struct swd_driver *swd = jtag_interface->swd; assert(swd); - return swd->read_reg(swd_cmd(true, true, reg), data); + int retval = swd_queue_ap_bankselect(dap, reg); + if (retval != ERROR_OK) + return retval; + + retval = swd->read_reg(swd_cmd(true, true, reg), data); + + if (retval != ERROR_OK) { + /* fault response */ + uint8_t ack = retval & 0xff; + swd_queue_ap_abort(dap, &ack); + } + + return retval; } static int (swd_queue_ap_write)(struct adiv5_dap *dap, unsigned reg, uint32_t data) { - /* REVISIT APSEL ... */ /* REVISIT status return ... */ const struct swd_driver *swd = jtag_interface->swd; assert(swd); - return swd->write_reg(swd_cmd(false, true, reg), data); -} + int retval = swd_queue_ap_bankselect(dap, reg); + if (retval != ERROR_OK) + return retval; -static int (swd_queue_ap_abort)(struct adiv5_dap *dap, uint8_t *ack) -{ - return ERROR_FAIL; + retval = swd->write_reg(swd_cmd(false, true, reg), data); + + if (retval != ERROR_OK) { + /* fault response */ + uint8_t ack = retval & 0xff; + swd_queue_ap_abort(dap, &ack); + } + + return retval; } /** Executes all queued DAP operations. */ @@ -356,8 +441,13 @@ static int swd_init(struct command_context *ctx) if (status == ERROR_OK) LOG_INFO("SWD IDCODE %#8.8" PRIx32, idcode); - return status; + /* force clear all sticky faults */ + swd_queue_ap_abort(dap, &ack); + + /* this is a workaround to get polling working */ + jtag_add_reset(0, 0); + return status; } static struct transport swd_transport = { diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c index 2154c0e..efddd66 100644 --- a/src/target/arm_adi_v5.c +++ b/src/target/arm_adi_v5.c @@ -664,6 +664,8 @@ int ahbap_debugport_init(struct adiv5_dap *dap) /* DP initialization */ + dap->dp_bank_value = 0; + retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL); if (retval != ERROR_OK) return retval; diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h index 131e9d9..d132c57 100644 --- a/src/target/arm_adi_v5.h +++ b/src/target/arm_adi_v5.h @@ -47,18 +47,20 @@ #define DPAP_WRITE 0 #define DPAP_READ 1 +#define BANK_REG(bank, reg) (((bank) << 4) | (reg)) + /* A[3:0] for DP registers; A[1:0] are always zero. * - JTAG accesses all of these via JTAG_DP_DPACC, except for * IDCODE (JTAG_DP_IDCODE) and ABORT (JTAG_DP_ABORT). * - SWD accesses these directly, sometimes needing SELECT.CTRLSEL */ -#define DP_IDCODE 0 /* SWD: read */ -#define DP_ABORT 0 /* SWD: write */ -#define DP_CTRL_STAT 0x4 /* r/w */ -#define DP_WCR 0x4 /* SWD: r/w (mux CTRLSEL) */ -#define DP_RESEND 0x8 /* SWD: read */ -#define DP_SELECT 0x8 /* JTAG: r/w; SWD: write */ -#define DP_RDBUFF 0xC /* read-only */ +#define DP_IDCODE BANK_REG(0x0, 0x0) /* SWD: read */ +#define DP_ABORT BANK_REG(0x0, 0x0) /* SWD: write */ +#define DP_CTRL_STAT BANK_REG(0x0, 0x4) /* r/w */ +#define DP_RESEND BANK_REG(0x0, 0x8) /* SWD: read */ +#define DP_SELECT BANK_REG(0x0, 0x8) /* JTAG: r/w; SWD: write */ +#define DP_RDBUFF BANK_REG(0x0, 0xC) /* read-only */ +#define DP_WCR BANK_REG(0x1, 0x4) /* SWD: r/w */ #define WCR_TO_TRN(wcr) ((uint32_t)(1 + (3 & ((wcr)) >> 8))) /* 1..4 clocks */ #define WCR_TO_PRESCALE(wcr) ((uint32_t)(7 & ((wcr)))) /* impl defined */ @@ -162,6 +164,13 @@ struct adiv5_dap { uint32_t ap_bank_value; /** + * Cache for DP_SELECT bits identifying the current four-word DP + * register bank. This caches DP register addresss bits 7:4; JTAG + * and SWD access primitves pass address bits 3:2; bits 1:0 are zero. + */ + uint32_t dp_bank_value; + + /** * Cache for (MEM-AP) AP_REG_CSW register value. This is written to * configure an access mode, such as autoincrementing AP_REG_TAR during * word access. "-1" indicates no cached value. |