initial SWD transport (SWD infrastructure #2)

[openocd] / src / jtag / core.c

diff --git a/src/jtag/core.c b/src/jtag/core.c
index dd3a2a61646c0037567947e121f0d95b6f576452..6e923bd47a3a3c95ecdab59cc0af19f86803f690 100644 (file)
--- a/src/jtag/core.c
+++ b/src/jtag/core.c
@@ -129,6 +129,9 @@ static int jtag_speed = 0;
 
 static struct jtag_interface *jtag = NULL;
 
+
+const struct swd_driver *swd = NULL;
+
 /* configuration */
 struct jtag_interface *jtag_interface = NULL;
 
@@ -296,28 +299,24 @@ int jtag_register_event_callback(jtag_event_handler_t callback, void *priv)
 
 int jtag_unregister_event_callback(jtag_event_handler_t callback, void *priv)
 {
-       struct jtag_event_callback **callbacks_p;
-       struct jtag_event_callback **next;
+       struct jtag_event_callback **p = &jtag_event_callbacks, *temp;
 
        if (callback == NULL)
        {
                return ERROR_INVALID_ARGUMENTS;
        }
 
-       for (callbacks_p = &jtag_event_callbacks;
-                       *callbacks_p != NULL;
-                       callbacks_p = next)
+       while (*p)
        {
-               next = &((*callbacks_p)->next);
-
-               if ((*callbacks_p)->priv != priv)
-                       continue;
-
-               if ((*callbacks_p)->callback == callback)
+               if (((*p)->priv != priv) || ((*p)->callback != callback))
                {
-                       free(*callbacks_p);
-                       *callbacks_p = *next;
+                       p = &(*p)->next;
+                       continue;
                }
+
+               temp = *p;
+               *p = (*p)->next;
+               free(temp);
        }
 
        return ERROR_OK;
@@ -1384,9 +1383,13 @@ int adapter_init(struct command_context *cmd_ctx)
 
        int requested_khz = jtag_get_speed_khz();
        int actual_khz = requested_khz;
-       int retval = jtag_get_speed_readable(&actual_khz);
+       int jtag_speed_var;
+       int retval = jtag_get_speed(&jtag_speed_var);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = jtag_get_speed_readable(&actual_khz);
        if (ERROR_OK != retval)
-               LOG_INFO("adapter-specific clock speed value %d", jtag_get_speed());
+               LOG_INFO("adapter-specific clock speed value %d", jtag_speed_var);
        else if (actual_khz)
        {
                /* Adaptive clocking -- JTAG-specific */
@@ -1632,31 +1635,33 @@ int jtag_config_rclk(unsigned fallback_speed_khz)
        return (ERROR_OK != retval) ? retval : jtag_set_speed(speed);
 }
 
-int jtag_get_speed(void)
+int jtag_get_speed(int *speed)
 {
-       int speed = 0; /* avoid -O3 warning */
        switch(clock_mode)
        {
                case CLOCK_MODE_SPEED:
-                       speed = jtag_speed;
+                       *speed = jtag_speed;
                        break;
                case CLOCK_MODE_KHZ:
-                       adapter_khz_to_speed(jtag_get_speed_khz(), &speed);
+                       adapter_khz_to_speed(jtag_get_speed_khz(), speed);
                        break;
                case CLOCK_MODE_RCLK:
-                       jtag_rclk_to_speed(rclk_fallback_speed_khz, &speed);
+                       jtag_rclk_to_speed(rclk_fallback_speed_khz, speed);
                        break;
                default:
                        LOG_ERROR("BUG: unknown jtag clock mode");
-                       speed = 0;
-                       break;
+                       return ERROR_FAIL;
        }
-       return speed;
+       return ERROR_OK;
 }
 
 int jtag_get_speed_readable(int *khz)
 {
-       return jtag ? jtag->speed_div(jtag_get_speed(), khz) : ERROR_OK;
+       int jtag_speed_var;
+       int retval = jtag_get_speed(&jtag_speed_var);
+       if (retval != ERROR_OK)
+               return retval;
+       return jtag ? jtag->speed_div(jtag_speed_var, khz) : ERROR_OK;
 }
 
 void jtag_set_verify(bool enable)