Diff: source/core/MicroBitFiber.cpp

Revision:

1:8aa5cdb4ab67

Child:

22:23d7b9a4b082

diff -r fb15f7887843 -r 8aa5cdb4ab67 source/core/MicroBitFiber.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/core/MicroBitFiber.cpp	Thu Apr 07 01:33:22 2016 +0100
@@ -0,0 +1,964 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2016 British Broadcasting Corporation.
+This software is provided by Lancaster University by arrangement with the BBC.
+
+Permission is hereby granted, free of charge, to any person obtaining a
+copy of this software and associated documentation files (the "Software"),
+to deal in the Software without restriction, including without limitation
+the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and/or sell copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+*/
+
+/**
+  * Functionality definitions for the MicroBit Fiber scheduler.
+  *
+  * This lightweight, non-preemptive scheduler provides a simple threading mechanism for two main purposes:
+  *
+  * 1) To provide a clean abstraction for application languages to use when building async behaviour (callbacks).
+  * 2) To provide ISR decoupling for EventModel events generated in an ISR context.
+  */
+#include "MicroBitConfig.h"
+#include "MicroBitFiber.h"
+#include "MicroBitSystemTimer.h"
+
+/*
+ * Statically allocated values used to create and destroy Fibers.
+ * required to be defined here to allow persistence during context switches.
+ */
+Fiber *currentFiber = NULL;                        // The context in which the current fiber is executing.
+static Fiber *forkedFiber = NULL;                  // The context in which a newly created child fiber is executing.
+static Fiber *idleFiber = NULL;                    // the idle task - performs a power efficient sleep, and system maintenance tasks.
+
+/*
+ * Scheduler state.
+ */
+static Fiber *runQueue = NULL;                     // The list of runnable fibers.
+static Fiber *sleepQueue = NULL;                   // The list of blocked fibers waiting on a fiber_sleep() operation.
+static Fiber *waitQueue = NULL;                    // The list of blocked fibers waiting on an event.
+static Fiber *fiberPool = NULL;                    // Pool of unused fibers, just waiting for a job to do.
+
+/*
+ * Scheduler wide flags
+ */
+static uint8_t fiber_flags = 0;
+
+
+/*
+ * Fibers may perform wait/notify semantics on events. If set, these operations will be permitted on this EventModel.
+ */
+static EventModel *messageBus = NULL;
+
+// Array of components which are iterated during idle thread execution, isIdleCallbackNeeded is polled during a systemTick.
+static MicroBitComponent* idleThreadComponents[MICROBIT_IDLE_COMPONENTS];
+
+/**
+  * Utility function to add the currenty running fiber to the given queue.
+  *
+  * Perform a simple add at the head, to avoid complexity,
+  *
+  * Queues are normally very short, so maintaining a doubly linked, sorted list typically outweighs the cost of
+  * brute force searching.
+  *
+  * @param f The fiber to add to the queue
+  *
+  * @param queue The run queue to add the fiber to.
+  */
+void queue_fiber(Fiber *f, Fiber **queue)
+{
+    __disable_irq();
+
+    // Record which queue this fiber is on.
+    f->queue = queue;
+
+    // Add the fiber to the tail of the queue. Although this involves scanning the
+    // list, it results in fairer scheduling.
+    if (*queue == NULL)
+    {
+        f->next = NULL;
+        f->prev = NULL;
+        *queue = f;
+    }
+    else
+    {
+        // Scan to the end of the queue.
+        // We don't maintain a tail pointer to save RAM (queues are nrmally very short).
+        Fiber *last = *queue;
+
+        while (last->next != NULL)
+            last = last->next;
+
+        last->next = f;
+        f->prev = last;
+        f->next = NULL;
+    }
+
+    __enable_irq();
+}
+
+/**
+  * Utility function to the given fiber from whichever queue it is currently stored on.
+  *
+  * @param f the fiber to remove.
+  */
+void dequeue_fiber(Fiber *f)
+{
+    // If this fiber is already dequeued, nothing the there's nothing to do.
+    if (f->queue == NULL)
+        return;
+
+    // Remove this fiber fromm whichever queue it is on.
+    __disable_irq();
+
+    if (f->prev != NULL)
+        f->prev->next = f->next;
+    else
+        *(f->queue) = f->next;
+
+    if(f->next)
+        f->next->prev = f->prev;
+
+    f->next = NULL;
+    f->prev = NULL;
+    f->queue = NULL;
+
+    __enable_irq();
+
+}
+
+/**
+  * Allocates a fiber from the fiber pool if availiable. Otherwise, allocates a new one from the heap.
+  */
+Fiber *getFiberContext()
+{
+    Fiber *f;
+
+    __disable_irq();
+
+    if (fiberPool != NULL)
+    {
+        f = fiberPool;
+        dequeue_fiber(f);
+        // dequeue_fiber() exits with irqs enabled, so no need to do this again!
+    }
+    else
+    {
+        __enable_irq();
+
+        f = new Fiber();
+
+        if (f == NULL)
+            return NULL;
+
+        f->stack_bottom = 0;
+        f->stack_top = 0;
+    }
+
+    // Ensure this fiber is in suitable state for reuse.
+    f->flags = 0;
+    f->tcb.stack_base = CORTEX_M0_STACK_BASE;
+
+    return f;
+}
+
+
+/**
+  * Initialises the Fiber scheduler.
+  * Creates a Fiber context around the calling thread, and adds it to the run queue as the current thread.
+  *
+  * This function must be called once only from the main thread, and before any other Fiber operation.
+  *
+  * @param _messageBus An event model, used to direct the priorities of the scheduler.
+  */
+void scheduler_init(EventModel &_messageBus)
+{
+    // If we're already initialised, then nothing to do.
+    if (fiber_scheduler_running())
+        return;
+
+	// Store a reference to the messageBus provided.
+	// This parameter will be NULL if we're being run without a message bus.
+	messageBus = &_messageBus;
+
+    // Create a new fiber context
+    currentFiber = getFiberContext();
+
+    // Add ourselves to the run queue.
+    queue_fiber(currentFiber, &runQueue);
+
+    // Create the IDLE fiber.
+    // Configure the fiber to directly enter the idle task.
+    idleFiber = getFiberContext();
+    idleFiber->tcb.SP = CORTEX_M0_STACK_BASE - 0x04;
+    idleFiber->tcb.LR = (uint32_t) &idle_task;
+
+	if (messageBus)
+	{
+		// Register to receive events in the NOTIFY channel - this is used to implement wait-notify semantics
+		messageBus->listen(MICROBIT_ID_NOTIFY, MICROBIT_EVT_ANY, scheduler_event, MESSAGE_BUS_LISTENER_IMMEDIATE);
+		messageBus->listen(MICROBIT_ID_NOTIFY_ONE, MICROBIT_EVT_ANY, scheduler_event, MESSAGE_BUS_LISTENER_IMMEDIATE);
+	}
+
+	// register a period callback to drive the scheduler and any other registered components.
+    new MicroBitSystemTimerCallback(scheduler_tick);
+
+	fiber_flags |= MICROBIT_SCHEDULER_RUNNING;
+}
+
+/**
+  * Determines if the fiber scheduler is operational.
+  *
+  * @return 1 if the fber scheduler is running, 0 otherwise.
+  */
+int fiber_scheduler_running()
+{
+	if (fiber_flags & MICROBIT_SCHEDULER_RUNNING)
+		return 1;
+
+	return 0;
+}
+
+/**
+  * The timer callback, called from interrupt context once every SYSTEM_TICK_PERIOD_MS milliseconds.
+  * This function checks to determine if any fibers blocked on the sleep queue need to be woken up
+  * and made runnable.
+  */
+void scheduler_tick()
+{
+    Fiber *f = sleepQueue;
+    Fiber *t;
+
+    // Check the sleep queue, and wake up any fibers as necessary.
+    while (f != NULL)
+    {
+        t = f->next;
+
+        if (system_timer_current_time() >= f->context)
+        {
+            // Wakey wakey!
+            dequeue_fiber(f);
+            queue_fiber(f,&runQueue);
+        }
+
+        f = t;
+    }
+}
+
+/**
+  * Event callback. Called from an instance of MicroBitMessageBus whenever an event is raised.
+  *
+  * This function checks to determine if any fibers blocked on the wait queue need to be woken up
+  * and made runnable due to the event.
+  *
+  * @param evt the event that has just been raised on an instance of MicroBitMessageBus.
+  */
+void scheduler_event(MicroBitEvent evt)
+{
+    Fiber *f = waitQueue;
+    Fiber *t;
+    int notifyOneComplete = 0;
+
+	// This should never happen.
+	// It is however, safe to simply ignore any events provided, as if no messageBus if recorded,
+	// no fibers are permitted to block on events.
+	if (messageBus == NULL)
+		return;
+
+    // Check the wait queue, and wake up any fibers as necessary.
+    while (f != NULL)
+    {
+        t = f->next;
+
+        // extract the event data this fiber is blocked on.
+        uint16_t id = f->context & 0xFFFF;
+        uint16_t value = (f->context & 0xFFFF0000) >> 16;
+
+        // Special case for the NOTIFY_ONE channel...
+        if ((evt.source == MICROBIT_ID_NOTIFY_ONE && id == MICROBIT_ID_NOTIFY) && (value == MICROBIT_EVT_ANY || value == evt.value))
+        {
+            if (!notifyOneComplete)
+            {
+                // Wakey wakey!
+                dequeue_fiber(f);
+                queue_fiber(f,&runQueue);
+                notifyOneComplete = 1;
+            }
+        }
+
+        // Normal case.
+        else if ((id == MICROBIT_ID_ANY || id == evt.source) && (value == MICROBIT_EVT_ANY || value == evt.value))
+        {
+            // Wakey wakey!
+            dequeue_fiber(f);
+            queue_fiber(f,&runQueue);
+        }
+
+        f = t;
+    }
+
+    // Unregister this event, as we've woken up all the fibers with this match.
+    if (evt.source != MICROBIT_ID_NOTIFY && evt.source != MICROBIT_ID_NOTIFY_ONE)
+        messageBus->ignore(evt.source, evt.value, scheduler_event);
+}
+
+
+/**
+  * Blocks the calling thread for the given period of time.
+  * The calling thread will be immediateley descheduled, and placed onto a
+  * wait queue until the requested amount of time has elapsed.
+  *
+  * @param t The period of time to sleep, in milliseconds.
+  *
+  * @note the fiber will not be be made runnable until after the elapsed time, but there
+  * are no guarantees precisely when the fiber will next be scheduled.
+  */
+void fiber_sleep(unsigned long t)
+{
+    Fiber *f = currentFiber;
+
+    // If the scheduler is not running, then simply perform a spin wait and exit.
+    if (!fiber_scheduler_running())
+    {
+        wait_ms(t);
+        return;
+    }
+
+    // Sleep is a blocking call, so if we're in a fork on block context,
+    // it's time to spawn a new fiber...
+    if (currentFiber->flags & MICROBIT_FIBER_FLAG_FOB)
+    {
+        // Allocate a new fiber. This will come from the fiber pool if availiable,
+        // else a new one will be allocated on the heap.
+        forkedFiber = getFiberContext();
+
+        // If we're out of memory, there's nothing we can do.
+        // keep running in the context of the current thread as a best effort.
+        if (forkedFiber != NULL)
+                f = forkedFiber;
+    }
+
+    // Calculate and store the time we want to wake up.
+    f->context = system_timer_current_time() + t;
+
+    // Remove fiber from the run queue
+    dequeue_fiber(f);
+
+    // Add fiber to the sleep queue. We maintain strict ordering here to reduce lookup times.
+    queue_fiber(f, &sleepQueue);
+
+    // Finally, enter the scheduler.
+    schedule();
+}
+
+/**
+  * Blocks the calling thread until the specified event is raised.
+  * The calling thread will be immediateley descheduled, and placed onto a
+  * wait queue until the requested event is received.
+  *
+  * @param id The ID field of the event to listen for (e.g. MICROBIT_ID_BUTTON_A)
+  *
+  * @param value The value of the event to listen for (e.g. MICROBIT_BUTTON_EVT_CLICK)
+  *
+  * @return MICROBIT_OK, or MICROBIT_NOT_SUPPORTED if the fiber scheduler is not running, or associated with an EventModel.
+  *
+  * @code
+  * fiber_wait_for_event(MICROBIT_ID_BUTTON_A, MICROBIT_BUTTON_EVT_CLICK);
+  * @endcode
+  *
+  * @note the fiber will not be be made runnable until after the event is raised, but there
+  * are no guarantees precisely when the fiber will next be scheduled.
+  */
+int fiber_wait_for_event(uint16_t id, uint16_t value)
+{
+    int ret = fiber_wake_on_event(id, value);
+
+    if(ret == MICROBIT_OK)
+        schedule();
+
+	return ret;
+}
+
+/**
+  * Configures the fiber context for the current fiber to block on an event ID
+  * and value, but does not deschedule the fiber.
+  *
+  * @param id The ID field of the event to listen for (e.g. MICROBIT_ID_BUTTON_A)
+  *
+  * @param value The value of the event to listen for (e.g. MICROBIT_BUTTON_EVT_CLICK)
+  *
+  * @return MICROBIT_OK, or MICROBIT_NOT_SUPPORTED if the fiber scheduler is not running, or associated with an EventModel.
+  *
+  * @code
+  * fiber_wake_on_event(MICROBIT_ID_BUTTON_A, MICROBIT_BUTTON_EVT_CLICK);
+  *
+  * //perform some time critical operation.
+  *
+  * //deschedule the current fiber manually, waiting for the previously configured event.
+  * schedule();
+  * @endcode
+  */
+int fiber_wake_on_event(uint16_t id, uint16_t value)
+{
+    Fiber *f = currentFiber;
+
+	if (messageBus == NULL || !fiber_scheduler_running())
+		return MICROBIT_NOT_SUPPORTED;
+
+    // Sleep is a blocking call, so if we'r ein a fork on block context,
+    // it's time to spawn a new fiber...
+    if (currentFiber->flags & MICROBIT_FIBER_FLAG_FOB)
+    {
+        // Allocate a TCB from the new fiber. This will come from the tread pool if availiable,
+        // else a new one will be allocated on the heap.
+        forkedFiber = getFiberContext();
+
+        // If we're out of memory, there's nothing we can do.
+        // keep running in the context of the current thread as a best effort.
+        if (forkedFiber != NULL)
+                f = forkedFiber;
+    }
+
+    // Encode the event data in the context field. It's handy having a 32 bit core. :-)
+    f->context = value << 16 | id;
+
+    // Remove ourselve from the run queue
+    dequeue_fiber(f);
+
+    // Add ourselves to the sleep queue. We maintain strict ordering here to reduce lookup times.
+    queue_fiber(f, &waitQueue);
+
+    // Register to receive this event, so we can wake up the fiber when it happens.
+    // Special case for teh notify channel, as we always stay registered for that.
+    if (id != MICROBIT_ID_NOTIFY && id != MICROBIT_ID_NOTIFY_ONE)
+        messageBus->listen(id, value, scheduler_event, MESSAGE_BUS_LISTENER_IMMEDIATE);
+
+    return MICROBIT_OK;
+}
+
+/**
+  * Executes the given function asynchronously if necessary.
+  *
+  * Fibers are often used to run event handlers, however many of these event handlers are very simple functions
+  * that complete very quickly, bringing unecessary RAM overhead.
+  *
+  * This function takes a snapshot of the current processor context, then attempts to optimistically call the given function directly.
+  * We only create an additional fiber if that function performs a block operation.
+  *
+  * @param entry_fn The function to execute.
+  *
+  * @return MICROBIT_OK, or MICROBIT_INVALID_PARAMETER.
+  */
+int invoke(void (*entry_fn)(void))
+{
+    // Validate our parameters.
+    if (entry_fn == NULL)
+        return MICROBIT_INVALID_PARAMETER;
+
+    if (!fiber_scheduler_running())
+		return MICROBIT_NOT_SUPPORTED;
+
+    if (currentFiber->flags & MICROBIT_FIBER_FLAG_FOB)
+    {
+        // If we attempt a fork on block whilst already in  fork n block context,
+        // simply launch a fiber to deal with the request and we're done.
+        create_fiber(entry_fn);
+        return MICROBIT_OK;
+    }
+
+    // Snapshot current context, but also update the Link Register to
+    // refer to our calling function.
+    save_register_context(&currentFiber->tcb);
+
+    // If we're here, there are two possibilities:
+    // 1) We're about to attempt to execute the user code
+    // 2) We've already tried to execute the code, it blocked, and we've backtracked.
+
+    // If we're returning from the user function and we forked another fiber then cleanup and exit.
+    if (currentFiber->flags & MICROBIT_FIBER_FLAG_PARENT)
+    {
+        currentFiber->flags &= ~MICROBIT_FIBER_FLAG_FOB;
+        currentFiber->flags &= ~MICROBIT_FIBER_FLAG_PARENT;
+        return MICROBIT_OK;
+    }
+
+    // Otherwise, we're here for the first time. Enter FORK ON BLOCK mode, and
+    // execute the function directly. If the code tries to block, we detect this and
+    // spawn a thread to deal with it.
+    currentFiber->flags |= MICROBIT_FIBER_FLAG_FOB;
+    entry_fn();
+    currentFiber->flags &= ~MICROBIT_FIBER_FLAG_FOB;
+
+    // If this is is an exiting fiber that for spawned to handle a blocking call, recycle it.
+    // The fiber will then re-enter the scheduler, so no need for further cleanup.
+    if (currentFiber->flags & MICROBIT_FIBER_FLAG_CHILD)
+        release_fiber();
+
+     return MICROBIT_OK;
+}
+
+/**
+  * Executes the given function asynchronously if necessary, and offers the ability to provide a parameter.
+  *
+  * Fibers are often used to run event handlers, however many of these event handlers are very simple functions
+  * that complete very quickly, bringing unecessary RAM. overhead
+  *
+  * This function takes a snapshot of the current fiber context, then attempt to optimistically call the given function directly.
+  * We only create an additional fiber if that function performs a block operation.
+  *
+  * @param entry_fn The function to execute.
+  *
+  * @param param an untyped parameter passed into the entry_fn and completion_fn.
+  *
+  * @return MICROBIT_OK, or MICROBIT_INVALID_PARAMETER.
+  */
+int invoke(void (*entry_fn)(void *), void *param)
+{
+    // Validate our parameters.
+    if (entry_fn == NULL)
+        return MICROBIT_INVALID_PARAMETER;
+
+    if (!fiber_scheduler_running())
+		return MICROBIT_NOT_SUPPORTED;
+
+    if (currentFiber->flags & (MICROBIT_FIBER_FLAG_FOB | MICROBIT_FIBER_FLAG_PARENT | MICROBIT_FIBER_FLAG_CHILD))
+    {
+        // If we attempt a fork on block whilst already in a fork on block context,
+        // simply launch a fiber to deal with the request and we're done.
+        create_fiber(entry_fn, param);
+        return MICROBIT_OK;
+    }
+
+    // Snapshot current context, but also update the Link Register to
+    // refer to our calling function.
+    save_register_context(&currentFiber->tcb);
+
+    // If we're here, there are two possibilities:
+    // 1) We're about to attempt to execute the user code
+    // 2) We've already tried to execute the code, it blocked, and we've backtracked.
+
+    // If we're returning from the user function and we forked another fiber then cleanup and exit.
+    if (currentFiber->flags & MICROBIT_FIBER_FLAG_PARENT)
+    {
+        currentFiber->flags &= ~MICROBIT_FIBER_FLAG_FOB;
+        currentFiber->flags &= ~MICROBIT_FIBER_FLAG_PARENT;
+        return MICROBIT_OK;
+    }
+
+    // Otherwise, we're here for the first time. Enter FORK ON BLOCK mode, and
+    // execute the function directly. If the code tries to block, we detect this and
+    // spawn a thread to deal with it.
+    currentFiber->flags |= MICROBIT_FIBER_FLAG_FOB;
+    entry_fn(param);
+    currentFiber->flags &= ~MICROBIT_FIBER_FLAG_FOB;
+
+    // If this is is an exiting fiber that for spawned to handle a blocking call, recycle it.
+    // The fiber will then re-enter the scheduler, so no need for further cleanup.
+    if (currentFiber->flags & MICROBIT_FIBER_FLAG_CHILD)
+        release_fiber(param);
+
+    return MICROBIT_OK;
+}
+
+/**
+ * Launches a fiber.
+ *
+ * @param ep the entry point for the fiber.
+ *
+ * @param cp the completion routine after ep has finished execution
+ */
+void launch_new_fiber(void (*ep)(void), void (*cp)(void))
+{
+    // Execute the thread's entrypoint
+    ep();
+
+    // Execute the thread's completion routine;
+    cp();
+
+    // If we get here, then the completion routine didn't recycle the fiber... so do it anyway. :-)
+    release_fiber();
+}
+
+/**
+ * Launches a fiber with a parameter
+ *
+ * @param ep the entry point for the fiber.
+ *
+ * @param cp the completion routine after ep has finished execution
+ *
+ * @param pm the parameter to provide to ep and cp.
+ */
+void launch_new_fiber_param(void (*ep)(void *), void (*cp)(void *), void *pm)
+{
+    // Execute the thread's entrypoint.
+    ep(pm);
+
+    // Execute the thread's completion routine.
+    cp(pm);
+
+    // If we get here, then the completion routine didn't recycle the fiber... so do it anyway. :-)
+    release_fiber(pm);
+}
+
+Fiber *__create_fiber(uint32_t ep, uint32_t cp, uint32_t pm, int parameterised)
+{
+    // Validate our parameters.
+    if (ep == 0 || cp == 0)
+        return NULL;
+
+    // Allocate a TCB from the new fiber. This will come from the fiber pool if availiable,
+    // else a new one will be allocated on the heap.
+    Fiber *newFiber = getFiberContext();
+
+    // If we're out of memory, there's nothing we can do.
+    if (newFiber == NULL)
+        return NULL;
+
+    newFiber->tcb.R0 = (uint32_t) ep;
+    newFiber->tcb.R1 = (uint32_t) cp;
+    newFiber->tcb.R2 = (uint32_t) pm;
+
+    // Set the stack and assign the link register to refer to the appropriate entry point wrapper.
+    newFiber->tcb.SP = CORTEX_M0_STACK_BASE - 0x04;
+    newFiber->tcb.LR = parameterised ? (uint32_t) &launch_new_fiber_param : (uint32_t) &launch_new_fiber;
+
+    // Add new fiber to the run queue.
+    queue_fiber(newFiber, &runQueue);
+
+    return newFiber;
+}
+
+/**
+  * Creates a new Fiber, and launches it.
+  *
+  * @param entry_fn The function the new Fiber will begin execution in.
+  *
+  * @param completion_fn The function called when the thread completes execution of entry_fn.
+  *                      Defaults to release_fiber.
+  *
+  * @return The new Fiber, or NULL if the operation could not be completed.
+  */
+Fiber *create_fiber(void (*entry_fn)(void), void (*completion_fn)(void))
+{
+    if (!fiber_scheduler_running())
+		return NULL;
+
+    return __create_fiber((uint32_t) entry_fn, (uint32_t)completion_fn, 0, 0);
+}
+
+
+/**
+  * Creates a new parameterised Fiber, and launches it.
+  *
+  * @param entry_fn The function the new Fiber will begin execution in.
+  *
+  * @param param an untyped parameter passed into the entry_fn and completion_fn.
+  *
+  * @param completion_fn The function called when the thread completes execution of entry_fn.
+  *                      Defaults to release_fiber.
+  *
+  * @return The new Fiber, or NULL if the operation could not be completed.
+  */
+Fiber *create_fiber(void (*entry_fn)(void *), void *param, void (*completion_fn)(void *))
+{
+    if (!fiber_scheduler_running())
+		return NULL;
+
+    return __create_fiber((uint32_t) entry_fn, (uint32_t)completion_fn, (uint32_t) param, 1);
+}
+
+/**
+  * Exit point for all fibers.
+  *
+  * Any fiber reaching the end of its entry function will return here  for recycling.
+  */
+void release_fiber(void *)
+{
+    if (!fiber_scheduler_running())
+		return;
+
+    release_fiber();
+}
+
+/**
+  * Exit point for all fibers.
+  *
+  * Any fiber reaching the end of its entry function will return here  for recycling.
+  */
+void release_fiber(void)
+{
+    if (!fiber_scheduler_running())
+		return;
+
+    // Remove ourselves form the runqueue.
+    dequeue_fiber(currentFiber);
+
+    // Add ourselves to the list of free fibers
+    queue_fiber(currentFiber, &fiberPool);
+
+    // Find something else to do!
+    schedule();
+}
+
+/**
+  * Resizes the stack allocation of the current fiber if necessary to hold the system stack.
+  *
+  * If the stack allocation is large enough to hold the current system stack, then this function does nothing.
+  * Otherwise, the the current allocation of the fiber is freed, and a larger block is allocated.
+  *
+  * @param f The fiber context to verify.
+  *
+  * @return The stack depth of the given fiber.
+  */
+void verify_stack_size(Fiber *f)
+{
+    // Ensure the stack buffer is large enough to hold the stack Reallocate if necessary.
+    uint32_t stackDepth;
+    uint32_t bufferSize;
+
+    // Calculate the stack depth.
+    stackDepth = f->tcb.stack_base - ((uint32_t) __get_MSP());
+
+    // Calculate the size of our allocated stack buffer
+    bufferSize = f->stack_top - f->stack_bottom;
+
+    // If we're too small, increase our buffer size.
+    if (bufferSize < stackDepth)
+    {
+        // To ease heap churn, we choose the next largest multple of 32 bytes.
+        bufferSize = (stackDepth + 32) & 0xffffffe0;
+
+        // Release the old memory
+        if (f->stack_bottom != 0)
+            free((void *)f->stack_bottom);
+
+        // Allocate a new one of the appropriate size.
+        f->stack_bottom = (uint32_t) malloc(bufferSize);
+
+        // Recalculate where the top of the stack is and we're done.
+        f->stack_top = f->stack_bottom + bufferSize;
+    }
+}
+
+/**
+  * Determines if any fibers are waiting to be scheduled.
+  *
+  * @return The number of fibers currently on the run queue
+  */
+int scheduler_runqueue_empty()
+{
+    return (runQueue == NULL);
+}
+
+/**
+  * Calls the Fiber scheduler.
+  * The calling Fiber will likely be blocked, and control given to another waiting fiber.
+  * Call this function to yield control of the processor when you have nothing more to do.
+  */
+void schedule()
+{
+    if (!fiber_scheduler_running())
+		return;
+
+    // First, take a reference to the currently running fiber;
+    Fiber *oldFiber = currentFiber;
+
+    // First, see if we're in Fork on Block context. If so, we simply want to store the full context
+    // of the currently running thread in a newly created fiber, and restore the context of the
+    // currently running fiber, back to the point where it entered FOB.
+
+    if (currentFiber->flags & MICROBIT_FIBER_FLAG_FOB)
+    {
+        // Record that the fibers have a parent/child relationship
+        currentFiber->flags |= MICROBIT_FIBER_FLAG_PARENT;
+        forkedFiber->flags |= MICROBIT_FIBER_FLAG_CHILD;
+
+        // Define the stack base of the forked fiber to be align with the entry point of the parent fiber
+        forkedFiber->tcb.stack_base = currentFiber->tcb.SP;
+
+        // Ensure the stack allocation of the new fiber is large enough
+        verify_stack_size(forkedFiber);
+
+        // Store the full context of this fiber.
+        save_context(&forkedFiber->tcb, forkedFiber->stack_top);
+
+        // We may now be either the newly created thread, or the one that created it.
+        // if the MICROBIT_FIBER_FLAG_PARENT flag is still set, we're the old thread, so
+        // restore the current fiber to its stored context and we're done.
+        if (currentFiber->flags & MICROBIT_FIBER_FLAG_PARENT)
+            restore_register_context(&currentFiber->tcb);
+
+        // If we're the new thread, we must have been unblocked by the scheduler, so simply return
+        // and continue processing.
+        return;
+    }
+
+    // We're in a normal scheduling context, so perform a round robin algorithm across runnable fibers.
+    // OK - if we've nothing to do, then run the IDLE task (power saving sleep)
+    if (runQueue == NULL)
+        currentFiber = idleFiber;
+
+    else if (currentFiber->queue == &runQueue)
+        // If the current fiber is on the run queue, round robin.
+        currentFiber = currentFiber->next == NULL ? runQueue : currentFiber->next;
+
+    else
+        // Otherwise, just pick the head of the run queue.
+        currentFiber = runQueue;
+
+    if (currentFiber == idleFiber && oldFiber->flags & MICROBIT_FIBER_FLAG_DO_NOT_PAGE)
+    {
+        // Run the idle task right here using the old fiber's stack.
+        // Keep idling while the runqueue is empty, or there is data to process.
+
+        // Run in the context of the original fiber, to preserve state of flags...
+        // as we are running on top of this fiber's stack.
+        currentFiber = oldFiber;
+
+        do
+        {
+            idle();
+        }
+        while (runQueue == NULL);
+
+        // Switch to a non-idle fiber.
+        // If this fiber is the same as the old one then there'll be no switching at all.
+        currentFiber = runQueue;
+    }
+
+    // Swap to the context of the chosen fiber, and we're done.
+    // Don't bother with the overhead of switching if there's only one fiber on the runqueue!
+    if (currentFiber != oldFiber)
+    {
+        // Special case for the idle task, as we don't maintain a stack context (just to save memory).
+        if (currentFiber == idleFiber)
+        {
+            idleFiber->tcb.SP = CORTEX_M0_STACK_BASE - 0x04;
+            idleFiber->tcb.LR = (uint32_t) &idle_task;
+        }
+
+        if (oldFiber == idleFiber)
+        {
+            // Just swap in the new fiber, and discard changes to stack and register context.
+            swap_context(NULL, &currentFiber->tcb, 0, currentFiber->stack_top);
+        }
+        else
+        {
+            // Ensure the stack allocation of the fiber being scheduled out is large enough
+            verify_stack_size(oldFiber);
+
+            // Schedule in the new fiber.
+            swap_context(&oldFiber->tcb, &currentFiber->tcb, oldFiber->stack_top, currentFiber->stack_top);
+        }
+    }
+}
+
+/**
+  * Adds a component to the array of idle thread components, which are processed
+  * when the run queue is empty.
+  *
+  * The system timer will poll isIdleCallbackNeeded on each component to determine
+  * if the scheduler should schedule the idle_task imminently.
+  *
+  * @param component The component to add to the array.
+  *
+  * @return MICROBIT_OK on success or MICROBIT_NO_RESOURCES if the fiber components array is full.
+  *
+  * @code
+  * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0);
+  *
+  * // heap allocated - otherwise it will be paged out!
+  * MicroBitAccelerometer* accelerometer = new MicroBitAccelerometer(i2c);
+  *
+  * fiber_add_idle_component(accelerometer);
+  * @endcode
+  */
+int fiber_add_idle_component(MicroBitComponent *component)
+{
+    int i = 0;
+
+    while(idleThreadComponents[i] != NULL && i < MICROBIT_IDLE_COMPONENTS)
+        i++;
+
+    if(i == MICROBIT_IDLE_COMPONENTS)
+        return MICROBIT_NO_RESOURCES;
+
+    idleThreadComponents[i] = component;
+
+    return MICROBIT_OK;
+}
+
+/**
+  * Remove a component from the array of idle thread components
+  *
+  * @param component The component to remove from the idle component array.
+  *
+  * @return MICROBIT_OK on success. MICROBIT_INVALID_PARAMETER is returned if the given component has not been previously added.
+  *
+  * @code
+  * MicroBitI2C i2c(I2C_SDA0, I2C_SCL0);
+  *
+  * // heap allocated - otherwise it will be paged out!
+  * MicroBitAccelerometer* accelerometer = new MicroBitAccelerometer(i2c);
+  *
+  * fiber_add_idle_component(accelerometer);
+  *
+  * fiber_remove_idle_component(accelerometer);
+  * @endcode
+  */
+int fiber_remove_idle_component(MicroBitComponent *component)
+{
+    int i = 0;
+
+    while(idleThreadComponents[i] != component && i < MICROBIT_IDLE_COMPONENTS)
+        i++;
+
+    if(i == MICROBIT_IDLE_COMPONENTS)
+        return MICROBIT_INVALID_PARAMETER;
+
+    idleThreadComponents[i] = NULL;
+
+    return MICROBIT_OK;
+}
+
+/**
+  * Set of tasks to perform when idle.
+  * Service any background tasks that are required, and attempt a power efficient sleep.
+  */
+void idle()
+{
+    // Service background tasks
+    for(int i = 0; i < MICROBIT_IDLE_COMPONENTS; i++)
+        if(idleThreadComponents[i] != NULL)
+            idleThreadComponents[i]->idleTick();
+
+    // If the above did create any useful work, enter power efficient sleep.
+    if(scheduler_runqueue_empty())
+    	__WFE();
+}
+
+/**
+  * The idle task, which is called when the runtime has no fibers that require execution.
+  *
+  * This function typically calls idle().
+  */
+void idle_task()
+{
+    while(1)
+    {
+        idle();
+        schedule();
+    }
+}