target/arm_adi_v5,arm_dap: introduce multidrop_targetsel and its configuration

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

diff --git a/src/target/arm_dap.c b/src/target/arm_dap.c
index 94edfc09d14116859955ffc3a263b5aad612b71d..bde1dfb5ee9279814b20dc88b08a9302c6f5f563 100644 (file)
--- a/src/target/arm_dap.c
+++ b/src/target/arm_dap.c
@@ -155,21 +155,24 @@ int dap_cleanup_all(void)
 enum dap_cfg_param {
        CFG_CHAIN_POSITION,
        CFG_IGNORE_SYSPWRUPACK,
+       CFG_DP_ID,
+       CFG_INSTANCE_ID,
 };
 
 static const struct jim_nvp nvp_config_opts[] = {
-       { .name = "-chain-position",   .value = CFG_CHAIN_POSITION },
+       { .name = "-chain-position",     .value = CFG_CHAIN_POSITION },
        { .name = "-ignore-syspwrupack", .value = CFG_IGNORE_SYSPWRUPACK },
+       { .name = "-dp-id",              .value = CFG_DP_ID },
+       { .name = "-instance-id",        .value = CFG_INSTANCE_ID },
        { .name = NULL, .value = -1 }
 };
 
 static int dap_configure(struct jim_getopt_info *goi, struct arm_dap_object *dap)
 {
-       struct jtag_tap *tap = NULL;
        struct jim_nvp *n;
        int e;
 
-       /* parse config or cget options ... */
+       /* parse config ... */
        while (goi->argc > 0) {
                Jim_SetEmptyResult(goi->interp);
 
@@ -184,30 +187,67 @@ static int dap_configure(struct jim_getopt_info *goi, struct arm_dap_object *dap
                        e = jim_getopt_obj(goi, &o_t);
                        if (e != JIM_OK)
                                return e;
+
+                       struct jtag_tap *tap;
                        tap = jtag_tap_by_jim_obj(goi->interp, o_t);
                        if (!tap) {
                                Jim_SetResultString(goi->interp, "-chain-position is invalid", -1);
                                return JIM_ERR;
                        }
+                       dap->dap.tap = tap;
                        /* loop for more */
                        break;
                }
                case CFG_IGNORE_SYSPWRUPACK:
                        dap->dap.ignore_syspwrupack = true;
                        break;
+               case CFG_DP_ID: {
+                       jim_wide w;
+                       e = jim_getopt_wide(goi, &w);
+                       if (e != JIM_OK) {
+                               Jim_SetResultFormatted(goi->interp,
+                                               "create %s: bad parameter %s",
+                                               dap->name, n->name);
+                               return JIM_ERR;
+                       }
+                       if (w < 0 || w > DP_TARGETSEL_DPID_MASK) {
+                               Jim_SetResultFormatted(goi->interp,
+                                               "create %s: %s out of range",
+                                               dap->name, n->name);
+                               return JIM_ERR;
+                       }
+                       dap->dap.multidrop_targetsel =
+                               (dap->dap.multidrop_targetsel & DP_TARGETSEL_INSTANCEID_MASK)
+                               | (w & DP_TARGETSEL_DPID_MASK);
+                       dap->dap.multidrop_dp_id_valid = true;
+                       break;
+               }
+               case CFG_INSTANCE_ID: {
+                       jim_wide w;
+                       e = jim_getopt_wide(goi, &w);
+                       if (e != JIM_OK) {
+                               Jim_SetResultFormatted(goi->interp,
+                                               "create %s: bad parameter %s",
+                                               dap->name, n->name);
+                               return JIM_ERR;
+                       }
+                       if (w < 0 || w > 15) {
+                               Jim_SetResultFormatted(goi->interp,
+                                               "create %s: %s out of range",
+                                               dap->name, n->name);
+                               return JIM_ERR;
+                       }
+                       dap->dap.multidrop_targetsel =
+                               (dap->dap.multidrop_targetsel & DP_TARGETSEL_DPID_MASK)
+                               | ((w << DP_TARGETSEL_INSTANCEID_SHIFT) & DP_TARGETSEL_INSTANCEID_MASK);
+                       dap->dap.multidrop_instance_id_valid = true;
+                       break;
+               }
                default:
                        break;
                }
        }
 
-       if (!tap) {
-               Jim_SetResultString(goi->interp, "-chain-position required when creating DAP", -1);
-               return JIM_ERR;
-       }
-
-       dap_instance_init(&dap->dap);
-       dap->dap.tap = tap;
-
        return JIM_OK;
 }
 
@@ -242,15 +282,21 @@ static int dap_create(struct jim_getopt_info *goi)
        if (!dap)
                return JIM_ERR;
 
-       e = dap_configure(goi, dap);
-       if (e != JIM_OK) {
-               free(dap);
-               return e;
-       }
+       dap_instance_init(&dap->dap);
 
        cp = Jim_GetString(new_cmd, NULL);
        dap->name = strdup(cp);
 
+       e = dap_configure(goi, dap);
+       if (e != JIM_OK)
+               goto err;
+
+       if (!dap->dap.tap) {
+               Jim_SetResultString(goi->interp, "-chain-position required when creating DAP", -1);
+               e = JIM_ERR;
+               goto err;
+       }
+
        struct command_registration dap_commands[] = {
                {
                        .name = cp,
@@ -267,12 +313,19 @@ static int dap_create(struct jim_getopt_info *goi)
                dap_commands[0].chain = NULL;
 
        e = register_commands_with_data(cmd_ctx, NULL, dap_commands, dap);
-       if (e != ERROR_OK)
-               return JIM_ERR;
+       if (e != ERROR_OK) {
+               e = JIM_ERR;
+               goto err;
+       }
 
        list_add_tail(&dap->lh, &all_dap);
 
        return JIM_OK;
+
+err:
+       free(dap->name);
+       free(dap);
+       return e;
 }
 
 static int jim_dap_create(Jim_Interp *interp, int argc, Jim_Obj *const *argv)