Index: rtai-3.6-cv/base/include/rtai_lxrt.h
===================================================================
--- rtai-3.6-cv.orig/base/include/rtai_lxrt.h	2009-02-17 15:09:28.000000000 +0100
+++ rtai-3.6-cv/base/include/rtai_lxrt.h	2009-03-03 15:26:12.000000000 +0100
@@ -331,7 +331,12 @@
 #define RT_SIGNAL_DISABLE	       226
 #define RT_SIGNAL_TRIGGER	       227
 
-#define MAX_LXRT_FUN		       230
+#define TASK_PREEMPTIBLE	       228        
+// System requests used for FPDS
+#define RT_SHOULD_YIELD		       229
+#define RT_FPDS_YIELD	           230
+
+#define MAX_LXRT_FUN	           235
 
 // not recovered yet 
 // Qblk's 
@@ -1085,6 +1090,27 @@
 	return rtai_lxrt(BIDX, SIZARG, LINUX_USE_FPU, &arg).i[LOW];
 }
 */
+
+RTAI_PROTO(int,rt_task_preemptible,(RT_TASK *task, int preemptible_flag))
+{
+	struct { RT_TASK *task; long preemptible_flag; } arg = { task, preemptible_flag };
+	return rtai_lxrt(BIDX, SIZARG, TASK_PREEMPTIBLE, &arg).i[LOW];
+	// FIXME: Allow this to be done in hard real-time
+}
+
+RTAI_PROTO(int, rt_should_yield, (void))
+{
+	struct { unsigned long dummy; } arg;
+	return rtai_lxrt(BIDX, SIZARG, RT_SHOULD_YIELD, &arg).i[LOW];
+}
+
+RTAI_PROTO(void, rt_fpds_yield, (void))
+{
+	struct { unsigned long dummy; } arg;
+	(void)rtai_lxrt(BIDX, SIZARG, RT_FPDS_YIELD, &arg).i[LOW];	
+}
+
+
 RTAI_PROTO(int, rt_get_priorities, (RT_TASK *task, int *priority, int *base_priority))
 {
 	int lpriority, lbase_priority, retval;
Index: rtai-3.6-cv/base/include/rtai_sched.h
===================================================================
--- rtai-3.6-cv.orig/base/include/rtai_sched.h	2009-02-17 15:09:28.000000000 +0100
+++ rtai-3.6-cv/base/include/rtai_sched.h	2009-03-03 14:26:06.000000000 +0100
@@ -48,6 +48,7 @@
 #define RT_SCHED_MBXSUSP    256
 #define RT_SCHED_SFTRDY     512
 #define RT_SCHED_SIGSUSP    (1 << 15)
+#define RT_FPDS_YIELDING	(1 << 16)
 
 #define RT_RWLINV     (11)  // keep this the highest
 #define RT_CHGPORTERR (10)
@@ -164,6 +165,7 @@
 	long *stack_bottom;
 	volatile int priority;
 	int base_priority;
+	int preemptible;
 	int policy;
 	int sched_lock_priority;
 	struct rt_task_struct *prio_passed_to;
@@ -223,6 +225,9 @@
 	struct rt_heap_t heap[2];
 
 	volatile int scheduler;
+	
+	/* FPDS */
+	volatile int should_yield;
 
 #ifdef CONFIG_RTAI_LONG_TIMED_LIST
 	rb_root_t rbr;
@@ -314,6 +319,13 @@
   
 void rt_linux_use_fpu(int use_fpu_flag);
 
+RTAI_SYSCALL_MODE int rt_task_preemptible(struct rt_task_struct *task,
+			int preemptible_flag);
+
+RTAI_SYSCALL_MODE int rt_should_yield(void);
+
+RTAI_SYSCALL_MODE void rt_fpds_yield(void);
+
 RTAI_SYSCALL_MODE int rt_hard_timer_tick_count(void);
 
 RTAI_SYSCALL_MODE int rt_hard_timer_tick_count_cpuid(int cpuid);
Index: rtai-3.6-cv/base/sched/api.c
===================================================================
--- rtai-3.6-cv.orig/base/sched/api.c	2009-02-17 15:09:28.000000000 +0100
+++ rtai-3.6-cv/base/sched/api.c	2009-03-03 14:36:35.000000000 +0100
@@ -588,6 +588,75 @@
 
 
 /**
+ * @anchor rt_task_preemptible
+ * @brief
+ *
+ * rt_task_preemptible sets a @e task to be preemptible (default) or not.
+ *
+ * @param task is a pointer to the real time task.
+ *
+ * @param preemptible_flag If this parameter has a nonzero value, the
+ * task can be preempted by higher priority tasks. If the value is zero,
+ * the task will not be preempted and will continue running until it
+ * blocks or explicitly calls the scheduler.
+ *
+ * @return 0 on succes. A negative value on failure as described below:
+ * - @b EINVAL: task does not refer to a valid task.
+ */
+RTAI_SYSCALL_MODE int rt_task_preemptible(RT_TASK *task, int preemptible_flag)
+{
+	if (task->magic != RT_TASK_MAGIC) {
+		return -EINVAL;
+	}
+	task->preemptible = !!preemptible_flag;
+	return 0;
+}
+
+/**
+ * @anchor rt_should_yield
+ * @brief
+ *
+ * rt_should_yield returns a boolean informing the calling task whether
+ * it should yield for a higher priority task in a FPDS system.
+ *
+ * @return 1 if the calling task should yield because a higher priority
+ * task is waiting, 0 if not.
+ * - @b EINVAL: task does not refer to a valid task. 
+ */
+RTAI_SYSCALL_MODE int rt_should_yield()
+{
+	return RT_CURRENT->should_yield;
+}
+
+/**
+ * @anchor rt_fpds_yield
+ * @brief
+ *
+ * rt_fpds_yield gives control of the CPU to other READY processes
+ * that have either equal *or* higher priority, unlike rt_task_yield().
+ * FIXME: does this claim of equal prio processes hold?
+ */
+RTAI_SYSCALL_MODE void rt_fpds_yield(void)
+{
+	RT_TASK *rt_current;
+	unsigned long flags;
+
+	flags = rt_global_save_flags_and_cli();
+	rt_current = RT_CURRENT;
+	
+	/*
+	 * Set the current task's state to be YIELDING, to inform the
+	 * scheduler that a context switch to a higher priority task
+	 * is allowed
+	 */
+	rt_current->state |= RT_FPDS_YIELDING;
+	rt_schedule(); /* Will not return until next context switch in */
+	rt_current->state &= ~RT_FPDS_YIELDING;
+
+	rt_global_restore_flags(flags);
+}
+
+/**
  * @anchor rt_task_signal_handler
  * @brief Set the signal handler of a task.
  *
@@ -2064,6 +2133,8 @@
 EXPORT_SYMBOL(rt_get_task_state);
 EXPORT_SYMBOL(rt_linux_use_fpu);
 EXPORT_SYMBOL(rt_task_use_fpu);
+EXPORT_SYMBOL(rt_should_yield);
+EXPORT_SYMBOL(rt_fpds_yield);
 EXPORT_SYMBOL(rt_task_signal_handler);
 EXPORT_SYMBOL(rt_gettimeorig);
 EXPORT_SYMBOL(rt_task_make_periodic_relative_ns);
Index: rtai-3.6-cv/base/sched/sched.c
===================================================================
--- rtai-3.6-cv.orig/base/sched/sched.c	2009-02-17 15:09:28.000000000 +0100
+++ rtai-3.6-cv/base/sched/sched.c	2009-03-03 15:11:05.000000000 +0100
@@ -289,6 +289,7 @@
 	(task->stack_bottom = (long *)&task->fpu_reg)[0] = 0;
 	task->magic = RT_TASK_MAGIC; 
 	task->policy = 0;
+	task->preemptible = 1;
 	task->owndres = 0;
 	task->prio_passed_to = 0;
 	task->period = 0;
@@ -315,6 +316,7 @@
 	task->msg_buf[0] = 0;
 	task->exectime[0] = task->exectime[1] = 0;
 	task->system_data_ptr = 0;
+	task->should_yield = 0;
 	atomic_inc((atomic_t *)(tasks_per_cpu + cpuid));
 	if (relink) {
 		task->priority = task->base_priority = priority;
@@ -410,6 +412,7 @@
 	*(task->stack_bottom = st) = 0;
 	task->magic = RT_TASK_MAGIC; 
 	task->policy = 0;
+	task->preemptible = 1;
 	task->suspdepth = 1;
 	task->state = (RT_SCHED_SUSPENDED | RT_SCHED_READY);
 	task->owndres = 0;
@@ -446,6 +449,8 @@
 	task->exectime[0] = task->exectime[1] = 0;
 	task->system_data_ptr = 0;
 
+	task->should_yield = 0;
+
 	task->max_msg_size[0] = (long)rt_thread;
 	task->max_msg_size[1] = data;
 	init_arch_stack();
@@ -794,6 +799,15 @@
 		RTAI_TASK_SWITCH_SIGNAL(); \
 	} while (0)
 
+
+inline int rt_fpds_preemptible(RT_TASK *task)
+{
+	return (task->priority == RT_SCHED_HIGHEST_PRIORITY
+		|| task->preemptible
+		|| (task->state & (RT_SCHED_DELAYED|RT_FPDS_YIELDING))
+		|| !(task->state & RT_SCHED_READY));
+}
+
 #ifdef CONFIG_SMP
 static void rt_schedule_on_schedule_ipi(void)
 {
@@ -945,12 +959,29 @@
 			rt_scheduling[cpuid].rqsted = 1;
 			goto sched_exit;
 		}
+		
+		/* If the current task is FPNS / non-preemptible,
+		 * don't preempt it but set a flag informing it that it should yield
+		 */
+		if (!rt_fpds_preemptible(rt_current)) {
+			/* Set a flag in the current task's TCB to signal that the current task
+			 * should yield to a higher priority task, and then exit the scheduler to
+			 * resume the current task.
+			 */
+			rt_current->should_yield = 1;
+			goto sched_exit;
+		}
+		
+		/* Reset the should_yield flag for the new task to be switched in */
+		new_task->should_yield = 0;
+		
 		if (USE_RTAI_TASKS && (!new_task->lnxtsk || !rt_current->lnxtsk)) {
 			if (!(new_task = switch_rtai_tasks(rt_current, new_task, cpuid))) {
 				goto sched_exit;
 			}
 		}
 		rt_smp_current[cpuid] = new_task;
+
 		if (new_task->is_hard > 0 || rt_current->is_hard > 0) {
 			struct task_struct *prev;
 			unsigned long sflags;
@@ -1293,6 +1324,22 @@
 			rt_scheduling[cpuid].rqsted = 1;
 			goto sched_exit;
 		}
+
+		/* If the current task is FPNS / non-preemptible,
+		 * don't preempt it but set a flag informing it that it should yield
+		 */
+		if (!rt_fpds_preemptible(rt_current)) {
+			/* Set a flag in the current task's TCB to signal that the current task
+			 * should yield to a higher priority task, and then exit the scheduler to
+			 * resume the current task.
+			 */
+			rt_current->should_yield = 1;
+			goto sched_exit;
+		}	
+
+		/* Reset the should_yield flag for the new task to be switched in */
+		new_task->should_yield = 0;
+
 		if (USE_RTAI_TASKS && (!new_task->lnxtsk || !rt_current->lnxtsk)) {
 			if (!(new_task = switch_rtai_tasks(rt_current, new_task, cpuid))) {
 				goto sched_exit;
@@ -2643,8 +2690,8 @@
 
 	PROC_PRINT("Number of forced hard/soft/hard transitions: traps %lu, syscalls %lu\n\n", traptrans, systrans);
 
-	PROC_PRINT("Priority  Period(ns)  FPU  Sig  State  CPU  Task  HD/SF  PID  RT_TASK *  TIME\n" );
-	PROC_PRINT("------------------------------------------------------------------------------\n" );
+	PROC_PRINT("Priority  Period(ns)  Preempt  FPU  Sig  State  CPU  Task  HD/SF  PID  RT_TASK *  TIME\n" );
+	PROC_PRINT("--------------------------------------------------------------------------------------\n" );
         for (cpuid = 0; cpuid < num_online_cpus(); cpuid++) {
                 task = &rt_linux_task;
 /*
@@ -2666,9 +2713,10 @@
 					t = 1000UL*(unsigned long)llimd(task->exectime[0], 10, tuned.cpu_freq)/den;
 				}				
 			}
-			PROC_PRINT("%-10d %-11lu %-4s %-3s 0x%-3x  %1lu:%1lu   %-4d   %-4d %-4d  %p   %-lu\n",
+			PROC_PRINT("%-10d %-11lu %-9s %-4s %-3s 0x%-3x  %1lu:%1lu   %-4d   %-4d %-4d  %p   %-lu\n",
                                task->priority,
                                (unsigned long)count2nano_cpuid(task->period, task->runnable_on_cpus),
+                               task->preemptible ? "Yes" : "No",
                                task->uses_fpu || task->lnxtsk ? "Yes" : "No",
                                task->signal ? "Yes" : "No",
                                task->state,
@@ -2745,6 +2793,8 @@
 	{ { 0, stop_rt_timer },			    STOP_TIMER },
 	{ { 0, rt_task_signal_handler },	    SIGNAL_HANDLER  },
 	{ { 0, rt_task_use_fpu },		    TASK_USE_FPU },
+	{ { 0, rt_task_preemptible },		TASK_PREEMPTIBLE },
+	{ { 1, rt_fpds_yield },				RT_FPDS_YIELD },
 	{ { 0, rt_hard_timer_tick_count },	    HARD_TIMER_COUNT },
 	{ { 0, rt_hard_timer_tick_count_cpuid },    HARD_TIMER_COUNT_CPUID },
 	{ { 0, count2nano },			    COUNT2NANO },
@@ -2803,6 +2853,7 @@
 	{ { 1, rt_enable_signal },		    RT_SIGNAL_ENABLE },
 	{ { 1, rt_disable_signal },		    RT_SIGNAL_DISABLE },
 	{ { 1, rt_trigger_signal }, 		    RT_SIGNAL_TRIGGER },
+	{ { 0, rt_should_yield },			RT_SHOULD_YIELD },
 	{ { 0, 0 },			            000 }
 };
 
@@ -2973,6 +3024,7 @@
 	for (cpuid = 0; cpuid < NR_RT_CPUS; cpuid++) {
 		rt_linux_task.uses_fpu = 1;
 		rt_linux_task.magic = 0;
+		rt_linux_task.preemptible = 1;
 		rt_linux_task.policy = rt_linux_task.is_hard = 0;
 		rt_linux_task.runnable_on_cpus = cpuid;
 		rt_linux_task.state = RT_SCHED_READY;