Code Review / openocd.git / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | review | tree
raw | inline | side by side
adi_v5_swd: Improve SWD support
[openocd.git] / src / target / adi_v5_swd.c
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 01782e5f4322936a666de23cbd617a9bb1f3af9e..c9c3ae586591de1189927b7a24a32c9c84acd336 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -55,11 +55,59 @@
 
 #include <jtag/swd.h>
 
+/* 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 ... */
-       return swd->read_reg(swd_cmd(true,  false, reg), data);
+       const struct swd_driver *swd = jtag_interface->swd;
+       assert(swd);
+
+       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,
@@ -76,30 +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 ... */
-       return swd->write_reg(swd_cmd(false,  false, reg), data);
+       const struct swd_driver *swd = jtag_interface->swd;
+       assert(swd);
+
+       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 ... */
-       return swd->read_reg(swd_cmd(true,  true, reg), data);
+       const struct swd_driver *swd = jtag_interface->swd;
+       assert(swd);
+
+       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 ... */
-       return swd->write_reg(swd_cmd(false,  true, reg), data);
-}
+       const struct swd_driver *swd = jtag_interface->swd;
+       assert(swd);
 
-static int (swd_queue_ap_abort)(struct adiv5_dap *dap, uint8_t *ack)
-{
-       return ERROR_FAIL;
+       int retval = swd_queue_ap_bankselect(dap, reg);
+       if (retval != ERROR_OK)
+               return retval;
+
+       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. */
@@ -185,8 +285,6 @@ int dap_to_swd(struct target *target)
        return retval;
 }
 
-
-
 COMMAND_HANDLER(handle_swd_wcr)
 {
        int retval;
@@ -209,7 +307,7 @@ COMMAND_HANDLER(handle_swd_wcr)
                }
 
                command_print(CMD_CTX,
-                       "turnaround=%d, prescale=%d",
+                       "turnaround=%" PRIu32 ", prescale=%" PRIu32,
                        WCR_TO_TRN(wcr),
                        WCR_TO_PRESCALE(wcr));
        return ERROR_OK;
@@ -280,7 +378,6 @@ static const struct command_registration swd_handlers[] = {
 
 static int swd_select(struct command_context *ctx)
 {
-       struct target *target = get_current_target(ctx);
        int retval;
 
        retval = register_commands(ctx, NULL, swd_handlers);
@@ -288,6 +385,8 @@ static int swd_select(struct command_context *ctx)
        if (retval != ERROR_OK)
                return retval;
 
+       const struct swd_driver *swd = jtag_interface->swd;
+
         /* be sure driver is in SWD mode; start
          * with hardware default TRN (1), it can be changed later
          */
@@ -296,14 +395,20 @@ static int swd_select(struct command_context *ctx)
                return ERROR_FAIL;
        }
 
-        retval = swd->init(1);
+       retval = swd->init(1);
        if (retval != ERROR_OK) {
                LOG_DEBUG("can't init SWD driver");
                return retval;
        }
 
        /* force DAP into SWD mode (not JTAG) */
-       retval = dap_to_swd(target);
+       /*retval = dap_to_swd(target);*/
+
+       if (ctx->current_target) {
+               /* force DAP into SWD mode (not JTAG) */
+               struct target *target = get_current_target(ctx);
+               retval = dap_to_swd(target);
+       }
 
        return retval;
 }
@@ -316,6 +421,10 @@ static int swd_init(struct command_context *ctx)
        uint32_t idcode;
        int status;
 
+       /* Force the DAP's ops vector for SWD mode.
+        * messy - is there a better way? */
+       arm->dap->ops = &swd_dap_ops;
+
        /* FIXME validate transport config ... is the
         * configured DAP present (check IDCODE)?
         * Is *only* one DAP configured?
@@ -330,10 +439,15 @@ static int swd_init(struct command_context *ctx)
        status = swd_queue_idcode_read(dap, &ack, &idcode);
 
        if (status == ERROR_OK)
-               LOG_INFO("SWD IDCODE %#8.8x", idcode);
+               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 = {
Open On-Chip Debugger

Linking to existing account procedure

If you already have an account and want to add another login method you MUST first sign in with your existing account and then change URL to read https://review.openocd.org/login/?link to get to this page again but this time it'll work for linking. Thank you.

SSH host keys fingerprints

1024 SHA256:YKx8b7u5ZWdcbp7/4AeXNaqElP49m6QrwfXaqQGJAOk gerrit-code-review@openocd.zylin.com (DSA)
384 SHA256:jHIbSQa4REvwCFG4cq5LBlBLxmxSqelQPem/EXIrxjk gerrit-code-review@openocd.org (ECDSA)
521 SHA256:UAOPYkU9Fjtcao0Ul/Rrlnj/OsQvt+pgdYSZ4jOYdgs gerrit-code-review@openocd.org (ECDSA)
256 SHA256:A13M5QlnozFOvTllybRZH6vm7iSt0XLxbA48yfc2yfY gerrit-code-review@openocd.org (ECDSA)
256 SHA256:spYMBqEYoAOtK7yZBrcwE8ZpYt6b68Cfh9yEVetvbXg gerrit-code-review@openocd.org (ED25519)
+--[ED25519 256]--+
|=..              |
|+o..   .         |
|*.o   . .        |
|+B . . .         |
|Bo. = o S        |
|Oo.+ + =         |
|oB=.* = . o      |
| =+=.+   + E     |
|. .=o   . o      |
+----[SHA256]-----+
2048 SHA256:0Onrb7/PHjpo6iVZ7xQX2riKN83FJ3KGU0TvI0TaFG4 gerrit-code-review@openocd.zylin.com (RSA)