Module: xenomai-forge
Branch: next
Commit: 89c3ce894ef5e471f0b4a157d8d8d8f82c555f1e
URL:    
http://git.xenomai.org/?p=xenomai-forge.git;a=commit;h=89c3ce894ef5e471f0b4a157d8d8d8f82c555f1e

Author: Philippe Gerum <r...@xenomai.org>
Date:   Sun Aug 18 18:11:49 2013 +0200

cobalt/kernel: tighten CPU affinity and migration management

- make sure timers fire over real-time CPUs only, preferably on the
same CPU than the timed thread (if known). To this end, the core timer
gains basic thread affinity, which is used to preset the target
scheduler appropriately.

- better track CPU migration of threads, between the primary and
  secondary domains. A lazy timer migration now happens for passive
  threads migrated when switching back to primary mode.

Basic rules:

* active CPU migration from primary mode can only be a self-directed
  operation via a call to xnthread_migrate().

* passive CPU migration from secondary mode is detected when switching
  back to primary mode, in the hardening path. The Xenomai scheduler
  affinity is updated for the thread when/if possible, otherwise it is
  aborted. An affinity can only be valid if the current CPU is at
  least part of the real-time CPU set (xnsched_realtime_cpus).

* updates to the affinity mask may only be done from the regular
  kernel, via set_cpus_allowed(). The new settings are checked and
  acted upon by the Cobalt core, causing passive threads to be moved
  immediately to the proper Xenomai scheduler queue. Active threads
  running in primary mode have to wait for the next
  secondary-to-primary transition for getting their affinity back in
  sync between the two kernels.

---

 include/cobalt/kernel/assert.h      |    4 +-
 include/cobalt/kernel/sched.h       |    6 +-
 include/cobalt/kernel/thread.h      |   18 +++-
 include/cobalt/kernel/timer.h       |   44 ++++++--
 include/cobalt/uapi/kernel/thread.h |    6 +-
 include/rtdm/rtdm_driver.h          |    2 +-
 kernel/cobalt/assert.c              |    2 +-
 kernel/cobalt/clock.c               |   17 +++-
 kernel/cobalt/init.c                |   15 ++-
 kernel/cobalt/intr.c                |   56 ----------
 kernel/cobalt/posix/thread.c        |   10 +--
 kernel/cobalt/posix/timer.c         |   19 +++-
 kernel/cobalt/rtdm/drvlib.c         |    2 +-
 kernel/cobalt/sched-sporadic.c      |   13 ++-
 kernel/cobalt/sched-tp.c            |    2 +-
 kernel/cobalt/sched.c               |  137 ++++++++++++++++++------
 kernel/cobalt/shadow.c              |  203 +++++++++++++++++++++++++++--------
 kernel/cobalt/thread.c              |  199 ++++++++++++++++++++--------------
 kernel/cobalt/timer.c               |   74 +++++++------
 kernel/drivers/testing/switchtest.c |   11 +--
 testsuite/latency/latency.c         |   20 ++--
 21 files changed, 544 insertions(+), 316 deletions(-)

diff --git a/include/cobalt/kernel/assert.h b/include/cobalt/kernel/assert.h
index f7fd7f0..158d51f 100644
--- a/include/cobalt/kernel/assert.h
+++ b/include/cobalt/kernel/assert.h
@@ -74,8 +74,8 @@
 #define primary_mode_only()    XENO_BUGON(CONTEXT, ipipe_root_p)
 #define secondary_mode_only()  XENO_BUGON(CONTEXT, !ipipe_root_p)
 #define interrupt_only()       XENO_BUGON(CONTEXT, !xnsched_interrupt_p())
-#define atomic_only()          XENO_BUGON(CONTEXT, (xnlock_is_owner(&nklock) \
-                                                    && spltest()) == 0)
+#define atomic_only()          XENO_BUGON(CONTEXT, (xnlock_is_owner(&nklock) 
&& spltest()) == 0)
+#define realtime_cpu_only()    XENO_BUGON(CONTEXT, 
!xnsched_supported_cpu(ipipe_processor_id()))
 
 void __xnsys_assert_failed(const char *file, int line, const char *msg);
 
diff --git a/include/cobalt/kernel/sched.h b/include/cobalt/kernel/sched.h
index 2606b6c..dc68c3f 100644
--- a/include/cobalt/kernel/sched.h
+++ b/include/cobalt/kernel/sched.h
@@ -217,11 +217,11 @@ static inline void xnsched_set_resched(struct xnsched 
*sched)
        }
 }
 
-#define xnsched_cpus xnarch_machdata.supported_cpus
+#define xnsched_realtime_cpus xnarch_machdata.supported_cpus
 
 static inline int xnsched_supported_cpu(int cpu)
 {
-       return cpu_isset(cpu, xnsched_cpus);
+       return cpu_isset(cpu, xnsched_realtime_cpus);
 }
 
 #else /* !CONFIG_SMP */
@@ -231,7 +231,7 @@ static inline void xnsched_set_resched(struct xnsched 
*sched)
        xnsched_set_self_resched(sched);
 }
 
-#define xnsched_cpus CPU_MASK_ALL
+#define xnsched_realtime_cpus CPU_MASK_ALL
 
 static inline int xnsched_supported_cpu(int cpu)
 {
diff --git a/include/cobalt/kernel/thread.h b/include/cobalt/kernel/thread.h
index c149771..58db4ed 100644
--- a/include/cobalt/kernel/thread.h
+++ b/include/cobalt/kernel/thread.h
@@ -46,13 +46,13 @@ struct xnpersonality;
 
 struct xnthread_init_attr {
        struct xnpersonality *personality;
+       cpumask_t affinity;
        int flags;
        const char *name;
 };
 
 struct xnthread_start_attr {
        int mode;
-       cpumask_t affinity;
        void (*entry)(void *cookie);
        void *cookie;
 };
@@ -407,8 +407,24 @@ void xnthread_cancel(struct xnthread *thread);
 
 void xnthread_join(struct xnthread *thread);
 
+#ifdef CONFIG_SMP
 int xnthread_migrate(int cpu);
 
+void xnthread_migrate_passive(struct xnthread *thread,
+                             struct xnsched *sched);
+#else
+
+static inline int xnthread_migrate(int cpu)
+{
+       return cpu ? -EINVAL : 0;
+}
+
+static inline void xnthread_migrate_passive(struct xnthread *thread,
+                                           struct xnsched *sched)
+{ }
+
+#endif
+
 int xnthread_set_schedparam(struct xnthread *thread,
                            struct xnsched_class *sched_class,
                            const union xnsched_policy_param *sched_param);
diff --git a/include/cobalt/kernel/timer.h b/include/cobalt/kernel/timer.h
index 7b3243f..fe83e74 100644
--- a/include/cobalt/kernel/timer.h
+++ b/include/cobalt/kernel/timer.h
@@ -296,14 +296,15 @@ static inline int xntimer_reload_p(struct xntimer *timer)
 
 void __xntimer_init(struct xntimer *timer,
                    struct xnclock *clock,
-                   void (*handler)(struct xntimer *timer));
+                   void (*handler)(struct xntimer *timer),
+                   struct xnthread *thread);
 
 #ifdef CONFIG_XENO_OPT_STATS
 
-#define xntimer_init(timer, clock, handler)            \
-       do {                                            \
-               __xntimer_init(timer, clock, handler);  \
-               (timer)->handler_name = #handler;       \
+#define xntimer_init(timer, clock, handler, thread)            \
+       do {                                                    \
+               __xntimer_init(timer, clock, handler, thread);  \
+               (timer)->handler_name = #handler;               \
        } while (0)
 
 static inline void xntimer_reset_stats(struct xntimer *timer)
@@ -350,10 +351,10 @@ void xntimer_switch_tracking(struct xntimer *timer,
                             struct xnclock *newclock) { }
 #endif
 
-#define xntimer_init_noblock(timer, clock, handler)    \
-       do {                                            \
-               xntimer_init(timer, clock, handler);    \
-               (timer)->status |= XNTIMER_NOBLCK;      \
+#define xntimer_init_noblock(timer, clock, handler, thread)    \
+       do {                                                    \
+               xntimer_init(timer, clock, handler, thread);    \
+               (timer)->status |= XNTIMER_NOBLCK;              \
        } while (0)
 
 void xntimer_destroy(struct xntimer *timer);
@@ -375,6 +376,8 @@ xnticks_t xntimer_get_timeout(struct xntimer *timer);
 
 xnticks_t xntimer_get_interval(struct xntimer *timer);
 
+int xntimer_heading_p(struct xntimer *timer);
+
 static inline void xntimer_stop(struct xntimer *timer)
 {
        if ((timer->status & XNTIMER_DEQUEUED) == 0)
@@ -415,12 +418,29 @@ void xntimer_cleanup_proc(void);
 unsigned long xntimer_get_overruns(struct xntimer *timer, xnticks_t now);
 
 #ifdef CONFIG_SMP
-int xntimer_migrate(struct xntimer *timer, struct xnsched *sched);
+
+void __xntimer_migrate(struct xntimer *timer, struct xnsched *sched);
+
+static inline
+void xntimer_migrate(struct xntimer *timer, struct xnsched *sched)
+{                              /* nklocked, IRQs off */
+       if (timer->sched != sched)
+               __xntimer_migrate(timer, sched);
+}
+
 #else /* ! CONFIG_SMP */
-#define xntimer_migrate(timer, sched)  do { } while(0)
+
+static inline void xntimer_migrate(struct xntimer *timer,
+                                  struct xnsched *sched)
+{ }
+
 #endif /* CONFIG_SMP */
 
-#define xntimer_set_sched(timer, sched)        xntimer_migrate(timer, sched)
+static inline void xntimer_set_sched(struct xntimer *timer,
+                                    struct xnsched *sched)
+{
+       xntimer_migrate(timer, sched);
+}
 
 char *xntimer_format_time(xnticks_t ns,
                          char *buf, size_t bufsz);
diff --git a/include/cobalt/uapi/kernel/thread.h 
b/include/cobalt/uapi/kernel/thread.h
index 069507b..860ef92 100644
--- a/include/cobalt/uapi/kernel/thread.h
+++ b/include/cobalt/uapi/kernel/thread.h
@@ -67,9 +67,9 @@
 #define XNKICKED  0x00000008 /**< Forced out of primary mode */
 #define XNWAKEN   0x00000010 /**< Thread waken up upon resource availability */
 #define XNROBBED  0x00000020 /**< Robbed from resource ownership */
-#define XNAFFSET  0x00000040 /**< CPU affinity changed from primary mode */
-#define XNCANCELD 0x00000080 /**< Cancellation request is pending */
-#define XNSWREP   0x00000100 /**< Mode switch already reported */
+#define XNCANCELD 0x00000040 /**< Cancellation request is pending */
+#define XNSWREP   0x00000080 /**< Mode switch already reported */
+#define XNMOVED   0x00000100 /**< CPU migration in primary mode occurred */
 
 /** @} */
 
diff --git a/include/rtdm/rtdm_driver.h b/include/rtdm/rtdm_driver.h
index ae986ff..cb80797 100644
--- a/include/rtdm/rtdm_driver.h
+++ b/include/rtdm/rtdm_driver.h
@@ -1023,7 +1023,7 @@ enum rtdm_timer_mode {
 #ifndef DOXYGEN_CPP /* Avoid broken doxygen output */
 #define rtdm_timer_init(timer, handler, name)          \
 ({                                                     \
-       xntimer_init((timer), &nkclock, &handler);      \
+       xntimer_init((timer), &nkclock, handler, NULL); \
        xntimer_set_name((timer), (name));              \
        0;                                              \
 })
diff --git a/kernel/cobalt/assert.c b/kernel/cobalt/assert.c
index b346b8d..8032c02 100644
--- a/kernel/cobalt/assert.c
+++ b/kernel/cobalt/assert.c
@@ -62,7 +62,7 @@ void __xnsys_fatal(const char *format, ...)
         * NOTE: &nkthreadq can't be empty, we have the root thread(s)
         * linked there at least.
         */
-       for_each_realtime_cpu(cpu) {
+       for_each_online_cpu(cpu) {
                sched = xnsched_struct(cpu);
                list_for_each_entry(thread, &nkthreadq, glink) {
                        if (thread->sched != sched)
diff --git a/kernel/cobalt/clock.c b/kernel/cobalt/clock.c
index 735b204..3363a47 100644
--- a/kernel/cobalt/clock.c
+++ b/kernel/cobalt/clock.c
@@ -393,7 +393,7 @@ static int clock_vfile_next(struct 
xnvfile_snapshot_iterator *it, void *data)
        p->timeout = xntimer_get_timeout(timer);
        p->interval = xntimer_get_interval(timer);
        p->status = timer->status;
-       strncpy(p->handler, timer->handler_name, 16)[17] = '\0';
+       strncpy(p->handler, timer->handler_name, 16)[16] = '\0';
        xnobject_copy_name(p->name, timer->name);
 
        return 1;
@@ -573,6 +573,9 @@ EXPORT_SYMBOL_GPL(xnclock_deregister);
  * - Interrupt service routine, nklock locked, interrupts off
  *
  * Rescheduling: never.
+ *
+ * @note The current CPU must be part of the real-time affinity set,
+ * otherwise weird things may happen.
  */
 void xnclock_tick(struct xnclock *clock)
 {
@@ -667,10 +670,18 @@ void xnclock_tick(struct xnclock *clock)
 #ifdef CONFIG_SMP
                /*
                 * Make sure to pick the right percpu queue, in case
-                * the timer was migrated over its timeout handler.
+                * the timer was migrated over its timeout
+                * handler. Since this timer was dequeued,
+                * xntimer_migrate() did not kick the remote CPU, so
+                * we have to do this now if required.
                 */
-               if (timer->sched != sched)
+               if (unlikely(timer->sched != sched)) {
                        timerq = xntimer_percpu_queue(timer);
+                       xntimer_enqueue(timer, timerq);
+                       if (xntimer_heading_p(timer))
+                               xnclock_remote_shot(clock, timer->sched);
+                       continue;
+               }
 #endif
                xntimer_enqueue(timer, timerq);
        }
diff --git a/kernel/cobalt/init.c b/kernel/cobalt/init.c
index da9276c..b378969 100644
--- a/kernel/cobalt/init.c
+++ b/kernel/cobalt/init.c
@@ -120,7 +120,7 @@ static void sys_shutdown(void)
 
        xnsched_run();
 
-       for_each_realtime_cpu(cpu) {
+       for_each_online_cpu(cpu) {
                sched = xnsched_struct(cpu);
                xnsched_destroy(sched);
        }
@@ -144,7 +144,7 @@ static int __init mach_setup(void)
        }
 #endif /* CONFIG_SMP */
 
-       ret = ipipe_select_timers(&xnsched_cpus);
+       ret = ipipe_select_timers(&xnsched_realtime_cpus);
        if (ret < 0)
                return ret;
 
@@ -313,7 +313,7 @@ static __init int sys_init(void)
        }
        xnheap_set_label(&kheap, "main heap");
 
-       for_each_realtime_cpu(cpu) {
+       for_each_online_cpu(cpu) {
                sched = &per_cpu(nksched, cpu);
                xnsched_init(sched, cpu);
        }
@@ -357,6 +357,13 @@ static int __init xenomai_init(void)
        if (ret)
                goto cleanup_proc;
 
+       cpus_and(nkaffinity, nkaffinity, xnsched_realtime_cpus);
+       if (cpus_empty(nkaffinity)) {
+               printk(XENO_ERR "no real-time CPU in global affinity mask\n");
+               ret = -EINVAL;
+               goto cleanup_mach;
+       }
+
        ret = xnheap_mount();
        if (ret)
                goto cleanup_mach;
@@ -387,8 +394,6 @@ static int __init xenomai_init(void)
        if (ret)
                goto cleanup_rtdm;
 
-       cpus_and(nkaffinity, nkaffinity, xnsched_cpus);
-
        printk(XENO_INFO "Cobalt v%s enabled%s\n",
               XENO_VERSION_STRING, boot_notice);
 
diff --git a/kernel/cobalt/intr.c b/kernel/cobalt/intr.c
index fecd190..3af0792 100644
--- a/kernel/cobalt/intr.c
+++ b/kernel/cobalt/intr.c
@@ -1073,69 +1073,13 @@ static struct xnvfile_regular irq_vfile = {
        .ops = &irq_vfile_ops,
 };
 
-#ifdef CONFIG_SMP
-
-static int affinity_vfile_show(struct xnvfile_regular_iterator *it,
-                              void *data)
-{
-       unsigned long val = 0;
-       int cpu;
-
-       for (cpu = 0; cpu < BITS_PER_LONG; cpu++)
-               if (cpu_isset(cpu, nkaffinity))
-                       val |= (1UL << cpu);
-
-       xnvfile_printf(it, "%08lx\n", val);
-
-       return 0;
-}
-
-static ssize_t affinity_vfile_store(struct xnvfile_input *input)
-{
-       cpumask_t new_affinity;
-       ssize_t ret;
-       long val;
-       int cpu;
-
-       ret = xnvfile_get_integer(input, &val);
-       if (ret < 0)
-               return ret;
-
-       cpus_clear(new_affinity);
-
-       for (cpu = 0; cpu < BITS_PER_LONG; cpu++, val >>= 1)
-               if (val & 1)
-                       cpu_set(cpu, new_affinity);
-
-       cpus_and(nkaffinity, new_affinity, xnsched_cpus);
-
-       return ret;
-}
-
-static struct xnvfile_regular_ops affinity_vfile_ops = {
-       .show = affinity_vfile_show,
-       .store = affinity_vfile_store,
-};
-
-static struct xnvfile_regular affinity_vfile = {
-       .ops = &affinity_vfile_ops,
-};
-
-#endif /* CONFIG_SMP */
-
 void xnintr_init_proc(void)
 {
        xnvfile_init_regular("irq", &irq_vfile, &nkvfroot);
-#ifdef CONFIG_SMP
-       xnvfile_init_regular("affinity", &affinity_vfile, &nkvfroot);
-#endif /* CONFIG_SMP */
 }
 
 void xnintr_cleanup_proc(void)
 {
-#ifdef CONFIG_SMP
-       xnvfile_destroy_regular(&affinity_vfile);
-#endif /* CONFIG_SMP */
        xnvfile_destroy_regular(&irq_vfile);
 }
 
diff --git a/kernel/cobalt/posix/thread.c b/kernel/cobalt/posix/thread.c
index ff2f100..b20c2cf 100644
--- a/kernel/cobalt/posix/thread.c
+++ b/kernel/cobalt/posix/thread.c
@@ -433,6 +433,7 @@ static inline int pthread_create(struct cobalt_thread 
**thread_p, const pthread_
        iattr.name = thread->attr.name;
        iattr.flags = XNUSER|XNFPU;
        iattr.personality = &cobalt_personality;
+       iattr.affinity = CPU_MASK_ALL;
 
        /*
         * When the weak scheduling class is compiled in, SCHED_WEAK
@@ -1444,13 +1445,4 @@ out:
        return ret;
 }
 
-void __xnthread_test_cancel(struct xnthread *curr)
-{
-       if (!xnthread_test_state(curr, XNRELAX))
-               xnshadow_relax(0, 0);
-
-       do_exit(0);
-}
-EXPORT_SYMBOL_GPL(__xnthread_test_cancel);
-
 /*@}*/
diff --git a/kernel/cobalt/posix/timer.c b/kernel/cobalt/posix/timer.c
index 0abb70c..cf043d4 100644
--- a/kernel/cobalt/posix/timer.c
+++ b/kernel/cobalt/posix/timer.c
@@ -92,7 +92,8 @@ timer_init(struct cobalt_timer *timer,
         * All standard clocks are based on the core clock, and we
         * want to deliver a signal when a timer elapses.
         */
-       xntimer_init(&timer->timerbase, &nkclock, cobalt_timer_handler);
+       xntimer_init(&timer->timerbase, &nkclock, cobalt_timer_handler,
+                    &target->threadbase);
 
        return target;
 }
@@ -271,6 +272,7 @@ timer_gettimeout(struct cobalt_timer *__restrict__ timer,
 static inline int timer_set(struct cobalt_timer *timer, int flags,
                            const struct itimerspec *__restrict__ value)
 {                              /* nklocked, IRQs off. */
+       struct cobalt_thread *thread;
        xnticks_t start, period;
        int ret;
 
@@ -297,7 +299,20 @@ static inline int timer_set(struct cobalt_timer *timer, 
int flags,
 
        start = ts2ns(&value->it_value) + 1;
        period = ts2ns(&value->it_interval);
-       xntimer_set_sched(&timer->timerbase, xnsched_current());
+
+       /*
+        * If the target thread vanished, simply don't start the
+        * timer.
+        */
+       thread = cobalt_thread_find(timer->target);
+       if (thread == NULL)
+               return 0;
+
+       /*
+        * Make the timer affine to the CPU running the thread to be
+        * signaled.
+        */
+       xntimer_set_sched(&timer->timerbase, thread->threadbase.sched);
        /*
         * Now start the timer. If the timeout data has already
         * passed, the caller will handle the case.
diff --git a/kernel/cobalt/rtdm/drvlib.c b/kernel/cobalt/rtdm/drvlib.c
index 15ec39b..c866377 100644
--- a/kernel/cobalt/rtdm/drvlib.c
+++ b/kernel/cobalt/rtdm/drvlib.c
@@ -144,6 +144,7 @@ int rtdm_task_init(rtdm_task_t *task, const char *name,
        iattr.name = name;
        iattr.flags = 0;
        iattr.personality = &rtdm_personality;
+       iattr.affinity = CPU_MASK_ALL;
        param.rt.prio = priority;
 
        err = xnthread_init(task, &iattr, &xnsched_class_rt, &param);
@@ -164,7 +165,6 @@ int rtdm_task_init(rtdm_task_t *task, const char *name,
        }
 
        sattr.mode = 0;
-       sattr.affinity = CPU_MASK_ALL;
        sattr.entry = task_proc;
        sattr.cookie = arg;
        err = xnthread_start(task, &sattr);
diff --git a/kernel/cobalt/sched-sporadic.c b/kernel/cobalt/sched-sporadic.c
index 5b5fca0..2041087 100644
--- a/kernel/cobalt/sched-sporadic.c
+++ b/kernel/cobalt/sched-sporadic.c
@@ -98,6 +98,12 @@ static void sporadic_schedule_drop(struct xnthread *thread)
        int ret;
 
        pss->resume_date = now;
+       /*
+        * Assuming this timer should not fire that often unless the
+        * monitored thread behaves badly, we don't pin it on the CPU
+        * the thread is running, trading cycles at firing time
+        * against cycles when arming the timer.
+        */
        ret = xntimer_start(&pss->drop_timer, now + pss->budget,
                            XN_INFINITE, XN_ABSOLUTE);
        if (ret == -ETIMEDOUT) {
@@ -133,6 +139,7 @@ retry:
        } while(--pss->repl_pending > 0);
 
        if (pss->repl_pending > 0) {
+               xntimer_set_sched(&pss->repl_timer, thread->sched);
                ret = xntimer_start(&pss->repl_timer, pss->repl_data[r].date,
                                    XN_INFINITE, XN_ABSOLUTE);
                if (ret == -ETIMEDOUT)
@@ -305,9 +312,11 @@ static int xnsched_sporadic_declare(struct xnthread 
*thread,
        if (pss == NULL)
                return -ENOMEM;
 
-       xntimer_init(&pss->repl_timer, &nkclock, sporadic_replenish_handler);
+       xntimer_init(&pss->repl_timer, &nkclock,
+                    sporadic_replenish_handler, thread);
        xntimer_set_name(&pss->repl_timer, "pss-replenish");
-       xntimer_init(&pss->drop_timer, &nkclock, sporadic_drop_handler);
+       xntimer_init(&pss->drop_timer, &nkclock,
+                    sporadic_drop_handler, thread);
        xntimer_set_name(&pss->drop_timer, "pss-drop");
 
        thread->pss = pss;
diff --git a/kernel/cobalt/sched-tp.c b/kernel/cobalt/sched-tp.c
index d6ee296..6d87d7b 100644
--- a/kernel/cobalt/sched-tp.c
+++ b/kernel/cobalt/sched-tp.c
@@ -100,7 +100,7 @@ static void xnsched_tp_init(struct xnsched *sched)
        tp->tps = NULL;
        tp->gps = NULL;
        INIT_LIST_HEAD(&tp->threads);
-       xntimer_init_noblock(&tp->tf_timer, &nkclock, tp_tick_handler);
+       xntimer_init_noblock(&tp->tf_timer, &nkclock, tp_tick_handler, NULL);
        xntimer_set_name(&tp->tf_timer, "tp-tick");
 }
 
diff --git a/kernel/cobalt/sched.c b/kernel/cobalt/sched.c
index f3c9b65..eeeab7e 100644
--- a/kernel/cobalt/sched.c
+++ b/kernel/cobalt/sched.c
@@ -149,25 +149,25 @@ void xnsched_init(struct xnsched *sched, int cpu)
        sched->lflags = 0;
        sched->inesting = 0;
        sched->curr = &sched->rootcb;
-       /*
-        * No direct handler here since the host timer processing is
-        * postponed to xnintr_irq_handler(), as part of the interrupt
-        * exit code.
-        */
-       xntimer_init(&sched->htimer, &nkclock, NULL);
-       xntimer_set_priority(&sched->htimer, XNTIMER_LOPRIO);
-       xntimer_set_name(&sched->htimer, htimer_name);
-       xntimer_set_sched(&sched->htimer, sched);
 
        attr.flags = XNROOT | XNFPU;
        attr.name = root_name;
        attr.personality = &xenomai_personality;
+       attr.affinity = cpumask_of_cpu(cpu);
        param.idle.prio = XNSCHED_IDLE_PRIO;
 
        __xnthread_init(&sched->rootcb, &attr,
                        sched, &xnsched_class_idle, &param);
 
-       sched->rootcb.affinity = cpumask_of_cpu(cpu);
+       /*
+        * No direct handler here since the host timer processing is
+        * postponed to xnintr_irq_handler(), as part of the interrupt
+        * exit code.
+        */
+       xntimer_init(&sched->htimer, &nkclock, NULL, &sched->rootcb);
+       xntimer_set_priority(&sched->htimer, XNTIMER_LOPRIO);
+       xntimer_set_name(&sched->htimer, htimer_name);
+
        xnstat_exectime_set_current(sched, &sched->rootcb.stat.account);
 #ifdef CONFIG_XENO_HW_FPU
        sched->fpuholder = &sched->rootcb;
@@ -178,10 +178,10 @@ void xnsched_init(struct xnsched *sched, int cpu)
        nknrthreads++;
 
 #ifdef CONFIG_XENO_OPT_WATCHDOG
-       xntimer_init_noblock(&sched->wdtimer, &nkclock, watchdog_handler);
+       xntimer_init_noblock(&sched->wdtimer, &nkclock,
+                            watchdog_handler, &sched->rootcb);
        xntimer_set_name(&sched->wdtimer, "[watchdog]");
        xntimer_set_priority(&sched->wdtimer, XNTIMER_LOPRIO);
-       xntimer_set_sched(&sched->wdtimer, sched);
 #endif /* CONFIG_XENO_OPT_WATCHDOG */
 }
 
@@ -394,11 +394,7 @@ void xnsched_track_policy(struct xnthread *thread,
        xnsched_set_resched(thread->sched);
 }
 
-/*
- * Must be called with nklock locked, interrupts off. thread must be
- * runnable.
- */
-void xnsched_migrate(struct xnthread *thread, struct xnsched *sched)
+static void migrate_thread(struct xnthread *thread, struct xnsched *sched)
 {
        struct xnsched_class *sched_class = thread->sched_class;
 
@@ -415,6 +411,15 @@ void xnsched_migrate(struct xnthread *thread, struct 
xnsched *sched)
         */
        xnsched_set_resched(thread->sched);
        thread->sched = sched;
+}
+
+/*
+ * Must be called with nklock locked, interrupts off. thread must be
+ * runnable.
+ */
+void xnsched_migrate(struct xnthread *thread, struct xnsched *sched)
+{
+       migrate_thread(thread, sched);
 
 #ifdef CONFIG_XENO_HW_UNLOCKED_SWITCH
        /*
@@ -428,25 +433,13 @@ void xnsched_migrate(struct xnthread *thread, struct 
xnsched *sched)
 #endif /* !CONFIG_XENO_HW_UNLOCKED_SWITCH */
 }
 
-/* Must be called with nklock locked, interrupts off. thread may be
- * blocked. */
+/*
+ * Must be called with nklock locked, interrupts off. Thread may be
+ * blocked.
+ */
 void xnsched_migrate_passive(struct xnthread *thread, struct xnsched *sched)
 {
-       struct xnsched_class *sched_class = thread->sched_class;
-
-       if (xnthread_test_state(thread, XNREADY)) {
-               xnsched_dequeue(thread);
-               xnthread_clear_state(thread, XNREADY);
-       }
-
-       if (sched_class->sched_migrate)
-               sched_class->sched_migrate(thread, sched);
-       /*
-        * WARNING: the scheduling class may have just changed as a
-        * result of calling the per-class migration hook.
-        */
-       xnsched_set_resched(thread->sched);
-       thread->sched = sched;
+       migrate_thread(thread, sched);
 
        if (!xnthread_test_state(thread, XNTHREAD_BLOCK_BITS)) {
                xnsched_requeue(thread);
@@ -1176,6 +1169,75 @@ static struct xnvfile_snapshot_ops vfile_schedacct_ops = 
{
 
 #endif /* CONFIG_XENO_OPT_STATS */
 
+#ifdef CONFIG_SMP
+
+static int affinity_vfile_show(struct xnvfile_regular_iterator *it,
+                              void *data)
+{
+       unsigned long val = 0;
+       int cpu;
+
+       for (cpu = 0; cpu < BITS_PER_LONG; cpu++)
+               if (cpu_isset(cpu, nkaffinity))
+                       val |= (1UL << cpu);
+
+       xnvfile_printf(it, "%08lx\n", val);
+
+       return 0;
+}
+
+static ssize_t affinity_vfile_store(struct xnvfile_input *input)
+{
+       cpumask_t affinity, set;
+       ssize_t ret;
+       long val;
+       int cpu;
+       spl_t s;
+
+       ret = xnvfile_get_integer(input, &val);
+       if (ret < 0)
+               return ret;
+
+       if (val == 0)
+               affinity = xnsched_realtime_cpus; /* Reset to default. */
+       else {
+               cpus_clear(affinity);
+               for (cpu = 0; cpu < BITS_PER_LONG; cpu++, val >>= 1) {
+                       if (val & 1)
+                               cpu_set(cpu, affinity);
+               }
+       }
+
+       cpus_and(set, affinity, *cpu_online_mask);
+       if (cpus_empty(set))
+               return -EINVAL;
+
+       /*
+        * The new dynamic affinity must be a strict subset of the
+        * static set of supported CPUs.
+        */
+       cpus_or(set, affinity, xnsched_realtime_cpus);
+       if (!cpus_equal(set, xnsched_realtime_cpus))
+               return -EINVAL;
+
+       xnlock_get_irqsave(&nklock, s);
+       nkaffinity = affinity;
+       xnlock_put_irqrestore(&nklock, s);
+
+       return ret;
+}
+
+static struct xnvfile_regular_ops affinity_vfile_ops = {
+       .show = affinity_vfile_show,
+       .store = affinity_vfile_store,
+};
+
+static struct xnvfile_regular affinity_vfile = {
+       .ops = &affinity_vfile_ops,
+};
+
+#endif /* CONFIG_SMP */
+
 int xnsched_init_proc(void)
 {
        struct xnsched_class *p;
@@ -1206,6 +1268,10 @@ int xnsched_init_proc(void)
                return ret;
 #endif /* CONFIG_XENO_OPT_STATS */
 
+#ifdef CONFIG_SMP
+       xnvfile_init_regular("affinity", &affinity_vfile, &nkvfroot);
+#endif /* CONFIG_SMP */
+
        return 0;
 }
 
@@ -1218,6 +1284,9 @@ void xnsched_cleanup_proc(void)
                        p->sched_cleanup_vfile(p);
        }
 
+#ifdef CONFIG_SMP
+       xnvfile_destroy_regular(&affinity_vfile);
+#endif /* CONFIG_SMP */
 #ifdef CONFIG_XENO_OPT_STATS
        xnvfile_destroy_snapshot(&schedacct_vfile);
        xnvfile_destroy_snapshot(&schedstat_vfile);
diff --git a/kernel/cobalt/shadow.c b/kernel/cobalt/shadow.c
index b8c63d9..fd3220f 100644
--- a/kernel/cobalt/shadow.c
+++ b/kernel/cobalt/shadow.c
@@ -365,6 +365,119 @@ static void lostage_task_signal(struct ipipe_work_header 
*work)
                send_sig(signo, p, 1);
 }
 
+#ifdef CONFIG_SMP
+
+static int handle_setaffinity_event(struct ipipe_cpu_migration_data *d)
+{
+       struct task_struct *p = d->task;
+       struct xnthread *thread;
+       struct xnsched *sched;
+       spl_t s;
+
+       thread = xnshadow_thread(p);
+       if (thread == NULL)
+               return EVENT_PROPAGATE;
+
+       /*
+        * The CPU affinity mask is always controlled from secondary
+        * mode, therefore we progagate any change to the real-time
+        * affinity mask accordingly.
+        */
+       xnlock_get_irqsave(&nklock, s);
+       cpus_and(thread->affinity, p->cpus_allowed, nkaffinity);
+       xnlock_put_irqrestore(&nklock, s);
+
+       /*
+        * If kernel and real-time CPU affinity sets are disjoints,
+        * there might be problems ahead for this thread next time it
+        * moves back to primary mode, if it ends up switching to an
+        * unsupported CPU.
+        *
+        * Otherwise, check_affinity() will extend the CPU affinity if
+        * possible, fixing up the thread's affinity mask. This means
+        * that a thread might be allowed to run with a broken
+        * (i.e. fully cleared) affinity mask until it leaves primary
+        * mode then switches back to it, in SMP configurations.
+        */
+       if (cpus_empty(thread->affinity))
+               printk(XENO_WARN "thread %s[%d] changed CPU affinity 
inconsistently\n",
+                      thread->name, xnthread_host_pid(thread));
+       else {
+               xnlock_get_irqsave(&nklock, s);
+               /*
+                * Threads running in primary mode may NOT be forcibly
+                * migrated by the regular kernel to another CPU. Such
+                * migration would have to wait until the thread
+                * switches back from secondary mode at some point
+                * later, or issues a call to xnthread_migrate().
+                */
+               if (!xnthread_test_state(thread, XNMIGRATE) &&
+                   xnthread_test_state(thread, XNTHREAD_BLOCK_BITS)) {
+                       sched = xnsched_struct(d->dest_cpu);
+                       xnthread_migrate_passive(thread, sched);
+               }
+               xnlock_put_irqrestore(&nklock, s);
+       }
+
+       return EVENT_PROPAGATE;
+}
+
+static inline void check_affinity(struct task_struct *p) /* nklocked, IRQs off 
*/
+{
+       struct xnthread *thread = xnshadow_thread(p);
+       struct xnsched *sched;
+       int cpu = task_cpu(p);
+
+       /*
+        * If the task moved to another CPU while in secondary mode,
+        * migrate the companion Xenomai shadow to reflect the new
+        * situation.
+        *
+        * In the weirdest case, the thread is about to switch to
+        * primary mode on a CPU Xenomai shall not use. This is
+        * hopeless, whine and kill that thread asap.
+        */
+       if (!cpu_isset(cpu, xnsched_realtime_cpus)) {
+               printk(XENO_WARN "thread %s[%d] switched to non-rt CPU, 
aborted.\n",
+                      thread->name, xnthread_host_pid(thread));
+               /*
+                * Can't call xnthread_cancel() from a migration
+                * point, that would break. Since we are on the wakeup
+                * path to hardening, just raise XNCANCELD to catch it
+                * in xnshadow_harden().
+                */
+               xnthread_set_info(thread, XNCANCELD);
+               return;
+       }
+
+       sched = xnsched_struct(cpu);
+       if (sched == thread->sched)
+               return;
+
+       /*
+        * The current thread moved to a supported real-time CPU,
+        * which is not part of its original affinity mask
+        * though. Assume user wants to extend this mask.
+        */
+       if (!cpu_isset(cpu, thread->affinity))
+               cpu_set(cpu, thread->affinity);
+
+       xnthread_migrate_passive(thread, sched);
+}
+
+#else /* !CONFIG_SMP */
+
+struct ipipe_cpu_migration_data;
+
+static int handle_setaffinity_event(struct ipipe_cpu_migration_data *d)
+{
+       return EVENT_PROPAGATE;
+}
+
+static inline void check_affinity(struct task_struct *p) { }
+
+#endif /* CONFIG_SMP */
+
 /*!
  * @internal
  * \fn int xnshadow_harden(void);
@@ -385,25 +498,10 @@ static void lostage_task_signal(struct ipipe_work_header 
*work)
 void ipipe_migration_hook(struct task_struct *p) /* hw IRQs off */
 {
        struct xnthread *thread = xnshadow_thread(p);
-       struct xnsched *sched __maybe_unused;
 
        xnlock_get(&nklock);
-
-#ifdef CONFIG_SMP
-       /*
-        * If the task moved to another CPU while in secondary mode,
-        * update the CPU of the underlying Xenomai shadow to reflect
-        * the new situation. We do not migrate the thread timers
-        * here, this would not work. For a "full" migration including
-        * timers, using xnthread_migrate() is required.
-        */
-       sched = xnsched_struct(task_cpu(p));
-       if (sched != thread->sched)
-               xnsched_migrate_passive(thread, sched);
-#endif /* CONFIG_SMP */
-
+       check_affinity(p);
        xnthread_resume(thread, XNRELAX);
-
        xnlock_put(&nklock);
 
        xnsched_run();
@@ -440,6 +538,8 @@ int xnshadow_harden(void)
        xnthread_switch_fpu(sched);
 
        xnlock_clear_irqon(&nklock);
+       xnsched_resched_after_unlocked_switch();
+       xnthread_test_cancel();
 
        trace_mark(xn_nucleus, shadow_hardened, "thread %p name %s",
                   thread, xnthread_name(thread));
@@ -457,8 +557,6 @@ int xnshadow_harden(void)
                return -ERESTARTSYS;
        }
 
-       xnsched_resched_after_unlocked_switch();
-
        return 0;
 }
 EXPORT_SYMBOL_GPL(xnshadow_harden);
@@ -496,6 +594,7 @@ void xnshadow_relax(int notify, int reason)
 {
        struct xnthread *thread = xnsched_current_thread();
        struct task_struct *p = current;
+       int cpu __maybe_unused;
        siginfo_t si;
 
        XENO_BUGON(NUCLEUS, xnthread_test_state(thread, XNROOT));
@@ -555,26 +654,20 @@ void xnshadow_relax(int notify, int reason)
                xnsynch_detect_claimed_relax(thread);
        }
 
-#ifdef CONFIG_SMP
-       /*
-        * If the shadow thread changed its CPU affinity while in
-        * primary mode, reset the CPU affinity of its Linux
-        * counter-part when returning to secondary mode. [Actually,
-        * there is no service changing the CPU affinity from primary
-        * mode available from the nucleus --rpm].
-        */
-       if (xnthread_test_info(thread, XNAFFSET)) {
-               xnthread_clear_info(thread, XNAFFSET);
-               set_cpus_allowed(p, xnthread_affinity(thread));
-       }
-#endif /* CONFIG_SMP */
-
        /*
         * "current" is now running into the Linux domain on behalf of
         * the root thread.
         */
        xnthread_sync_window(thread);
 
+#ifdef CONFIG_SMP
+       if (xnthread_test_info(thread, XNMOVED)) {
+               xnthread_clear_info(thread, XNMOVED);
+               cpu = xnsched_cpu(thread->sched);
+               set_cpus_allowed(p, cpumask_of_cpu(cpu));
+       }
+#endif
+
        trace_mark(xn_nucleus, shadow_relaxed,
                  "thread %p thread_name %s comm %s",
                  thread, xnthread_name(thread), p->comm);
@@ -722,6 +815,36 @@ static inline void destroy_threadinfo(void)
        p->mm = NULL;
 }
 
+static void pin_to_initial_cpu(struct xnthread *thread)
+{
+       struct task_struct *p = current;
+       struct xnsched *sched;
+       int cpu;
+       spl_t s;
+
+       /*
+        * @thread is the Xenomai extension of the current kernel
+        * task. If the current CPU is part of the affinity mask of
+        * this thread, pin the latter on this CPU. Otherwise pin it
+        * to the first CPU of that mask.
+        */
+       cpu = task_cpu(p);
+       if (!cpu_isset(cpu, thread->affinity))
+               cpu = first_cpu(thread->affinity);
+
+       set_cpus_allowed(p, cpumask_of_cpu(cpu));
+       /*
+        * @thread is still unstarted Xenomai-wise, we are precisely
+        * in the process of mapping the current kernel task to
+        * it. Therefore xnthread_migrate_passive() is the right way
+        * to pin it on a real-time CPU.
+        */
+       xnlock_get_irqsave(&nklock, s);
+       sched = xnsched_struct(cpu);
+       xnthread_migrate_passive(thread, sched);
+       xnlock_put_irqrestore(&nklock, s);
+}
+
 /**
  * @fn int xnshadow_map_user(struct xnthread *thread, unsigned long __user 
*u_window_offset)
  * @internal
@@ -770,7 +893,6 @@ int xnshadow_map_user(struct xnthread *thread,
        struct xnthread_start_attr attr;
        struct xnsys_ppd *sys_ppd;
        struct xnheap *sem_heap;
-       cpumask_t affinity;
        spl_t s;
        int ret;
 
@@ -817,11 +939,7 @@ int xnshadow_map_user(struct xnthread *thread,
        }
        thread->u_window = u_window;
        __xn_put_user(xnheap_mapped_offset(sem_heap, u_window), 
u_window_offset);
-
-       /* Restrict affinity to a single CPU of nkaffinity & current set. */
-       cpus_and(affinity, p->cpus_allowed, nkaffinity);
-       affinity = cpumask_of_cpu(first_cpu(affinity));
-       set_cpus_allowed(p, affinity);
+       pin_to_initial_cpu(thread);
 
        trace_mark(xn_nucleus, shadow_map_user,
                   "thread %p thread_name %s pid %d priority %d",
@@ -851,7 +969,6 @@ int xnshadow_map_user(struct xnthread *thread,
        ipipe_enable_notifier(current);
 
        attr.mode = 0;
-       attr.affinity = affinity;
        attr.entry = NULL;
        attr.cookie = NULL;
        ret = xnthread_start(thread, &attr);
@@ -944,7 +1061,6 @@ int xnshadow_map_kernel(struct xnthread *thread, struct 
completion *done)
 {
        struct xnpersonality *personality = thread->personality;
        struct task_struct *p = current;
-       cpumask_t affinity;
        int ret;
        spl_t s;
 
@@ -959,9 +1075,7 @@ int xnshadow_map_kernel(struct xnthread *thread, struct 
completion *done)
                return ret;
 
        thread->u_window = NULL;
-       cpus_and(affinity, p->cpus_allowed, nkaffinity);
-       affinity = cpumask_of_cpu(first_cpu(affinity));
-       set_cpus_allowed(p, affinity);
+       pin_to_initial_cpu(thread);
 
        trace_mark(xn_nucleus, shadow_map_kernel,
                   "thread %p thread_name %s pid %d priority %d",
@@ -2378,6 +2492,9 @@ int ipipe_kevent_hook(int kevent, void *data)
        case IPIPE_KEVT_HOSTRT:
                ret = handle_hostrt_event(data);
                break;
+       case IPIPE_KEVT_SETAFFINITY:
+               ret = handle_setaffinity_event(data);
+               break;
        default:
                ret = EVENT_PROPAGATE;
        }
diff --git a/kernel/cobalt/thread.c b/kernel/cobalt/thread.c
index 976f1e1..1f9ed50 100644
--- a/kernel/cobalt/thread.c
+++ b/kernel/cobalt/thread.c
@@ -65,12 +65,18 @@ static void periodic_handler(struct xntimer *timer)
         */
        if (xnthread_test_state(thread, XNDELAY|XNPEND) == XNDELAY)
                xnthread_resume(thread, XNDELAY);
+       /*
+        * The thread a periodic timer is affine to might have been
+        * migrated to another CPU while passive. Fix this up.
+        */
+       xntimer_set_sched(timer, thread->sched);
 }
 
 static void roundrobin_handler(struct xntimer *timer)
 {
        struct xnthread *thread = container_of(timer, struct xnthread, 
rrbtimer);
        xnsched_tick(thread);
+       xntimer_set_sched(timer, thread->sched);
 }
 
 struct kthread_arg {
@@ -165,20 +171,12 @@ int __xnthread_init(struct xnthread *thread,
        else
                snprintf(thread->name, sizeof(thread->name), "%p", thread);
 
-       xntimer_init(&thread->rtimer, &nkclock, timeout_handler);
-       xntimer_set_name(&thread->rtimer, thread->name);
-       xntimer_set_priority(&thread->rtimer, XNTIMER_HIPRIO);
-       xntimer_init(&thread->ptimer, &nkclock, periodic_handler);
-       xntimer_set_name(&thread->ptimer, thread->name);
-       xntimer_set_priority(&thread->ptimer, XNTIMER_HIPRIO);
-       xntimer_init(&thread->rrbtimer, &nkclock, roundrobin_handler);
-       xntimer_set_name(&thread->rrbtimer, thread->name);
-       xntimer_set_priority(&thread->rrbtimer, XNTIMER_LOPRIO);
-
+       thread->personality = attr->personality;
+       cpus_and(thread->affinity, attr->affinity, nkaffinity);
+       thread->sched = sched;
        thread->state = flags;
        thread->info = 0;
        thread->schedlck = 0;
-
        thread->rrperiod = XN_INFINITE;
        thread->wchan = NULL;
        thread->wwake = NULL;
@@ -187,18 +185,23 @@ int __xnthread_init(struct xnthread *thread,
        thread->registry.handle = XN_NO_HANDLE;
        thread->registry.waitkey = NULL;
        memset(&thread->stat, 0, sizeof(thread->stat));
-
+       thread->selector = NULL;
+       INIT_LIST_HEAD(&thread->claimq);
        /* These will be filled by xnthread_start() */
        thread->imode = 0;
        thread->entry = NULL;
        thread->cookie = NULL;
 
-       thread->selector = NULL;
-       INIT_LIST_HEAD(&thread->claimq);
-
-       thread->personality = attr->personality;
+       xntimer_init(&thread->rtimer, &nkclock, timeout_handler, thread);
+       xntimer_set_name(&thread->rtimer, thread->name);
+       xntimer_set_priority(&thread->rtimer, XNTIMER_HIPRIO);
+       xntimer_init(&thread->ptimer, &nkclock, periodic_handler, thread);
+       xntimer_set_name(&thread->ptimer, thread->name);
+       xntimer_set_priority(&thread->ptimer, XNTIMER_HIPRIO);
+       xntimer_init(&thread->rrbtimer, &nkclock, roundrobin_handler, thread);
+       xntimer_set_name(&thread->rrbtimer, thread->name);
+       xntimer_set_priority(&thread->rrbtimer, XNTIMER_LOPRIO);
 
-       thread->sched = sched;
        thread->init_class = sched_class;
        thread->base_class = NULL; /* xnsched_set_policy() will set it. */
        thread->init_schedparam = *sched_param;
@@ -553,9 +556,9 @@ void __xnthread_cleanup(struct xnthread *curr)
  * the floating-point unit. XNFPU is implicitly assumed for user-space
  * threads even if not set in @a flags.
  *
- * - ops: A pointer to a structure defining the class-level operations
- * available for this thread. Fields from this structure must have
- * been set appropriately by the caller.
+ * - affinity: The processor affinity of this thread. Passing
+ * CPU_MASK_ALL means "any cpu" from the allowed core affinity mask
+ * (nkaffinity). Passing an empty set is invalid.
  *
  * @param sched_class The initial scheduling class the new thread
  * should be assigned to.
@@ -567,7 +570,8 @@ void __xnthread_cleanup(struct xnthread *curr)
  * @return 0 is returned on success. Otherwise, the following error
  * code indicates the cause of the failure:
  *
- * - -EINVAL is returned if @a attr->flags has invalid bits set.
+ * - -EINVAL is returned if @a attr->flags has invalid bits set, or @a
+ *   attr->affinity is invalid (e.g. empty).
  *
  * Side-effect: This routine does not call the rescheduling procedure.
  *
@@ -580,13 +584,25 @@ int xnthread_init(struct xnthread *thread,
                  struct xnsched_class *sched_class,
                  const union xnsched_policy_param *sched_param)
 {
-       struct xnsched *sched = xnsched_current();
+       struct xnsched *sched;
+       cpumask_t affinity;
        spl_t s;
        int ret;
 
        if (attr->flags & ~(XNFPU | XNUSER | XNSUSP))
                return -EINVAL;
 
+       /*
+        * Pick an initial CPU for the new thread which is part of its
+        * affinity mask, and therefore also part of the supported
+        * CPUs. This CPU may change in pin_to_initial_cpu().
+        */
+       cpus_and(affinity, attr->affinity, nkaffinity);
+       if (cpus_empty(affinity))
+               return -EINVAL;
+
+       sched = xnsched_struct(first_cpu(affinity));
+
        ret = __xnthread_init(thread, attr, sched, sched_class, sched_param);
        if (ret)
                return ret;
@@ -635,10 +651,6 @@ EXPORT_SYMBOL_GPL(xnthread_init);
  * case, the thread will have to be explicitly resumed using the
  * xnthread_resume() service for its execution to actually begin.
  *
- * - affinity: The processor affinity of this thread. Passing
- * CPU_MASK_ALL or an empty affinity set means "any cpu" from the
- * allowed core affinity mask (nkaffinity).
- *
  * - entry: The address of the thread's body routine. In other words,
  * it is the thread entry point.
  *
@@ -649,8 +661,6 @@ EXPORT_SYMBOL_GPL(xnthread_init);
  *
  * @retval -EBUSY if @a thread was not dormant or stopped ;
  *
- * @retval -EINVAL if the value of @a attr->affinity is invalid.
- *
  * Environments:
  *
  * This service can be called from:
@@ -664,33 +674,14 @@ EXPORT_SYMBOL_GPL(xnthread_init);
 int xnthread_start(struct xnthread *thread,
                   const struct xnthread_start_attr *attr)
 {
-       struct xnsched *sched __maybe_unused;
-       cpumask_t affinity;
-       int ret = 0;
        spl_t s;
 
        xnlock_get_irqsave(&nklock, s);
 
        if (!xnthread_test_state(thread, XNDORMANT)) {
-               ret = -EBUSY;
-               goto unlock_and_exit;
-       }
-
-       affinity = attr->affinity;
-       cpus_and(affinity, affinity, nkaffinity);
-       thread->affinity = *cpu_online_mask;
-       cpus_and(thread->affinity, affinity, thread->affinity);
-
-       if (cpus_empty(thread->affinity)) {
-               ret = -EINVAL;
-               goto unlock_and_exit;
-       }
-#ifdef CONFIG_SMP
-       if (!cpu_isset(xnsched_cpu(thread->sched), thread->affinity)) {
-               sched = xnsched_struct(first_cpu(thread->affinity));
-               xnsched_migrate_passive(thread, sched);
+               xnlock_put_irqrestore(&nklock, s);
+               return -EBUSY;
        }
-#endif /* CONFIG_SMP */
 
        xnthread_set_state(thread, attr->mode & (XNTHREAD_MODE_BITS | XNSUSP));
        thread->imode = (attr->mode & XNTHREAD_MODE_BITS);
@@ -702,10 +693,10 @@ int xnthread_start(struct xnthread *thread,
 
        xnthread_resume(thread, XNDORMANT);
        xnsched_run();
-unlock_and_exit:
+
        xnlock_put_irqrestore(&nklock, s);
 
-       return ret;
+       return 0;
 }
 EXPORT_SYMBOL_GPL(xnthread_start);
 
@@ -1308,7 +1299,7 @@ EXPORT_SYMBOL_GPL(xnthread_unblock);
 int xnthread_set_periodic(xnthread_t *thread, xnticks_t idate,
                          xntmode_t timeout_mode, xnticks_t period)
 {
-       int err = 0;
+       int ret = 0;
        spl_t s;
 
        xnlock_get_irqsave(&nklock, s);
@@ -1331,30 +1322,29 @@ int xnthread_set_periodic(xnthread_t *thread, xnticks_t 
idate,
                 * clock gravity. This can't work, caller must have
                 * messed up with arguments.
                 */
-               err = -EINVAL;
+               ret = -EINVAL;
                goto unlock_and_exit;
        }
 
        xntimer_set_sched(&thread->ptimer, thread->sched);
 
-       if (idate == XN_INFINITE) {
+       if (idate == XN_INFINITE)
                xntimer_start(&thread->ptimer, period, period, XN_RELATIVE);
-       } else {
+       else {
                if (timeout_mode == XN_REALTIME)
                        idate -= xnclock_get_offset(&nkclock);
                else if (timeout_mode != XN_ABSOLUTE) {
-                       err = -EINVAL;
+                       ret = -EINVAL;
                        goto unlock_and_exit;
                }
-               err = xntimer_start(&thread->ptimer, idate + period, period,
+               ret = xntimer_start(&thread->ptimer, idate + period, period,
                                    XN_ABSOLUTE);
        }
 
-      unlock_and_exit:
-
+unlock_and_exit:
        xnlock_put_irqrestore(&nklock, s);
 
-       return err;
+       return ret;
 }
 EXPORT_SYMBOL_GPL(xnthread_set_periodic);
 
@@ -1497,6 +1487,7 @@ int xnthread_set_slice(struct xnthread *thread, xnticks_t 
quantum)
                        return -EINVAL;
                }
                xnthread_set_state(thread, XNRRB);
+               xntimer_set_sched(&thread->rrbtimer, thread->sched);
                xntimer_start(&thread->rrbtimer,
                              quantum, quantum, XN_RELATIVE);
        } else
@@ -1624,16 +1615,21 @@ EXPORT_SYMBOL_GPL(xnthread_join);
  * @fn int xnthread_migrate(int cpu)
  * @brief Migrate the current thread.
  *
- * This call makes the current thread migrate to another CPU if its
- * affinity allows it.
+ * This call makes the current thread migrate to another (real-time)
+ * CPU if its affinity allows it. This call is available from
+ * primary mode only.
  *
  * @param cpu The destination CPU.
  *
  * @retval 0 if the thread could migrate ;
- * @retval -EPERM if the calling context is asynchronous, or the
- * current thread affinity forbids this migration ;
- * @retval -EBUSY if the scheduler is locked.
+ * @retval -EPERM if the calling context is invalid, or the
+ * scheduler is locked.
+ * @retval -EINVAL if the current thread affinity forbids this
+ * migration.
  */
+
+#ifdef CONFIG_SMP
+
 int xnthread_migrate(int cpu)
 {
        struct xnthread *thread;
@@ -1641,39 +1637,39 @@ int xnthread_migrate(int cpu)
        int ret = 0;
        spl_t s;
 
-       if (xnsched_interrupt_p())
-               return -EPERM;
-
-       if (xnsched_locked_p())
-               return -EBUSY;
-
        xnlock_get_irqsave(&nklock, s);
 
-       thread = xnsched_current_thread();
-
-       if (!cpu_isset(cpu, thread->affinity)) {
+       if (!xnsched_primary_p() || xnsched_locked_p()) {
                ret = -EPERM;
                goto unlock_and_exit;
        }
 
-       if (cpu == ipipe_processor_id())
+       thread = xnsched_current_thread();
+       if (!cpu_isset(cpu, thread->affinity)) {
+               ret = -EINVAL;
                goto unlock_and_exit;
+       }
 
        sched = xnsched_struct(cpu);
+       if (sched == xnthread_sched(thread))
+               goto unlock_and_exit;
 
        trace_mark(xn_nucleus, thread_migrate,
                   "thread %p thread_name %s cpu %d",
                   thread, xnthread_name(thread), cpu);
 
-       release_fpu(thread);
-
        /* Move to remote scheduler. */
        xnsched_migrate(thread, sched);
 
-       /* Migrate the thread's periodic timer. */
-       xntimer_set_sched(&thread->ptimer, sched);
-
-       xnsched_run();
+       /*
+        * Migrate the thread's periodic and round-robin timers. We
+        * don't have to care about the resource timer, since we can
+        * only deal with the current thread, which is, well, running,
+        * so it can't be sleeping on any timed wait at the moment.
+        */
+       __xntimer_migrate(&thread->ptimer, sched);
+       if (xnthread_test_state(thread, XNRRB))
+               __xntimer_migrate(&thread->rrbtimer, sched);
 
        /*
         * Reset execution time measurement period so that we don't
@@ -1681,14 +1677,44 @@ int xnthread_migrate(int cpu)
         */
        xnstat_exectime_reset_stats(&thread->stat.lastperiod);
 
-      unlock_and_exit:
+       /*
+        * So that xnshadow_relax() will pin the linux mate on the
+        * same CPU next time the thread switches to secondary mode.
+        */
+       xnthread_set_info(thread, XNMOVED);
+
+       xnsched_run();
 
+ unlock_and_exit:
        xnlock_put_irqrestore(&nklock, s);
 
        return ret;
 }
 EXPORT_SYMBOL_GPL(xnthread_migrate);
 
+void xnthread_migrate_passive(struct xnthread *thread, struct xnsched *sched)
+{                              /* nklocked, IRQs off */
+       trace_mark(xn_nucleus, thread_migrate_passive,
+                  "thread %p thread_name %s cpu %d",
+                  thread, xnthread_name(thread), xnsched_cpu(sched));
+
+       XENO_BUGON(NUCLEUS, !cpu_isset(xnsched_cpu(sched), 
xnsched_realtime_cpus));
+
+       if (thread->sched == sched)
+               return;
+       /*
+        * Timer migration is postponed until the next timeout happens
+        * for the periodic and rrb timers. The resource timer will be
+        * moved to the right CPU next time it is armed in
+        * xnthread_suspend().
+        */
+       xnsched_migrate_passive(thread, sched);
+
+       xnstat_exectime_reset_stats(&thread->stat.lastperiod);
+}
+
+#endif /* CONFIG_SMP */
+
 /**
  * @fn int xnthread_set_schedparam(struct xnthread *thread,struct 
xnsched_class *sched_class,const union xnsched_policy_param *sched_param)
  * @brief Change the base scheduling parameters of a thread.
@@ -1810,4 +1836,13 @@ unlock_and_exit:
 }
 EXPORT_SYMBOL_GPL(xnthread_set_schedparam);
 
+void __xnthread_test_cancel(struct xnthread *curr)
+{
+       if (!xnthread_test_state(curr, XNRELAX))
+               xnshadow_relax(0, 0);
+
+       do_exit(0);
+}
+EXPORT_SYMBOL_GPL(__xnthread_test_cancel);
+
 /*@}*/
diff --git a/kernel/cobalt/timer.c b/kernel/cobalt/timer.c
index ff71fc1..267c2c7 100644
--- a/kernel/cobalt/timer.c
+++ b/kernel/cobalt/timer.c
@@ -45,7 +45,7 @@
 #include <cobalt/kernel/trace.h>
 #include <cobalt/kernel/arith.h>
 
-static inline int xntimer_heading_p(struct xntimer *timer)
+int xntimer_heading_p(struct xntimer *timer)
 {
        struct xnsched *sched = timer->sched;
        xntimerq_it_t it;
@@ -313,7 +313,7 @@ xnticks_t xntimer_get_interval(struct xntimer *timer)
 EXPORT_SYMBOL_GPL(xntimer_get_interval);
 
 /*!
- * \fn void xntimer_init(struct xntimer *timer,struct xnclock *clock,void 
(*handler)(struct xntimer *timer))
+ * \fn void xntimer_init(struct xntimer *timer,struct xnclock *clock,void 
(*handler)(struct xntimer *timer), struct xnthread *thread)
  * \brief Initialize a timer object.
  *
  * Creates a timer. When created, a timer is left disarmed; it must be
@@ -331,6 +331,11 @@ EXPORT_SYMBOL_GPL(xntimer_get_interval);
  *
  * @param handler The routine to call upon expiration of the timer.
  *
+ * @param thread The optional thread object the new timer is affine
+ * to. If non-NULL, the timer will fire on the same CPU @a thread
+ * currently runs on by default. A call to xntimer_set_sched() may
+ * change this setting.
+ *
  * There is no limitation on the number of timers which can be
  * created/active concurrently.
  *
@@ -344,14 +349,17 @@ EXPORT_SYMBOL_GPL(xntimer_get_interval);
  */
 #ifdef DOXYGEN_CPP
 void xntimer_init(struct xntimer *timer, struct xnclock *clock,
-                 void (*handler)(struct xntimer *timer));
+                 void (*handler)(struct xntimer *timer),
+                 struct xnthread *thread);
 #endif
 
 void __xntimer_init(struct xntimer *timer,
                    struct xnclock *clock,
-                   void (*handler)(struct xntimer *timer))
+                   void (*handler)(struct xntimer *timer),
+                   struct xnthread *thread)
 {
        spl_t s __maybe_unused;
+       int cpu;
 
 #ifdef CONFIG_XENO_OPT_EXTCLOCK
        timer->clock = clock;
@@ -362,7 +370,18 @@ void __xntimer_init(struct xntimer *timer,
        timer->status = XNTIMER_DEQUEUED;
        timer->handler = handler;
        timer->interval = 0;
-       timer->sched = xnsched_current();
+       /*
+        * Timers have to run on a real-time CPU, i.e. a member of the
+        * xnsched_realtime_cpus mask. If the new timer is affine to a
+        * thread, we assign it the same CPU (which has to be correct),
+        * otherwise pick the first valid real-time CPU by default.
+        */
+       if (thread)
+               timer->sched = thread->sched;
+       else {
+               cpu = first_cpu(xnsched_realtime_cpus);
+               timer->sched = xnsched_struct(cpu);
+       }
 
 #ifdef CONFIG_XENO_OPT_STATS
 #ifdef CONFIG_XENO_OPT_EXTCLOCK
@@ -458,54 +477,37 @@ EXPORT_SYMBOL_GPL(xntimer_destroy);
  *
  * @param timer The address of the timer object to be migrated.
  *
- * @param sched The address of the destination per-CPU scheduler slot.
- *
- * @retval -EINVAL if @a timer is queued on another CPU than current.
- * @retval 0 otherwise.
+ * @param sched The address of the destination per-CPU scheduler
+ * slot.
  *
+ * @note Must be called with nklock held, IRQs off.
  */
-int xntimer_migrate(struct xntimer *timer, struct xnsched *sched)
-{
+void __xntimer_migrate(struct xntimer *timer, struct xnsched *sched)
+{                              /* nklocked, IRQs off */
        struct xnclock *clock;
        xntimerq_t *q;
-       int ret = 0;
-       int queued;
-       spl_t s;
 
        trace_mark(xn_nucleus, timer_migrate, "timer %p cpu %d",
                   timer, (int)xnsched_cpu(sched));
 
-       xnlock_get_irqsave(&nklock, s);
+       XENO_BUGON(NUCLEUS, !cpu_isset(xnsched_cpu(sched), 
xnsched_realtime_cpus));
 
        if (sched == timer->sched)
-               goto unlock_and_exit;
+               return;
 
-       queued = (timer->status & XNTIMER_DEQUEUED) == 0;
-       if (queued) {
-               if (timer->sched != xnsched_current()) {
-                       ret = -EINVAL;
-                       goto unlock_and_exit;
-               }
+       if (timer->status & XNTIMER_DEQUEUED)
+               timer->sched = sched;
+       else {
                xntimer_stop(timer);
-       }
-
-       timer->sched = sched;
-
-       if (queued) {
+               timer->sched = sched;
                clock = xntimer_clock(timer);
                q = xntimer_percpu_queue(timer);
                xntimer_enqueue(timer, q);
                if (xntimer_heading_p(timer))
-                       xnclock_remote_shot(clock, timer->sched);
+                       xnclock_remote_shot(clock, sched);
        }
-
-unlock_and_exit:
-
-       xnlock_put_irqrestore(&nklock, s);
-
-       return ret;
 }
-EXPORT_SYMBOL_GPL(xntimer_migrate);
+EXPORT_SYMBOL_GPL(__xntimer_migrate);
 
 #endif /* CONFIG_SMP */
 
diff --git a/kernel/drivers/testing/switchtest.c 
b/kernel/drivers/testing/switchtest.c
index 235c0e5..2be9a5c 100644
--- a/kernel/drivers/testing/switchtest.c
+++ b/kernel/drivers/testing/switchtest.c
@@ -476,25 +476,16 @@ static int rtswitch_create_ktask(rtswitch_context_t *ctx,
 
        init_flags = (ptask->flags & RTTST_SWTEST_FPU) ? XNFPU : 0;
 
-       /*
-        * Migrate the calling thread to the same CPU as the created
-        * task, in order to be sure that the created task is
-        * suspended when this function returns. This also allow us to
-        * use the stack to pass the parameters to the created
-        * task.
-        */
-       set_cpus_allowed(current, cpumask_of_cpu(ctx->cpu));
-
        iattr.name = name;
        iattr.flags = init_flags;
        iattr.personality = &xenomai_personality;
+       iattr.affinity = cpumask_of_cpu(ctx->cpu);
        param.rt.prio = 1;
 
        err = xnthread_init(&task->ktask,
                            &iattr, &xnsched_class_rt, &param);
        if (!err) {
                sattr.mode = 0;
-               sattr.affinity = cpumask_of_cpu(ctx->cpu);
                sattr.entry = rtswitch_ktask;
                sattr.cookie = &arg;
                err = xnthread_start(&task->ktask, &sattr);
diff --git a/testsuite/latency/latency.c b/testsuite/latency/latency.c
index b06e2eb..7c5099b 100644
--- a/testsuite/latency/latency.c
+++ b/testsuite/latency/latency.c
@@ -522,7 +522,7 @@ static void sigdebug(int sig, siginfo_t *si, void *context)
 int main(int argc, char *const *argv)
 {
        struct sigaction sa __attribute__((unused));
-       int c, err, sig, cpu = 0;
+       int c, err, sig, cpu = -1;
        char task_name[16];
        cpu_set_t cpus;
        sigset_t mask;
@@ -723,8 +723,6 @@ int main(int argc, char *const *argv)
        }
 
        if (test_mode == USER_TASK) {
-               CPU_ZERO(&cpus);
-               CPU_SET(cpu, &cpus);
                snprintf(task_name, sizeof(task_name), "sampling-%d", getpid());
                err =
                    rt_task_create(&latency_task, task_name, 0, priority,
@@ -737,12 +735,16 @@ int main(int argc, char *const *argv)
                        return 0;
                }
 
-               err = rt_task_set_affinity(&latency_task, &cpus);
-               if (err) {
-                       fprintf(stderr,
-                               "latency: failed to set CPU affinity, code 
%d\n",
-                               err);
-                       return 0;
+               if (cpu >= 0) {
+                       CPU_ZERO(&cpus);
+                       CPU_SET(cpu, &cpus);
+                       err = rt_task_set_affinity(&latency_task, &cpus);
+                       if (err) {
+                               fprintf(stderr,
+                                       "latency: failed to set CPU affinity, 
code %d\n",
+                                       err);
+                               return 0;
+                       }
                }
 
                err = rt_task_start(&latency_task, &latency, NULL);


_______________________________________________
Xenomai-git mailing list
Xenomai-git@xenomai.org
http://www.xenomai.org/mailman/listinfo/xenomai-git

Reply via email to