]> git.sur5r.net Git - openocd/blobdiff - src/target/arm_adi_v5.c
target/arm_adi_v5: allow commands apsel and apcsw during init phase
[openocd] / src / target / arm_adi_v5.c
index ae6c2c629fdb62f71bf8ad60ef8aebd843ee8b49..302ea78913cdc939ef9f92952fc670d8b345169c 100644 (file)
@@ -76,6 +76,7 @@
 #include <helper/jep106.h>
 #include <helper/time_support.h>
 #include <helper/list.h>
+#include <helper/jim-nvp.h>
 
 /* ARM ADI Specification requires at least 10 bits used for TAR autoincrement  */
 
@@ -96,14 +97,15 @@ static uint32_t max_tar_block_size(uint32_t tar_autoincr_block, uint32_t address
 
 static int mem_ap_setup_csw(struct adiv5_ap *ap, uint32_t csw)
 {
-       csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT |
-               ap->csw_default;
+       csw |= ap->csw_default;
 
        if (csw != ap->csw_value) {
                /* LOG_DEBUG("DAP: Set CSW %x",csw); */
                int retval = dap_queue_ap_write(ap, MEM_AP_REG_CSW, csw);
-               if (retval != ERROR_OK)
+               if (retval != ERROR_OK) {
+                       ap->csw_value = 0;
                        return retval;
+               }
                ap->csw_value = csw;
        }
        return ERROR_OK;
@@ -114,8 +116,10 @@ static int mem_ap_setup_tar(struct adiv5_ap *ap, uint32_t tar)
        if (!ap->tar_valid || tar != ap->tar_value) {
                /* LOG_DEBUG("DAP: Set TAR %x",tar); */
                int retval = dap_queue_ap_write(ap, MEM_AP_REG_TAR, tar);
-               if (retval != ERROR_OK)
+               if (retval != ERROR_OK) {
+                       ap->tar_valid = false;
                        return retval;
+               }
                ap->tar_value = tar;
                ap->tar_valid = true;
        }
@@ -152,6 +156,8 @@ static uint32_t mem_ap_get_tar_increment(struct adiv5_ap *ap)
                        return 2;
                case CSW_32BIT:
                        return 4;
+               default:
+                       return 0;
                }
        case CSW_ADDRINC_PACKED:
                return 4;
@@ -325,7 +331,7 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
        const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
        uint32_t csw_size;
        uint32_t addr_xor;
-       int retval;
+       int retval = ERROR_OK;
 
        /* TI BE-32 Quirks mode:
         * Writes on big-endian TMS570 behave very strangely. Observed behavior:
@@ -378,33 +384,34 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
                /* How many source bytes each transfer will consume, and their location in the DRW,
                 * depends on the type of transfer and alignment. See ARM document IHI0031C. */
                uint32_t outvalue = 0;
+               uint32_t drw_byte_idx = address;
                if (dap->ti_be_32_quirks) {
                        switch (this_size) {
                        case 4:
-                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx & 3) ^ addr_xor);
                                break;
                        case 2:
-                               outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (address++ & 3) ^ addr_xor);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (address++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx & 3) ^ addr_xor);
                                break;
                        case 1:
-                               outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (address++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (drw_byte_idx & 3) ^ addr_xor);
                                break;
                        }
                } else {
                        switch (this_size) {
                        case 4:
-                               outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
                                /* fallthrough */
                        case 2:
-                               outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
                                /* fallthrough */
                        case 1:
-                               outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx & 3);
                        }
                }
 
@@ -415,6 +422,8 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
                        break;
 
                mem_ap_update_tar_cache(ap);
+               if (addrinc)
+                       address += this_size;
        }
 
        /* REVISIT: Might want to have a queued version of this function that does not run. */
@@ -452,7 +461,7 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
        const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
        uint32_t csw_size;
        uint32_t address = adr;
-       int retval;
+       int retval = ERROR_OK;
 
        /* TI BE-32 Quirks mode:
         * Reads on big-endian TMS570 behave strangely differently than writes.
@@ -476,7 +485,8 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
        /* Allocate buffer to hold the sequence of DRW reads that will be made. This is a significant
         * over-allocation if packed transfers are going to be used, but determining the real need at
         * this point would be messy. */
-       uint32_t *read_buf = malloc(count * sizeof(uint32_t));
+       uint32_t *read_buf = calloc(count, sizeof(uint32_t));
+       /* Multiplication count * sizeof(uint32_t) may overflow, calloc() is safe */
        uint32_t *read_ptr = read_buf;
        if (read_buf == NULL) {
                LOG_ERROR("Failed to allocate read buffer");
@@ -509,7 +519,8 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
                        break;
 
                nbytes -= this_size;
-               address += this_size;
+               if (addrinc)
+                       address += this_size;
 
                mem_ap_update_tar_cache(ap);
        }
@@ -609,33 +620,8 @@ int mem_ap_write_buf_noincr(struct adiv5_ap *ap,
 
 #define DAP_POWER_DOMAIN_TIMEOUT (10)
 
-/* FIXME don't import ... just initialize as
- * part of DAP transport setup
-*/
-extern const struct dap_ops jtag_dp_ops;
-
 /*--------------------------------------------------------------------------*/
 
-/**
- * Create a new DAP
- */
-struct adiv5_dap *dap_init(void)
-{
-       struct adiv5_dap *dap = calloc(1, sizeof(struct adiv5_dap));
-       int i;
-       /* Set up with safe defaults */
-       for (i = 0; i <= 255; i++) {
-               dap->ap[i].dap = dap;
-               dap->ap[i].ap_num = i;
-               /* memaccess_tck max is 255 */
-               dap->ap[i].memaccess_tck = 255;
-               /* Number of bits for tar autoincrement, impl. dep. at least 10 */
-               dap->ap[i].tar_autoincr_block = (1<<10);
-       }
-       INIT_LIST_HEAD(&dap->cmd_journal);
-       return dap;
-}
-
 /**
  * Invalidate cached DP select and cached TAR and CSW of all APs
  */
@@ -662,14 +648,7 @@ int dap_dp_init(struct adiv5_dap *dap)
 {
        int retval;
 
-       LOG_DEBUG(" ");
-       /* JTAG-DP or SWJ-DP, in JTAG mode
-        * ... for SWD mode this is patched as part
-        * of link switchover
-        * FIXME: This should already be setup by the respective transport specific DAP creation.
-        */
-       if (!dap->ops)
-               dap->ops = &jtag_dp_ops;
+       LOG_DEBUG("%s", adiv5_dap_name(dap));
 
        dap_invalidate_cache(dap);
 
@@ -702,12 +681,14 @@ int dap_dp_init(struct adiv5_dap *dap)
        if (retval != ERROR_OK)
                return retval;
 
-       LOG_DEBUG("DAP: wait CSYSPWRUPACK");
-       retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
-                                     CSYSPWRUPACK, CSYSPWRUPACK,
-                                     DAP_POWER_DOMAIN_TIMEOUT);
-       if (retval != ERROR_OK)
-               return retval;
+       if (!dap->ignore_syspwrupack) {
+               LOG_DEBUG("DAP: wait CSYSPWRUPACK");
+               retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
+                                             CSYSPWRUPACK, CSYSPWRUPACK,
+                                             DAP_POWER_DOMAIN_TIMEOUT);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
 
        retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
        if (retval != ERROR_OK)
@@ -1036,12 +1017,14 @@ static const struct {
        { ARM_ID, 0x4a2, "Cortex-A57 ROM",             "(ROM Table)", },
        { ARM_ID, 0x4a3, "Cortex-A53 ROM",             "(v7 Memory Map ROM Table)", },
        { ARM_ID, 0x4a4, "Cortex-A72 ROM",             "(ROM Table)", },
+       { ARM_ID, 0x4a9, "Cortex-A9 ROM",              "(ROM Table)", },
        { ARM_ID, 0x4af, "Cortex-A15 ROM",             "(ROM Table)", },
        { ARM_ID, 0x4c0, "Cortex-M0+ ROM",             "(ROM Table)", },
        { ARM_ID, 0x4c3, "Cortex-M3 ROM",              "(ROM Table)", },
        { ARM_ID, 0x4c4, "Cortex-M4 ROM",              "(ROM Table)", },
        { ARM_ID, 0x4c7, "Cortex-M7 PPB ROM",          "(Private Peripheral Bus ROM Table)", },
        { ARM_ID, 0x4c8, "Cortex-M7 ROM",              "(ROM Table)", },
+       { ARM_ID, 0x4b5, "Cortex-R5 ROM",              "(ROM Table)", },
        { ARM_ID, 0x470, "Cortex-M1 ROM",              "(ROM Table)", },
        { ARM_ID, 0x471, "Cortex-M0 ROM",              "(ROM Table)", },
        { ARM_ID, 0x906, "CoreSight CTI",              "(Cross Trigger)", },
@@ -1082,7 +1065,7 @@ static const struct {
        { ARM_ID, 0x9a9, "Cortex-M7 TPIU",             "(Trace Port Interface Unit)", },
        { ARM_ID, 0x9ae, "Cortex-A17 PMU",             "(Performance Monitor Unit)", },
        { ARM_ID, 0x9af, "Cortex-A15 PMU",             "(Performance Monitor Unit)", },
-       { ARM_ID, 0x9b7, "Cortex-R7 PMU",              "(Performance Monitoring Unit)", },
+       { ARM_ID, 0x9b7, "Cortex-R7 PMU",              "(Performance Monitor Unit)", },
        { ARM_ID, 0x9d3, "Cortex-A53 PMU",             "(Performance Monitor Unit)", },
        { ARM_ID, 0x9d7, "Cortex-A57 PMU",             "(Performance Monitor Unit)", },
        { ARM_ID, 0x9d8, "Cortex-A72 PMU",             "(Performance Monitor Unit)", },
@@ -1105,6 +1088,11 @@ static const struct {
        { 0x0c1,  0x1ed, "XMC1000 ROM",                "(ROM Table)" },
        { 0x0E5,  0x000, "SHARC+/Blackfin+",           "", },
        { 0x0F0,  0x440, "Qualcomm QDSS Component v1", "(Qualcomm Designed CoreSight Component v1)", },
+       { 0x3eb,  0x181, "Tegra 186 ROM",              "(ROM Table)", },
+       { 0x3eb,  0x211, "Tegra 210 ROM",              "(ROM Table)", },
+       { 0x3eb,  0x202, "Denver ETM",                 "(Denver Embedded Trace)", },
+       { 0x3eb,  0x302, "Denver Debug",               "(Debug Unit)", },
+       { 0x3eb,  0x402, "Denver PMU",                 "(Performance Monitor Unit)", },
        /* legacy comment: 0x113: what? */
        { ANY_ID, 0x120, "TI SDTI",                    "(System Debug Trace Interface)", }, /* from OMAP3 memmap */
        { ANY_ID, 0x343, "TI DAPCTL",                  "", }, /* from OMAP3 memmap */
@@ -1364,7 +1352,7 @@ static int dap_rom_display(struct command_context *cmd_ctx,
        return ERROR_OK;
 }
 
-static int dap_info_command(struct command_context *cmd_ctx,
+int dap_info_command(struct command_context *cmd_ctx,
                struct adiv5_ap *ap)
 {
        int retval;
@@ -1422,46 +1410,131 @@ static int dap_info_command(struct command_context *cmd_ctx,
        return ERROR_OK;
 }
 
+enum adiv5_cfg_param {
+       CFG_DAP,
+       CFG_AP_NUM
+};
+
+static const Jim_Nvp nvp_config_opts[] = {
+       { .name = "-dap",    .value = CFG_DAP },
+       { .name = "-ap-num", .value = CFG_AP_NUM },
+       { .name = NULL, .value = -1 }
+};
+
 int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
 {
        struct adiv5_private_config *pc;
-       const char *arg;
-       jim_wide ap_num;
        int e;
 
-       /* check if argv[0] is for us */
-       arg = Jim_GetString(goi->argv[0], NULL);
-       if (strcmp(arg, "-ap-num"))
-               return JIM_CONTINUE;
+       pc = (struct adiv5_private_config *)target->private_config;
+       if (pc == NULL) {
+               pc = calloc(1, sizeof(struct adiv5_private_config));
+               pc->ap_num = -1;
+               target->private_config = pc;
+       }
 
-       e = Jim_GetOpt_String(goi, &arg, NULL);
-       if (e != JIM_OK)
-               return e;
+       target->has_dap = true;
+
+       if (goi->argc > 0) {
+               Jim_Nvp *n;
+
+               Jim_SetEmptyResult(goi->interp);
+
+               /* check first if topmost item is for us */
+               e = Jim_Nvp_name2value_obj(goi->interp, nvp_config_opts,
+                                                                  goi->argv[0], &n);
+               if (e != JIM_OK)
+                       return JIM_CONTINUE;
+
+               e = Jim_GetOpt_Obj(goi, NULL);
+               if (e != JIM_OK)
+                       return e;
+
+               switch (n->value) {
+               case CFG_DAP:
+                       if (goi->isconfigure) {
+                               Jim_Obj *o_t;
+                               struct adiv5_dap *dap;
+                               e = Jim_GetOpt_Obj(goi, &o_t);
+                               if (e != JIM_OK)
+                                       return e;
+                               dap = dap_instance_by_jim_obj(goi->interp, o_t);
+                               if (dap == NULL) {
+                                       Jim_SetResultString(goi->interp, "DAP name invalid!", -1);
+                                       return JIM_ERR;
+                               }
+                               if (pc->dap != NULL && pc->dap != dap) {
+                                       Jim_SetResultString(goi->interp,
+                                               "DAP assignment cannot be changed after target was created!", -1);
+                                       return JIM_ERR;
+                               }
+                               if (target->tap_configured) {
+                                       Jim_SetResultString(goi->interp,
+                                               "-chain-position and -dap configparams are mutually exclusive!", -1);
+                                       return JIM_ERR;
+                               }
+                               pc->dap = dap;
+                               target->tap = dap->tap;
+                               target->dap_configured = true;
+                       } else {
+                               if (goi->argc != 0) {
+                                       Jim_WrongNumArgs(goi->interp,
+                                                                               goi->argc, goi->argv,
+                                       "NO PARAMS");
+                                       return JIM_ERR;
+                               }
+
+                               if (pc->dap == NULL) {
+                                       Jim_SetResultString(goi->interp, "DAP not configured", -1);
+                                       return JIM_ERR;
+                               }
+                               Jim_SetResultString(goi->interp, adiv5_dap_name(pc->dap), -1);
+                       }
+                       break;
 
-       if (goi->argc == 0) {
-               Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "-ap-num ?ap-number? ...");
-               return JIM_ERR;
+               case CFG_AP_NUM:
+                       if (goi->isconfigure) {
+                               jim_wide ap_num;
+                               e = Jim_GetOpt_Wide(goi, &ap_num);
+                               if (e != JIM_OK)
+                                       return e;
+                               pc->ap_num = ap_num;
+                       } else {
+                               if (goi->argc != 0) {
+                                       Jim_WrongNumArgs(goi->interp,
+                                                                        goi->argc, goi->argv,
+                                         "NO PARAMS");
+                                       return JIM_ERR;
+                               }
+
+                               if (pc->ap_num < 0) {
+                                       Jim_SetResultString(goi->interp, "AP number not configured", -1);
+                                       return JIM_ERR;
+                               }
+                               Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, (int)pc->ap_num));
+                       }
+                       break;
+               }
        }
 
-       e = Jim_GetOpt_Wide(goi, &ap_num);
-       if (e != JIM_OK)
-               return e;
+       return JIM_OK;
+}
 
-       if (target->private_config == NULL) {
-               pc = calloc(1, sizeof(struct adiv5_private_config));
-               target->private_config = pc;
-               pc->ap_num = ap_num;
-       }
+int adiv5_verify_config(struct adiv5_private_config *pc)
+{
+       if (pc == NULL)
+               return ERROR_FAIL;
 
+       if (pc->dap == NULL)
+               return ERROR_FAIL;
 
-       return JIM_OK;
+       return ERROR_OK;
 }
 
+
 COMMAND_HANDLER(handle_dap_info_command)
 {
-       struct target *target = get_current_target(CMD_CTX);
-       struct arm *arm = target_to_arm(target);
-       struct adiv5_dap *dap = arm->dap;
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
        uint32_t apsel;
 
        switch (CMD_ARGC) {
@@ -1482,10 +1555,7 @@ COMMAND_HANDLER(handle_dap_info_command)
 
 COMMAND_HANDLER(dap_baseaddr_command)
 {
-       struct target *target = get_current_target(CMD_CTX);
-       struct arm *arm = target_to_arm(target);
-       struct adiv5_dap *dap = arm->dap;
-
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
        uint32_t apsel, baseaddr;
        int retval;
 
@@ -1522,10 +1592,7 @@ COMMAND_HANDLER(dap_baseaddr_command)
 
 COMMAND_HANDLER(dap_memaccess_command)
 {
-       struct target *target = get_current_target(CMD_CTX);
-       struct arm *arm = target_to_arm(target);
-       struct adiv5_dap *dap = arm->dap;
-
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
        uint32_t memaccess_tck;
 
        switch (CMD_ARGC) {
@@ -1548,17 +1615,13 @@ COMMAND_HANDLER(dap_memaccess_command)
 
 COMMAND_HANDLER(dap_apsel_command)
 {
-       struct target *target = get_current_target(CMD_CTX);
-       struct arm *arm = target_to_arm(target);
-       struct adiv5_dap *dap = arm->dap;
-
-       uint32_t apsel, apid;
-       int retval;
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
+       uint32_t apsel;
 
        switch (CMD_ARGC) {
        case 0:
-               apsel = dap->apsel;
-               break;
+               command_print(CMD_CTX, "%" PRIi32, dap->apsel);
+               return ERROR_OK;
        case 1:
                COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
                /* AP address is in bits 31:24 of DP_SELECT */
@@ -1570,42 +1633,40 @@ COMMAND_HANDLER(dap_apsel_command)
        }
 
        dap->apsel = apsel;
-
-       retval = dap_queue_ap_read(dap_ap(dap, apsel), AP_REG_IDR, &apid);
-       if (retval != ERROR_OK)
-               return retval;
-       retval = dap_run(dap);
-       if (retval != ERROR_OK)
-               return retval;
-
-       command_print(CMD_CTX, "ap %" PRIi32 " selected, identification register 0x%8.8" PRIx32,
-                       apsel, apid);
-
-       return retval;
+       return ERROR_OK;
 }
 
 COMMAND_HANDLER(dap_apcsw_command)
 {
-       struct target *target = get_current_target(CMD_CTX);
-       struct arm *arm = target_to_arm(target);
-       struct adiv5_dap *dap = arm->dap;
-
-       uint32_t apcsw = dap->ap[dap->apsel].csw_default, sprot = 0;
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
+       uint32_t apcsw = dap->ap[dap->apsel].csw_default;
+       uint32_t csw_val, csw_mask;
 
        switch (CMD_ARGC) {
        case 0:
-               command_print(CMD_CTX, "apsel %" PRIi32 " selected, csw 0x%8.8" PRIx32,
-                       (dap->apsel), apcsw);
-               break;
+               command_print(CMD_CTX, "ap %" PRIi32 " selected, csw 0x%8.8" PRIx32,
+                       dap->apsel, apcsw);
+               return ERROR_OK;
        case 1:
-               COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], sprot);
-               /* AP address is in bits 31:24 of DP_SELECT */
-               if (sprot > 1)
-                       return ERROR_COMMAND_SYNTAX_ERROR;
-               if (sprot)
-                       apcsw |= CSW_SPROT;
+               if (strcmp(CMD_ARGV[0], "default") == 0)
+                       csw_val = CSW_DEFAULT;
                else
-                       apcsw &= ~CSW_SPROT;
+                       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], csw_val);
+
+               if (csw_val & (CSW_SIZE_MASK | CSW_ADDRINC_MASK)) {
+                       LOG_ERROR("CSW value cannot include 'Size' and 'AddrInc' bit-fields");
+                       return ERROR_COMMAND_SYNTAX_ERROR;
+               }
+               apcsw = csw_val;
+               break;
+       case 2:
+               COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], csw_val);
+               COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], csw_mask);
+               if (csw_mask & (CSW_SIZE_MASK | CSW_ADDRINC_MASK)) {
+                       LOG_ERROR("CSW mask cannot include 'Size' and 'AddrInc' bit-fields");
+                       return ERROR_COMMAND_SYNTAX_ERROR;
+               }
+               apcsw = (apcsw & ~csw_mask) | (csw_val & csw_mask);
                break;
        default:
                return ERROR_COMMAND_SYNTAX_ERROR;
@@ -1619,10 +1680,7 @@ COMMAND_HANDLER(dap_apcsw_command)
 
 COMMAND_HANDLER(dap_apid_command)
 {
-       struct target *target = get_current_target(CMD_CTX);
-       struct arm *arm = target_to_arm(target);
-       struct adiv5_dap *dap = arm->dap;
-
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
        uint32_t apsel, apid;
        int retval;
 
@@ -1654,11 +1712,9 @@ COMMAND_HANDLER(dap_apid_command)
 
 COMMAND_HANDLER(dap_apreg_command)
 {
-       struct target *target = get_current_target(CMD_CTX);
-       struct arm *arm = target_to_arm(target);
-       struct adiv5_dap *dap = arm->dap;
-
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
        uint32_t apsel, reg, value;
+       struct adiv5_ap *ap;
        int retval;
 
        if (CMD_ARGC < 2 || CMD_ARGC > 3)
@@ -1668,6 +1724,7 @@ COMMAND_HANDLER(dap_apreg_command)
        /* AP address is in bits 31:24 of DP_SELECT */
        if (apsel >= 256)
                return ERROR_COMMAND_SYNTAX_ERROR;
+       ap = dap_ap(dap, apsel);
 
        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], reg);
        if (reg >= 256 || (reg & 3))
@@ -1675,9 +1732,21 @@ COMMAND_HANDLER(dap_apreg_command)
 
        if (CMD_ARGC == 3) {
                COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], value);
-               retval = dap_queue_ap_write(dap_ap(dap, apsel), reg, value);
+               switch (reg) {
+               case MEM_AP_REG_CSW:
+                       ap->csw_default = 0;  /* invalid, force write */
+                       retval = mem_ap_setup_csw(ap, value);
+                       break;
+               case MEM_AP_REG_TAR:
+                       ap->tar_valid = false;  /* invalid, force write */
+                       retval = mem_ap_setup_tar(ap, value);
+                       break;
+               default:
+                       retval = dap_queue_ap_write(ap, reg, value);
+                       break;
+               }
        } else {
-               retval = dap_queue_ap_read(dap_ap(dap, apsel), reg, &value);
+               retval = dap_queue_ap_read(ap, reg, &value);
        }
        if (retval == ERROR_OK)
                retval = dap_run(dap);
@@ -1691,12 +1760,40 @@ COMMAND_HANDLER(dap_apreg_command)
        return retval;
 }
 
-COMMAND_HANDLER(dap_ti_be_32_quirks_command)
+COMMAND_HANDLER(dap_dpreg_command)
 {
-       struct target *target = get_current_target(CMD_CTX);
-       struct arm *arm = target_to_arm(target);
-       struct adiv5_dap *dap = arm->dap;
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
+       uint32_t reg, value;
+       int retval;
+
+       if (CMD_ARGC < 1 || CMD_ARGC > 2)
+               return ERROR_COMMAND_SYNTAX_ERROR;
 
+       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], reg);
+       if (reg >= 256 || (reg & 3))
+               return ERROR_COMMAND_SYNTAX_ERROR;
+
+       if (CMD_ARGC == 2) {
+               COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
+               retval = dap_queue_dp_write(dap, reg, value);
+       } else {
+               retval = dap_queue_dp_read(dap, reg, &value);
+       }
+       if (retval == ERROR_OK)
+               retval = dap_run(dap);
+
+       if (retval != ERROR_OK)
+               return retval;
+
+       if (CMD_ARGC == 1)
+               command_print(CMD_CTX, "0x%08" PRIx32, value);
+
+       return retval;
+}
+
+COMMAND_HANDLER(dap_ti_be_32_quirks_command)
+{
+       struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
        uint32_t enable = dap->ti_be_32_quirks;
 
        switch (CMD_ARGC) {
@@ -1717,7 +1814,7 @@ COMMAND_HANDLER(dap_ti_be_32_quirks_command)
        return 0;
 }
 
-static const struct command_registration dap_commands[] = {
+const struct command_registration dap_instance_commands[] = {
        {
                .name = "info",
                .handler = handle_dap_info_command,
@@ -1729,7 +1826,7 @@ static const struct command_registration dap_commands[] = {
        {
                .name = "apsel",
                .handler = dap_apsel_command,
-               .mode = COMMAND_EXEC,
+               .mode = COMMAND_ANY,
                .help = "Set the currently selected AP (default 0) "
                        "and display the result",
                .usage = "[ap_num]",
@@ -1737,9 +1834,9 @@ static const struct command_registration dap_commands[] = {
        {
                .name = "apcsw",
                .handler = dap_apcsw_command,
-               .mode = COMMAND_EXEC,
-               .help = "Set csw access bit ",
-               .usage = "[sprot]",
+               .mode = COMMAND_ANY,
+               .help = "Set CSW default bits",
+               .usage = "[value [mask]]",
        },
 
        {
@@ -1758,6 +1855,14 @@ static const struct command_registration dap_commands[] = {
                        "(reg is byte address of a word register, like 0 4 8...)",
                .usage = "ap_num reg [value]",
        },
+       {
+               .name = "dpreg",
+               .handler = dap_dpreg_command,
+               .mode = COMMAND_EXEC,
+               .help = "read/write a register from DP "
+                       "(reg is byte address (bank << 4 | reg) of a word register, like 0 4 8...)",
+               .usage = "reg [value]",
+       },
        {
                .name = "baseaddr",
                .handler = dap_baseaddr_command,
@@ -1783,14 +1888,3 @@ static const struct command_registration dap_commands[] = {
        },
        COMMAND_REGISTRATION_DONE
 };
-
-const struct command_registration dap_command_handlers[] = {
-       {
-               .name = "dap",
-               .mode = COMMAND_EXEC,
-               .help = "DAP command group",
-               .usage = "",
-               .chain = dap_commands,
-       },
-       COMMAND_REGISTRATION_DONE
-};