Skip to content

Commit

Permalink
target/arm_adi_v5: fix DP SELECT logic
Browse files Browse the repository at this point in the history 
The original code supported ADIv5 only, just one SELECT register
with some reserved bits - the pseudo value DP_SELECT_INVALID was
just fine to indicate the DP SELECT register is in an unknown state.

Added ADIv6 support required DP SELECT and SELECT1 registers
without reserved bits. Therefore DP_SELECT_INVALID value became
reachable as a (fortunately not really used) ADIv6 AP ADDR.

JTAG DPBANKSEL setting support introduced with ADIv6 does not
honor DP_SELECT_INVALID correctly: required select value
gets compared to DP_SELECT_INVALID value and the most common zero
bank does not trigger DP SELECT write.

DP banked registers need just to set DP SELECT. ADIv6 AP register
addressing scheme may use both DP SELECT and SELECT1. This further
complicates using a single invalid value.

Moreover the difference how the SWD line reset influences
DPBANKSEL field between ADIv5 and ADIv6 deserves better handling
than setting select cache to zero and then to DP_SELECT_INVALID
in a very specific code positions.

Introduce bool flags indicating the validity of each SELECT
register and one SWD specific for DPBANKSEL field.
Use the latter to prevent selecting DP BANK before taking
the connection out of reset by reading DPIDR.

Treat DP SELECT and SELECT1 individually in ADIv6 64-bit mode.

Update comments to reflect the difference between ADIv5 and ADIv6
in SWD line reset.

Change-Id: Ibbb0b06cb592be072571218b666566a13d8dff0e
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: https://review.openocd.org/c/openocd/+/7541
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
  • Loading branch information
@tom-van
tom-van committed Dec 29, 2023
1 parent 492dc7c commit ee3fb5a
Show file tree
Hide file tree
Showing 4 changed files with 164 additions and 101 deletions.
89 changes: 61 additions & 28 deletions src/target/adi_v5_jtag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -353,17 +353,25 @@ static int adi_jtag_dp_scan_u32(struct adiv5_dap *dap,
uint64_t sel = (reg_addr >> 4) & DP_SELECT_DPBANK;

/* No need to change SELECT or RDBUFF as they are not banked */
if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF &&
sel != (dap->select & 0xf)) {
if (dap->select != DP_SELECT_INVALID)
sel |= dap->select & ~0xfull;
dap->select = sel;
LOG_DEBUG("DP BANKSEL: %x", (uint32_t)sel);
if (instr == JTAG_DP_DPACC && reg_addr != DP_SELECT && reg_addr != DP_RDBUFF
&& (!dap->select_valid || sel != (dap->select & DP_SELECT_DPBANK))) {
/* Use the AP part of dap->select regardless of dap->select_valid:
* if !dap->select_valid
* dap->select contains a speculative value likely going to be used
* in the following swd_queue_ap_bankselect() */
sel |= dap->select & SELECT_AP_MASK;

LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, (uint32_t)sel);

buf_set_u32(out_value_buf, 0, 32, (uint32_t)sel);

retval = adi_jtag_dp_scan(dap, JTAG_DP_DPACC,
DP_SELECT, DPAP_WRITE, out_value_buf, NULL, 0, NULL);
if (retval != ERROR_OK)
return retval;

dap->select = sel;
dap->select_valid = true;
}
buf_set_u32(out_value_buf, 0, 32, outvalue);

Expand Down Expand Up @@ -520,7 +528,10 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
/* timeout happened */
if (tmp->ack == JTAG_ACK_WAIT) {
LOG_ERROR("Timeout during WAIT recovery");
dap->select = DP_SELECT_INVALID;
dap->select_valid = false;
dap->select1_valid = false;
/* Keep dap->select unchanged, the same AP and AP bank
* is likely going to be used further */
jtag_ap_q_abort(dap, NULL);
/* clear the sticky overrun condition */
adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
Expand Down Expand Up @@ -580,7 +591,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)

/* TODO: ADIv6 DP SELECT1 handling */

dap->select = DP_SELECT_INVALID;
dap->select_valid = false;
}

list_for_each_entry_safe(el, tmp, &replay_list, lh) {
Expand Down Expand Up @@ -615,7 +626,10 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
if (retval == ERROR_OK) {
if (el->ack == JTAG_ACK_WAIT) {
LOG_ERROR("Timeout during WAIT recovery");
dap->select = DP_SELECT_INVALID;
dap->select_valid = false;
dap->select1_valid = false;
/* Keep dap->select unchanged, the same AP and AP bank
* is likely going to be used further */
jtag_ap_q_abort(dap, NULL);
/* clear the sticky overrun condition */
adi_jtag_scan_inout_check_u32(dap, JTAG_DP_DPACC,
Expand Down Expand Up @@ -748,41 +762,60 @@ static int jtag_dp_q_write(struct adiv5_dap *dap, unsigned reg,
return retval;
}

/** Select the AP register bank matching bits 7:4 of reg. */
/** Select the AP register bank */
static int jtag_ap_q_bankselect(struct adiv5_ap *ap, unsigned reg)
{
int retval;
struct adiv5_dap *dap = ap->dap;
uint64_t sel;

if (is_adiv6(dap)) {
if (is_adiv6(dap))
sel = ap->ap_num | (reg & 0x00000FF0);
if (sel == (dap->select & ~0xfull))
return ERROR_OK;
else
sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);

if (dap->select != DP_SELECT_INVALID)
sel |= dap->select & 0xf;
dap->select = sel;
LOG_DEBUG("AP BANKSEL: %" PRIx64, sel);
uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;

bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
bool set_select1 = is_adiv6(dap) && dap->asize > 32
&& (!dap->select1_valid
|| sel_diff & (0xffffffffull << 32));

if (set_select && set_select1) {
/* Prepare DP bank for DP_SELECT1 now to save one write */
sel |= (DP_SELECT1 >> 4) & DP_SELECT_DPBANK;
} else {
/* Use the DP part of dap->select regardless of dap->select_valid:
* if !dap->select_valid
* dap->select contains a speculative value likely going to be used
* in the following swd_queue_dp_bankselect().
* Moreover dap->select_valid should never be false here as a DP bank
* is always selected before selecting an AP bank */
sel |= dap->select & DP_SELECT_DPBANK;
}

if (set_select) {
LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);

retval = jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
if (retval != ERROR_OK)
if (retval != ERROR_OK) {
dap->select_valid = false;
return retval;

if (dap->asize > 32)
return jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
return ERROR_OK;
}
}

/* ADIv5 */
sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
if (set_select1) {
LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));

if (sel == dap->select)
return ERROR_OK;
retval = jtag_dp_q_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
if (retval != ERROR_OK) {
dap->select1_valid = false;
return retval;
}
}

dap->select = sel;

return jtag_dp_q_write(dap, DP_SELECT, (uint32_t)sel);
return ERROR_OK;
}

static int jtag_ap_q_read(struct adiv5_ap *ap, unsigned reg,
Expand Down
148 changes: 80 additions & 68 deletions src/target/adi_v5_swd.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,27 +99,31 @@ 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. */
/** Select the DP register bank */
static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
{
/* Only register address 0 and 4 are banked. */
/* Only register address 0 (ADIv6 only) and 4 are banked. */
if ((reg & 0xf) > 4)
return ERROR_OK;

uint64_t sel = (reg & 0x000000F0) >> 4;
if (dap->select != DP_SELECT_INVALID)
sel |= dap->select & ~0xfULL;
uint32_t sel = (reg >> 4) & DP_SELECT_DPBANK;

if (sel == dap->select)
/* DP register 0 is not mapped according to ADIv5
* whereas ADIv6 ensures DPBANKSEL = 0 after line reset */
if ((dap->select_valid || ((reg & 0xf) == 0 && dap->select_dpbanksel_valid))
&& (sel == (dap->select & DP_SELECT_DPBANK)))
return ERROR_OK;

dap->select = sel;
/* Use the AP part of dap->select regardless of dap->select_valid:
* if !dap->select_valid
* dap->select contains a speculative value likely going to be used
* in the following swd_queue_ap_bankselect() */
sel |= (uint32_t)(dap->select & SELECT_AP_MASK);

int retval = swd_queue_dp_write_inner(dap, DP_SELECT, (uint32_t)sel);
if (retval != ERROR_OK)
dap->select = DP_SELECT_INVALID;
LOG_DEBUG_IO("DP BANK SELECT: %" PRIx32, sel);

return retval;
/* dap->select cache gets updated in the following call */
return swd_queue_dp_write_inner(dap, DP_SELECT, sel);
}

static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg,
Expand Down Expand Up @@ -147,24 +151,31 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
swd_finish_read(dap);

if (reg == DP_SELECT) {
dap->select = data & (ADIV5_DP_SELECT_APSEL | ADIV5_DP_SELECT_APBANK | DP_SELECT_DPBANK);
dap->select = data | (dap->select & (0xffffffffull << 32));

swd->write_reg(swd_cmd(false, false, reg), data, 0);

retval = check_sync(dap);
if (retval != ERROR_OK)
dap->select = DP_SELECT_INVALID;
dap->select_valid = (retval == ERROR_OK);
dap->select_dpbanksel_valid = dap->select_valid;

return retval;
}

if (reg == DP_SELECT1)
dap->select = ((uint64_t)data << 32) | (dap->select & 0xffffffffull);

retval = swd_queue_dp_bankselect(dap, reg);
if (retval != ERROR_OK)
return retval;
if (retval == ERROR_OK) {
swd->write_reg(swd_cmd(false, false, reg), data, 0);

retval = check_sync(dap);
}

swd->write_reg(swd_cmd(false, false, reg), data, 0);
if (reg == DP_SELECT1)
dap->select1_valid = (retval == ERROR_OK);

return check_sync(dap);
return retval;
}


Expand All @@ -177,19 +188,17 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
assert(dap_is_multidrop(dap));

swd_send_sequence(dap, LINE_RESET);
/* From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
* sequence":
* - line reset sets DP_SELECT_DPBANK to zero;
* - read of DP_DPIDR takes the connection out of reset;
* - write of DP_TARGETSEL keeps the connection in reset;
* - other accesses return protocol error (SWDIO not driven by target).
*
* Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
* skip the write to DP_SELECT, avoiding the protocol error. Set again
* dap->select to DP_SELECT_INVALID because the rest of the register is
* unknown after line reset.
/*
* Zero dap->select and set dap->select_dpbanksel_valid
* to skip the write to DP_SELECT before DPIDR read, avoiding
* the protocol error.
* Clear the other validity flags because the rest of the DP
* SELECT and SELECT1 registers is unknown after line reset.
*/
dap->select = 0;
dap->select_dpbanksel_valid = true;
dap->select_valid = false;
dap->select1_valid = false;

retval = swd_queue_dp_write_inner(dap, DP_TARGETSEL, dap->multidrop_targetsel);
if (retval != ERROR_OK)
Expand All @@ -209,8 +218,6 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
return retval;
}

dap->select = DP_SELECT_INVALID;

retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr);
if (retval != ERROR_OK)
return retval;
Expand Down Expand Up @@ -335,19 +342,20 @@ static int swd_connect_single(struct adiv5_dap *dap)

/* The sequences to enter in SWD (JTAG_TO_SWD and DORMANT_TO_SWD) end
* with a SWD line reset sequence (50 clk with SWDIO high).
* From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
* sequence":
* - line reset sets DP_SELECT_DPBANK to zero;
* From ARM IHI 0031F ADIv5.2 and ARM IHI 0074C ADIv6.0,
* chapter B4.3.3 "Connection and line reset sequence":
* - DPv3 (ADIv6) only: line reset sets DP_SELECT_DPBANK to zero;
* - read of DP_DPIDR takes the connection out of reset;
* - write of DP_TARGETSEL keeps the connection in reset;
* - other accesses return protocol error (SWDIO not driven by target).
*
* Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
* skip the write to DP_SELECT, avoiding the protocol error. Set again
* dap->select to DP_SELECT_INVALID because the rest of the register is
* unknown after line reset.
* dap_invalidate_cache() sets dap->select to zero and all validity
* flags to invalid. Set dap->select_dpbanksel_valid only
* to skip the write to DP_SELECT, avoiding the protocol error.
* Read DP_DPIDR to get out of reset.
*/
dap->select = 0;
dap->select_dpbanksel_valid = true;

retval = swd_queue_dp_read_inner(dap, DP_DPIDR, &dpidr);
if (retval == ERROR_OK) {
retval = swd_run_inner(dap);
Expand All @@ -360,8 +368,6 @@ static int swd_connect_single(struct adiv5_dap *dap)
dap->switch_through_dormant = !dap->switch_through_dormant;
} while (timeval_ms() < timeout);

dap->select = DP_SELECT_INVALID;

if (retval != ERROR_OK) {
LOG_ERROR("Error connecting DP: cannot read IDR");
return retval;
Expand Down Expand Up @@ -494,49 +500,55 @@ static int swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg,
return swd_queue_dp_write_inner(dap, reg, data);
}

/** Select the AP register bank matching bits 7:4 of reg. */
/** Select the AP register bank */
static int swd_queue_ap_bankselect(struct adiv5_ap *ap, unsigned reg)
{
int retval;
struct adiv5_dap *dap = ap->dap;
uint64_t sel;

if (is_adiv6(dap)) {
if (is_adiv6(dap))
sel = ap->ap_num | (reg & 0x00000FF0);
if (sel == (dap->select & ~0xfULL))
return ERROR_OK;

if (dap->select != DP_SELECT_INVALID)
sel |= dap->select & 0xf;
dap->select = sel;
LOG_DEBUG("AP BANKSEL: %" PRIx64, sel);

retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel);
else
sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);

if (retval == ERROR_OK && dap->asize > 32)
retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
uint64_t sel_diff = (sel ^ dap->select) & SELECT_AP_MASK;

if (retval != ERROR_OK)
dap->select = DP_SELECT_INVALID;
bool set_select = !dap->select_valid || (sel_diff & 0xffffffffull);
bool set_select1 = is_adiv6(dap) && dap->asize > 32
&& (!dap->select1_valid
|| sel_diff & (0xffffffffull << 32));

return retval;
if (set_select && set_select1) {
/* Prepare DP bank for DP_SELECT1 now to save one write */
sel |= (DP_SELECT1 & 0x000000f0) >> 4;
} else {
/* Use the DP part of dap->select regardless of dap->select_valid:
* if !dap->select_valid
* dap->select contains a speculative value likely going to be used
* in the following swd_queue_dp_bankselect().
* Moreover dap->select_valid should never be false here as a DP bank
* is always selected before selecting an AP bank */
sel |= dap->select & DP_SELECT_DPBANK;
}

/* ADIv5 */
sel = (ap->ap_num << 24) | (reg & ADIV5_DP_SELECT_APBANK);
if (dap->select != DP_SELECT_INVALID)
sel |= dap->select & DP_SELECT_DPBANK;
if (set_select) {
LOG_DEBUG_IO("AP BANK SELECT: %" PRIx32, (uint32_t)sel);

if (sel == dap->select)
return ERROR_OK;
retval = swd_queue_dp_write(dap, DP_SELECT, (uint32_t)sel);
if (retval != ERROR_OK)
return retval;
}

dap->select = sel;
if (set_select1) {
LOG_DEBUG_IO("AP BANK SELECT1: %" PRIx32, (uint32_t)(sel >> 32));

retval = swd_queue_dp_write_inner(dap, DP_SELECT, sel);
if (retval != ERROR_OK)
dap->select = DP_SELECT_INVALID;
retval = swd_queue_dp_write(dap, DP_SELECT1, (uint32_t)(sel >> 32));
if (retval != ERROR_OK)
return retval;
}

return retval;
return ERROR_OK;
}

static int swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg,
Expand Down
6 changes: 5 additions & 1 deletion src/target/arm_adi_v5.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -655,7 +655,11 @@ int mem_ap_write_buf_noincr(struct adiv5_ap *ap,
*/
void dap_invalidate_cache(struct adiv5_dap *dap)
{
dap->select = DP_SELECT_INVALID;
dap->select = 0; /* speculate the first AP access will select AP 0, bank 0 */
dap->select_valid = false;
dap->select1_valid = false;
dap->select_dpbanksel_valid = false;

dap->last_read = NULL;

int i;
Expand Down

0 comments on commit ee3fb5a

Please sign in to comment.