X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fkern%2Fsem.c;h=dc19f4abc4a4e7e56fed0e9441d3a34b4ecff605;hb=c0bf24abc6e74b7de8237af5fe843d19d31b3b87;hp=3c645e957ae0723f2b863fb6a0c395eab64a7f17;hpb=6a3c9d13073fcab59f86255408e5371804d5d455;p=bertos.git

diff --git a/bertos/kern/sem.c b/bertos/kern/sem.c
index 3c645e95..dc19f4ab 100644
--- a/bertos/kern/sem.c
+++ b/bertos/kern/sem.c
@@ -32,7 +32,6 @@
  *
  * \brief Semaphore based synchronization services.
  *
- * \version $Id$
  * \author Bernie Innocenti <bernie@codewiz.org>
  */
 
@@ -54,6 +53,120 @@ 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))
+
+
+/**
+ * Priority inheritance update algorithm.
+ *
+ * The algorithm checks and boosts the priority of the semaphore's
+ * current owner and also processes in that block the owner, which
+ * form a chain of blocking processes.
+ *
+ * Note that the priority of a process in the chain of blocked
+ * processes is always greater or equal than the priority of a process
+ * before in the chain. See the diagram below:
+ * P1  --. S1 ---> P2 --. S2 ---> P3
+ * prio_proc(P2) >= prio_proc(P1) always.
+ */
+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);
+	}
+}
+
+
+/**
+ * Priority inheritance unblock algorithm.
+ *
+ * Pass the priority inheritance list from the current owner to the
+ * process that will take ownership of the semaphore next, potentially
+ * boosting its priority.
+ *
+ * \param proc The process that will take ownership of the semaphore.
+ */
+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.
  */
@@ -122,6 +235,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
@@ -170,14 +286,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();