5 files changed, 337 insertions, 1 deletions

diff --git a/kernel/sched.c b/kernel/sched.c
index 328a67c55287..ee894ee8a0bb 100644
--- a/kernel/sched.c
+++ b/kernel/sched.c
@@ -5533,6 +5533,9 @@ need_resched_nonpreemptible:
 
         if (need_resched())
                 goto need_resched;
+
+        if (srp_active())
+                srp_ceiling_block();
 }
 EXPORT_SYMBOL(schedule);
 
diff --git a/litmus/Kconfig b/litmus/Kconfig
index f8c642658a2f..31cd527c25ef 100644
--- a/litmus/Kconfig
+++ b/litmus/Kconfig
@@ -1,5 +1,18 @@
 menu "LITMUS^RT"
 
+menu "Real-Time Synchronization"
+
+config SRP
+        bool "Stack Resource Policy (SRP)"
+        default n
+        help
+          Include support for Baker's Stack Resource Policy.
+
+          Say Yes if you want FMLP local long
+          critical section synchronization support.
+
+endmenu
+
 menu "Tracing"
 
 config FEATHER_TRACE
diff --git a/litmus/Makefile b/litmus/Makefile
index 3d18cff62cee..612457fa773e 100644
--- a/litmus/Makefile
+++ b/litmus/Makefile
@@ -8,6 +8,7 @@ obj-y     = sched_plugin.o litmus.o \
             rt_domain.o \
             edf_common.o \
             fdso.o \
+            srp.o \
             heap.o
 
 obj-$(CONFIG_FEATHER_TRACE) += ft_event.o ftdev.o
diff --git a/litmus/fdso.c b/litmus/fdso.c
index 323efac17a47..16e3a43a67d1 100644
--- a/litmus/fdso.c
+++ b/litmus/fdso.c
@@ -18,6 +18,7 @@
 
 #include <litmus/fdso.h>
 
+extern struct fdso_ops srp_sem_ops;
 
 static struct fdso_ops dummy_ops = {
         .create  = NULL
@@ -25,7 +26,7 @@ static struct fdso_ops dummy_ops = {
 
 static const struct fdso_ops* fdso_ops[] = {
         &dummy_ops,
-        &dummy_ops,
+        &srp_sem_ops,
 };
 
 static void* fdso_create(obj_type_t type)
diff --git a/litmus/srp.c b/litmus/srp.c
new file mode 100644
index 000000000000..71639b991630
--- /dev/null
+++ b/litmus/srp.c
@@ -0,0 +1,318 @@
+/* ************************************************************************** */
+/*                          STACK RESOURCE POLICY                             */
+/* ************************************************************************** */
+
+#include <asm/atomic.h>
+#include <linux/wait.h>
+#include <litmus/litmus.h>
+#include <litmus/sched_plugin.h>
+
+#include <litmus/fdso.h>
+
+#include <litmus/trace.h>
+
+
+#ifdef CONFIG_SRP
+
+struct srp_priority {
+        struct list_head        list;
+        unsigned int            period;
+        pid_t                   pid;
+};
+
+#define list2prio(l) list_entry(l, struct srp_priority, list)
+
+/* SRP task priority comparison function. Smaller periods have highest
+ * priority, tie-break is PID. Special case: period == 0 <=> no priority
+ */
+static int srp_higher_prio(struct srp_priority* first,
+                           struct srp_priority* second)
+{
+        if (!first->period)
+                return 0;
+        else
+                return  !second->period ||
+                        first->period < second->period || (
+                        first->period == second->period &&
+                        first->pid < second->pid);
+}
+
+struct srp {
+        struct list_head        ceiling;
+        wait_queue_head_t       ceiling_blocked;
+};
+
+
+atomic_t srp_objects_in_use = ATOMIC_INIT(0);
+
+DEFINE_PER_CPU(struct srp, srp);
+
+
+/* Initialize SRP semaphores at boot time. */
+static int __init srp_init(void)
+{
+        int i;
+
+        printk("Initializing SRP per-CPU ceilings...");
+        for (i = 0; i < NR_CPUS; i++) {
+                init_waitqueue_head(&per_cpu(srp, i).ceiling_blocked);
+                INIT_LIST_HEAD(&per_cpu(srp, i).ceiling);
+        }
+        printk(" done!\n");
+
+        return 0;
+}
+module_init(srp_init);
+
+
+#define system_ceiling(srp) list2prio(srp->ceiling.next)
+
+
+#define UNDEF_SEM -2
+
+
+/* struct for uniprocessor SRP "semaphore" */
+struct srp_semaphore {
+        struct srp_priority ceiling;
+        struct task_struct* owner;
+        int cpu; /* cpu associated with this "semaphore" and resource */
+};
+
+#define ceiling2sem(c) container_of(c, struct srp_semaphore, ceiling)
+
+static int srp_exceeds_ceiling(struct task_struct* first,
+                               struct srp* srp)
+{
+        return list_empty(&srp->ceiling) ||
+               get_rt_period(first) < system_ceiling(srp)->period ||
+               (get_rt_period(first) == system_ceiling(srp)->period &&
+                first->pid < system_ceiling(srp)->pid) ||
+                ceiling2sem(system_ceiling(srp))->owner == first;
+}
+
+static void srp_add_prio(struct srp* srp, struct srp_priority* prio)
+{
+        struct list_head *pos;
+        if (in_list(&prio->list)) {
+                printk(KERN_CRIT "WARNING: SRP violation detected, prio is already in "
+                       "ceiling list! cpu=%d, srp=%p\n", smp_processor_id(), ceiling2sem(prio));
+                return;
+        }
+        list_for_each(pos, &srp->ceiling)
+                if (unlikely(srp_higher_prio(prio, list2prio(pos)))) {
+                        __list_add(&prio->list, pos->prev, pos);
+                        return;
+                }
+
+        list_add_tail(&prio->list, &srp->ceiling);
+}
+
+
+static void* create_srp_semaphore(void)
+{
+        struct srp_semaphore* sem;
+
+        sem = kmalloc(sizeof(*sem), GFP_KERNEL);
+        if (!sem)
+                return NULL;
+
+        INIT_LIST_HEAD(&sem->ceiling.list);
+        sem->ceiling.period = 0;
+        sem->cpu     = UNDEF_SEM;
+        sem->owner   = NULL;
+        atomic_inc(&srp_objects_in_use);
+        return sem;
+}
+
+static noinline int open_srp_semaphore(struct od_table_entry* entry, void* __user arg)
+{
+        struct srp_semaphore* sem = (struct srp_semaphore*) entry->obj->obj;
+        int ret = 0;
+        struct task_struct* t = current;
+        struct srp_priority t_prio;
+
+        TRACE("opening SRP semaphore %p, cpu=%d\n", sem, sem->cpu);
+        if (!srp_active())
+                return -EBUSY;
+
+        if (sem->cpu == UNDEF_SEM)
+                sem->cpu = get_partition(t);
+        else if (sem->cpu != get_partition(t))
+                ret = -EPERM;
+
+        if (ret == 0) {
+                t_prio.period = get_rt_period(t);
+                t_prio.pid    = t->pid;
+                if (srp_higher_prio(&t_prio, &sem->ceiling)) {
+                        sem->ceiling.period = t_prio.period;
+                        sem->ceiling.pid    = t_prio.pid;
+                }
+        }
+
+        return ret;
+}
+
+static void destroy_srp_semaphore(void* sem)
+{
+        /* XXX invariants */
+        atomic_dec(&srp_objects_in_use);
+        kfree(sem);
+}
+
+struct fdso_ops srp_sem_ops = {
+        .create  = create_srp_semaphore,
+        .open    = open_srp_semaphore,
+        .destroy = destroy_srp_semaphore
+};
+
+
+static void do_srp_down(struct srp_semaphore* sem)
+{
+        /* Update ceiling. */
+        srp_add_prio(&__get_cpu_var(srp), &sem->ceiling);
+        WARN_ON(sem->owner != NULL);
+        sem->owner = current;
+        TRACE_CUR("acquired srp 0x%p\n", sem);
+}
+
+static void do_srp_up(struct srp_semaphore* sem)
+{
+        /* Determine new system priority ceiling for this CPU. */
+        WARN_ON(!in_list(&sem->ceiling.list));
+        if (in_list(&sem->ceiling.list))
+                list_del(&sem->ceiling.list);
+
+        sem->owner = NULL;
+
+        /* Wake tasks on this CPU, if they exceed current ceiling. */
+        TRACE_CUR("released srp 0x%p\n", sem);
+        wake_up_all(&__get_cpu_var(srp).ceiling_blocked);
+}
+
+/* Adjust the system-wide priority ceiling if resource is claimed. */
+asmlinkage long sys_srp_down(int sem_od)
+{
+        int cpu;
+        int ret = -EINVAL;
+        struct srp_semaphore* sem;
+
+        /* disabling preemptions is sufficient protection since
+         * SRP is strictly per CPU and we don't interfere with any
+         * interrupt handlers
+         */
+        preempt_disable();
+        TS_SRP_DOWN_START;
+
+        cpu = smp_processor_id();
+        sem = lookup_srp_sem(sem_od);
+        if (sem && sem->cpu == cpu) {
+                do_srp_down(sem);
+                ret = 0;
+        }
+
+        TS_SRP_DOWN_END;
+        preempt_enable();
+        return ret;
+}
+
+/* Adjust the system-wide priority ceiling if resource is freed. */
+asmlinkage long sys_srp_up(int sem_od)
+{
+        int cpu;
+        int ret = -EINVAL;
+        struct srp_semaphore* sem;
+
+        preempt_disable();
+        TS_SRP_UP_START;
+
+        cpu = smp_processor_id();
+        sem = lookup_srp_sem(sem_od);
+
+        if (sem && sem->cpu == cpu) {
+                do_srp_up(sem);
+                ret = 0;
+        }
+
+        TS_SRP_UP_END;
+        preempt_enable();
+        return ret;
+}
+
+static int srp_wake_up(wait_queue_t *wait, unsigned mode, int sync,
+                       void *key)
+{
+        int cpu = smp_processor_id();
+        struct task_struct *tsk = wait->private;
+        if (cpu != get_partition(tsk))
+                TRACE_TASK(tsk, "srp_wake_up on wrong cpu, partition is %d\b",
+                           get_partition(tsk));
+        else if (srp_exceeds_ceiling(tsk, &__get_cpu_var(srp)))
+                return default_wake_function(wait, mode, sync, key);
+        return 0;
+}
+
+
+
+static void do_ceiling_block(struct task_struct *tsk)
+{
+        wait_queue_t wait = {
+                .private   = tsk,
+                .func      = srp_wake_up,
+                .task_list = {NULL, NULL}
+        };
+
+        tsk->state = TASK_UNINTERRUPTIBLE;
+        add_wait_queue(&__get_cpu_var(srp).ceiling_blocked, &wait);
+        tsk->rt_param.srp_non_recurse = 1;
+        preempt_enable_no_resched();
+        schedule();
+        preempt_disable();
+        tsk->rt_param.srp_non_recurse = 0;
+        remove_wait_queue(&__get_cpu_var(srp).ceiling_blocked, &wait);
+}
+
+/* Wait for current task priority to exceed system-wide priority ceiling.
+ */
+void srp_ceiling_block(void)
+{
+        struct task_struct *tsk = current;
+
+        /* Only applies to real-time tasks, but optimize for RT tasks. */
+        if (unlikely(!is_realtime(tsk)))
+                return;
+
+        /* Avoid recursive ceiling blocking. */
+        if (unlikely(tsk->rt_param.srp_non_recurse))
+                return;
+
+        /* Bail out early if there aren't any SRP resources around. */
+        if (likely(!atomic_read(&srp_objects_in_use)))
+                return;
+
+        preempt_disable();
+        if (!srp_exceeds_ceiling(tsk, &__get_cpu_var(srp))) {
+                TRACE_CUR("is priority ceiling blocked.\n");
+                while (!srp_exceeds_ceiling(tsk, &__get_cpu_var(srp)))
+                        do_ceiling_block(tsk);
+                TRACE_CUR("finally exceeds system ceiling.\n");
+        } else
+                TRACE_CUR("is not priority ceiling blocked\n");
+        preempt_enable();
+}
+
+
+#else
+
+asmlinkage long sys_srp_down(int sem_od)
+{
+        return -ENOSYS;
+}
+
+asmlinkage long sys_srp_up(int sem_od)
+{
+        return -ENOSYS;
+}
+
+struct fdso_ops srp_sem_ops = {};
+
+#endif