Add scheduling context support in seL4_libs

Only works if CONFIG_KERNEL_RT is enabled, however most of the code
is compiled no mater what to lighten the maintenance burden
diff --git a/libsel4simple-default/src/libsel4simple-default.c b/libsel4simple-default/src/libsel4simple-default.c
index a3313ee..536c29e 100644
--- a/libsel4simple-default/src/libsel4simple-default.c
+++ b/libsel4simple-default/src/libsel4simple-default.c
@@ -180,6 +180,17 @@
     debug_print_bootinfo(data);
 }
 
+seL4_CPtr simple_default_sched_control(void *data, int core)
+{
+    assert(core < simple_default_core_count(data));
+#if CONFIG_KERNEL_RT
+    return ((seL4_BootInfo) data)->schedcontrol.start + core;
+#else
+    ZF_LOGW("not implemented");
+    return seL4_CapNull;
+#endif
+}
+
 seL4_Word simple_default_arch_info(void *data) {
     if (data == NULL) {
         ZF_LOGE("Data is null!");
@@ -251,6 +262,7 @@
     simple->core_count = &simple_default_core_count;
     simple->nth_userimage = &simple_default_nth_userimage;
     simple->print = &simple_default_print;
+    simple->sched_ctrl = &simple_default_sched_control;
     simple->arch_info = &simple_default_arch_info;
     simple->extended_bootinfo_len = &simple_default_get_extended_bootinfo_size;
     simple->extended_bootinfo = &simple_default_get_extended_bootinfo;
diff --git a/libsel4simple/include/simple/simple.h b/libsel4simple/include/simple/simple.h
index 505b544..e73a493 100644
--- a/libsel4simple/include/simple/simple.h
+++ b/libsel4simple/include/simple/simple.h
@@ -178,6 +178,12 @@
 typedef seL4_Error (*simple_get_iospace_fn)(void *data, uint16_t domainID, uint16_t deviceID, cspacepath_t *path);
 #endif
 
+/*
+ * Get the sched ctrl for the requested core (0 for uniprocessor).
+ * @return seL4_CapNull if CONFIG_RT is disabled
+ */
+typedef seL4_CPtr (*simple_get_sched_ctrl_fn)(void *data, int core);
+
 /**
  *
  * Get simple to print all the information it has about its environment
@@ -232,6 +238,7 @@
     simple_get_nth_userimage_fn nth_userimage;
     simple_get_core_count_fn core_count;
     simple_print_fn print;
+    simple_get_sched_ctrl_fn sched_ctrl;
     simple_get_arch_info_fn arch_info;
     simple_get_extended_bootinfo_len_fn extended_bootinfo_len;
     simple_get_extended_bootinfo_fn extended_bootinfo;
@@ -409,6 +416,16 @@
 }
 
 static inline seL4_CPtr
+simple_get_sc(UNUSED simple_t *simple)
+{
+#ifdef CONFIG_KERNEL_RT
+    return simple_init_cap(simple, seL4_CapInitThreadSC);
+#else
+    return seL4_CapNull;
+#endif
+}
+
+static inline seL4_CPtr
 simple_get_pd(simple_t *simple)
 {
     return simple_init_cap(simple, seL4_CapInitThreadPD);
@@ -576,6 +593,25 @@
     simple->print(simple->data);
 }
 
+static inline seL4_CPtr
+simple_get_sched_ctrl(simple_t *simple, int core)
+{
+    if (!simple) {
+        ZF_LOGE("Simple is NULL");
+        return seL4_CapNull;
+    }
+    if (!simple->sched_ctrl) {
+        ZF_LOGE("%s not implemented", __FUNCTION__);
+        return seL4_CapNull;
+    }
+    if (core >= simple_get_core_count(simple)) {
+        ZF_LOGE("invalid core");
+        return seL4_CapNull;
+    }
+
+    return simple->sched_ctrl(simple->data, core);
+}
+
 static inline seL4_Word
 simple_get_arch_info(simple_t *simple)
 {
diff --git a/libsel4utils/include/sel4utils/process.h b/libsel4utils/include/sel4utils/process.h
index 7abe9cd..ec3731c 100644
--- a/libsel4utils/include/sel4utils/process.h
+++ b/libsel4utils/include/sel4utils/process.h
@@ -86,8 +86,11 @@
     /* the slot for this processes tcb */
     SEL4UTILS_TCB_SLOT = 5,
 
+    /* the slot for this processes sc */
+    SEL4UTILS_SCHED_CONTEXT_SLOT = 6,
+
     /* First free slot in the cspace configured by sel4utils */
-    SEL4UTILS_FIRST_FREE = 6
+    SEL4UTILS_FIRST_FREE = 7
 };
 
 /**
@@ -148,6 +151,8 @@
  * It creates a simple cspace and vspace for you, allocates a fault endpoint and puts
  * it into the new cspace. The process will start at priority 0.
  *
+ * If CONFIG_RT is enabled, it will not have a scheduling context.
+ *
  * It loads the elf file into the new vspace, parses the elf file and stores the entry point
  * into process->entry_point.
  *
diff --git a/libsel4utils/include/sel4utils/process_config.h b/libsel4utils/include/sel4utils/process_config.h
index 8c6aeee..f60764b 100644
--- a/libsel4utils/include/sel4utils/process_config.h
+++ b/libsel4utils/include/sel4utils/process_config.h
@@ -185,5 +185,13 @@
     config = process_config_create_cnode(config, CONFIG_SEL4UTILS_CSPACE_SIZE_BITS);
     config = process_config_create_vspace(config, NULL, 0);
     config = process_config_create_fault_endpoint(config);
+
+    uint64_t timeslice = 0;
+#ifdef CONFIG_BOOT_THREAD_TIME_SLICE
+    timeslice = CONFIG_BOOT_THREAD_TIME_SLICE;
+#else
+    timeslice = CONFIG_TIMER_TICK_MS;
+#endif
+    config.sched_params = sched_params_round_robin(config.sched_params, simple, 0, timeslice * US_IN_MS);
     return process_config_priority(config, prio);
 }
diff --git a/libsel4utils/include/sel4utils/thread.h b/libsel4utils/include/sel4utils/thread.h
index 138aba7..e1128e5 100644
--- a/libsel4utils/include/sel4utils/thread.h
+++ b/libsel4utils/include/sel4utils/thread.h
@@ -38,11 +38,13 @@
 
 typedef struct sel4utils_thread {
     vka_object_t tcb;
+    vka_object_t sched_context;
     void *stack_top;
     void *initial_stack_pointer;
     size_t stack_size;
     seL4_CPtr ipc_buffer;
     seL4_Word ipc_buffer_addr;
+    bool own_sc;
 } sel4utils_thread_t;
 
 typedef struct sel4utils_checkpoint {
@@ -56,6 +58,8 @@
 /**
  * Configure a thread, allocating any resources required. The thread will start at priority 0.
  *
+ * If CONFIG_RT is enabled, the thread will not have a scheduling context, so it will not be able to run.
+ *
  * @param vka initialised vka to allocate objects with
  * @param parent vspace structure of the thread calling this function, used for temporary mappings
  * @param alloc initialised vspace structure to allocate virtual memory with
@@ -175,6 +179,8 @@
  * Start a fault handling thread that will print the name of the thread that faulted
  * as well as debugging information. The thread will start at priority 0.
  *
+ * If CONFIG_RT it be passive (not have a scheulding context) and will run on the SC of the faulter.
+ *
  * @param fault_endpoint the fault_endpoint to wait on
  * @param vka allocator
  * @param vspace vspace (this library must be mapped into that vspace).
diff --git a/libsel4utils/include/sel4utils/thread_config.h b/libsel4utils/include/sel4utils/thread_config.h
index 169b8ad..62adbef 100644
--- a/libsel4utils/include/sel4utils/thread_config.h
+++ b/libsel4utils/include/sel4utils/thread_config.h
@@ -29,6 +29,17 @@
     uint8_t mcp;
     /* TCB to derive MCP from when setting priority/mcp (in future API) */
     seL4_CPtr auth;
+    /* true if sel4utils should create an sc */
+    bool create_sc;
+    /* sel4 sched control cap for creating sc */
+    seL4_CPtr sched_ctrl;
+    /* scheduling parameters */
+    uint64_t period;
+    uint64_t budget;
+    seL4_Word extra_refills;
+    seL4_Word badge;
+    /* otherwise use this provided sc */
+    seL4_CPtr sched_context;
 } sched_params_t;
 
 typedef struct sel4utils_thread_config {
@@ -48,6 +59,36 @@
     sched_params_t sched_params;
 } sel4utils_thread_config_t;
 
+static inline sched_params_t
+sched_params_periodic(sched_params_t params, simple_t *simple, seL4_Word core, uint64_t period_us,
+                      uint64_t budget_us, seL4_Word extra_refills, seL4_Word badge)
+{
+    if (!config_set(CONFIG_KERNEL_RT)) {
+        ZF_LOGW("Setting sched params on non-RT kernel will have no effect");
+    }
+    params.sched_ctrl = simple_get_sched_ctrl(simple, core);
+    params.period = period_us;
+    params.budget = budget_us;
+    params.extra_refills = extra_refills;
+    params.badge = badge;
+    params.create_sc = true;
+    return params;
+}
+
+static inline sched_params_t
+sched_params_round_robin(sched_params_t params, simple_t *simple, seL4_Word core, uint64_t timeslice_us)
+{
+    return sched_params_periodic(params, simple, core, timeslice_us, timeslice_us, 0, 0);
+}
+
+static inline sel4utils_thread_config_t
+thread_config_sched_context(sel4utils_thread_config_t config, seL4_CPtr sched_context)
+{
+    config.sched_params.create_sc = false;
+    config.sched_params.sched_context = sched_context;
+    return config;
+}
+
 static inline sel4utils_thread_config_t
 thread_config_cspace(sel4utils_thread_config_t config, seL4_CPtr cspace_root, seL4_CapData_t cspace_root_data)
 {
@@ -115,5 +156,13 @@
     config = thread_config_cspace(config, cnode, data);
     config = thread_config_fault_endpoint(config, fault_ep);
     config = thread_config_priority(config, prio);
+    uint64_t timeslice;
+#ifdef CONFIG_BOOT_THREAD_TIME_SLICE
+    timeslice = CONFIG_BOOT_THREAD_TIME_SLICE;
+#else
+    timeslice = CONFIG_TIMER_TICK_MS;
+#endif
+    config.sched_params = sched_params_round_robin(config.sched_params, simple, 0, timeslice * US_IN_MS);
+    config = thread_config_create_reply(config);
     return config;
 }
diff --git a/libsel4utils/src/process.c b/libsel4utils/src/process.c
index 5f67ecc..4e5d4f4 100644
--- a/libsel4utils/src/process.c
+++ b/libsel4utils/src/process.c
@@ -621,6 +621,11 @@
     thread_config.sched_params = config.sched_params;
     error = sel4utils_configure_thread_config(vka, spawner_vspace, &process->vspace, thread_config,
                                               &process->thread);
+    if (error) {
+        ZF_LOGE("ERROR: failed to configure thread for new process %d\n", error);
+        goto error;
+    }
+
     /* copy tcb cap to cspace */
     if (config.create_cspace) {
         cspacepath_t src;
@@ -630,11 +635,16 @@
         process->dest_tcb_cptr = SEL4UTILS_TCB_SLOT;
     } else {
         process->dest_tcb_cptr = config.dest_cspace_tcb_cptr;
+        /* skip the tcb slot */
+        allocate_next_slot(process);
     }
 
-    if (error) {
-        ZF_LOGE("ERROR: failed to configure thread for new process %d\n", error);
-        goto error;
+    if (config.create_cspace && config_set(CONFIG_KERNEL_RT)) {
+        slot = sel4utils_copy_cap_to_process(process, vka, process->thread.sched_context.cptr);
+        assert(slot == SEL4UTILS_SCHED_CONTEXT_SLOT);
+    } else {
+        /* skip the sc slot */
+        allocate_next_slot(process);
     }
 
     return 0;
@@ -730,6 +740,10 @@
         return SEL4UTILS_PD_SLOT;
     case seL4_CapInitThreadASIDPool:
         return SEL4UTILS_ASID_POOL_SLOT;
+#ifdef CONFIG_KERNEL_RT
+    case seL4_CapInitThreadSC:
+        return SEL4UTILS_SCHED_CONTEXT_SLOT;
+#endif
     default:
         ZF_LOGE("sel4utils does not copy this cap (%zu) to new processes", cap);
         return seL4_CapNull;
@@ -758,4 +772,4 @@
                 vka, from->objs[i].obj.cptr);
     }
     return 0;
-}
\ No newline at end of file
+}
diff --git a/libsel4utils/src/thread.c b/libsel4utils/src/thread.c
index 98399ee..7d42149 100644
--- a/libsel4utils/src/thread.c
+++ b/libsel4utils/src/thread.c
@@ -78,9 +78,36 @@
         }
     }
 
+    if (config_set(CONFIG_KERNEL_RT) && config.sched_params.create_sc) {
+        /* allocate a scheduling context */
+        if (vka_alloc_sched_context(vka, &res->sched_context)) {
+            ZF_LOGE("Failed to allocate sched context");
+            sel4utils_clean_up_thread(vka, alloc, res);
+            return -1;
+        }
+
+        /* configure the scheduling context */
+#ifdef CONFIG_KERNEL_RT
+        error = seL4_SchedControl_Configure(config.sched_params.sched_ctrl, res->sched_context.cptr,
+                                            config.sched_params.budget, config.sched_params.period,
+                                            config.sched_params.extra_refills);
+#endif
+        if (error != seL4_NoError) {
+            ZF_LOGE("Failed to configure sched context");
+            sel4utils_clean_up_thread(vka, alloc, res);
+            return -1;
+        }
+        res->own_sc = true;
+    } else {
+        res->sched_context.cptr = config.sched_params.sched_context;
+    }
     seL4_CapData_t null_cap_data = {{0}};
     error = seL4_TCB_Configure(res->tcb.cptr, config.fault_endpoint,
-                               seL4_PrioProps_new(config.sched_params.mcp, config.sched_params.priority), config.cspace,
+                               seL4_PrioProps_new(config.sched_params.mcp, config.sched_params.priority),
+#ifdef CONFIG_KERNEL_RT
+                               res->sched_context.cptr,
+#endif
+                               config.cspace,
                                config.cspace_root_data, vspace_get_root(alloc), null_cap_data, res->ipc_buffer_addr, res->ipc_buffer);
 
     if (error != seL4_NoError) {
@@ -146,6 +173,10 @@
         vspace_free_sized_stack(alloc, thread->stack_top, thread->stack_size);
     }
 
+    if (thread->own_sc && thread->sched_context.cptr != 0) {
+        vka_free_object(vka, &thread->sched_context);
+    }
+
     memset(thread, 0, sizeof(sel4utils_thread_t));
 }
 
diff --git a/libsel4vka/include/vka/kobject_t.h b/libsel4vka/include/vka/kobject_t.h
index 4a21499..aa82f7e 100644
--- a/libsel4vka/include/vka/kobject_t.h
+++ b/libsel4vka/include/vka/kobject_t.h
@@ -28,6 +28,7 @@
     KOBJECT_UNTYPED,
     KOBJECT_ENDPOINT,
     KOBJECT_NOTIFICATION,
+    KOBJECT_SCHED_CONTEXT,
 #ifdef CONFIG_CACHE_COLORING
     KOBJECT_KERNEL_IMAGE,
 #endif
@@ -63,6 +64,10 @@
         return seL4_PageDirBits;
     case KOBJECT_PAGE_TABLE:
         return seL4_PageTableBits;
+#ifdef CONFIG_KERNEL_RT
+    case KOBJECT_SCHED_CONTEXT:
+        return objectSize > seL4_MinSchedContextBits ? objectSize : seL4_MinSchedContextBits;
+#endif
 #ifdef CONFIG_CACHE_COLORING
     case KOBJECT_KERNEL_IMAGE:
         return seL4_KernelImageBits;
@@ -89,6 +94,10 @@
         return seL4_EndpointObject;
     case KOBJECT_NOTIFICATION:
         return seL4_NotificationObject;
+#ifdef CONFIG_KERNEL_RT
+    case KOBJECT_SCHED_CONTEXT:
+        return seL4_SchedContextObject;
+#endif
 #ifdef CONFIG_CACHE_COLORING
     case KOBJECT_KERNEL_IMAGE:
         return seL4_KernelImageObject;
diff --git a/libsel4vka/include/vka/object.h b/libsel4vka/include/vka/object.h
index fc3fa8f..d18019f 100644
--- a/libsel4vka/include/vka/object.h
+++ b/libsel4vka/include/vka/object.h
@@ -14,6 +14,7 @@
 #define __VKA_OBJECT_H__
 
 #include <assert.h>
+#include <errno.h>
 #include <vka/vka.h>
 #include <vka/kobject_t.h>
 #include <stdio.h>
@@ -143,6 +144,31 @@
 {
     return vka_alloc_object(vka, seL4_TCBObject, seL4_TCBBits, result);
 }
+
+static inline int vka_alloc_sched_context(UNUSED vka_t *vka, UNUSED vka_object_t *result)
+{
+#ifdef CONFIG_KERNEL_RT
+    return vka_alloc_object(vka, seL4_SchedContextObject, seL4_MinSchedContextBits, result);
+#else
+    ZF_LOGW("Allocating sched context on non RT kernel");
+    return ENOSYS;
+#endif
+}
+
+static inline int vka_alloc_sched_context_size(UNUSED vka_t *vka, UNUSED vka_object_t *result, UNUSED uint32_t size_bits)
+{
+#ifdef CONFIG_KERNEL_RT
+    if (size_bits < seL4_MinSchedContextBits) {
+        ZF_LOGE("Invalid size bits for sc");
+        return -1;
+    }
+    return vka_alloc_object(vka, seL4_SchedContextObject, size_bits, result);
+#else
+    ZF_LOGW("Allocating sched context on non RT kernel");
+    return ENOSYS;
+#endif
+}
+
 static inline int vka_alloc_endpoint(vka_t *vka, vka_object_t *result)
 {
     return vka_alloc_object(vka, seL4_EndpointObject, seL4_EndpointBits, result);
@@ -228,6 +254,7 @@
 LEAKY(notification)
 LEAKY(page_directory)
 LEAKY(page_table)
+LEAKY(sched_context)
 
 static inline DEPRECATED("use vka_alloc_notification_leaky") seL4_CPtr
 vka_alloc_async_endpoint_leaky(vka_t *vka)
@@ -265,6 +292,10 @@
         return objectSize;
     case seL4_TCBObject:
         return seL4_TCBBits;
+#ifdef CONFIG_KERNEL_RT
+    case seL4_SchedContextObject:
+        return objectSize > seL4_MinSchedContextBits ? objectSize : seL4_MinSchedContextBits;
+#endif
     case seL4_EndpointObject:
         return seL4_EndpointBits;
     case seL4_NotificationObject:
diff --git a/libsel4vmm/include/vmm/vmm.h b/libsel4vmm/include/vmm/vmm.h
index 252002b..597f96b 100644
--- a/libsel4vmm/include/vmm/vmm.h
+++ b/libsel4vmm/include/vmm/vmm.h
@@ -109,6 +109,8 @@
     /* TCB of the VMM thread
      * TODO: Should eventually have one vmm thread per vcpu */
     seL4_CPtr tcb;
+    seL4_CPtr sc;
+    seL4_CPtr sched_ctrl;
 
     /* platform callback functions */
     platform_callbacks_t plat_callbacks;
diff --git a/libsel4vmm/src/platform/boot.c b/libsel4vmm/src/platform/boot.c
index 9338eca..1b92d1f 100644
--- a/libsel4vmm/src/platform/boot.c
+++ b/libsel4vmm/src/platform/boot.c
@@ -37,6 +37,8 @@
     vmm->host_vspace = vspace;
     vmm->plat_callbacks = callbacks;
     vmm->tcb = simple_get_tcb(&simple);
+    vmm->sc = simple_get_sc(&simple);
+    vmm->sched_ctrl = simple_get_sched_ctrl(&simple, 0);
     // Currently set this to 4k pages by default
     vmm->page_size = seL4_PageBits;
     err = vmm_pci_init(&vmm->pci);