adi_v5: enforce check on AP number value

[openocd.git] / src / target / arm_adi_v5.c

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 6795b31af6c80a9798d549ac36222bbb964744c4..b4e252bb495a7f927eaa4f0f1f6b0c7677d08548 100644 (file)
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -102,8 +102,10 @@ static int mem_ap_setup_csw(struct adiv5_ap *ap, uint32_t csw)
        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;
        }
@@ -789,7 +793,7 @@ int dap_find_ap(struct adiv5_dap *dap, enum ap_type type_to_find, struct adiv5_a
        int ap_num;
 
        /* Maximum AP number is 255 since the SELECT register is 8 bits */
-       for (ap_num = 0; ap_num <= 255; ap_num++) {
+       for (ap_num = 0; ap_num <= DP_APSEL_MAX; ap_num++) {
 
                /* read the IDR register of the Access Port */
                uint32_t id_val = 0;
@@ -1425,7 +1429,7 @@ int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
        pc = (struct adiv5_private_config *)target->private_config;
        if (pc == NULL) {
                pc = calloc(1, sizeof(struct adiv5_private_config));
-               pc->ap_num = -1;
+               pc->ap_num = DP_APSEL_INVALID;
                target->private_config = pc;
        }
 
@@ -1494,6 +1498,10 @@ int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
                                e = Jim_GetOpt_Wide(goi, &ap_num);
                                if (e != JIM_OK)
                                        return e;
+                               if (ap_num < 0 || ap_num > DP_APSEL_MAX) {
+                                       Jim_SetResultString(goi->interp, "Invalid AP number!", -1);
+                                       return JIM_ERR;
+                               }
                                pc->ap_num = ap_num;
                        } else {
                                if (goi->argc != 0) {
@@ -1503,7 +1511,7 @@ int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
                                        return JIM_ERR;
                                }
 
-                               if (pc->ap_num < 0) {
+                               if (pc->ap_num == DP_APSEL_INVALID) {
                                        Jim_SetResultString(goi->interp, "AP number not configured", -1);
                                        return JIM_ERR;
                                }
@@ -1539,7 +1547,7 @@ COMMAND_HANDLER(handle_dap_info_command)
                break;
        case 1:
                COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
-               if (apsel >= 256)
+               if (apsel > DP_APSEL_MAX)
                        return ERROR_COMMAND_SYNTAX_ERROR;
                break;
        default:
@@ -1562,7 +1570,7 @@ COMMAND_HANDLER(dap_baseaddr_command)
        case 1:
                COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
                /* AP address is in bits 31:24 of DP_SELECT */
-               if (apsel >= 256)
+               if (apsel > DP_APSEL_MAX)
                        return ERROR_COMMAND_SYNTAX_ERROR;
                break;
        default:
@@ -1612,17 +1620,16 @@ COMMAND_HANDLER(dap_memaccess_command)
 COMMAND_HANDLER(dap_apsel_command)
 {
        struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
-       uint32_t apsel, apid;
-       int retval;
+       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 */
-               if (apsel >= 256)
+               if (apsel > DP_APSEL_MAX)
                        return ERROR_COMMAND_SYNTAX_ERROR;
                break;
        default:
@@ -1630,18 +1637,7 @@ 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)
@@ -1699,7 +1695,7 @@ COMMAND_HANDLER(dap_apid_command)
        case 1:
                COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
                /* AP address is in bits 31:24 of DP_SELECT */
-               if (apsel >= 256)
+               if (apsel > DP_APSEL_MAX)
                        return ERROR_COMMAND_SYNTAX_ERROR;
                break;
        default:
@@ -1730,7 +1726,7 @@ COMMAND_HANDLER(dap_apreg_command)
 
        COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], apsel);
        /* AP address is in bits 31:24 of DP_SELECT */
-       if (apsel >= 256)
+       if (apsel > DP_APSEL_MAX)
                return ERROR_COMMAND_SYNTAX_ERROR;
        ap = dap_ap(dap, apsel);
 
@@ -1768,6 +1764,37 @@ COMMAND_HANDLER(dap_apreg_command)
        return retval;
 }
 
+COMMAND_HANDLER(dap_dpreg_command)
+{
+       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);
@@ -1803,7 +1830,7 @@ const struct command_registration dap_instance_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]",
@@ -1811,7 +1838,7 @@ const struct command_registration dap_instance_commands[] = {
        {
                .name = "apcsw",
                .handler = dap_apcsw_command,
-               .mode = COMMAND_EXEC,
+               .mode = COMMAND_ANY,
                .help = "Set CSW default bits",
                .usage = "[value [mask]]",
        },
@@ -1832,6 +1859,14 @@ const struct command_registration dap_instance_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,