Add priority inheritance implementation for Semaphores.

Introduce the support for the Priority Inheritance protocol (PI [1])
when using kernel semaphores. The aim of the protocol is to avvoid
the phenomenon known as priority inversion. This happens when a
high priority process blocks on a mutex/semaphore own by a low priority
one, which is then preempted by a third process, with priority in the
middle between the previous two and not using any semaphore. What
happens is, in fact, that the high priority process is delayed by
the medium priority one, and this is usually a bad thing... Feel
free to ask NASA [2] !!!

So, in case we have the following `process(prio)', sharing a couple of
semaphores:
> test1(2): Start.
> test2(3): Start.
> test3(4): Start.
> test4(5): Start.
> test6(7): Start.
> test7(8): Start.
> test8(9): Start.

And the following one, not using any semaphore at all.
> test5(6): Start.

Then without this commit, the finishing time of all the processes is as
follows:
> Main: I-O latency of 1 = 375ms
> Main: I-O latency of 2 = 374ms
> Main: I-O latency of 3 = 374ms
> Main: I-O latency of 4 = 373ms
> Main: I-O latency of 5 = 156ms
> Main: I-O latency of 6 = 372ms
> Main: I-O latency of 7 = 372ms
> Main: I-O latency of 8 = 371ms

With the protocol enabled we have:
> Main: I-O latency of 1 = 376ms
> Main: I-O latency of 2 = 376ms
> Main: I-O latency of 3 = 375ms
> Main: I-O latency of 4 = 375ms
> Main: I-O latency of 5 = 374ms
> Main: I-O latency of 6 = 217ms
> Main: I-O latency of 7 = 217ms
> Main: I-O latency of 8 = 216ms

As it is clear, in the non-PI enabled case, test5 'disturbs' the
processes with priority higher than 6, i.e., test{6,7,8}, by preventing
the lower priority processes that are owning the semaphore to run and
release it. On the other hand, when PI is on duty, the highest priority
processes are able to complete without being interrupted by test5,
thanks to the priority lending enacted by the protocol.

In order of making it possible to enable the implemented bits at
configure/compile time, the CONFIG_KERN_PRI_INHERIT switch has been
added, and it is part of cfg_proc.h, mainly because priority inheritance is
often considered a feature of the scheduler, more than of the blocking
mechanisms (but all this can be changed easily!).

Introduced for and only tested in emulation mode on x86.

[1] L. Sha, R. Rajkumar, and J. P. Lehoczky, "Priority Inheritance
    Protocols: An Approach to Real-Time Synchronization". IEEE Transactions
    on Computers 39 (9): 1175–1185.
[2] http://research.microsoft.com/en-us/um/people/mbj/Mars_Pathfinder/

Signed-off-by: Dario Faggioli <raistlin@linux.it>

git-svn-id: https://src.develer.com/svnoss/bertos/trunk@4908 38d2e660-2303-0410-9eaa-f027e97ec537

bertos/cfg/cfg_proc.h

@@ -74,6 +74,12 @@
  */
 #define CONFIG_KERN_PRI 0
 
+/**
+ * Priority-inheritance protocol.
+ * $WIZ$ type = "boolean"
+ */
+#define CONFIG_KERN_PRI_INHERIT 0
+
 /**
  * Dynamic memory allocation for processes.
  * $WIZ$ type = "boolean"

bertos/kern/proc.c

@@ -193,6 +193,12 @@ static void proc_initStruct(Process *proc)
 
 #if CONFIG_KERN_PRI
 	proc->link.pri = 0;
+
+# if CONFIG_KERN_PRI_INHERIT
+	proc->orig_pri = proc->inh_link.pri = proc->link.pri;
+	proc->inh_blocked_by = NULL;
+	LIST_INIT(&proc->inh_list);
+# endif
 #endif
 }
 
@@ -439,10 +445,33 @@ void proc_rename(struct Process *proc, const char *name)
  */
 void proc_setPri(struct Process *proc, int pri)
 {
+#if CONFIG_KERN_PRI_INHERIT
+	int new_pri;
+
+	/*
+	 * Whatever it will happen below, this is the new
+	 * original priority of the process, i.e., the priority
+	 * it has without taking inheritance under account.
+	 */
+	proc->orig_pri = pri;
+
+	/* If not changing anything we can just leave */
+	if ((new_pri = __prio_proc(proc)) == proc->link.pri)
+		return;
+
+	/*
+	 * Actual process priority is the highest among its
+	 * own priority and the one of the top-priority
+	 * process that it is blocking (returned by
+	 * __prio_proc()).
+	 */
+	proc->link.pri = new_pri;
+#else
 	if (proc->link.pri == pri)
 		return;
 
 	proc->link.pri = pri;
+#endif // CONFIG_KERN_PRI_INHERIT
 
 	if (proc != current_process)
 		ATOMIC(sched_reenqueue(proc));

bertos/kern/proc.h

@@ -92,6 +92,7 @@
 #ifndef KERN_PROC_H
 #define KERN_PROC_H
 
+#include "sem.h"
 #include "cfg/cfg_proc.h"
 #include "cfg/cfg_signal.h"
 #include "cfg/cfg_monitor.h"
@@ -115,6 +116,12 @@ typedef struct Process
 {
 #if CONFIG_KERN_PRI
 	PriNode      link;        /**< Link Process into scheduler lists */
+# if CONFIG_KERN_PRI_INHERIT
+	PriNode      inh_link;    /**< Link Process into priority inheritance lists */
+	List         inh_list;    /**< Priority inheritance list for this Process */
+	Semaphore    *inh_blocked_by;  /**< Semaphore blocking this Process */
+	int          orig_pri;    /**< Process priority without considering inheritance */
+# endif
 #else
 	Node         link;        /**< Link Process into scheduler lists */
 #endif

bertos/kern/proc_p.h

@@ -78,6 +78,13 @@ extern REGISTER Process	*current_process;
 extern REGISTER List     proc_ready_list;
 
 #if CONFIG_KERN_PRI
+# if CONFIG_KERN_PRI_INHERIT
+	#define __prio_orig(proc) (proc->orig_pri)
+	#define __prio_inh(proc) (LIST_EMPTY(&(proc)->inh_list) ? INT_MIN : \
+					((PriNode *)LIST_HEAD(&proc->inh_list))->pri)
+	#define __prio_proc(proc) (__prio_inh(proc) > __prio_orig(proc) ? \
+					__prio_inh(proc) : __prio_orig(proc))
+# endif
 	#define prio_next()	(LIST_EMPTY(&proc_ready_list) ? INT_MIN : \
 					((PriNode *)LIST_HEAD(&proc_ready_list))->pri)
 	#define prio_proc(proc)	(proc->link.pri)

bertos/kern/sem.c

@@ -53,6 +53,98 @@ INLINE void sem_verify(struct Semaphore *s)
 	ASSERT(s->nest_count < 128);   // heuristic max
 }
 
+#if CONFIG_KERN_PRI && CONFIG_KERN_PRI_INHERIT
+
+#define proc_updatePri(proc) (proc_setPri(proc, (proc)->orig_pri))
+
+
+INLINE void pri_inheritBlock(Semaphore *s)
+{
+	Process *owner = s->owner;
+
+	/*
+	 * Enqueue the blocking process in the owner's inheritance
+	 * list. Notice that such process might have inherited its
+	 * current priority from someone else.
+	 */
+	current_process->inh_link.pri = __prio_proc(current_process);
+	LIST_ENQUEUE(&owner->inh_list, &current_process->inh_link);
+	current_process->inh_blocked_by = s;
+
+	/*
+	 * As long as a process has the power of boosting the priority
+	 * of its lock owner...
+	 */
+	while (current_process->inh_link.pri > prio_proc(owner)) {
+		Process *p = owner;
+
+		 /* Boost the priority of the owner */
+		proc_updatePri(p);
+
+		/* If the owner is not blocked, we're done */
+		if (!p->inh_blocked_by)
+			break;
+
+		/*
+		 * Otherwise update the position of the owner
+		 * (which is `p' at each round!) in the inheritance
+		 * list it lies in and set up `owner' for the
+		 * next round.
+		 */
+		REMOVE(&p->inh_link.link);
+		p->inh_link.pri = prio_proc(p);
+		owner = p->inh_blocked_by->owner;
+		LIST_ENQUEUE(&owner->inh_list, &p->inh_link);
+	}
+}
+
+
+INLINE void pri_inheritUnblock(Semaphore *s, Process *proc)
+{
+	Process *owner = s->owner;
+	Node *n, *temp;
+	Process *p;
+
+	/*
+	 * This process has nothing more to do on a priority
+	 * inheritance list.
+	 */
+	REMOVE(&proc->inh_link.link);
+	proc->inh_blocked_by = NULL;
+
+	/*
+	 * Each process in the former owner's priority inheritance
+	 * list that is blocked on 's' needs to be removed from
+	 * there and added to the priority inheritance list of
+	 * this process, since it's going to be the new owner for
+	 * that semaphore.
+	 */
+	FOREACH_NODE_SAFE(n, temp, &owner->inh_list) {
+		p = containerof(n, Process, inh_link.link);
+		/* Ensures only the processes blocked on 's' are affected! */
+		if (p->inh_blocked_by == s) {
+			REMOVE(&p->inh_link.link);
+			LIST_ENQUEUE(&proc->inh_list, &p->inh_link);
+
+			/* And again, update the priority of the new owner */
+			if (p->inh_link.pri > prio_proc(proc))
+				proc_updatePri(proc);
+		}
+	}
+
+	proc_updatePri(owner);
+}
+#else
+INLINE void pri_inheritBlock(UNUSED_ARG(Semaphore *, s))
+{
+}
+
+INLINE void pri_inheritUnblock(UNUSED_ARG(Semaphore *, s), UNUSED_ARG(Process *, proc))
+{
+}
+#endif /* CONFIG_KERN_PRI_INHERIT */
+
+
 /**
  * \brief Initialize a Semaphore structure.
  */
@@ -121,6 +213,9 @@ void sem_obtain(struct Semaphore *s)
 		/* Append calling process to the wait queue */
 		ADDTAIL(&s->wait_queue, (Node *)current_process);
 
+		/* Trigger priority inheritance logic, if enabled */
+		pri_inheritBlock(s);
+
 		/*
 		 * We will wake up only when the current owner calls
 		 * sem_release(). Then, the semaphore will already
@@ -169,14 +264,17 @@ void sem_release(struct Semaphore *s)
 	 */
 	if (--s->nest_count == 0)
 	{
-		/* Disown semaphore */
-		s->owner = NULL;
-
 		/* Give semaphore to the first applicant, if any */
 		if (UNLIKELY((proc = (Process *)list_remHead(&s->wait_queue))))
 		{
+			/* Undo the effects of priority inheritance, if enabled */
+			pri_inheritUnblock(s, proc);
+
 			s->nest_count = 1;
 			s->owner = proc;
+		} else {
+			/* Disown semaphore */
+			s->owner = NULL;
 		}
 	}
 	proc_permit();

bertos/kern/sem_test.c

@@ -57,6 +57,8 @@
  * $test$: echo "#define CONFIG_KERN 1" >> $cfgdir/cfg_proc.h
  * $test$: echo  "#undef CONFIG_KERN_PRI" >> $cfgdir/cfg_proc.h
  * $test$: echo "#define CONFIG_KERN_PRI 1" >> $cfgdir/cfg_proc.h
+ * $test$: echo  "#undef CONFIG_KERN_PRI_INHERIT" >> $cfgdir/cfg_proc.h
+ * $test$: echo "#define CONFIG_KERN_PRI_INHERIT 1" >> $cfgdir/cfg_proc.h
  * $test$: cp bertos/cfg/cfg_sem.h $cfgdir/
  * $test$: echo  "#undef CONFIG_KERN_SEMAPHORES" >> $cfgdir/cfg_sem.h
  * $test$: echo "#define CONFIG_KERN_SEMAPHORES 1" >> $cfgdir/cfg_sem.h

bertos/struct/list.h

@@ -216,6 +216,21 @@ typedef struct _PriNode
 		(n) = (TYPEOF_OR_VOIDPTR(n))(((Node *)(n))->pred) \
 	)
 
+/**
+ * Iterate on the list safely against node removal.
+ *
+ * This macro generates a "for" statement using the following parameters:
+ * \param n   Node pointer to be used in each iteration.
+ * \param p   Temporal storage for the iterator.
+ * \param l   Pointer to list.
+ */
+#define FOREACH_NODE_SAFE(n, p, l) \
+	for( \
+		(n) = (TYPEOF_OR_VOIDPTR(n))LIST_HEAD(l), (p) = ((Node *)(n))->succ; \
+		((Node *)(n))->succ; \
+		(n) = (p), (p) = (TYPEOF_OR_VOIDPTR(n))(((Node *)(n))->succ) \
+	)
+
 /** Initialize a list. */
 #define LIST_INIT(l) \
 	do { \

examples/demo/cfg/cfg_proc.h

@@ -80,6 +80,12 @@
  */
 #define CONFIG_KERN_PRI 1
 
+/**
+ * Priority-inheritance protocol.
+ * $WIZ$ type = "boolean"
+ */
+#define CONFIG_KERN_PRI_INHERIT 1
+
 /**
  * Time sharing quantum (a prime number prevents interference effects) [ms].
  *