5 files changed, 32 insertions, 21 deletions

diff --git a/core.c b/core.c
index cbc3bc2..1c247f0 100644
--- a/core.c
+++ b/core.c
@@ -213,31 +213,39 @@ hal_error_t hal_core_alloc(const char *name, hal_core_t **pcore)
   if (name == NULL)
     name = core->info.name;
 
-  hal_critical_section_start();
   if (core != NULL) {
     /* if we can reallocate the same core, do it now */
     if (!core->busy) {
+      hal_critical_section_start();
       core->busy = 1;
       hal_critical_section_end();
       return HAL_OK;
     }
     /* else fall through to search */
   }
-  for (core = hal_core_iterate(NULL); core != NULL; core = core->next) {
-    if (name_matches(core, name)) {
-      if (core->busy) {
-        err = HAL_ERROR_CORE_BUSY;
-        continue;
-      }
-      else {
-        err = HAL_OK;
-        *pcore = core;
-        core->busy = 1;
-        break;
+
+  while (1) {
+    hal_critical_section_start();
+    for (core = hal_core_iterate(NULL); core != NULL; core = core->next) {
+      if (name_matches(core, name)) {
+        if (core->busy) {
+          err = HAL_ERROR_CORE_BUSY;
+          continue;
+        }
+        else {
+          err = HAL_OK;
+          *pcore = core;
+          core->busy = 1;
+          break;
+        }
       }
     }
-  }
-  hal_critical_section_end();
+    hal_critical_section_end();
+    if (err == HAL_ERROR_CORE_BUSY)
+      hal_task_yield();
+    else
+      break;
+  } 
 
   return err;
 }
@@ -248,6 +256,7 @@ void hal_core_free(hal_core_t *core)
     hal_critical_section_start();
     core->busy = 0;
     hal_critical_section_end();
+    hal_task_yield();
   }
 }
 
diff --git a/hal_internal.h b/hal_internal.h
index f17179c..659cd27 100644
--- a/hal_internal.h
+++ b/hal_internal.h
@@ -97,6 +97,7 @@ extern void hal_critical_section_start(void);
 extern void hal_critical_section_end(void);
 extern void hal_ks_lock(void);
 extern void hal_ks_unlock(void);
+extern void hal_task_yield(void);
 
 /*
  * Logging.
diff --git a/hal_io_fmc.c b/hal_io_fmc.c
index 7aa4b19..76d6883 100644
--- a/hal_io_fmc.c
+++ b/hal_io_fmc.c
@@ -159,6 +159,8 @@ hal_error_t hal_io_wait(const hal_core_t *core, uint8_t status, int *count)
     if (count && (*count > 0) && (i >= *count))
       return HAL_ERROR_IO_TIMEOUT;
 
+    hal_task_yield();
+
     if ((err = hal_io_read(core, ADDR_STATUS, buf, sizeof(buf))) != HAL_OK)
       return err;
 
diff --git a/locks.c b/locks.c
index 2165753..9b81769 100644
--- a/locks.c
+++ b/locks.c
@@ -101,6 +101,11 @@ WEAK_FUNCTION void hal_ks_unlock(void)
   return;
 }
 
+WEAK_FUNCTION void hal_task_yield(void)
+{
+  return;
+}
+
 /*
  * Local variables:
  * indent-tabs-mode: nil
diff --git a/rpc_server.c b/rpc_server.c
index 8d8af18..a01572e 100644
--- a/rpc_server.c
+++ b/rpc_server.c
@@ -966,13 +966,7 @@ hal_error_t hal_rpc_server_dispatch(const uint8_t * const ibuf, const size_t ile
     }
 
     if (handler)
-        for (int i = 0; i < 3; ++i) {
-            ret = handler(&iptr, ilimit, &optr, olimit);
-            if (ret != HAL_ERROR_CORE_BUSY)
-                break;
-            iptr = ibuf + 4;
-            optr = obuf + 12;
-        }
+        ret = handler(&iptr, ilimit, &optr, olimit);
     else
         ret = HAL_ERROR_RPC_BAD_FUNCTION;