[PATCH 1/5] cobalt/rtdm: add new interface to init task on specified cpu

[PATCH 1/5] cobalt/rtdm: add new interface to init task on specified cpu

[hongzha1]

Initialise and start a real-time task on specified cpu

Signed-off-by: hongzha1 <hongzhan.chen@intel.com>

diff --git a/include/cobalt/kernel/rtdm/driver.h b/include/cobalt/kernel/rtdm/driver.h
index 733c49df8..6cdace324 100644
--- a/include/cobalt/kernel/rtdm/driver.h
+++ b/include/cobalt/kernel/rtdm/driver.h
@@ -1024,6 +1024,9 @@ typedef void (*rtdm_task_proc_t)(void *arg);
 int rtdm_task_init(rtdm_task_t *task, const char *name,
 		   rtdm_task_proc_t task_proc, void *arg,
 		   int priority, nanosecs_rel_t period);
+int rtdm_task_init_on_cpu(rtdm_task_t *task, int cpu, const char *name,
+		rtdm_task_proc_t task_proc, void *arg,
+		int priority, nanosecs_rel_t period);
 int __rtdm_task_sleep(xnticks_t timeout, xntmode_t mode);
 void rtdm_task_busy_sleep(nanosecs_rel_t delay);
 
diff --git a/kernel/cobalt/rtdm/drvlib.c b/kernel/cobalt/rtdm/drvlib.c
index b914fa312..7452850b7 100644
--- a/kernel/cobalt/rtdm/drvlib.c
+++ b/kernel/cobalt/rtdm/drvlib.c
@@ -157,6 +157,79 @@ int rtdm_task_init(rtdm_task_t *task, const char *name,
 
 EXPORT_SYMBOL_GPL(rtdm_task_init);
 
+/**
+ * @brief Initialise and start a real-time task on specified cpu
+ *
+ * After initialising a task, the task handle remains valid and can be
+ * passed to RTDM services until either rtdm_task_destroy() or
+ * rtdm_task_join() was invoked.
+ *
+ * @param[in,out] task Task handle
+ * @param[in] cpu that task want to run on
+ * @param[in] name Optional task name
+ * @param[in] task_proc Procedure to be executed by the task
+ * @param[in] arg Custom argument passed to @c task_proc() on entry
+ * @param[in] priority Priority of the task, see also
+ * @ref rtdmtaskprio "Task Priority Range"
+ * @param[in] period Period in nanoseconds of a cyclic task, 0 for non-cyclic
+ * mode. Waiting for the first and subsequent periodic events is
+ * done using rtdm_task_wait_period().
+ *
+ * @return 0 on success, otherwise negative error code
+ *
+ * @coretags{secondary-only, might-switch}
+ */
+int rtdm_task_init_on_cpu(rtdm_task_t *task, int cpu, const char *name,
+		rtdm_task_proc_t task_proc, void *arg,
+		int priority, nanosecs_rel_t period)
+{
+	union xnsched_policy_param param;
+	struct xnthread_start_attr sattr;
+	struct xnthread_init_attr iattr;
+	int err;
+
+	if (!realtime_core_enabled())
+		return -ENOSYS;
+
+	iattr.name = name;
+	iattr.flags = 0;
+	iattr.personality = &xenomai_personality;
+	iattr.affinity = *cpumask_of(cpu);
+	param.rt.prio = priority;
+
+	err = xnthread_init(task, &iattr, &xnsched_class_rt, &param);
+	if (err)
+		return err;
+
+	/* We need an anonymous registry entry to obtain a handle for fast
+	 * mutex locking.
+	 */
+	err = xnthread_register(task, "");
+	if (err)
+		goto cleanup_out;
+
+	if (period > 0) {
+		err = xnthread_set_periodic(task, XN_INFINITE,
+					    XN_RELATIVE, period);
+		if (err)
+			goto cleanup_out;
+	}
+
+	sattr.mode = 0;
+	sattr.entry = task_proc;
+	sattr.cookie = arg;
+	err = xnthread_start(task, &sattr);
+	if (err)
+		goto cleanup_out;
+
+	return 0;
+
+cleanup_out:
+	xnthread_cancel(task);
+	return err;
+}
+EXPORT_SYMBOL_GPL(rtdm_task_init_on_cpu);
+
 #ifdef DOXYGEN_CPP /* Only used for doxygen doc generation */
 /**
  * @brief Destroy a real-time task
-- 
2.17.1

[PATCH 2/5] cobalt/rtdm: add interface to init timer on specified cpu

[hongzha1]

Initialise timer on specified cpu

Signed-off-by: hongzha1 <hongzhan.chen@intel.com>

diff --git a/include/cobalt/kernel/rtdm/driver.h b/include/cobalt/kernel/rtdm/driver.h
index 6cdace324..6a431e7c5 100644
--- a/include/cobalt/kernel/rtdm/driver.h
+++ b/include/cobalt/kernel/rtdm/driver.h
@@ -966,6 +966,9 @@ enum rtdm_timer_mode {
 int rtdm_timer_init(rtdm_timer_t *timer, rtdm_timer_handler_t handler,
 		    const char *name);
 
+int rtdm_timer_init_on_cpu(rtdm_timer_t *timer, rtdm_timer_handler_t handler,
+		    const char *name, int cpu);
+
 void rtdm_timer_destroy(rtdm_timer_t *timer);
 
 int rtdm_timer_start(rtdm_timer_t *timer, nanosecs_abs_t expiry,
diff --git a/kernel/cobalt/rtdm/drvlib.c b/kernel/cobalt/rtdm/drvlib.c
index 7452850b7..e6145b353 100644
--- a/kernel/cobalt/rtdm/drvlib.c
+++ b/kernel/cobalt/rtdm/drvlib.c
@@ -601,6 +601,34 @@ int rtdm_timer_init(rtdm_timer_t *timer, rtdm_timer_handler_t handler,
 
 EXPORT_SYMBOL_GPL(rtdm_timer_init);
 
+/**
+ * @brief Initialise a timer on specified cpu
+ *
+ * @param[in,out] timer Timer handle
+ * @param[in] handler Handler to be called on timer expiry
+ * @param[in] name Optional timer name
+ * @param[in] cpu that run on
+ *
+ * @return 0 on success, otherwise negative error code
+ *
+ * @coretags{task-unrestricted}
+ */
+int rtdm_timer_init_on_cpu(rtdm_timer_t *timer, rtdm_timer_handler_t handler,
+		    const char *name, int cpu)
+{
+	struct xnsched *sched = xnsched_struct(cpu);
+
+	if (!realtime_core_enabled())
+		return -ENOSYS;
+
+	sched = xnsched_struct(cpu);
+	xntimer_init((timer), &nkclock, handler, sched, XNTIMER_IGRAVITY);
+	xntimer_set_name((timer), (name));
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rtdm_timer_init_on_cpu);
+
 /**
  * @brief Destroy a timer
  *
-- 
2.17.1

[PATCH 3/5] rtdm/testing: latmus: introduce latmus driver

[hongzha1]

Signed-off-by: hongzha1 <hongzhan.chen@intel.com>

diff --git a/include/rtdm/uapi/testing.h b/include/rtdm/uapi/testing.h
index f8207b8c7..b1723b9f8 100644
--- a/include/rtdm/uapi/testing.h
+++ b/include/rtdm/uapi/testing.h
@@ -118,6 +118,52 @@ struct rttst_heap_stathdr {
 	struct rttst_heap_stats *buf;
 };
 
+/* Latmus context types. */
+#define COBALT_LAT_IRQ   0
+#define COBALT_LAT_KERN  1
+#define COBALT_LAT_USER  2
+#define COBALT_LAT_SIRQ  3
+#define COBALT_LAT_LAST	COBALT_LAT_SIRQ
+
+struct latmus_setup {
+	__u32 type;
+	__u64 period;
+	__s32 priority;
+	__u32 cpu;
+	union {
+		struct {
+			__u32 verbosity;
+		} tune;
+		struct {
+			__u32 xfd;
+			__u32 hcells;
+		} measure;
+	} u;
+};
+
+/*
+ * The measurement record which the driver sends to userland each
+ * second through an xbuf channel.
+ */
+struct latmus_measurement {
+	__s64 sum_lat;
+	__s32 min_lat;
+	__s32 max_lat;
+	__u32 overruns;
+	__u32 samples;
+};
+
+struct latmus_measurement_result {
+	__u64 last_ptr;		/* (struct latmus_measurement __user *last) */
+	__u64 histogram_ptr;	/* (__s32 __user *histogram) */
+	__u32 len;
+};
+
+struct latmus_result {
+	__u64 data_ptr;		/* (void __user *data) */
+	__u32 len;
+};
+
 #define RTIOC_TYPE_TESTING		RTDM_CLASS_TESTING
 
 /*!
@@ -133,6 +179,8 @@ struct rttst_heap_stathdr {
 #define RTDM_SUBCLASS_RTDMTEST		3
 /** subclase name: "heapcheck" */
 #define RTDM_SUBCLASS_HEAPCHECK		4
+/** subclase name: "latmus" */
+#define RTDM_SUBCLASS_LATMUS		5
 /** @} */
 
 /*!
@@ -193,6 +241,21 @@ struct rttst_heap_stathdr {
 #define RTTST_RTIOC_HEAP_STAT_COLLECT \
 	_IOR(RTIOC_TYPE_TESTING, 0x45, int)
 
+#define RTTST_RTIOC_COBALT_LATIOC_TUNE \
+	_IOWR(RTIOC_TYPE_TESTING, 0x50, struct latmus_setup)
+
+#define RTTST_RTIOC_COBALT_LATIOC_MEASURE \
+	_IOWR(RTIOC_TYPE_TESTING, 0x51, struct latmus_setup)
+
+#define RTTST_RTIOC_COBALT_LATIOC_RUN \
+	_IOR(RTIOC_TYPE_TESTING, 0x52, struct latmus_result)
+
+#define RTTST_RTIOC_COBALT_LATIOC_PULSE \
+	_IOW(RTIOC_TYPE_TESTING, 0x53, __u64)
+
+#define RTTST_RTIOC_COBALT_LATIOC_RESET	\
+	_IO(RTIOC_TYPE_TESTING, 0x54)
+
 /** @} */
 
 #endif /* !_RTDM_UAPI_TESTING_H */
diff --git a/kernel/drivers/testing/Kconfig b/kernel/drivers/testing/Kconfig
index 88c043c82..5478dfe97 100644
--- a/kernel/drivers/testing/Kconfig
+++ b/kernel/drivers/testing/Kconfig
@@ -26,4 +26,14 @@ config XENO_DRIVERS_RTDMTEST
 	help
 	Kernel driver for performing RTDM unit tests.
 
+config COBALT_LATMUS
+	bool "Timer latency calibration and measurement"
+	depends on XENOMAI
+	default y
+	help
+	  This driver supports the latmus application for
+	  determining the best gravity values for the cobalt core
+	  clock, and measuring the response time to timer events.
+	  If in doubt, say 'Y'.
+
 endmenu
diff --git a/kernel/drivers/testing/Makefile b/kernel/drivers/testing/Makefile
index 09b076348..cad4ad50d 100644
--- a/kernel/drivers/testing/Makefile
+++ b/kernel/drivers/testing/Makefile
@@ -3,6 +3,7 @@ obj-$(CONFIG_XENO_DRIVERS_TIMERBENCH) += xeno_timerbench.o
 obj-$(CONFIG_XENO_DRIVERS_SWITCHTEST) += xeno_switchtest.o
 obj-$(CONFIG_XENO_DRIVERS_RTDMTEST)   += xeno_rtdmtest.o
 obj-$(CONFIG_XENO_DRIVERS_HEAPCHECK)   += xeno_heapcheck.o
+obj-$(CONFIG_COBALT_LATMUS)		+= latmus.o
 
 xeno_timerbench-y := timerbench.o
 
diff --git a/kernel/drivers/testing/latmus.c b/kernel/drivers/testing/latmus.c
new file mode 100644
index 000000000..bef662260
--- /dev/null
+++ b/kernel/drivers/testing/latmus.c
@@ -0,0 +1,1237 @@
+/*
+ * SPDX-License-Identifier: GPL-2.0
+ *
+ * Derived from Xenomai Cobalt's autotune driver, https://xenomai.org/
+ * Copyright (C) 2014, 2018 Philippe Gerum  <rpm@xenomai.org>
+ */
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/sort.h>
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/fcntl.h>
+#include <cobalt/kernel/pipe.h>
+#include <cobalt/kernel/sched.h>
+#include <rtdm/ipc.h>
+#include <rtdm/testing.h>
+#include <rtdm/driver.h>
+#include <rtdm/compat.h>
+
+#define ONE_BILLION  1000000000
+#define TUNER_SAMPLING_TIME	500000000UL
+#define TUNER_WARMUP_STEPS	10
+#define TUNER_RESULT_STEPS	40
+
+#define progress(__runner, __fmt, __args...)				\
+	do {								\
+		if ((__runner)->verbosity > 1)				\
+			printk(XENO_INFO "latmus(%s) " __fmt "\n",	\
+			       (__runner)->name, ##__args);		\
+	} while (0)
+
+#define cobalt_init_xntimer_on_cpu(__timer, __cpu, __handler)		\
+		rtdm_timer_init_on_cpu(__timer, __handler,		\
+			#__handler, __cpu)
+
+struct latmus_message {
+	struct xnpipe_mh mh;
+	char data[];
+};
+
+struct tuning_score {
+	int pmean;
+	int stddev;
+	int minlat;
+	unsigned int step;
+	unsigned int gravity;
+};
+
+struct runner_state {
+	ktime_t ideal;
+	int offset;
+	int min_lat;
+	int max_lat;
+	int allmax_lat;
+	int prev_mean;
+	int prev_sqs;
+	int cur_sqs;
+	int sum;
+	unsigned int overruns;
+	unsigned int cur_samples;
+	unsigned int max_samples;
+};
+
+struct latmus_runner {
+	const char *name;
+	unsigned int (*get_gravity)(struct latmus_runner *runner);
+	void (*set_gravity)(struct latmus_runner *runner, unsigned int gravity);
+	unsigned int (*adjust_gravity)(struct latmus_runner *runner, int adjust);
+	int (*start)(struct latmus_runner *runner, ktime_t start_time);
+	void (*stop)(struct latmus_runner *runner);
+	void (*destroy)(struct latmus_runner *runner);
+	int (*add_sample)(struct latmus_runner *runner, ktime_t timestamp);
+	int (*run)(struct rtdm_fd *fd, struct latmus_runner *runner,
+			struct latmus_result *result);
+	void (*cleanup)(struct latmus_runner *runner);
+	struct runner_state state;
+	struct rtdm_event done;
+	int status;
+	int verbosity;
+	ktime_t period;
+	union {
+		struct {
+			struct tuning_score scores[TUNER_RESULT_STEPS];
+			int nscores;
+		};
+		struct {
+			unsigned int warmup_samples;
+			unsigned int warmup_limit;
+			int socketfd;
+			void *xbuf;
+			u32 hcells;
+			s32 *histogram;
+		};
+	};
+};
+
+struct irq_runner {
+	rtdm_timer_t timer;
+	struct latmus_runner runner;
+};
+
+struct kthread_runner {
+	rtdm_task_t kthread;
+	struct rtdm_event barrier;
+	ktime_t start_time;
+	struct latmus_runner runner;
+};
+
+struct uthread_runner {
+	rtdm_timer_t timer;
+	struct rtdm_event pulse;
+	struct latmus_runner runner;
+};
+
+struct sirq_runner {
+	int sirq;
+	struct sirq_runner * __percpu *sirq_percpu;
+	rtdm_timer_t timer;
+	struct latmus_runner runner;
+};
+
+struct latmus_state {
+	struct latmus_runner *runner;
+	rtdm_lock_t latmus_lock;
+};
+
+static inline void init_runner_base(struct latmus_runner *runner)
+{
+	rtdm_event_init(&runner->done, 0);
+	runner->status = 0;
+	runner->socketfd = -1;
+}
+
+static inline void destroy_runner_base(struct latmus_runner *runner)
+{
+	rtdm_event_destroy(&runner->done);
+	if (runner->cleanup)
+		runner->cleanup(runner);
+}
+
+static inline void done_sampling(struct latmus_runner *runner,
+				 int status)
+{
+	runner->status = status;
+	rtdm_event_signal(&runner->done);
+}
+
+static void send_measurement(struct latmus_runner *runner)
+{
+	struct runner_state *state = &runner->state;
+	struct latmus_measurement *meas;
+	struct latmus_message *mbuf;
+	int len;
+
+	len = sizeof(*meas) + sizeof(*mbuf);
+	mbuf = xnmalloc(len);
+	if (mbuf == NULL)
+		return;
+
+	meas = (struct latmus_measurement *)mbuf->data;
+	meas->min_lat = state->min_lat;
+	meas->max_lat = state->max_lat;
+	meas->sum_lat = state->sum;
+	meas->overruns = state->overruns;
+	meas->samples = state->cur_samples;
+
+	runner->xbuf = mbuf;
+	xnpipe_send(runner->socketfd, &mbuf->mh, len, XNPIPE_NORMAL);
+
+	/* Reset counters for next round. */
+	state->min_lat = INT_MAX;
+	state->max_lat = INT_MIN;
+	state->sum = 0;
+	state->overruns = 0;
+	state->cur_samples = 0;
+}
+
+static int add_measurement_sample(struct latmus_runner *runner,
+				  ktime_t timestamp)
+{
+	struct runner_state *state = &runner->state;
+	ktime_t period = runner->period;
+	int delta, cell, offset_delta;
+
+	/* Skip samples in warmup time. */
+	if (runner->warmup_samples < runner->warmup_limit) {
+		runner->warmup_samples++;
+		state->ideal = ktime_add(state->ideal, period);
+		return 0;
+	}
+
+	delta = (int)ktime_to_ns(ktime_sub(timestamp, state->ideal));
+	offset_delta = delta - state->offset;
+	if (offset_delta < state->min_lat)
+		state->min_lat = offset_delta;
+	if (offset_delta > state->max_lat)
+		state->max_lat = offset_delta;
+	if (offset_delta > state->allmax_lat) {
+		state->allmax_lat = offset_delta;
+		//trace_evl_latspot(offset_delta);
+		//trace_evl_trigger("latmus");
+	}
+
+	if (runner->histogram) {
+		cell = (offset_delta < 0 ? -offset_delta : offset_delta) / 1000; /* us */
+		if (cell >= runner->hcells)
+			cell = runner->hcells - 1;
+		runner->histogram[cell]++;
+	}
+
+	state->sum += offset_delta;
+	state->ideal = ktime_add(state->ideal, period);
+
+	while (delta > 0 &&
+		(unsigned int)delta > ktime_to_ns(period)) { /* period > 0 */
+		state->overruns++;
+		state->ideal = ktime_add(state->ideal, period);
+		delta -= ktime_to_ns(period);
+	}
+
+	if (++state->cur_samples >= state->max_samples)
+		send_measurement(runner);
+
+	return 0;	/* Always keep going. */
+}
+
+static int add_tuning_sample(struct latmus_runner *runner,
+			     ktime_t timestamp)
+{
+	struct runner_state *state = &runner->state;
+	int n, delta, cur_mean;
+
+	delta = (int)ktime_to_ns(ktime_sub(timestamp, state->ideal));
+	if (delta < state->min_lat)
+		state->min_lat = delta;
+	if (delta > state->max_lat)
+		state->max_lat = delta;
+	if (delta < 0)
+		delta = 0;
+
+	state->sum += delta;
+	state->ideal = ktime_add(state->ideal, runner->period);
+	n = ++state->cur_samples;
+
+	/* TAOCP (Vol 2), single-pass computation of variance. */
+	if (n == 1)
+		state->prev_mean = delta;
+	else {
+		cur_mean = state->prev_mean + (delta - state->prev_mean) / n;
+		state->cur_sqs = state->prev_sqs + (delta - state->prev_mean)
+			* (delta - cur_mean);
+		state->prev_mean = cur_mean;
+		state->prev_sqs = state->cur_sqs;
+	}
+
+	if (n >= state->max_samples) {
+		done_sampling(runner, 0);
+		return 1;	/* Finished. */
+	}
+
+	return 0;	/* Keep going. */
+}
+
+static void latmus_irq_handler(rtdm_timer_t *timer) /* hard irqs off */
+{
+	struct irq_runner *irq_runner;
+	ktime_t now;
+
+	irq_runner = container_of(timer, struct irq_runner, timer);
+	now = xnclock_read_raw(&nkclock);
+	if (irq_runner->runner.add_sample(&irq_runner->runner, now))
+		rtdm_timer_stop(timer);
+}
+
+static void destroy_irq_runner(struct latmus_runner *runner)
+{
+	struct irq_runner *irq_runner;
+
+	irq_runner = container_of(runner, struct irq_runner, runner);
+	rtdm_timer_destroy(&irq_runner->timer);
+	destroy_runner_base(runner);
+	kfree(irq_runner);
+}
+
+static unsigned int get_irq_gravity(struct latmus_runner *runner)
+{
+	return nkclock.gravity.irq;
+}
+
+static void set_irq_gravity(struct latmus_runner *runner, unsigned int gravity)
+{
+	nkclock.gravity.irq = gravity;
+}
+
+static unsigned int adjust_irq_gravity(struct latmus_runner *runner, int adjust)
+{
+	return nkclock.gravity.irq += adjust;
+}
+
+static int start_irq_runner(struct latmus_runner *runner,
+			    ktime_t start_time)
+{
+	struct irq_runner *irq_runner;
+
+	irq_runner = container_of(runner, struct irq_runner, runner);
+
+	rtdm_timer_start(&irq_runner->timer, start_time,
+				runner->period, RTDM_TIMERMODE_ABSOLUTE);
+
+	return 0;
+}
+
+static void stop_irq_runner(struct latmus_runner *runner)
+{
+	struct irq_runner *irq_runner;
+
+	irq_runner = container_of(runner, struct irq_runner, runner);
+
+	rtdm_timer_stop(&irq_runner->timer);
+}
+
+static struct latmus_runner *create_irq_runner(int cpu)
+{
+	struct irq_runner *irq_runner;
+
+	irq_runner = kzalloc(sizeof(*irq_runner), GFP_KERNEL);
+	if (irq_runner == NULL)
+		return NULL;
+
+	irq_runner->runner = (struct latmus_runner){
+		.name = "irqhand",
+		.destroy = destroy_irq_runner,
+		.get_gravity = get_irq_gravity,
+		.set_gravity = set_irq_gravity,
+		.adjust_gravity = adjust_irq_gravity,
+		.start = start_irq_runner,
+		.stop = stop_irq_runner,
+	};
+
+	init_runner_base(&irq_runner->runner);
+	cobalt_init_xntimer_on_cpu(&irq_runner->timer, cpu, latmus_irq_handler);
+
+	return &irq_runner->runner;
+}
+
+static irqreturn_t latmus_sirq_handler(int sirq, void *dev_id)
+{
+	struct sirq_runner * __percpu *self_percpu = dev_id;
+	struct sirq_runner *sirq_runner = *self_percpu;
+	ktime_t now;
+
+	now = xnclock_read_raw(&nkclock);
+	if (sirq_runner->runner.add_sample(&sirq_runner->runner, now))
+		rtdm_timer_stop(&sirq_runner->timer);
+
+	return IRQ_HANDLED;
+}
+
+static void latmus_sirq_timer_handler(rtdm_timer_t *timer) /* hard irqs off */
+{
+	struct sirq_runner *sirq_runner;
+	struct runner_state *state;
+	ktime_t now;
+
+	now = xnclock_read_raw(&nkclock);
+	sirq_runner = container_of(timer, struct sirq_runner, timer);
+	state = &sirq_runner->runner.state;
+	state->offset = (int)ktime_to_ns(ktime_sub(now, state->ideal));
+	irq_post_inband(sirq_runner->sirq);
+}
+
+static void destroy_sirq_runner(struct latmus_runner *runner)
+{
+	struct sirq_runner *sirq_runner;
+
+	sirq_runner = container_of(runner, struct sirq_runner, runner);
+	rtdm_timer_destroy(&sirq_runner->timer);
+	free_percpu_irq(sirq_runner->sirq, sirq_runner->sirq_percpu);
+	free_percpu(sirq_runner->sirq_percpu);
+	irq_dispose_mapping(sirq_runner->sirq);
+	destroy_runner_base(runner);
+	kfree(sirq_runner);
+}
+
+static int start_sirq_runner(struct latmus_runner *runner,
+			ktime_t start_time)
+{
+	struct sirq_runner *sirq_runner;
+
+	sirq_runner = container_of(runner, struct sirq_runner, runner);
+
+	rtdm_timer_start(&sirq_runner->timer, start_time,
+				runner->period, RTDM_TIMERMODE_ABSOLUTE);
+
+	return 0;
+}
+
+static void stop_sirq_runner(struct latmus_runner *runner)
+{
+	struct sirq_runner *sirq_runner;
+
+	sirq_runner = container_of(runner, struct sirq_runner, runner);
+
+	rtdm_timer_stop(&sirq_runner->timer);
+}
+
+static struct latmus_runner *create_sirq_runner(int cpu)
+{
+	struct sirq_runner * __percpu *sirq_percpu;
+	struct sirq_runner *sirq_runner;
+	int sirq, ret, _cpu;
+
+	sirq_percpu = alloc_percpu(struct sirq_runner *);
+	if (sirq_percpu == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	sirq_runner = kzalloc(sizeof(*sirq_runner), GFP_KERNEL);
+	if (sirq_runner == NULL) {
+		free_percpu(sirq_percpu);
+		return NULL;
+	}
+
+	sirq = irq_create_direct_mapping(synthetic_irq_domain);
+	if (sirq == 0) {
+		free_percpu(sirq_percpu);
+		kfree(sirq_runner);
+		return ERR_PTR(-EAGAIN);
+	}
+
+	sirq_runner->runner = (struct latmus_runner){
+		.name = "sirqhand",
+		.destroy = destroy_sirq_runner,
+		.start = start_sirq_runner,
+		.stop = stop_sirq_runner,
+	};
+	sirq_runner->sirq = sirq;
+	sirq_runner->sirq_percpu = sirq_percpu;
+	init_runner_base(&sirq_runner->runner);
+	cobalt_init_xntimer_on_cpu(&sirq_runner->timer, cpu,
+			latmus_sirq_timer_handler);
+
+	for_each_possible_cpu(_cpu)
+		*per_cpu_ptr(sirq_percpu, _cpu) = sirq_runner;
+
+	ret = __request_percpu_irq(sirq, latmus_sirq_handler,
+				IRQF_NO_THREAD,
+				"latmus sirq",
+				sirq_percpu);
+	if (ret) {
+		rtdm_timer_destroy(&sirq_runner->timer);
+		irq_dispose_mapping(sirq);
+		free_percpu(sirq_percpu);
+		kfree(sirq_runner);
+		return ERR_PTR(ret);
+	}
+
+	return &sirq_runner->runner;
+}
+
+void kthread_handler(void *arg)
+{
+	struct kthread_runner *k_runner = arg;
+	ktime_t now;
+	int ret = 0;
+
+	for (;;) {
+		if (rtdm_task_should_stop())
+			break;
+
+		ret = rtdm_event_wait(&k_runner->barrier);
+		if (ret)
+			break;
+
+		ret = rtdm_task_set_period(&k_runner->kthread,
+					k_runner->start_time,
+					k_runner->runner.period);
+		if (ret)
+			break;
+
+		for (;;) {
+			ret = rtdm_task_wait_period(NULL);
+			if (ret && ret != -ETIMEDOUT)
+				goto out;
+
+			now = xnclock_read_raw(&nkclock);
+			if (k_runner->runner.add_sample(&k_runner->runner, now)) {
+				rtdm_task_set_period(&k_runner->kthread, 0, 0);
+				break;
+			}
+		}
+	}
+out:
+	done_sampling(&k_runner->runner, ret);
+	rtdm_task_destroy(&k_runner->kthread);
+}
+
+static void destroy_kthread_runner(struct latmus_runner *runner)
+{
+	struct kthread_runner *k_runner;
+
+	k_runner = container_of(runner, struct kthread_runner, runner);
+	rtdm_task_destroy(&k_runner->kthread);
+	rtdm_event_destroy(&k_runner->barrier);
+	destroy_runner_base(runner);
+	kfree(k_runner);
+}
+
+static unsigned int get_kthread_gravity(struct latmus_runner *runner)
+{
+	return nkclock.gravity.kernel;
+}
+
+static void
+set_kthread_gravity(struct latmus_runner *runner, unsigned int gravity)
+{
+	nkclock.gravity.kernel = gravity;
+}
+
+static unsigned int
+adjust_kthread_gravity(struct latmus_runner *runner, int adjust)
+{
+	return nkclock.gravity.kernel += adjust;
+}
+
+static int start_kthread_runner(struct latmus_runner *runner,
+				ktime_t start_time)
+{
+	struct kthread_runner *k_runner;
+
+	k_runner = container_of(runner, struct kthread_runner, runner);
+
+	k_runner->start_time = start_time;
+	rtdm_event_signal(&k_runner->barrier);
+
+	return 0;
+}
+
+static struct latmus_runner *
+create_kthread_runner(int priority, int cpu)
+{
+	struct kthread_runner *k_runner;
+	int ret;
+	char *taskname;
+
+	k_runner = kzalloc(sizeof(*k_runner), GFP_KERNEL);
+	if (k_runner == NULL)
+		return NULL;
+
+	k_runner->runner = (struct latmus_runner){
+		.name = "kthread",
+		.destroy = destroy_kthread_runner,
+		.get_gravity = get_kthread_gravity,
+		.set_gravity = set_kthread_gravity,
+		.adjust_gravity = adjust_kthread_gravity,
+		.start = start_kthread_runner,
+	};
+
+	init_runner_base(&k_runner->runner);
+	rtdm_event_init(&k_runner->barrier, 0);
+
+	taskname = kasprintf(GFP_KERNEL, "latmus-klat:%d",
+			task_pid_nr(current));
+	ret = rtdm_task_init_on_cpu(&k_runner->kthread, cpu,
+				taskname,
+				kthread_handler,
+				k_runner,
+				priority,
+				0);
+	kfree(taskname);
+	if (ret) {
+		kfree(k_runner);
+		return ERR_PTR(ret);
+	}
+
+	return &k_runner->runner;
+}
+
+static void latmus_pulse_handler(rtdm_timer_t *timer)
+{
+	struct uthread_runner *u_runner;
+
+	u_runner = container_of(timer, struct uthread_runner, timer);
+	rtdm_event_signal(&u_runner->pulse);
+}
+
+static void destroy_uthread_runner(struct latmus_runner *runner)
+{
+	struct uthread_runner *u_runner;
+
+	u_runner = container_of(runner, struct uthread_runner, runner);
+	rtdm_timer_destroy(&u_runner->timer);
+	rtdm_event_destroy(&u_runner->pulse);
+	destroy_runner_base(runner);
+	kfree(u_runner);
+}
+
+static unsigned int get_uthread_gravity(struct latmus_runner *runner)
+{
+	return nkclock.gravity.user;
+}
+
+static void set_uthread_gravity(struct latmus_runner *runner,
+				unsigned int gravity)
+{
+	nkclock.gravity.user = gravity;
+}
+
+static unsigned int
+adjust_uthread_gravity(struct latmus_runner *runner, int adjust)
+{
+	return nkclock.gravity.user += adjust;
+}
+
+static int start_uthread_runner(struct latmus_runner *runner,
+				ktime_t start_time)
+{
+	struct uthread_runner *u_runner;
+
+	u_runner = container_of(runner, struct uthread_runner, runner);
+
+	rtdm_timer_start(&u_runner->timer, start_time,
+				runner->period, RTDM_TIMERMODE_ABSOLUTE);
+
+	return 0;
+}
+
+static void stop_uthread_runner(struct latmus_runner *runner)
+{
+	struct uthread_runner *u_runner;
+
+	u_runner = container_of(runner, struct uthread_runner, runner);
+
+	rtdm_timer_stop(&u_runner->timer);
+}
+
+static int add_uthread_sample(struct latmus_runner *runner,
+			      ktime_t user_timestamp)
+{
+	struct uthread_runner *u_runner;
+	int ret;
+
+	u_runner = container_of(runner, struct uthread_runner, runner);
+
+	if (user_timestamp &&
+	    u_runner->runner.add_sample(runner, user_timestamp)) {
+		rtdm_timer_stop(&u_runner->timer);
+		/* Tell the caller to park until next round. */
+		ret = -EPIPE;
+	} else {
+		ret = rtdm_event_wait(&u_runner->pulse);
+	}
+
+	return ret;
+}
+
+static struct latmus_runner *create_uthread_runner(int cpu)
+{
+	struct uthread_runner *u_runner;
+
+	u_runner = kzalloc(sizeof(*u_runner), GFP_KERNEL);
+	if (u_runner == NULL)
+		return NULL;
+
+	u_runner->runner = (struct latmus_runner){
+		.name = "uthread",
+		.destroy = destroy_uthread_runner,
+		.get_gravity = get_uthread_gravity,
+		.set_gravity = set_uthread_gravity,
+		.adjust_gravity = adjust_uthread_gravity,
+		.start = start_uthread_runner,
+		.stop = stop_uthread_runner,
+	};
+
+	init_runner_base(&u_runner->runner);
+	cobalt_init_xntimer_on_cpu(&u_runner->timer, cpu, latmus_pulse_handler);
+	xntimer_set_gravity(&u_runner->timer, XNTIMER_UGRAVITY);
+	rtdm_event_init(&u_runner->pulse, 0);
+
+	return &u_runner->runner;
+}
+
+static inline void build_score(struct latmus_runner *runner, int step)
+{
+	struct runner_state *state = &runner->state;
+	unsigned int variance, n;
+
+	n = state->cur_samples;
+	runner->scores[step].pmean = state->sum / n;
+	variance = n > 1 ? state->cur_sqs / (n - 1) : 0;
+	runner->scores[step].stddev = int_sqrt(variance);
+	runner->scores[step].minlat = state->min_lat;
+	runner->scores[step].gravity = runner->get_gravity(runner);
+	runner->scores[step].step = step;
+	runner->nscores++;
+}
+
+static int cmp_score_mean(const void *c, const void *r)
+{
+	const struct tuning_score *sc = c, *sr = r;
+
+	return sc->pmean - sr->pmean;
+}
+
+static int cmp_score_stddev(const void *c, const void *r)
+{
+	const struct tuning_score *sc = c, *sr = r;
+
+	return sc->stddev - sr->stddev;
+}
+
+static int cmp_score_minlat(const void *c, const void *r)
+{
+	const struct tuning_score *sc = c, *sr = r;
+
+	return sc->minlat - sr->minlat;
+}
+
+static int cmp_score_gravity(const void *c, const void *r)
+{
+	const struct tuning_score *sc = c, *sr = r;
+
+	return sc->gravity - sr->gravity;
+}
+
+static int filter_mean(struct latmus_runner *runner)
+{
+	sort(runner->scores, runner->nscores,
+	     sizeof(struct tuning_score),
+	     cmp_score_mean, NULL);
+
+	/* Top half of the best pondered means. */
+
+	return (runner->nscores + 1) / 2;
+}
+
+static int filter_stddev(struct latmus_runner *runner)
+{
+	sort(runner->scores, runner->nscores,
+	     sizeof(struct tuning_score),
+	     cmp_score_stddev, NULL);
+
+	/* Top half of the best standard deviations. */
+
+	return (runner->nscores + 1) / 2;
+}
+
+static int filter_minlat(struct latmus_runner *runner)
+{
+	sort(runner->scores, runner->nscores,
+	     sizeof(struct tuning_score),
+	     cmp_score_minlat, NULL);
+
+	/* Top half of the minimum latencies. */
+
+	return (runner->nscores + 1) / 2;
+}
+
+static int filter_gravity(struct latmus_runner *runner)
+{
+	sort(runner->scores, runner->nscores,
+	     sizeof(struct tuning_score),
+	     cmp_score_gravity, NULL);
+
+	/* Smallest gravity required among the shortest latencies. */
+
+	return runner->nscores;
+}
+
+static void dump_scores(struct latmus_runner *runner)
+{
+	int n;
+
+	if (runner->verbosity < 2)
+		return;
+
+	for (n = 0; n < runner->nscores; n++)
+		printk(XENO_INFO
+		       ".. S%.2d pmean=%d stddev=%d minlat=%d gravity=%u\n",
+		       runner->scores[n].step,
+		       runner->scores[n].pmean,
+		       runner->scores[n].stddev,
+		       runner->scores[n].minlat,
+		       runner->scores[n].gravity);
+}
+
+static inline void filter_score(struct latmus_runner *runner,
+				int (*filter)(struct latmus_runner *runner))
+{
+	runner->nscores = filter(runner);
+	dump_scores(runner);
+}
+
+static void __latmus_free_handler(void *buf, void *skarg) /* nklock free */
+{
+	struct latmus_runner *runner = skarg;
+
+	xnfree(runner->xbuf);
+}
+
+static int measure_continously(struct latmus_runner *runner)
+{
+	struct runner_state *state = &runner->state;
+	ktime_t period = runner->period;
+	int ret;
+
+	state->max_samples = ONE_BILLION / (int)ktime_to_ns(period);
+	runner->add_sample = add_measurement_sample;
+	state->min_lat = INT_MAX;
+	state->max_lat = INT_MIN;
+	state->allmax_lat = INT_MIN;
+	state->sum = 0;
+	state->overruns = 0;
+	state->cur_samples = 0;
+	state->offset = 0;	/* for SIRQ latency only. */
+	state->ideal = ktime_add(xnclock_read_raw(&nkclock), period);
+
+	ret = runner->start(runner, state->ideal);
+	if (ret)
+		goto out;
+
+	ret = rtdm_event_wait(&runner->done) ?: runner->status;
+
+	if (runner->stop)
+		runner->stop(runner);
+out:
+	return ret;
+}
+
+static int tune_gravity(struct latmus_runner *runner)
+{
+	struct runner_state *state = &runner->state;
+	int ret, step, gravity_limit, adjust;
+	ktime_t period = runner->period;
+	unsigned int orig_gravity;
+
+	state->max_samples = TUNER_SAMPLING_TIME / (int)ktime_to_ns(period);
+	orig_gravity = runner->get_gravity(runner);
+	runner->add_sample = add_tuning_sample;
+	runner->set_gravity(runner, 0);
+	runner->nscores = 0;
+	adjust = 500; /* Gravity adjustment step */
+	gravity_limit = 0;
+	progress(runner, "warming up...");
+
+	for (step = 0; step < TUNER_WARMUP_STEPS + TUNER_RESULT_STEPS; step++) {
+		state->ideal = ktime_add_ns(xnclock_read_raw(&nkclock),
+			    ktime_to_ns(period) * TUNER_WARMUP_STEPS);
+		state->min_lat = INT_MAX;
+		state->max_lat = INT_MIN;
+		state->prev_mean = 0;
+		state->prev_sqs = 0;
+		state->cur_sqs = 0;
+		state->sum = 0;
+		state->cur_samples = 0;
+
+		ret = runner->start(runner, state->ideal);
+		if (ret)
+			goto fail;
+
+		/* Runner stops when posting. */
+		ret = rtdm_event_wait(&runner->done);
+		if (ret)
+			goto fail;
+
+		ret = runner->status;
+		if (ret)
+			goto fail;
+
+		if (step < TUNER_WARMUP_STEPS) {
+			if (state->min_lat > gravity_limit) {
+				gravity_limit = state->min_lat;
+				progress(runner, "gravity limit set to %u ns (%d)",
+					 gravity_limit, state->min_lat);
+			}
+			continue;
+		}
+
+		if (state->min_lat < 0) {
+			if (runner->get_gravity(runner) < -state->min_lat) {
+				printk(XENO_WARNING
+				       "latmus(%s) failed with early shot (%d ns)\n",
+				       runner->name,
+				       -(runner->get_gravity(runner) + state->min_lat));
+				ret = -EAGAIN;
+				goto fail;
+			}
+			break;
+		}
+
+		if (((step - TUNER_WARMUP_STEPS) % 5) == 0)
+			progress(runner, "calibrating... (slice %d)",
+				 (step - TUNER_WARMUP_STEPS) / 5 + 1);
+
+		build_score(runner, step - TUNER_WARMUP_STEPS);
+
+		/*
+		 * Anticipating by more than the minimum latency
+		 * detected at warmup would make no sense: cap the
+		 * gravity we may try.
+		 */
+		if (runner->adjust_gravity(runner, adjust) > gravity_limit) {
+			progress(runner, "beyond gravity limit at %u ns",
+				 runner->get_gravity(runner));
+			break;
+		}
+	}
+
+	progress(runner, "calibration scores");
+	dump_scores(runner);
+	progress(runner, "pondered mean filter");
+	filter_score(runner, filter_mean);
+	progress(runner, "standard deviation filter");
+	filter_score(runner, filter_stddev);
+	progress(runner, "minimum latency filter");
+	filter_score(runner, filter_minlat);
+	progress(runner, "gravity filter");
+	filter_score(runner, filter_gravity);
+	runner->set_gravity(runner, runner->scores[0].gravity);
+
+	return 0;
+fail:
+	runner->set_gravity(runner, orig_gravity);
+
+	return ret;
+}
+
+static int setup_tuning(struct latmus_runner *runner,
+			struct latmus_setup *setup)
+{
+	runner->verbosity = setup->u.tune.verbosity;
+	runner->period = setup->period;
+
+	return 0;
+}
+
+static int run_tuning(struct rtdm_fd *fd,
+		struct latmus_runner *runner,
+		struct latmus_result *result)
+{
+	__u32 gravity;
+	int ret;
+
+	ret = tune_gravity(runner);
+	if (ret)
+		return ret;
+
+	gravity = runner->get_gravity(runner);
+
+	if (rtdm_safe_copy_to_user(fd, (void *)(long)result->data_ptr,
+				&gravity, sizeof(gravity)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int setup_measurement(struct latmus_runner *runner,
+			     struct latmus_setup *setup)
+{
+	struct xnpipe_operations ops;
+	int xnpipefd;
+
+	memset(&ops, 0, sizeof(ops));
+	ops.free_obuf = &__latmus_free_handler;
+	xnpipefd = xnpipe_connect(-1, &ops, runner);
+	if (xnpipefd < 0)
+		return xnpipefd;
+	runner->socketfd = xnpipefd;
+	runner->period = setup->period;
+	runner->warmup_limit = ONE_BILLION / (int)ktime_to_ns(setup->period); /* 1s warmup */
+	runner->histogram = NULL;
+	runner->hcells = setup->u.measure.hcells;
+	if (runner->hcells == 0)
+		return 0;
+
+	if (runner->hcells > 1000) /* LART */
+		return -EINVAL;
+
+	runner->histogram = kzalloc(runner->hcells * sizeof(s32),
+				    GFP_KERNEL);
+
+	return runner->histogram ? 0 : -ENOMEM;
+}
+
+static int run_measurement(struct rtdm_fd *fd,
+			struct latmus_runner *runner,
+			struct latmus_result *result)
+{
+	struct runner_state *state = &runner->state;
+	struct latmus_measurement_result mr;
+	struct latmus_measurement last;
+	size_t len;
+	int ret;
+
+	if (result->len != sizeof(mr))
+		return -EINVAL;
+
+	if (rtdm_safe_copy_from_user(fd, &mr, (void *)(long)result->data_ptr,
+				sizeof(mr)))
+		return -EFAULT;
+
+	ret = measure_continously(runner);
+	if (ret != -EINTR)
+		return ret;
+
+	/*
+	 * Copy the last bulk of consolidated measurements and the
+	 * histogram distribution data back to userland.
+	 */
+	last.min_lat = state->min_lat;
+	last.max_lat = state->max_lat;
+	last.sum_lat = state->sum;
+	last.overruns = state->overruns;
+	last.samples = state->cur_samples;
+	if (rtdm_safe_copy_to_user(fd, (void *)(long)mr.last_ptr,
+				&last, sizeof(last)))
+		return -EFAULT;
+
+	if (runner->histogram) {
+		len = runner->hcells * sizeof(s32);
+		if (len > mr.len)
+			len = result->len;
+		if (len > 0 &&
+		    rtdm_safe_copy_to_user(fd, (void *)(long)mr.histogram_ptr,
+					   runner->histogram, len))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static void cleanup_measurement(struct latmus_runner *runner)
+{
+	if (runner->socketfd >= 0) {
+		xnpipe_disconnect(runner->socketfd);
+		runner->socketfd = -1;
+	}
+
+	if (runner->histogram)
+		kfree(runner->histogram);
+}
+
+static int latmus_ioctl(struct rtdm_fd *fd, unsigned int cmd, void *arg)
+{
+	struct latmus_state *ls = rtdm_fd_to_private(fd);
+	int (*setup)(struct latmus_runner *runner,
+		     struct latmus_setup *setup_data);
+	int (*run)(struct rtdm_fd *fd,
+		struct latmus_runner *runner,
+		struct latmus_result *result);
+	void (*cleanup)(struct latmus_runner *runner);
+	struct latmus_setup setup_data;
+	struct latmus_runner *runner;
+	int ret;
+
+	if (cmd == RTTST_RTIOC_COBALT_LATIOC_RESET) {
+		xnclock_reset_gravity(&nkclock);
+		return 0;
+	}
+
+	/* All other cmds require a setup struct to be passed. */
+
+	if (rtdm_copy_from_user(fd, &setup_data, arg, sizeof(setup_data)))
+		return -EFAULT;
+
+	if (setup_data.type == COBALT_LAT_SIRQ &&
+			cmd != RTTST_RTIOC_COBALT_LATIOC_MEASURE)
+		return -EINVAL;
+
+	switch (cmd) {
+	case RTTST_RTIOC_COBALT_LATIOC_TUNE:
+		setup = setup_tuning;
+		run = run_tuning;
+		cleanup = NULL;
+		break;
+	case RTTST_RTIOC_COBALT_LATIOC_MEASURE:
+		setup = setup_measurement;
+		run = run_measurement;
+		cleanup = cleanup_measurement;
+		break;
+	default:
+		return -ENOSYS;
+	}
+
+	if (setup_data.period <= 0 ||
+	    setup_data.period > ONE_BILLION)
+		return -EINVAL;
+
+	if (setup_data.priority < 1 ||
+	    setup_data.priority > XNSCHED_FIFO_MAX_PRIO)
+		return -EINVAL;
+
+	if (setup_data.cpu >= num_possible_cpus() ||
+		!xnsched_supported_cpu(setup_data.cpu))
+		return -EINVAL;
+
+	/* Clear previous runner. */
+	runner = ls->runner;
+	if (runner) {
+		runner->destroy(runner);
+		ls->runner = NULL;
+	}
+
+	switch (setup_data.type) {
+	case COBALT_LAT_IRQ:
+		runner = create_irq_runner(setup_data.cpu);
+		break;
+	case COBALT_LAT_KERN:
+		runner = create_kthread_runner(setup_data.priority,
+					       setup_data.cpu);
+		break;
+	case COBALT_LAT_USER:
+		runner = create_uthread_runner(setup_data.cpu);
+		break;
+	case COBALT_LAT_SIRQ:
+		runner = create_sirq_runner(setup_data.cpu);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (IS_ERR(runner))
+		return PTR_ERR(runner);
+
+	ret = setup(runner, &setup_data);
+	if (ret) {
+		runner->destroy(runner);
+		return ret;
+	}
+
+	runner->run = run;
+	runner->cleanup = cleanup;
+	ls->runner = runner;
+
+	return ((cmd == RTTST_RTIOC_COBALT_LATIOC_MEASURE) ?
+			runner->socketfd : 0);
+}
+
+static int latmus_oob_ioctl(struct rtdm_fd *fd, unsigned int cmd, void *arg)
+{
+	struct latmus_state *ls = rtdm_fd_to_private(fd);
+	struct latmus_runner *runner;
+	struct latmus_result result;
+	__u64 timestamp;
+	int ret;
+
+	switch (cmd) {
+	case RTTST_RTIOC_COBALT_LATIOC_RUN:
+		runner = ls->runner;
+		if (runner == NULL)
+			return -EAGAIN;
+		ret = rtdm_safe_copy_from_user(fd, &result,
+				arg, sizeof(result));
+
+		if (ret)
+			return -EFAULT;
+		ret = runner->run(fd, runner, &result);
+		break;
+	case RTTST_RTIOC_COBALT_LATIOC_PULSE:
+		runner = ls->runner;
+		if (runner == NULL)
+			return -EAGAIN;
+		if (runner->start != start_uthread_runner)
+			return -EINVAL;
+		ret = rtdm_safe_copy_from_user(fd,  &timestamp,
+						arg, sizeof(timestamp));
+		if (ret)
+			return -EFAULT;
+		ret = add_uthread_sample(runner, ns_to_ktime(timestamp));
+		break;
+	default:
+		ret = -ENOSYS;
+	}
+
+	return ret;
+}
+
+static int latmus_open(struct rtdm_fd *fd, int oflags)
+{
+	struct latmus_state *ls;
+
+	ls = rtdm_fd_to_private(fd);
+	ls->runner = NULL;
+	rtdm_lock_init(&ls->latmus_lock);
+
+	return 0;
+}
+
+static void latmus_close(struct rtdm_fd *fd)
+{
+	struct latmus_state *ls = rtdm_fd_to_private(fd);
+	struct latmus_runner *runner;
+
+	runner = ls->runner;
+	if (runner)
+		runner->destroy(runner);
+
+}
+
+static struct rtdm_driver latmus_driver = {
+	.profile_info		= RTDM_PROFILE_INFO(latmus,
+						    RTDM_CLASS_TESTING,
+						    RTDM_SUBCLASS_LATMUS,
+						    RTTST_PROFILE_VER),
+	.device_flags		= RTDM_NAMED_DEVICE | RTDM_EXCLUSIVE,
+	.device_count		= 1,
+	.context_size		= sizeof(struct latmus_state),
+	.ops = {
+		.open		= latmus_open,
+		.close		= latmus_close,
+		.ioctl_rt	= latmus_oob_ioctl,
+		.ioctl_nrt	= latmus_ioctl,
+	},
+};
+
+static struct rtdm_device device = {
+	.driver = &latmus_driver,
+	.label = "latmus",
+};
+
+static int __init __latmus_init(void)
+{
+	return rtdm_dev_register(&device);
+}
+
+static void __latmus_exit(void)
+{
+	rtdm_dev_unregister(&device);
+}
+
+module_init(__latmus_init);
+module_exit(__latmus_exit);
+
+MODULE_LICENSE("GPL");
-- 
2.17.1

[PATCH 4/5] drivers/gpio: core: introduce helper to find gpiochip

[hongzha1]

To find gpiochip for non-OF platforms like x86

Signed-off-by: hongzha1 <hongzhan.chen@intel.com>

diff --git a/include/cobalt/kernel/rtdm/gpio.h b/include/cobalt/kernel/rtdm/gpio.h
index 00055ec0a..1d1e67bf7 100644
--- a/include/cobalt/kernel/rtdm/gpio.h
+++ b/include/cobalt/kernel/rtdm/gpio.h
@@ -61,6 +61,9 @@ int rtdm_gpiochip_add_by_name(struct rtdm_gpio_chip *rgc,
 int rtdm_gpiochip_post_event(struct rtdm_gpio_chip *rgc,
 			     unsigned int offset);
 
+int rtdm_gpiochip_find(struct device_node *from,
+			  void *compat, int type);
+
 #ifdef CONFIG_OF
 
 int rtdm_gpiochip_scan_of(struct device_node *from,
diff --git a/kernel/drivers/gpio/gpio-core.c b/kernel/drivers/gpio/gpio-core.c
index 600ef9789..7ab989e94 100644
--- a/kernel/drivers/gpio/gpio-core.c
+++ b/kernel/drivers/gpio/gpio-core.c
@@ -554,6 +554,34 @@ static int match_gpio_chip(struct gpio_chip *gc, void *data)
 	return 0;
 }
 
+static int chip_match_name(struct gpio_chip *chip, void *data)
+{
+	return !strncmp(chip->label, data, strlen(data));
+}
+
+int rtdm_gpiochip_find(struct device_node *from, void *compat,
+			  int type)
+{
+	struct rtdm_gpio_chip *rgc;
+	struct gpio_chip *chip;
+	int ret = -ENODEV;
+
+	if (!rtdm_available())
+		return -ENOSYS;
+
+	chip = gpiochip_find(compat, chip_match_name);
+	if (chip == NULL)
+		return ret;
+
+	ret = 0;
+	rgc = rtdm_gpiochip_alloc(chip, type);
+	if (IS_ERR(rgc))
+		ret = PTR_ERR(rgc);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(rtdm_gpiochip_find);
+
 int rtdm_gpiochip_scan_of(struct device_node *from, const char *compat,
 			  int type)
 {
-- 
2.17.1

[PATCH 5/5] testsuite/latmus: introduce latmus app

[hongzha1]

Signed-off-by: hongzha1 <hongzhan.chen@intel.com>

diff --git a/configure.ac b/configure.ac
index abe538dbd..8416bc013 100644
--- a/configure.ac
+++ b/configure.ac
@@ -954,6 +954,7 @@ AC_CONFIG_FILES([ \
 	lib/trank/Makefile \
 	testsuite/Makefile \
 	testsuite/latency/Makefile \
+	testsuite/latmus/Makefile \
 	testsuite/switchtest/Makefile \
 	testsuite/gpiotest/Makefile \
 	testsuite/gpiobench/Makefile \
diff --git a/include/rtdm/uapi/testing.h b/include/rtdm/uapi/testing.h
index b1723b9f8..d7e2707e1 100644
--- a/include/rtdm/uapi/testing.h
+++ b/include/rtdm/uapi/testing.h
@@ -256,6 +256,22 @@ struct latmus_result {
 #define RTTST_RTIOC_COBALT_LATIOC_RESET	\
 	_IO(RTIOC_TYPE_TESTING, 0x54)
 
+#define LATMON_NET_PORT 2306
+
+struct latmon_net_request {
+	uint32_t period_usecs;
+	uint32_t histogram_cells;
+} __attribute__((__packed__));
+
+struct latmon_net_data {
+	int32_t sum_lat_hi;
+	int32_t sum_lat_lo;
+	int32_t min_lat;
+	int32_t max_lat;
+	uint32_t overruns;
+	uint32_t samples;
+} __attribute__((__packed__));
+
 /** @} */
 
 #endif /* !_RTDM_UAPI_TESTING_H */
diff --git a/testsuite/Makefile.am b/testsuite/Makefile.am
index 4932f6d33..4baff2f19 100644
--- a/testsuite/Makefile.am
+++ b/testsuite/Makefile.am
@@ -1,10 +1,11 @@
 
-SUBDIRS = latency smokey gpiobench
+SUBDIRS = latency latmus smokey gpiobench
 
 if XENO_COBALT
 SUBDIRS += 		\
 	clocktest	\
 	gpiotest	\
+	latmus		\
 	spitest		\
 	switchtest	\
 	xeno-test
@@ -15,6 +16,7 @@ DIST_SUBDIRS =		\
 	gpiotest	\
 	gpiobench   \
 	latency		\
+	latmus		\
 	smokey		\
 	spitest		\
 	switchtest	\
diff --git a/testsuite/latmus/Makefile.am b/testsuite/latmus/Makefile.am
new file mode 100644
index 000000000..8eab3d740
--- /dev/null
+++ b/testsuite/latmus/Makefile.am
@@ -0,0 +1,18 @@
+testdir = @XENO_TEST_DIR@
+
+CCLD = $(top_srcdir)/scripts/wrap-link.sh $(CC)
+
+test_PROGRAMS = latmus
+
+latmus_SOURCES = latmus.c
+
+latmus_CPPFLAGS = 		\
+	$(XENO_USER_CFLAGS)	\
+	-I$(top_srcdir)/include
+
+latmus_LDFLAGS = @XENO_AUTOINIT_LDFLAGS@ $(XENO_POSIX_WRAPPERS)
+
+latmus_LDADD =			\
+	@XENO_CORE_LDADD@		\
+	@XENO_USER_LDADD@		\
+	-lpthread -lrt -lm
diff --git a/testsuite/latmus/latmus.c b/testsuite/latmus/latmus.c
new file mode 100644
index 000000000..e1abba216
--- /dev/null
+++ b/testsuite/latmus/latmus.c
@@ -0,0 +1,1444 @@
+/*
+ * SPDX-License-Identifier: MIT
+ *
+ * Derived from Xenomai Cobalt's latency & autotune utilities
+ * (http://git.xenomai.org/xenomai-3.git/)
+ * Copyright (C) 2014 Gilles Chanteperdrix <gch@xenomai.org>
+ * Copyright (C) 2018-2020 Philippe Gerum  <rpm@xenomai.org>
+ */
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <getopt.h>
+#include <netdb.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <signal.h>
+#include <pthread.h>
+#include <semaphore.h>
+#include <limits.h>
+#include <time.h>
+#include <string.h>
+#include <memory.h>
+#include <poll.h>
+#include <signal.h>
+#include <error.h>
+#include <errno.h>
+#include <unistd.h>
+#include <ctype.h>
+#include <sys/ioctl.h>
+#include <arpa/inet.h>
+#include <linux/gpio.h>
+#include <rtdm/testing.h>
+#include <rtdm/gpio.h>
+
+#define COBALT_CPU_OOB      (1 << 0)
+#define COBALT_CPU_ISOL     (1 << 1)
+#define COBALT_CPU_OFFLINE  (1 << 2)
+
+#define ISOLATED_CPU_LIST "/sys/devices/system/cpu/isolated"
+#define OOB_CPU_LIST	"/sys/module/xenomai/parameters/supported_cpus"
+
+static cpu_set_t isolated_cpus;
+
+#define OOB_GPIO_LAT    1
+#define INBAND_GPIO_LAT 2
+
+#define LATMON_TIMEOUT_SECS  5
+
+static int test_irqlat, test_klat,
+	test_ulat, test_sirqlat,
+	test_gpiolat;
+
+static bool reset, background,
+	abort_on_switch = true;
+
+static int verbosity = 1,
+	abort_threshold;
+
+static bool tuning;
+
+static time_t timeout;
+
+static time_t start_time;
+
+static unsigned int period_usecs = 1000; /* 1ms */
+
+static int responder_priority = 98;
+
+static int responder_cpu = -1;
+
+static int responder_cpu_state;
+
+static struct in_addr gpio_monitor_ip;
+
+static sigset_t sigmask;
+
+static int latmus_fd = -1;
+
+static int gpio_infd = -1, gpio_outfd = -1;
+
+static int gpio_inpin, gpio_outpin;
+
+static int gpio_hdinflags = GPIOHANDLE_REQUEST_INPUT,
+	gpio_hdoutflags = GPIOHANDLE_REQUEST_OUTPUT;
+
+static int gpio_evinflags;
+
+static pthread_t responder, logger;
+
+static sem_t logger_done;
+
+static bool c_state_restricted;
+
+static bool force_cpu;
+
+#define short_optlist "ikusrqbKmtp:A:T:v::l:g::H:P:c:Z:z:I:O:C:"
+
+static const struct option options[] = {
+	{
+		.name = "irq",
+		.has_arg = no_argument,
+		.val = 'i'
+	},
+	{
+		.name = "kernel",
+		.has_arg = no_argument,
+		.val = 'k'
+	},
+	{
+		.name = "user",
+		.has_arg = no_argument,
+		.val = 'u'
+	},
+	{
+		.name = "sirq",
+		.has_arg = no_argument,
+		.val = 's'
+	},
+	{
+		.name = "reset",
+		.has_arg = no_argument,
+		.val = 'r'
+	},
+	{
+		.name = "quiet",
+		.has_arg = no_argument,
+		.val = 'q'
+	},
+	{
+		.name = "background",
+		.has_arg = no_argument,
+		.val = 'b'
+	},
+	{
+		.name = "keep-going",
+		.has_arg = no_argument,
+		.val = 'K'
+	},
+	{
+		.name = "measure",
+		.has_arg = no_argument,
+		.val = 'm',
+	},
+	{
+		.name = "tune",
+		.has_arg = no_argument,
+		.val = 't',
+	},
+	{
+		.name = "period",
+		.has_arg = required_argument,
+		.val = 'p',
+	},
+	{
+		.name = "timeout",
+		.has_arg = required_argument,
+		.val = 'T',
+	},
+	{
+		.name = "maxlat-abort",
+		.has_arg = required_argument,
+		.val = 'A',
+	},
+	{
+		.name = "verbose",
+		.has_arg = optional_argument,
+		.val = 'v',
+	},
+	{
+		.name = "lines",
+		.has_arg = required_argument,
+		.val = 'l',
+	},
+	{
+		.name = "plot",
+		.has_arg = optional_argument,
+		.val = 'g',
+	},
+	{
+		.name = "histogram",
+		.has_arg = required_argument,
+		.val = 'H',
+	},
+	{
+		.name = "priority",
+		.has_arg = required_argument,
+		.val = 'P',
+	},
+	{
+		.name = "cpu",
+		.has_arg = required_argument,
+		.val = 'c',
+	},
+	{
+		.name = "force-cpu",
+		.has_arg = required_argument,
+		.val = 'C',
+	},
+	{
+		.name = "oob-gpio",
+		.has_arg = required_argument,
+		.val = 'Z',
+	},
+	{
+		.name = "inband-gpio",
+		.has_arg = required_argument,
+		.val = 'z',
+	},
+	{
+		.name = "gpio-in",
+		.has_arg = required_argument,
+		.val = 'I',
+	},
+	{
+		.name = "gpio-out",
+		.has_arg = required_argument,
+		.val = 'O',
+	},
+	{ /* Sentinel */ }
+};
+
+static inline unsigned int stack_size(unsigned int size)
+{
+	return size > PTHREAD_STACK_MIN ? size : PTHREAD_STACK_MIN;
+}
+
+static void create_responder(pthread_t *tid, void *(*responder)(void *))
+{
+	struct sched_param param;
+	pthread_attr_t attr;
+	int ret;
+
+	pthread_attr_init(&attr);
+	pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
+	pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
+	pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
+	param.sched_priority = responder_priority;
+	pthread_attr_setschedparam(&attr, &param);
+	pthread_attr_setstacksize(&attr,  stack_size(65536));
+	ret = pthread_create(tid, &attr, responder, NULL);
+	pthread_attr_destroy(&attr);
+	if (ret)
+		error(1, ret, "sampling thread");
+}
+
+#define ONE_BILLION	1000000000
+#define TEN_MILLIONS	10000000
+
+static int lat_xfd = -1;
+
+static int lat_sock = -1;
+
+static int data_lines = 21;
+
+static int32_t *histogram;
+
+static unsigned int histogram_cells = 200;
+
+static struct latmus_measurement last_bulk;
+
+static unsigned int all_overruns;
+
+static unsigned int spurious_inband_switches;
+
+static int32_t all_minlat = TEN_MILLIONS, all_maxlat = -TEN_MILLIONS;
+
+static int64_t all_sum;
+
+static int64_t all_samples;
+
+static time_t peak_time;
+
+static FILE *plot_fp;
+
+#define COBALT_LAT_OOB_GPIO      (COBALT_LAT_LAST + 1)
+#define COBALT_LAT_INBAND_GPIO   (COBALT_LAT_OOB_GPIO + 1)
+
+static int context_type = COBALT_LAT_USER;
+
+const char *context_labels[] = {
+	[COBALT_LAT_IRQ] = "irq",
+	[COBALT_LAT_SIRQ] = "sirq",
+	[COBALT_LAT_KERN] = "kernel",
+	[COBALT_LAT_USER] = "user",
+	[COBALT_LAT_OOB_GPIO] = "oob-gpio",
+	[COBALT_LAT_INBAND_GPIO] = "inband-gpio",
+};
+
+static void __log_results(struct latmus_measurement *meas)
+{
+	if (meas->min_lat < all_minlat)
+		all_minlat = meas->min_lat;
+	if (meas->max_lat > all_maxlat) {
+		peak_time = time(NULL) - start_time - 1;
+		all_maxlat = meas->max_lat;
+		if (abort_threshold && all_maxlat > abort_threshold) {
+			fprintf(stderr, "latency threshold is exceeded"
+				" (%d >= %.3f), aborting.\n",
+				abort_threshold,
+				(double)all_maxlat / 1000.0);
+			exit(102);
+		}
+	}
+
+	all_sum += meas->sum_lat;
+	all_samples += meas->samples;
+	all_overruns += meas->overruns;
+}
+
+static void log_results(struct latmus_measurement *meas,
+			unsigned int round)
+{
+	double min, avg, max, best, worst;
+	bool oops = false;
+	time_t now, dt;
+
+	if (verbosity > 0 && data_lines && (round % data_lines) == 0) {
+		time(&now);
+		dt = now - start_time - 1; /* -1s warm-up time */
+		printf("RTT|  %.2ld:%.2ld:%.2ld  (%s, %u us period,",
+			dt / 3600, (dt / 60) % 60, dt % 60,
+			context_labels[context_type], period_usecs);
+		if (context_type != COBALT_LAT_IRQ && context_type != COBALT_LAT_SIRQ)
+			printf(" priority %d,", responder_priority);
+		printf(" CPU%d%s)\n",
+			responder_cpu,
+			responder_cpu_state & COBALT_CPU_ISOL ? "" : "-noisol");
+		printf("RTH|%11s|%11s|%11s|%8s|%6s|%11s|%11s\n",
+		       "----lat min", "----lat avg",
+		       "----lat max", "-overrun", "---msw",
+		       "---lat best", "--lat worst");
+	}
+
+	__log_results(meas);
+	min = (double)meas->min_lat / 1000.0;
+	avg = (double)(meas->sum_lat / (int)meas->samples) / 1000.0;
+	max = (double)meas->max_lat / 1000.0;
+	best = (double)all_minlat / 1000.0;
+	worst = (double)all_maxlat / 1000.0;
+
+	/*
+	 * A trivial check on the reported values, so that we detect
+	 * and stop on obviously inconsistent results.
+	 */
+	if (min > max || min > avg || avg > max ||
+		min > worst || max > worst || avg > worst ||
+		best > worst || worst < best) {
+		oops = true;
+		verbosity = 1;
+	}
+
+	if (verbosity > 0)
+		printf("RTD|%11.3f|%11.3f|%11.3f|%8d|%6u|%11.3f|%11.3f\n",
+			min, avg, max,
+			all_overruns, spurious_inband_switches,
+			best, worst);
+
+	if (oops) {
+		fprintf(stderr, "results look weird, aborting.\n");
+		exit(103);
+	}
+}
+
+static inline void notify_start(void)
+{
+	if (timeout)
+		alarm(timeout + 1); /* +1 warm-up time */
+}
+
+static void create_logger(pthread_t *tid, void *(*logger)(void *), void *arg)
+{
+	struct sched_param param;
+	pthread_attr_t attr;
+	int ret;
+
+	sem_init(&logger_done, 0, 0);
+
+	pthread_attr_init(&attr);
+	pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
+	pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
+	pthread_attr_setschedpolicy(&attr, SCHED_OTHER);
+	param.sched_priority = 0;
+	pthread_attr_setschedparam(&attr, &param);
+	pthread_attr_setstacksize(&attr, stack_size(65536));
+	ret = pthread_create(tid, &attr, logger, arg);
+	pthread_attr_destroy(&attr);
+	if (ret)
+		error(1, ret, "logger thread");
+}
+
+static void *xbuf_logger_thread(void *arg)
+{
+	struct latmus_measurement meas;
+	ssize_t ret, round = 0;
+
+	for (;;) {
+		ret = read(lat_xfd, &meas, sizeof(meas));
+		if (ret != sizeof(meas))
+			break;
+		log_results(&meas, round++);
+	}
+
+	/* Nobody waits for logger_done in timer mode. */
+
+	return NULL;
+}
+
+static void *timer_responder(void *arg)
+{
+	__u64 timestamp = 0;
+	struct timespec now;
+	int ret;
+
+	for (;;) {
+		ret = ioctl(latmus_fd, RTTST_RTIOC_COBALT_LATIOC_PULSE,
+				&timestamp);
+		if (ret) {
+			if (errno != EPIPE)
+				error(1, errno, "pulse failed");
+			timestamp = 0; /* Next period. */
+		} else {
+			clock_gettime(CLOCK_MONOTONIC, &now);
+			timestamp = (__u64)now.tv_sec * 1000000000 + now.tv_nsec;
+		}
+	}
+
+	return NULL;
+}
+
+static void *timer_test_sitter(void *arg)
+{
+	struct latmus_measurement_result mr;
+	struct latmus_result result;
+	int ret;
+
+	mr.last_ptr = (__u64)(uintptr_t)&last_bulk;
+	mr.histogram_ptr = (__u64)(uintptr_t)histogram;
+	mr.len = histogram ? histogram_cells * sizeof(int32_t) : 0;
+
+	result.data_ptr = (__u64)(uintptr_t)&mr;
+	result.len = sizeof(mr);
+
+	notify_start();
+
+	/* Run test until signal. */
+	ret = ioctl(latmus_fd, RTTST_RTIOC_COBALT_LATIOC_RUN, &result);
+	if (ret)
+		error(1, errno, "measurement failed");
+
+	return NULL;
+}
+
+static void setup_measurement_on_timer(void)
+{
+	struct latmus_setup setup;
+	pthread_attr_t attr;
+	pthread_t sitter;
+	int ret, sig;
+	char *rtp;
+
+	memset(&setup, 0, sizeof(setup));
+	setup.type = context_type;
+	setup.period = period_usecs * 1000ULL; /* ns */
+	setup.priority = responder_priority;
+	setup.cpu = responder_cpu;
+	setup.u.measure.hcells = histogram ? histogram_cells : 0;
+	ret = ioctl(latmus_fd, RTTST_RTIOC_COBALT_LATIOC_MEASURE, &setup);
+	if (ret < 0)
+		error(1, errno, "measurement setup failed");
+
+	/*open xnpipe to exchange message*/
+	if (asprintf(&rtp, "/dev/rtp%d", ret) < 0)
+		error(1, ENOMEM, "resolve rtp name");
+	lat_xfd = open(rtp, O_RDWR);
+	if (lat_xfd < 0)
+		error(1, -lat_xfd, "cannot create rtp");
+	free(rtp);
+	create_logger(&logger, xbuf_logger_thread, NULL);
+
+	if (context_type == COBALT_LAT_USER)
+		create_responder(&responder, timer_responder);
+
+	pthread_attr_init(&attr);
+	pthread_attr_setstacksize(&attr, stack_size(65536));
+	ret = pthread_create(&sitter, &attr, timer_test_sitter, NULL);
+	pthread_attr_destroy(&attr);
+	if (ret)
+		error(1, ret, "timer_test_sitter");
+
+	sigwait(&sigmask, &sig);
+	pthread_cancel(sitter);
+	pthread_join(sitter, NULL);
+
+	/*
+	 * Add results from the last incomplete bulk once the sitter
+	 * has returned to user-space from oob_ioctl(COBALT_LATIOC_RUN)
+	 * then exited, at which point such bulk contains meaningful
+	 * data.
+	 */
+	if (last_bulk.samples > 0)
+		__log_results(&last_bulk);
+}
+
+static void setup_gpio_pins(int *fds)
+{
+	int value;
+	int ret;
+
+	ret = ioctl(gpio_infd, GPIO_RTIOC_IRQEN, &gpio_evinflags);
+	if (ret)
+		error(1, errno, "ioctl(GPIO_RTIOC_IRQEN)");
+
+	value = 1;
+	ret = ioctl(gpio_infd, GPIO_RTIOC_TS, &value);
+	if (ret)
+		error(1, errno, "ioctl(GPIO_RTIOC_TS)");
+
+	value = 1;
+	ret = ioctl(gpio_outfd, GPIO_RTIOC_DIR_OUT, &value);
+	if (ret)
+		error(1, errno, "ioctl(GPIO_RTIOC_DIR_OUT)");
+
+	fds[0] = gpio_infd;
+	fds[1] = gpio_outfd;
+}
+
+static void *gpio_responder_thread(void *arg)
+{
+	int value;
+	struct rtdm_gpio_readout rdo;
+	const int ackval = 0;	/* Remote observes falling edges. */
+	int fds[2], ret;
+
+	setup_gpio_pins(fds);
+
+	for (;;) {
+		value = !ackval;
+		ret = write(fds[1], &value, sizeof(value));
+		if (ret < 0)
+			error(1, errno, "write failed");
+
+		ret = read(fds[0], &rdo,
+				sizeof(struct rtdm_gpio_readout));
+		if (ret != sizeof(struct rtdm_gpio_readout))
+			break;
+
+		value = ackval;
+		ret = write(fds[1], &value, sizeof(value));
+		if (ret < 0)
+			error(1, errno, "write failed");
+	}
+
+	return NULL;
+}
+
+static ssize_t read_net_data(void *buf, size_t len)
+{
+	ssize_t count = 0, ret;
+	struct pollfd pollfd;
+
+	pollfd.fd = lat_sock;
+	pollfd.events = POLLIN;
+	pollfd.revents = 0;
+
+	do {
+		/* Make sure to detect latmon unresponsivess. */
+		ret = poll(&pollfd, 1, LATMON_TIMEOUT_SECS * 1000);
+		if (ret <= 0)
+			return -ETIMEDOUT;
+		ret = recv(lat_sock, buf + count, len - count, 0);
+		if (ret <= 0)
+			return ret;
+		count += ret;
+	} while (count < len);
+
+	return count;
+}
+
+static void *sock_logger_thread(void *arg)
+{
+	struct latmus_measurement meas;
+	struct latmon_net_data ndata;
+	bool *no_response = arg;
+	ssize_t ret, round = 0;
+	int cell;
+
+	for (;;) {
+		ret = read_net_data(&ndata, sizeof(ndata));
+		if (ret <= 0)
+			goto unresponsive;
+
+		/*
+		 * Receiving an empty data record means that we got
+		 * the trailing data in the previous round, so
+		 * we are done with sample bulks now.
+		 */
+		if (ndata.samples == 0)
+			break;
+
+		/* This is valid sample data, log it. */
+		meas.sum_lat = ((__s64)ntohl(ndata.sum_lat_hi)) << 32 |
+			ntohl(ndata.sum_lat_lo);
+		meas.min_lat = ntohl(ndata.min_lat);
+		meas.max_lat = ntohl(ndata.max_lat);
+		meas.overruns = ntohl(ndata.overruns);
+		meas.samples = ntohl(ndata.samples);
+		log_results(&meas, round++);
+	}
+
+	if (histogram == NULL)
+		goto out;
+
+	ret = read_net_data(histogram, histogram_cells * sizeof(int32_t));
+	if (ret <= 0) {
+unresponsive:
+		*no_response = true;
+		kill(getpid(), SIGHUP);
+	} else {
+		for (cell = 0; cell < histogram_cells; cell++)
+			histogram[cell] = ntohl(histogram[cell]);
+	}
+out:
+	sem_post(&logger_done);
+
+	return NULL;
+}
+
+static void setup_measurement_on_gpio(void)
+{
+	struct latmon_net_request req;
+	struct sockaddr_in in_addr;
+	bool latmon_hung = false;
+	struct timespec timeout;
+	int ret, sig;
+
+	lat_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+	if (lat_sock < 0)
+		error(1, errno, "socket()");
+
+	if (verbosity)
+		printf("connecting to latmon at %s:%d...\n",
+			inet_ntoa(gpio_monitor_ip), LATMON_NET_PORT);
+
+	memset(&in_addr, 0, sizeof(in_addr));
+	in_addr.sin_family = AF_INET;
+	in_addr.sin_addr = gpio_monitor_ip;
+	in_addr.sin_port = htons(LATMON_NET_PORT);
+	ret = connect(lat_sock, (struct sockaddr *)&in_addr,
+		sizeof(in_addr));
+	if (ret)
+		error(1, errno, "connect()");
+
+	if (verbosity && context_type == COBALT_LAT_INBAND_GPIO)
+		printf("CAUTION: measuring in-band response time (no COBALT there)\n");
+
+	create_responder(&responder, gpio_responder_thread);
+	create_logger(&logger, sock_logger_thread, &latmon_hung);
+
+	notify_start();
+	req.period_usecs = htonl(period_usecs); /* Non-zero, means start. */
+	req.histogram_cells = histogram ? htonl(histogram_cells) : 0;
+	ret = send(lat_sock, &req, sizeof(req), 0);
+	if (ret != sizeof(req))
+		error(1, errno, "send() start");
+
+	sigwait(&sigmask, &sig);
+
+	/*
+	 * From now on, we may wait up to LATMON_TIMEOUT_SECS
+	 * max. between messages from the remote latency monitor
+	 * before declaring it unresponsive.
+	 */
+	if (!latmon_hung) {
+		req.period_usecs = 0; /* Zero means stop. */
+		req.histogram_cells = 0;
+		ret = send(lat_sock, &req, sizeof(req), 0);
+		if (ret != sizeof(req)) {
+			error(1, errno, "send() stop");
+			latmon_hung = true;
+		} else {
+			clock_gettime(CLOCK_REALTIME, &timeout);
+			timeout.tv_sec += LATMON_TIMEOUT_SECS;
+			if (sem_timedwait(&logger_done, &timeout))
+				latmon_hung = true;
+		}
+	}
+
+	if (latmon_hung)
+		error(1, ETIMEDOUT, "latmon at %s is unresponsive",
+			inet_ntoa(gpio_monitor_ip));
+
+	close(lat_sock);
+}
+
+static void paste_file_in(const char *path, const char *header)
+{
+	char buf[BUFSIZ];
+	FILE *fp;
+
+	fp = fopen(path, "r");
+	if (fp == NULL)
+		return;
+
+	fprintf(plot_fp, "# %s", header ?: "");
+
+	while (fgets(buf, sizeof(buf), fp))
+		fputs(buf, plot_fp);
+
+	fclose(fp);
+}
+
+static void dump_gnuplot(time_t duration)
+{
+	int first, last, n;
+
+	if (all_samples == 0)
+		return;
+
+	fprintf(plot_fp, "# test started on: %s", ctime(&start_time));
+	paste_file_in("/proc/version", NULL);
+	paste_file_in("/proc/cmdline", NULL);
+	//fprintf(plot_fp, "# libevl version: %s\n", evl_get_version().version_string);
+	fprintf(plot_fp, "# sampling period: %u microseconds\n", period_usecs);
+	paste_file_in("/sys/devices/virtual/clock/monotonic/gravity",
+		"clock gravity: ");
+	paste_file_in("/sys/devices/system/clocksource/clocksource0/current_clocksource",
+		"clocksource: ");
+	paste_file_in("/sys/devices/system/clocksource/clocksource0/vdso_clocksource",
+		"vDSO access: ");
+	fprintf(plot_fp, "# context: %s\n", context_labels[context_type]);
+	if (!(test_irqlat || test_sirqlat)) {
+		fprintf(plot_fp, "# thread priority: %d\n", responder_priority);
+		fprintf(plot_fp, "# thread affinity: CPU%d%s\n",
+			responder_cpu,
+			responder_cpu_state & COBALT_CPU_ISOL ? "" : "-noisol");
+	}
+	if (c_state_restricted)
+		fprintf(plot_fp, "# C-state restricted\n");
+	fprintf(plot_fp, "# duration (hhmmss): %.2ld:%.2ld:%.2ld\n",
+		duration / 3600, (duration / 60) % 60, duration % 60);
+	fprintf(plot_fp, "# peak (hhmmss): %.2ld:%.2ld:%.2ld\n",
+		peak_time / 3600, (peak_time / 60) % 60, peak_time % 60);
+	if (all_overruns > 0)
+		fprintf(plot_fp, "# OVERRUNS: %u\n", all_overruns);
+	if (spurious_inband_switches > 0)
+		fprintf(plot_fp, "# IN-BAND SWITCHES: %u\n", spurious_inband_switches);
+	fprintf(plot_fp, "# min latency: %.3f\n",
+		(double)all_minlat / 1000.0);
+	fprintf(plot_fp, "# avg latency: %.3f\n",
+		(double)(all_sum / all_samples) / 1000.0);
+	fprintf(plot_fp, "# max latency: %.3f\n",
+		(double)all_maxlat / 1000.0);
+	fprintf(plot_fp, "# sample count: %lld\n",
+		(long long)all_samples);
+
+	for (n = 0; n < histogram_cells && histogram[n] == 0; n++)
+		;
+	first = n;
+
+	for (n = histogram_cells - 1; n >= 0 && histogram[n] == 0; n--)
+		;
+	last = n;
+
+	for (n = first; n < last; n++)
+		fprintf(plot_fp, "%d %d\n", n, histogram[n]);
+
+	/*
+	 * If we have outliers, display a '+' sign after the last cell
+	 * index.
+	 */
+	fprintf(plot_fp, "%d%s %d\n", last,
+		all_maxlat / 1000 >= histogram_cells ? "+" : "",
+		histogram[last]);
+}
+
+static void do_measurement(int type)
+{
+	const char *cpu_s = "";
+	time_t duration;
+
+	context_type = type;
+
+	if (plot_fp) {
+		histogram = malloc(histogram_cells * sizeof(int32_t));
+		if (histogram == NULL)
+			error(1, ENOMEM, "cannot get memory");
+	}
+
+	if (!(responder_cpu_state & COBALT_CPU_ISOL))
+		cpu_s = " (not isolated)";
+
+	if (verbosity > 0)
+		fprintf(stderr, "warming up on CPU%d%s...\n", responder_cpu, cpu_s);
+	else
+		fprintf(stderr, "running quietly for %ld seconds on CPU%d%s\n",
+			timeout, responder_cpu, cpu_s);
+
+	switch (type) {
+	case COBALT_LAT_OOB_GPIO:
+	case COBALT_LAT_INBAND_GPIO:
+		setup_measurement_on_gpio();
+		break;
+	default:
+		setup_measurement_on_timer();
+	}
+
+	duration = time(NULL) - start_time - 1; /* -1s warm-up time */
+	if (plot_fp) {
+		dump_gnuplot(duration);
+		if (plot_fp != stdout)
+			fclose(plot_fp);
+		free(histogram);
+	}
+
+	if (!timeout)
+		timeout = duration;
+
+	if (all_samples > 0)
+		printf("---|-----------|-----------|-----------|--------"
+			"|------|-------------------------\n"
+			"RTS|%11.3f|%11.3f|%11.3f|%8d|%6u|    "
+			"%.2ld:%.2ld:%.2ld/%.2ld:%.2ld:%.2ld\n",
+			(double)all_minlat / 1000.0,
+			(double)(all_sum / all_samples) / 1000.0,
+			(double)all_maxlat / 1000.0,
+			all_overruns, spurious_inband_switches,
+			duration / 3600, (duration / 60) % 60,
+			duration % 60, duration / 3600,
+			(timeout / 60) % 60, timeout % 60);
+
+	if (spurious_inband_switches > 0) {
+		if (all_samples > 0)
+			fputc('\n', stderr);
+		fprintf(stderr, "*** WARNING: unexpected switches to in-band mode detected,\n"
+		       "             latency figures displayed are NOT reliable.\n"
+		       "             Please submit a bug report upstream.\n");
+		if (abort_on_switch) {
+			abort_on_switch = false;
+			fprintf(stderr, "*** OOPS: aborting upon spurious switch to in-band mode.\n");
+		}
+	}
+
+	if (responder)
+		pthread_cancel(responder);
+
+	if (logger)
+		pthread_cancel(logger);
+}
+
+static void do_tuning(int type)
+{
+	struct latmus_result result;
+	struct latmus_setup setup;
+	pthread_t responder;
+	__s32 gravity;
+	int ret;
+
+	if (verbosity) {
+		printf("%s gravity...", context_labels[type]);
+		fflush(stdout);
+	}
+
+	memset(&setup, 0, sizeof(setup));
+	setup.type = type;
+	setup.period = period_usecs * 1000ULL; /* ns */
+	setup.priority = responder_priority;
+	setup.cpu = responder_cpu;
+	setup.u.tune.verbosity = verbosity;
+	ret = ioctl(latmus_fd, RTTST_RTIOC_COBALT_LATIOC_TUNE, &setup);
+	if (ret)
+		error(1, errno, "tuning setup failed (%s)", context_labels[type]);
+
+	if (type == COBALT_LAT_USER)
+		create_responder(&responder, timer_responder);
+
+	pthread_sigmask(SIG_UNBLOCK, &sigmask, NULL);
+
+	notify_start();
+
+	result.data_ptr = (__u64)(uintptr_t)&gravity;
+	result.len = sizeof(gravity);
+	ret = ioctl(latmus_fd, RTTST_RTIOC_COBALT_LATIOC_RUN, &result);
+	if (ret)
+		error(1, errno, "measurement failed");
+
+	if (type == COBALT_LAT_USER)
+		pthread_cancel(responder);
+
+	if (verbosity)
+		printf("%u ns\n", gravity);
+}
+
+#ifdef CONFIG_XENO_COBALT
+
+#include <cobalt/uapi/syscall.h>
+
+static void sigdebug_handler(int sig, siginfo_t *si, void *context)
+{
+	unsigned int reason = sigdebug_reason(si);
+
+	if (reason > SIGDEBUG_WATCHDOG)
+		reason = SIGDEBUG_UNDEFINED;
+
+	switch (reason) {
+	case SIGDEBUG_UNDEFINED:
+	case SIGDEBUG_NOMLOCK:
+	case SIGDEBUG_WATCHDOG:
+		exit(99);
+	}
+
+	spurious_inband_switches++;
+	if (abort_on_switch)
+		kill(getpid(), SIGHUP);
+}
+#endif /* CONFIG_XENO_COBALT */
+
+static void set_cpu_affinity(void)
+{
+	cpu_set_t cpu_set;
+	int ret;
+
+	CPU_ZERO(&cpu_set);
+	CPU_SET(responder_cpu, &cpu_set);
+	ret = sched_setaffinity(0, sizeof(cpu_set), &cpu_set);
+	if (ret)
+		error(1, errno, "cannot set affinity to CPU%d",
+		      responder_cpu);
+}
+
+static void restrict_c_state(void)
+{
+	__s32 val = 0;
+	int fd;
+
+	fd = open("/dev/cpu_dma_latency", O_WRONLY);
+	if (fd < 0)
+		return;
+
+	if (write(fd, &val, sizeof(val) == sizeof(val)))
+		c_state_restricted = true;
+}
+
+static void parse_host_spec(const char *host, struct in_addr *in_addr)
+{
+	struct addrinfo hints, *res;
+	int ret;
+
+	memset(&hints, 0, sizeof(hints));
+	hints.ai_family = AF_INET;
+	hints.ai_socktype = SOCK_STREAM;
+	hints.ai_flags = AI_ADDRCONFIG;
+	ret = getaddrinfo(host, NULL, &hints, &res);
+	if (ret)
+		error(1, ret == EAI_SYSTEM ? errno : ESRCH,
+			"getaddrinfo(%s)", host);
+
+	*in_addr = ((struct sockaddr_in *)res->ai_addr)->sin_addr;
+}
+
+static int parse_gpio_spec(const char *spec, int *pin,
+			int *hdflags, int *evflags)
+{
+	char *s, *dev, *p, *endptr, *devname;
+	int fd, ret;
+
+	s = strdup(spec);
+	dev = strtok(s, ",");
+	if (dev == NULL)
+		error(1, EINVAL, "no GPIO device in spec: %s", spec);
+
+	p = strtok(NULL, ",");
+	if (p == NULL)
+		error(1, EINVAL, "no GPIO pin in spec: %s", spec);
+
+	*pin = (int)strtol(p, &endptr, 10);
+	if (*pin < 0 || endptr == p)
+		error(1, EINVAL, "invalid GPIO pin number in spec: %s",
+			spec);
+
+	ret = asprintf(&devname, "/dev/rtdm/%s/gpio%d", dev, *pin);
+	if (ret < 0)
+		error(1, ENOMEM, "asprintf()");
+
+	p = strtok(NULL, ",");
+	if (evflags) {
+		if (p) {
+			if (!strcmp(p, "rising-edge"))
+				*evflags = GPIO_TRIGGER_EDGE_FALLING;
+			else if  (!strcmp(p, "falling-edge"))
+				*evflags = GPIO_TRIGGER_EDGE_RISING;
+			else
+				error(1, EINVAL, "invalid edge type in spec: %s",
+					spec);
+		} else	/* Default is rising edge. */
+			*evflags = GPIO_TRIGGER_EDGE_RISING;
+	} else if (p)
+		error(1, EINVAL, "trailing garbage in spec: %s",
+			spec);
+
+	fd = open(devname, O_RDONLY);
+	if (fd < 0)
+		error(1, errno, "open(%s)", devname);
+
+	free(devname);
+	free(s);
+
+	return fd;
+}
+
+static int get_realtime_cpu_set(cpu_set_t *cpuset)
+{
+	unsigned long long cpumask;
+	char buf[BUFSIZ], *p;
+	FILE *fp;
+	int cpu;
+
+	fp = fopen("/sys/module/xenomai/parameters/supported_cpus", "r");
+	if (fp == NULL)
+		return -ENOENT;
+
+	p = fgets(buf, sizeof(buf), fp);
+	fclose(fp);
+	if (p == NULL)
+		return -EBADF;
+
+	errno = 0;
+	cpumask = strtoll(p, NULL, 10);
+	if (cpumask == LLONG_MAX && errno == ERANGE)
+		cpumask = ULLONG_MAX;
+	for (cpu = 0; cpumask != 0; cpu++, cpumask >>= 1) {
+		if (cpumask & 1ULL)
+			CPU_SET(cpu, cpuset);
+	}
+
+	return 0;
+}
+
+static void parse_cpu_list(const char *path, cpu_set_t *cpuset)
+{
+	char *p, *range, *range_p = NULL, *id, *id_r;
+	int start, end, cpu;
+	char buf[BUFSIZ];
+	FILE *fp;
+
+	CPU_ZERO(cpuset);
+
+	fp = fopen(path, "r");
+	if (fp == NULL)
+		return;
+
+	if (!fgets(buf, sizeof(buf), fp))
+		goto out;
+
+	p = buf;
+	while ((range = strtok_r(p, ",", &range_p)) != NULL) {
+		if (*range == '\0' || *range == '\n')
+			goto next;
+		end = -1;
+		id = strtok_r(range, "-", &id_r);
+		if (id) {
+			start = atoi(id);
+			id = strtok_r(NULL, "-", &id_r);
+			if (id)
+				end = atoi(id);
+			else if (end < 0)
+				end = start;
+			for (cpu = start; cpu <= end; cpu++)
+				CPU_SET(cpu, cpuset);
+		}
+next:
+		p = NULL;
+	}
+out:
+	fclose(fp);
+}
+
+static void determine_responder_cpu(bool inband_test)
+{
+	cpu_set_t oob_cpus, best_cpus;
+	int cpu;
+
+	parse_cpu_list(ISOLATED_CPU_LIST, &isolated_cpus);
+	get_realtime_cpu_set(&oob_cpus);
+	if (responder_cpu >= 0) {
+		if (!inband_test && !CPU_ISSET(responder_cpu, &oob_cpus)) {
+			if (verbosity)
+				printf("CPU%d is not OOB-capable, "
+					"picking a better one\n",
+					responder_cpu);
+			goto pick_oob;
+		}
+		goto finish;
+	}
+
+	if (force_cpu)
+		goto finish;
+
+	if (inband_test)
+		goto pick_isolated;
+
+	/*
+	 * Pick a default CPU among the ones which are both
+	 * OOB-capable and isolated. If COBALT is not enabled, oob_cpus
+	 * is empty so there is no best choice.
+	 */
+	CPU_AND(&best_cpus, &isolated_cpus, &oob_cpus);
+	for (cpu = 0; cpu < CPU_SETSIZE; cpu++) {
+		if (CPU_ISSET(cpu, &best_cpus)) {
+			responder_cpu = cpu;
+			goto finish;
+		}
+	}
+
+	/*
+	 * If no best choice, pick the first OOB-capable CPU we can
+	 * find (if any).
+	 */
+pick_oob:
+	for (cpu = 0; cpu < CPU_SETSIZE; cpu++) {
+		if (CPU_ISSET(cpu, &oob_cpus)) {
+			responder_cpu = cpu;
+			goto finish;
+		}
+	}
+
+pick_isolated:
+	/*
+	 * This must be a kernel with no COBALT support or we
+	 * specifically need an isolated CPU.
+	 */
+	for (cpu = 0; cpu < CPU_SETSIZE; cpu++) {
+		if (CPU_ISSET(cpu, &isolated_cpus)) {
+			responder_cpu = cpu;
+			goto finish;
+		}
+	}
+
+	/* Out of luck, run on the current CPU. */
+	if (responder_cpu < 0)
+		responder_cpu = sched_getcpu();
+finish:
+	if (CPU_ISSET(responder_cpu, &isolated_cpus))
+		responder_cpu_state = COBALT_CPU_ISOL;
+}
+
+static void usage(void)
+{
+	fprintf(stderr, "usage: latmus [options]:\n");
+	fprintf(stderr, "-m --measure            measure latency on timer event [default]\n");
+	fprintf(stderr, "-t --tune               tune the COBALT core timer\n");
+	fprintf(stderr, "-i --irq                measure/tune interrupt latency\n");
+	fprintf(stderr, "-k --kernel             measure/tune kernel scheduling latency\n");
+	fprintf(stderr, "-u --user               measure/tune user scheduling latency\n");
+	fprintf(stderr, "    [ if none of --irq, --kernel or --user is given,\n"
+			"      tune for all contexts ]\n");
+	fprintf(stderr, "-s --sirq               measure in-band response time to synthetic irq\n");
+	fprintf(stderr, "-p --period=<us>        sampling period\n");
+	fprintf(stderr, "-P --priority=<prio>    responder thread priority [=90]\n");
+	fprintf(stderr, "-c --cpu=<n>            pin responder thread to CPU [=current]\n");
+	fprintf(stderr, "-C --force-cpu=<n>      similar to -c, accept non-isolated CPU\n");
+	fprintf(stderr, "-r --reset              reset core timer gravity to factory default\n");
+	fprintf(stderr, "-b --background         run in the background (daemon mode)\n");
+	fprintf(stderr, "-K --keep-going         keep going on unexpected switch to in-band mode\n");
+	fprintf(stderr, "-A --max-abort=<us>     abort if maximum latency exceeds threshold\n");
+	fprintf(stderr, "-T --timeout=<t>[dhms]  stop measurement after <t> [d(ays)|h(ours)|m(inutes)|s(econds)]\n");
+	fprintf(stderr, "-v --verbose[=level]    set verbosity level [=1]\n");
+	fprintf(stderr, "-q --quiet              quiet mode (i.e. --verbose=0)\n");
+	fprintf(stderr, "-l --lines=<num>        result lines per page, 0 = no pagination [=21]\n");
+	fprintf(stderr, "-H --histogram[=<nr>]   set histogram size to <nr> cells [=200]\n");
+	fprintf(stderr, "-g --plot=<filename>    dump histogram data to file (gnuplot format)\n");
+	fprintf(stderr, "-Z --oob-gpio=<host>    measure COBALT response time to GPIO event via <host>\n");
+	fprintf(stderr, "-z --inband-gpio=<host> measure in-band response time to GPIO event via <host>\n");
+	fprintf(stderr, "-I --gpio-in=<spec>     input GPIO line configuration\n");
+	fprintf(stderr, "   with <spec> = gpiochip-devname,pin-number[,rising-edge|falling-edge]\n");
+	fprintf(stderr, "-O --gpio-out=<spec>    output GPIO line configuration\n");
+	fprintf(stderr, "   with <spec> = gpiochip-devname,pin-number\n");
+}
+
+static void bad_usage(int argc, char *const argv[])
+{
+	int last = optind < argc ? optind : argc - 1;
+
+	printf("** Uh, you lost me somewhere near '%s' (arg #%d)\n", argv[last], last);
+	usage();
+}
+
+int main(int argc, char *const argv[])
+{
+	int ret, c, spec, type, max_prio, lindex;
+	const char *plot_filename = NULL;
+	struct sigaction sa;
+	char *endptr;
+
+	opterr = 0;
+
+	for (;;) {
+		c = getopt_long(argc, argv, short_optlist, options, &lindex);
+		if (c == EOF)
+			break;
+
+		switch (c) {
+		case 0:
+			break;
+		case 'i':
+			test_irqlat = 1;
+			break;
+		case 'k':
+			test_klat = 1;
+			break;
+		case 'u':
+			test_ulat = 1;
+			break;
+		case 's':
+			test_sirqlat = 1;
+			break;
+		case 'r':
+			reset = true;
+			break;
+		case 'q':
+			verbosity = 0;
+			break;
+		case 'b':
+			background = true;
+			break;
+		case 'K':
+			abort_on_switch = false;
+			break;
+		case 'm':
+			tuning = false;
+			break;
+		case 't':
+			tuning = true;
+			break;
+		case 'p':
+			period_usecs = atoi(optarg);
+			if (period_usecs <= 0 || period_usecs > 1000000)
+				error(1, EINVAL, "invalid sampling period "
+				      "(0 < period < 1000000)");
+			break;
+		case 'A':
+			abort_threshold = atoi(optarg) * 1000; /* ns */
+			if (abort_threshold <= 0)
+				error(1, EINVAL, "invalid timeout");
+			break;
+		case 'T':
+			timeout = (int)strtol(optarg, &endptr, 10);
+			if (timeout < 0 || endptr == optarg)
+				error(1, EINVAL, "invalid timeout");
+			switch (*endptr) {
+			case 'd':
+				timeout *= 24;
+				/* Falldown wanted. */
+			case 'h':
+				timeout *= 60;
+				/* Falldown wanted. */
+			case 'm':
+				timeout *= 60;
+				break;
+			case 's':
+			case '\0':
+				break;
+			default:
+				error(1, EINVAL, "invalid time modifier: '%c'",
+					*endptr);
+			}
+			break;
+		case 'v':
+			verbosity = optarg ? atoi(optarg) : 1;
+			break;
+		case 'l':
+			data_lines = atoi(optarg);
+			break;
+		case 'g':
+			if (optarg && strcmp(optarg, "-"))
+				plot_filename = optarg;
+			else
+				plot_fp = stdout;
+			break;
+		case 'H':
+			histogram_cells = atoi(optarg);
+			if (histogram_cells < 1 || histogram_cells > 1000)
+				error(1, EINVAL, "invalid number of histogram cells "
+				      "(0 < cells <= 1000)");
+			break;
+		case 'P':
+			max_prio = sched_get_priority_max(SCHED_FIFO);
+			responder_priority = atoi(optarg);
+			if (responder_priority < 0 || responder_priority > max_prio)
+				error(1, EINVAL, "invalid thread priority "
+				      "(0 < priority < %d)", max_prio);
+			break;
+		case 'C':
+			force_cpu = true;
+			/* fallthrough */
+		case 'c':
+			responder_cpu = atoi(optarg);
+			if (responder_cpu < 0 || responder_cpu >= CPU_SETSIZE)
+				error(1, EINVAL, "invalid CPU number");
+			break;
+		case 'z':
+		case 'Z':
+			test_gpiolat = (c == 'z') + 1;
+			parse_host_spec(optarg, &gpio_monitor_ip);
+			break;
+		case 'I':
+			gpio_infd = parse_gpio_spec(optarg, &gpio_inpin,
+					&gpio_hdinflags, &gpio_evinflags);
+			break;
+		case 'O':
+			gpio_outfd = parse_gpio_spec(optarg, &gpio_outpin,
+					&gpio_hdoutflags, NULL);
+			break;
+		case '?':
+		default:
+			bad_usage(argc, argv);
+			return 1;
+		}
+	}
+
+	if (optind < argc) {
+		bad_usage(argc, argv);
+		return 1;
+	}
+
+	determine_responder_cpu(test_gpiolat == INBAND_GPIO_LAT);
+
+	setlinebuf(stdout);
+	setlinebuf(stderr);
+
+	if (!tuning && !timeout && !verbosity) {
+		fprintf(stderr, "--quiet requires --timeout, ignoring --quiet\n");
+		verbosity = 1;
+	}
+
+	if (background && verbosity) {
+		fprintf(stderr, "--background requires --quiet, taming verbosity down\n");
+		verbosity = 0;
+	}
+
+	if (tuning && (plot_filename || plot_fp)) {
+		fprintf(stderr, "--plot implies --measure, ignoring --plot\n");
+		plot_filename = NULL;
+		plot_fp = NULL;
+	}
+
+	if (background) {
+		signal(SIGHUP, SIG_IGN);
+		ret = daemon(0, 0);
+		if (ret)
+			error(1, errno, "cannot daemonize");
+	}
+
+	set_cpu_affinity();
+	restrict_c_state();
+
+	sigaddset(&sigmask, SIGINT);
+	sigaddset(&sigmask, SIGTERM);
+	sigaddset(&sigmask, SIGHUP);
+	sigaddset(&sigmask, SIGALRM);
+	pthread_sigmask(SIG_BLOCK, &sigmask, NULL);
+
+#ifdef CONFIG_XENO_COBALT
+	sigemptyset(&sa.sa_mask);
+	sa.sa_sigaction = sigdebug_handler;
+	sa.sa_flags = SA_SIGINFO | SA_RESTART;
+	sigaction(SIGDEBUG, &sa, NULL);
+#endif
+
+	spec = test_irqlat || test_klat || test_ulat || test_sirqlat || test_gpiolat;
+	if (!tuning) {
+		if (!spec)
+			test_ulat = 1;
+		else if (test_irqlat + test_klat + test_ulat + test_sirqlat +
+			(!!test_gpiolat) > 1)
+			error(1, EINVAL, "only one of -u, -k, -i, -s, -z or -Z "
+			      "in measurement mode");
+	} else {
+		/* Default to tune for all contexts. */
+		if (!spec)
+			test_irqlat = test_klat = test_ulat = 1;
+		else if (test_sirqlat || test_gpiolat)
+			error(1, EINVAL, "-s/-z and -t are mutually exclusive");
+	}
+
+	if (!test_gpiolat) {
+		latmus_fd = open("/dev/rtdm/latmus", O_RDWR);
+		if (latmus_fd < 0)
+			error(1, errno, "cannot open latmus device");
+
+		if (reset) {
+			ret = ioctl(latmus_fd, RTTST_RTIOC_COBALT_LATIOC_RESET);
+			if (ret)
+				error(1, errno, "reset failed");
+		}
+	} else {
+		if (gpio_infd < 0 || gpio_outfd < 0)
+			error(1, EINVAL, "-[zZ] require -I, -O for GPIO settings");
+		if (test_gpiolat == OOB_GPIO_LAT) {
+			//gpio_hdinflags |= GPIOHANDLE_REQUEST_OOB;
+			//gpio_hdoutflags |= GPIOHANDLE_REQUEST_OOB;
+		}
+	}
+
+	time(&start_time);
+
+	if (!tuning) {
+		if (plot_filename) {
+			if (!access(plot_filename, F_OK))
+				error(1, EINVAL, "declining to overwrite %s",
+				      plot_filename);
+			plot_fp = fopen(plot_filename, "w");
+			if (plot_fp == NULL)
+				error(1, errno, "cannot open %s for writing",
+				      plot_filename);
+		}
+		type = test_irqlat ? COBALT_LAT_IRQ : test_klat ?
+			COBALT_LAT_KERN : test_sirqlat ? COBALT_LAT_SIRQ :
+			test_ulat ? COBALT_LAT_USER :
+			COBALT_LAT_LAST + test_gpiolat;
+		do_measurement(type);
+	} else {
+		if (verbosity)
+			printf("== latmus started for core tuning, "
+			       "period=%d microseconds (may take a while)\n",
+			       period_usecs);
+
+		if (test_irqlat)
+			do_tuning(COBALT_LAT_IRQ);
+
+		if (test_klat)
+			do_tuning(COBALT_LAT_KERN);
+
+		if (test_ulat)
+			do_tuning(COBALT_LAT_USER);
+
+		if (verbosity)
+			printf("== tuning completed after %ds\n",
+			       (int)(time(NULL) - start_time));
+	}
+
+	return 0;
+}
-- 
2.17.1

Re: [PATCH 1/5] cobalt/rtdm: add new interface to init task on specified cpu

[chensong]

why do we need a new interface instead of adding a new parameter(cpu) in 
old interface, in this case rtdm_task_init.

For backward compatibility?

On 2021年02月20日 13:53, hongzha1 via Xenomai wrote:
> Initialise and start a real-time task on specified cpu
>
> Signed-off-by: hongzha1 <hongzhan.chen@intel.com>
>
> diff --git a/include/cobalt/kernel/rtdm/driver.h b/include/cobalt/kernel/rtdm/driver.h
> index 733c49df8..6cdace324 100644
> --- a/include/cobalt/kernel/rtdm/driver.h
> +++ b/include/cobalt/kernel/rtdm/driver.h
> @@ -1024,6 +1024,9 @@ typedef void (*rtdm_task_proc_t)(void *arg);
>   int rtdm_task_init(rtdm_task_t *task, const char *name,
>   		   rtdm_task_proc_t task_proc, void *arg,
>   		   int priority, nanosecs_rel_t period);
> +int rtdm_task_init_on_cpu(rtdm_task_t *task, int cpu, const char *name,
> +		rtdm_task_proc_t task_proc, void *arg,
> +		int priority, nanosecs_rel_t period);
>   int __rtdm_task_sleep(xnticks_t timeout, xntmode_t mode);
>   void rtdm_task_busy_sleep(nanosecs_rel_t delay);
>
> diff --git a/kernel/cobalt/rtdm/drvlib.c b/kernel/cobalt/rtdm/drvlib.c
> index b914fa312..7452850b7 100644
> --- a/kernel/cobalt/rtdm/drvlib.c
> +++ b/kernel/cobalt/rtdm/drvlib.c
> @@ -157,6 +157,79 @@ int rtdm_task_init(rtdm_task_t *task, const char *name,
>
>   EXPORT_SYMBOL_GPL(rtdm_task_init);
>
> +/**
> + * @brief Initialise and start a real-time task on specified cpu
> + *
> + * After initialising a task, the task handle remains valid and can be
> + * passed to RTDM services until either rtdm_task_destroy() or
> + * rtdm_task_join() was invoked.
> + *
> + * @param[in,out] task Task handle
> + * @param[in] cpu that task want to run on
> + * @param[in] name Optional task name
> + * @param[in] task_proc Procedure to be executed by the task
> + * @param[in] arg Custom argument passed to @c task_proc() on entry
> + * @param[in] priority Priority of the task, see also
> + * @ref rtdmtaskprio "Task Priority Range"
> + * @param[in] period Period in nanoseconds of a cyclic task, 0 for non-cyclic
> + * mode. Waiting for the first and subsequent periodic events is
> + * done using rtdm_task_wait_period().
> + *
> + * @return 0 on success, otherwise negative error code
> + *
> + * @coretags{secondary-only, might-switch}
> + */
> +int rtdm_task_init_on_cpu(rtdm_task_t *task, int cpu, const char *name,
> +		rtdm_task_proc_t task_proc, void *arg,
> +		int priority, nanosecs_rel_t period)
> +{
> +	union xnsched_policy_param param;
> +	struct xnthread_start_attr sattr;
> +	struct xnthread_init_attr iattr;
> +	int err;
> +
> +	if (!realtime_core_enabled())
> +		return -ENOSYS;
> +
> +	iattr.name = name;
> +	iattr.flags = 0;
> +	iattr.personality = &xenomai_personality;
> +	iattr.affinity = *cpumask_of(cpu);
> +	param.rt.prio = priority;
> +
> +	err = xnthread_init(task, &iattr, &xnsched_class_rt, &param);
> +	if (err)
> +		return err;
> +
> +	/* We need an anonymous registry entry to obtain a handle for fast
> +	 * mutex locking.
> +	 */
> +	err = xnthread_register(task, "");
> +	if (err)
> +		goto cleanup_out;
> +
> +	if (period > 0) {
> +		err = xnthread_set_periodic(task, XN_INFINITE,
> +					    XN_RELATIVE, period);
> +		if (err)
> +			goto cleanup_out;
> +	}
> +
> +	sattr.mode = 0;
> +	sattr.entry = task_proc;
> +	sattr.cookie = arg;
> +	err = xnthread_start(task, &sattr);
> +	if (err)
> +		goto cleanup_out;
> +
> +	return 0;
> +
> +cleanup_out:
> +	xnthread_cancel(task);
> +	return err;
> +}
> +EXPORT_SYMBOL_GPL(rtdm_task_init_on_cpu);
> +
>   #ifdef DOXYGEN_CPP /* Only used for doxygen doc generation */
>   /**
>    * @brief Destroy a real-time task
>

RE: [PATCH 1/5] cobalt/rtdm: add new interface to init task on specified cpu

[Chen, Hongzhan]

>-----Original Message-----
>From: chensong <chensong@kylinos.cn> 
>Sent: Saturday, February 20, 2021 5:40 PM
>To: Chen, Hongzhan <hongzhan.chen@intel.com>; xenomai@xenomai.org
>Subject: Re: [PATCH 1/5] cobalt/rtdm: add new interface to init task on specified cpu
>
>why do we need a new interface instead of adding a new parameter(cpu) in 
>old interface, in this case rtdm_task_init.
>
>For backward compatibility?

We discussed it about introducing rtdm_task_init_on_cpu in [1]. Actually most of contents of
rtdm_task_init_on_cpu is really same with rtdm_task_init except cpu affinity. But rtdm_task_init 
was already called by several drivers such as net and testing. If we modify definition of 
rtdm_task_init , we have to modify all of these drivers that calling rtdm_task_init. 

[1]: https://xenomai.org/pipermail/xenomai/2021-February/044250.html
>
>On 2021?02?20? 13:53, hongzha1 via Xenomai wrote:
>> Initialise and start a real-time task on specified cpu
>>
>> Signed-off-by: hongzha1 <hongzhan.chen@intel.com>
>>
>> diff --git a/include/cobalt/kernel/rtdm/driver.h b/include/cobalt/kernel/rtdm/driver.h
>> index 733c49df8..6cdace324 100644
>> --- a/include/cobalt/kernel/rtdm/driver.h
>> +++ b/include/cobalt/kernel/rtdm/driver.h
>> @@ -1024,6 +1024,9 @@ typedef void (*rtdm_task_proc_t)(void *arg);
>>   int rtdm_task_init(rtdm_task_t *task, const char *name,
>>   		   rtdm_task_proc_t task_proc, void *arg,
>>   		   int priority, nanosecs_rel_t period);
>> +int rtdm_task_init_on_cpu(rtdm_task_t *task, int cpu, const char *name,
>> +		rtdm_task_proc_t task_proc, void *arg,
>> +		int priority, nanosecs_rel_t period);
>>   int __rtdm_task_sleep(xnticks_t timeout, xntmode_t mode);
>>   void rtdm_task_busy_sleep(nanosecs_rel_t delay);
>>
>> diff --git a/kernel/cobalt/rtdm/drvlib.c b/kernel/cobalt/rtdm/drvlib.c
>> index b914fa312..7452850b7 100644
>> --- a/kernel/cobalt/rtdm/drvlib.c
>> +++ b/kernel/cobalt/rtdm/drvlib.c
>> @@ -157,6 +157,79 @@ int rtdm_task_init(rtdm_task_t *task, const char *name,
>>
>>   EXPORT_SYMBOL_GPL(rtdm_task_init);
>>
>> +/**
>> + * @brief Initialise and start a real-time task on specified cpu
>> + *
>> + * After initialising a task, the task handle remains valid and can be
>> + * passed to RTDM services until either rtdm_task_destroy() or
>> + * rtdm_task_join() was invoked.
>> + *
>> + * @param[in,out] task Task handle
>> + * @param[in] cpu that task want to run on
>> + * @param[in] name Optional task name
>> + * @param[in] task_proc Procedure to be executed by the task
>> + * @param[in] arg Custom argument passed to @c task_proc() on entry
>> + * @param[in] priority Priority of the task, see also
>> + * @ref rtdmtaskprio "Task Priority Range"
>> + * @param[in] period Period in nanoseconds of a cyclic task, 0 for non-cyclic
>> + * mode. Waiting for the first and subsequent periodic events is
>> + * done using rtdm_task_wait_period().
>> + *
>> + * @return 0 on success, otherwise negative error code
>> + *
>> + * @coretags{secondary-only, might-switch}
>> + */
>> +int rtdm_task_init_on_cpu(rtdm_task_t *task, int cpu, const char *name,
>> +		rtdm_task_proc_t task_proc, void *arg,
>> +		int priority, nanosecs_rel_t period)
>> +{
>> +	union xnsched_policy_param param;
>> +	struct xnthread_start_attr sattr;
>> +	struct xnthread_init_attr iattr;
>> +	int err;
>> +
>> +	if (!realtime_core_enabled())
>> +		return -ENOSYS;
>> +
>> +	iattr.name = name;
>> +	iattr.flags = 0;
>> +	iattr.personality = &xenomai_personality;
>> +	iattr.affinity = *cpumask_of(cpu);
>> +	param.rt.prio = priority;
>> +
>> +	err = xnthread_init(task, &iattr, &xnsched_class_rt, &param);
>> +	if (err)
>> +		return err;
>> +
>> +	/* We need an anonymous registry entry to obtain a handle for fast
>> +	 * mutex locking.
>> +	 */
>> +	err = xnthread_register(task, "");
>> +	if (err)
>> +		goto cleanup_out;
>> +
>> +	if (period > 0) {
>> +		err = xnthread_set_periodic(task, XN_INFINITE,
>> +					    XN_RELATIVE, period);
>> +		if (err)
>> +			goto cleanup_out;
>> +	}
>> +
>> +	sattr.mode = 0;
>> +	sattr.entry = task_proc;
>> +	sattr.cookie = arg;
>> +	err = xnthread_start(task, &sattr);
>> +	if (err)
>> +		goto cleanup_out;
>> +
>> +	return 0;
>> +
>> +cleanup_out:
>> +	xnthread_cancel(task);
>> +	return err;
>> +}
>> +EXPORT_SYMBOL_GPL(rtdm_task_init_on_cpu);
>> +
>>   #ifdef DOXYGEN_CPP /* Only used for doxygen doc generation */
>>   /**
>>    * @brief Destroy a real-time task
>>

Re: [PATCH 1/5] cobalt/rtdm: add new interface to init task on specified cpu

[Philippe Gerum]

Chen, Hongzhan via Xenomai <xenomai@xenomai.org> writes:

>>-----Original Message-----
>>From: chensong <chensong@kylinos.cn> 
>>Sent: Saturday, February 20, 2021 5:40 PM
>>To: Chen, Hongzhan <hongzhan.chen@intel.com>; xenomai@xenomai.org
>>Subject: Re: [PATCH 1/5] cobalt/rtdm: add new interface to init task on specified cpu
>>
>>why do we need a new interface instead of adding a new parameter(cpu) in 
>>old interface, in this case rtdm_task_init.
>>
>>For backward compatibility?
>
> We discussed it about introducing rtdm_task_init_on_cpu in [1]. Actually most of contents of
> rtdm_task_init_on_cpu is really same with rtdm_task_init except cpu affinity. But rtdm_task_init 
> was already called by several drivers such as net and testing. If we modify definition of 
> rtdm_task_init , we have to modify all of these drivers that calling rtdm_task_init. 
>
> [1]: https://xenomai.org/pipermail/xenomai/2021-February/044250.html

Agreed. Not to speak of the custom drivers in the field, which would
break at build due to a new prototype of rtdm_task_init().

Besides, adding such a variant matches the mainline kernel naming scheme
(kthread_create vs kthread_create_on_node vs
kthread_create_on_cpu).

-- 
Philippe.

Re: [PATCH 1/5] cobalt/rtdm: add new interface to init task on specified cpu

[Philippe Gerum]

hongzha1 via Xenomai <xenomai@xenomai.org> writes:

> Initialise and start a real-time task on specified cpu
>
> Signed-off-by: hongzha1 <hongzhan.chen@intel.com>
>
> diff --git a/include/cobalt/kernel/rtdm/driver.h b/include/cobalt/kernel/rtdm/driver.h
> index 733c49df8..6cdace324 100644
> --- a/include/cobalt/kernel/rtdm/driver.h
> +++ b/include/cobalt/kernel/rtdm/driver.h
> @@ -1024,6 +1024,9 @@ typedef void (*rtdm_task_proc_t)(void *arg);
>  int rtdm_task_init(rtdm_task_t *task, const char *name,
>  		   rtdm_task_proc_t task_proc, void *arg,
>  		   int priority, nanosecs_rel_t period);
> +int rtdm_task_init_on_cpu(rtdm_task_t *task, int cpu, const char *name,
> +		rtdm_task_proc_t task_proc, void *arg,
> +		int priority, nanosecs_rel_t period);
>  int __rtdm_task_sleep(xnticks_t timeout, xntmode_t mode);
>  void rtdm_task_busy_sleep(nanosecs_rel_t delay);
>  
> diff --git a/kernel/cobalt/rtdm/drvlib.c b/kernel/cobalt/rtdm/drvlib.c
> index b914fa312..7452850b7 100644
> --- a/kernel/cobalt/rtdm/drvlib.c
> +++ b/kernel/cobalt/rtdm/drvlib.c
> @@ -157,6 +157,79 @@ int rtdm_task_init(rtdm_task_t *task, const char *name,
>  
>  EXPORT_SYMBOL_GPL(rtdm_task_init);
>  
> +/**
> + * @brief Initialise and start a real-time task on specified cpu
> + *
> + * After initialising a task, the task handle remains valid and can be
> + * passed to RTDM services until either rtdm_task_destroy() or
> + * rtdm_task_join() was invoked.
> + *
> + * @param[in,out] task Task handle
> + * @param[in] cpu that task want to run on
> + * @param[in] name Optional task name
> + * @param[in] task_proc Procedure to be executed by the task
> + * @param[in] arg Custom argument passed to @c task_proc() on entry
> + * @param[in] priority Priority of the task, see also
> + * @ref rtdmtaskprio "Task Priority Range"
> + * @param[in] period Period in nanoseconds of a cyclic task, 0 for non-cyclic
> + * mode. Waiting for the first and subsequent periodic events is
> + * done using rtdm_task_wait_period().
> + *
> + * @return 0 on success, otherwise negative error code
> + *
> + * @coretags{secondary-only, might-switch}
> + */
> +int rtdm_task_init_on_cpu(rtdm_task_t *task, int cpu, const char *name,
> +		rtdm_task_proc_t task_proc, void *arg,
> +		int priority, nanosecs_rel_t period)
> +{
> +	union xnsched_policy_param param;
> +	struct xnthread_start_attr sattr;
> +	struct xnthread_init_attr iattr;
> +	int err;
> +
> +	if (!realtime_core_enabled())
> +		return -ENOSYS;
> +
> +	iattr.name = name;
> +	iattr.flags = 0;
> +	iattr.personality = &xenomai_personality;
> +	iattr.affinity = *cpumask_of(cpu);

We should probably check cpu for validity first, I don't think
cpumask_of() does it (e.g. cpu_online(cpu) would ensure both validity
and online presence).

> +	param.rt.prio = priority;
> +
> +	err = xnthread_init(task, &iattr, &xnsched_class_rt, &param);
> +	if (err)
> +		return err;
> +
> +	/* We need an anonymous registry entry to obtain a handle for fast
> +	 * mutex locking.
> +	 */
> +	err = xnthread_register(task, "");
> +	if (err)
> +		goto cleanup_out;
> +
> +	if (period > 0) {
> +		err = xnthread_set_periodic(task, XN_INFINITE,
> +					    XN_RELATIVE, period);
> +		if (err)
> +			goto cleanup_out;
> +	}
> +
> +	sattr.mode = 0;
> +	sattr.entry = task_proc;
> +	sattr.cookie = arg;
> +	err = xnthread_start(task, &sattr);
> +	if (err)
> +		goto cleanup_out;
> +
> +	return 0;
> +
> +cleanup_out:
> +	xnthread_cancel(task);
> +	return err;
> +}
> +EXPORT_SYMBOL_GPL(rtdm_task_init_on_cpu);
> +
>  #ifdef DOXYGEN_CPP /* Only used for doxygen doc generation */
>  /**
>   * @brief Destroy a real-time task

Looks good. We may want to implement rtdm_task_init_cpu() as an inline
variant calling rtdm_task_init_on_cpu() with the current CPU number, to
prevent code duplication.

-- 
Philippe.