From 81afe6e3fdb1253659d7d5a1be9f4f444c2c18d5 Mon Sep 17 00:00:00 2001 From: Tomas Vanek Date: Mon, 5 Apr 2021 05:57:46 +0200 Subject: [PATCH] target/adi_v5_swd: introduce swd_queue_dp_read/write_inner() This is a preparatory change for swd multidrop, mostly refactoring. Split swd_queue_dp_read/write() to inner and outer parts. Use the inner parts in swd_queue_dp_bankselect(), swd_connect() they do not need to check reconnect. Use the outer parts exclusively in swd_dap_ops. Rearrange the code to reduce forward declarations. Change-Id: I47b7f0cb037e0032a267463f06ba02123ba96fe7 Signed-off-by: Tomas Vanek Reviewed-on: https://review.openocd.org/c/openocd/+/6139 Reviewed-by: Antonio Borneo Tested-by: jenkins --- src/target/adi_v5_swd.c | 166 ++++++++++++++++++++++------------------ 1 file changed, 92 insertions(+), 74 deletions(-) diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c index 5e4fd2222c..6e403e8b22 100644 --- a/src/target/adi_v5_swd.c +++ b/src/target/adi_v5_swd.c @@ -56,6 +56,20 @@ /* for debug, set do_sync to true to force synchronous transfers */ static bool do_sync; + +static int swd_run(struct adiv5_dap *dap); +static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg, + uint32_t data); + + +static int swd_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq) +{ + const struct swd_driver *swd = adiv5_dap_swd_driver(dap); + assert(swd); + + return swd->switch_seq(seq); +} + static void swd_finish_read(struct adiv5_dap *dap) { const struct swd_driver *swd = adiv5_dap_swd_driver(dap); @@ -65,11 +79,6 @@ static void swd_finish_read(struct adiv5_dap *dap) } } -static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg, - uint32_t data); -static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg, - uint32_t *data); - static void swd_clear_sticky_errors(struct adiv5_dap *dap) { const struct swd_driver *swd = adiv5_dap_swd_driver(dap); @@ -94,6 +103,79 @@ static int swd_run_inner(struct adiv5_dap *dap) return retval; } +static inline int check_sync(struct adiv5_dap *dap) +{ + return do_sync ? swd_run_inner(dap) : ERROR_OK; +} + +/** Select the DP register bank matching bits 7:4 of reg. */ +static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg) +{ + /* Only register address 4 is banked. */ + if ((reg & 0xf) != 4) + return ERROR_OK; + + uint32_t select_dp_bank = (reg & 0x000000F0) >> 4; + uint32_t sel = select_dp_bank + | (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK)); + + if (sel == dap->select) + return ERROR_OK; + + dap->select = sel; + + int retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel); + if (retval != ERROR_OK) + dap->select = DP_SELECT_INVALID; + + return retval; +} + +static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg, + uint32_t *data) +{ + const struct swd_driver *swd = adiv5_dap_swd_driver(dap); + assert(swd); + + int retval = swd_queue_dp_bankselect(dap, reg); + if (retval != ERROR_OK) + return retval; + + swd->read_reg(swd_cmd(true, false, reg), data, 0); + + return check_sync(dap); +} + +static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg, + uint32_t data) +{ + int retval; + const struct swd_driver *swd = adiv5_dap_swd_driver(dap); + assert(swd); + + swd_finish_read(dap); + + if (reg == DP_SELECT) { + dap->select = data & (DP_SELECT_APSEL | DP_SELECT_APBANK | DP_SELECT_DPBANK); + + swd->write_reg(swd_cmd(false, false, reg), data, 0); + + retval = check_sync(dap); + if (retval != ERROR_OK) + dap->select = DP_SELECT_INVALID; + + return retval; + } + + retval = swd_queue_dp_bankselect(dap, reg); + if (retval != ERROR_OK) + return retval; + + swd->write_reg(swd_cmd(false, false, reg), data, 0); + + return check_sync(dap); +} + static int swd_connect(struct adiv5_dap *dap) { const struct swd_driver *swd = adiv5_dap_swd_driver(dap); @@ -130,7 +212,7 @@ static int swd_connect(struct adiv5_dap *dap) dap->do_reconnect = false; dap_invalidate_cache(dap); - status = swd_queue_dp_read(dap, DP_DPIDR, &dpidr); + status = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr); if (status == ERROR_OK) { status = swd_run_inner(dap); if (status == ERROR_OK) @@ -186,19 +268,6 @@ static int swd_connect(struct adiv5_dap *dap) return status; } -static int swd_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq) -{ - const struct swd_driver *swd = adiv5_dap_swd_driver(dap); - assert(swd); - - return swd->switch_seq(seq); -} - -static inline int check_sync(struct adiv5_dap *dap) -{ - return do_sync ? swd_run_inner(dap) : ERROR_OK; -} - static int swd_check_reconnect(struct adiv5_dap *dap) { if (dap->do_reconnect) @@ -212,51 +281,19 @@ static int swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack) const struct swd_driver *swd = adiv5_dap_swd_driver(dap); assert(swd); - swd->write_reg(swd_cmd(false, false, DP_ABORT), + swd->write_reg(swd_cmd(false, false, DP_ABORT), DAPABORT | STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR, 0); return check_sync(dap); } -/** Select the DP register bank matching bits 7:4 of reg. */ -static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned reg) -{ - /* Only register address 4 is banked. */ - if ((reg & 0xf) != 4) - return ERROR_OK; - - uint32_t select_dp_bank = (reg & 0x000000F0) >> 4; - uint32_t sel = select_dp_bank - | (dap->select & (DP_SELECT_APSEL | DP_SELECT_APBANK)); - - if (sel == dap->select) - return ERROR_OK; - - dap->select = sel; - - int retval = swd_queue_dp_write(dap, DP_SELECT, sel); - if (retval != ERROR_OK) - dap->select = DP_SELECT_INVALID; - - return retval; -} - static int swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg, uint32_t *data) { - const struct swd_driver *swd = adiv5_dap_swd_driver(dap); - assert(swd); - int retval = swd_check_reconnect(dap); if (retval != ERROR_OK) return retval; - retval = swd_queue_dp_bankselect(dap, reg); - if (retval != ERROR_OK) - return retval; - - swd->read_reg(swd_cmd(true, false, reg), data, 0); - - return check_sync(dap); + return swd_queue_dp_read_inner(dap, reg, data); } static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg, @@ -269,26 +306,7 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg, if (retval != ERROR_OK) return retval; - swd_finish_read(dap); - if (reg == DP_SELECT) { - dap->select = data & (DP_SELECT_APSEL | DP_SELECT_APBANK | DP_SELECT_DPBANK); - - swd->write_reg(swd_cmd(false, false, reg), data, 0); - - retval = check_sync(dap); - if (retval != ERROR_OK) - dap->select = DP_SELECT_INVALID; - - return retval; - } - - retval = swd_queue_dp_bankselect(dap, reg); - if (retval != ERROR_OK) - return retval; - - swd->write_reg(swd_cmd(false, false, reg), data, 0); - - return check_sync(dap); + return swd_queue_dp_write_inner(dap, reg, data); } /** Select the AP register bank matching bits 7:4 of reg. */ @@ -304,7 +322,7 @@ static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg) dap->select = sel; - int retval = swd_queue_dp_write(dap, DP_SELECT, sel); + int retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel); if (retval != ERROR_OK) dap->select = DP_SELECT_INVALID; -- 2.30.2