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

[PATCH 1/3] 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 2/3] 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..c5fb20f25 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,
+			  const char *label, 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..b3d801fa1 100644
--- a/kernel/drivers/gpio/gpio-core.c
+++ b/kernel/drivers/gpio/gpio-core.c
@@ -520,6 +520,29 @@ int rtdm_gpiochip_add_by_name(struct rtdm_gpio_chip *rgc,
 }
 EXPORT_SYMBOL_GPL(rtdm_gpiochip_add_by_name);
 
+int rtdm_gpiochip_find(struct device_node *from, const char *label,
+			  int type)
+{
+	struct rtdm_gpio_chip *rgc;
+	struct gpio_chip *chip;
+	int ret = -ENODEV;
+
+	if (!rtdm_available())
+		return -ENOSYS;
+
+	chip = find_chip_by_name(label);
+	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);
+
 #ifdef CONFIG_OF
 
 #include <linux/of_platform.h>
-- 
2.17.1

[PATCH 3/3] testsuite/latmus: introduce latmus benchmark

[hongzha1]

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

diff --git a/configure.ac b/configure.ac
index bd5fd5ba9..e8516e103 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..016984444
--- /dev/null
+++ b/testsuite/latmus/latmus.c
@@ -0,0 +1,1446 @@
+/*
+ * 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 (max > 1000)
+		exit(103);
+	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/3] rtdm/testing: latmus: introduce latmus driver

[Jan Kiszka]

Missing commit message in this patch, and in patch 3.

Also, could you provide cover letters with your patch series (for series
> 1 patch)? That letter would provide a chance to set the context,
explain potential dependencies.

Does this series already work over next? Or does it have dependencies on
the dovetail patches?

Given that some hardware setup will be needed, a patch adding
documentation would be useful. What does the user have to do to be able
to use the benchmark?

Thanks,
Jan

On 14.04.21 04:16, hongzha1 via Xenomai wrote:
> 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");
> 

-- 
Siemens AG, T RDA IOT
Corporate Competence Center Embedded Linux

RE: [PATCH 1/3] rtdm/testing: latmus: introduce latmus driver

[Chen, Hongzhan]

>
>
>-----Original Message-----
>From: Jan Kiszka <jan.kiszka@siemens.com> 
>Sent: Wednesday, April 14, 2021 5:08 PM
>To: Chen, Hongzhan <hongzhan.chen@intel.com>; xenomai@xenomai.org
>Subject: Re: [PATCH 1/3] rtdm/testing: latmus: introduce latmus driver
>
>Missing commit message in this patch, and in patch 3.
>
>Also, could you provide cover letters with your patch series (for series
>> 1 patch)? That letter would provide a chance to set the context,
>explain potential dependencies.

OK. Thanks for your suggestions.

>
>Does this series already work over next? Or does it have dependencies on
>the dovetail patches?

Yes, I have validated it based on https://lab.xenomai.org/xenomai-rpm.git/log/?h=for-upstream/dovetail
with Rock PI X and FRDM boards. Even though still can not get correct gpio latency because of hardware
 issue that GPIO voltage (1.8 V <> 3.3 V) mismatch between RockPIX and FRDM K64F boards , basic function
works.   It depends on dovetail because it port from evl.

>
>Given that some hardware setup will be needed, a patch adding
>documentation would be useful. What does the user have to do to be able
>to use the benchmark?

Yes, you are right. It also took me long time to setup hardware environment and understand to use it.

>
>Thanks,
>Jan
>
>On 14.04.21 04:16, hongzha1 via Xenomai wrote:
>> Signed-off-by: hongzha1 <hongzhan.chen@intel.com>
>> 
>> diff --git a/include/rtdm/uapi/testing.h b/include/rtdm/uapi/testing.h

Re: [PATCH 1/3] rtdm/testing: latmus: introduce latmus driver

[Philippe Gerum]

Jan Kiszka via Xenomai <xenomai@xenomai.org> writes:

> Missing commit message in this patch, and in patch 3.
>
> Also, could you provide cover letters with your patch series (for series
>> 1 patch)? That letter would provide a chance to set the context,
> explain potential dependencies.
>
> Does this series already work over next? Or does it have dependencies on
> the dovetail patches?
>
> Given that some hardware setup will be needed, a patch adding
> documentation would be useful. What does the user have to do to be able
> to use the benchmark?
>

Since this a backport from EVL to Cobalt, the procedure is fully
described here: https://evlproject.org/core/benchmarks/

This is a documentation which could be shared between the two cores,
based on making latmus part of the CXP.

-- 
Philippe.

[PATCH 0/3] add support for cherryview gpio controller driver

[Hongzhan Chen]

1. move out of OF config conditional compilation so that non-OF platform
  call same API to remove rtdm gpio chip device.
2. Introduce helper to find gpiochip as referring to pair of 
   rtdm_gpiochip_scan_of and rtdm_gpiochip_scan_array_of. 
3. Add Intel Cherryview pinctrl driver based on on 1 and 2.

I also did following tests with this patchset:
1. run /usr/lib/xenomai/testsuite/gpiobench -i 334 -i 335 -c INT33FF:02
   to validate patch 9afea5ff2d7ba97db96b22a005a9a7fcf5f2d892 when
   setting GPIO_RTIOC_TS
2. apply following patch, and rerun 1.

index f83d7689f..50afbd418 100644
--- a/testsuite/gpiobench/gpiobench.c
+++ b/testsuite/gpiobench/gpiobench.c
@@ -619,7 +619,7 @@ int main(int argc, char **argv)
                        goto out;
                }

-               ret = ioctl(ti.fd_dev_intr, GPIO_RTIOC_TS, &value);
+               ret = ioctl(ti.fd_dev_intr, GPIO_RTIOC_TS_MONO, &value);
                if (ret) {
                        printf("ioctl gpio port ts, failed\n");
                        goto out;

Hardware env:
1. Rock PI X V1.4.
2. GPIO loopback connection between GPIO 334 and 335.
 

Hongzhan Chen (3):
  drivers/gpio: core: move out of OF config conditional compilation
  drivers/gpio: core: Introduce helper to find gpiochip
  driver/gpio: Add Intel Cherryview pinctrl driver

 include/cobalt/kernel/rtdm/gpio.h     | 10 ++++--
 kernel/drivers/gpio/Kconfig           |  7 ++++
 kernel/drivers/gpio/Makefile          |  2 ++
 kernel/drivers/gpio/gpio-cherryview.c | 42 ++++++++++++++++++++++++
 kernel/drivers/gpio/gpio-core.c       | 46 +++++++++++++++++++++++++--
 5 files changed, 103 insertions(+), 4 deletions(-)
 create mode 100644 kernel/drivers/gpio/gpio-cherryview.c

-- 
2.17.1

[PATCH 2/3] drivers/gpio: core: Introduce helper to find gpiochip

[Hongzhan Chen]

To find gpiochip for non-OF platforms like x86

Signed-off-by: Hongzhan Chen <hongzhan.chen@intel.com>
---
 include/cobalt/kernel/rtdm/gpio.h |  7 ++++++
 kernel/drivers/gpio/gpio-core.c   | 42 +++++++++++++++++++++++++++++++
 2 files changed, 49 insertions(+)

diff --git a/include/cobalt/kernel/rtdm/gpio.h b/include/cobalt/kernel/rtdm/gpio.h
index c307b1572..07093e0be 100644
--- a/include/cobalt/kernel/rtdm/gpio.h
+++ b/include/cobalt/kernel/rtdm/gpio.h
@@ -62,6 +62,13 @@ 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,
+			  const char *label, int type);
+
+int rtdm_gpiochip_array_find(struct device_node *from,
+				const char *compat[],
+				int nentries, 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 ddda62b24..6bcf04ea3 100644
--- a/kernel/drivers/gpio/gpio-core.c
+++ b/kernel/drivers/gpio/gpio-core.c
@@ -528,6 +528,48 @@ int rtdm_gpiochip_add_by_name(struct rtdm_gpio_chip *rgc,
 }
 EXPORT_SYMBOL_GPL(rtdm_gpiochip_add_by_name);
 
+int rtdm_gpiochip_find(struct device_node *from, const char *label,
+			  int type)
+{
+	struct rtdm_gpio_chip *rgc;
+	struct gpio_chip *chip;
+	int ret = -ENODEV;
+
+	if (!rtdm_available())
+		return -ENOSYS;
+
+	chip = find_chip_by_name(label);
+	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_array_find(struct device_node *from,
+				const char *compat[],
+				int nentries, int type)
+{
+	int ret = -ENODEV, _ret, n;
+
+	for (n = 0; n < nentries; n++) {
+		_ret = rtdm_gpiochip_find(from, compat[n], type);
+		if (_ret) {
+			if (_ret != -ENODEV)
+				return _ret;
+		} else
+			ret = 0;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(rtdm_gpiochip_array_find);
+
 #ifdef CONFIG_OF
 
 #include <linux/of_platform.h>
-- 
2.17.1

[PATCH V2 0/3] add support for cherryview gpio controller driver

[Hongzhan Chen]

1. move out of OF config conditional compilation so that non-OF platform
  call same API to remove rtdm gpio chip device and remove _of for api
  name.
2. Introduce helper to find gpiochip as referring to pair of 
   rtdm_gpiochip_scan_of and rtdm_gpiochip_scan_array_of. 
3. Add Intel Cherryview pinctrl driver based on on 1 and 2.

I also did following tests with this patchset:
1. run /usr/lib/xenomai/testsuite/gpiobench -i 334 -i 335 -c INT33FF:02
   to validate patch 9afea5ff2d7ba97db96b22a005a9a7fcf5f2d892 when
   setting GPIO_RTIOC_TS
2. apply following patch, and rerun 1.

index f83d7689f..50afbd418 100644
--- a/testsuite/gpiobench/gpiobench.c
+++ b/testsuite/gpiobench/gpiobench.c
@@ -619,7 +619,7 @@ int main(int argc, char **argv)
                        goto out;
                }

-               ret = ioctl(ti.fd_dev_intr, GPIO_RTIOC_TS, &value);
+               ret = ioctl(ti.fd_dev_intr, GPIO_RTIOC_TS_MONO, &value);
                if (ret) {
                        printf("ioctl gpio port ts, failed\n");
                        goto out;

3. test result for inner related output:
   REALTIME clock:
	# Total: 000999999
	# Min Latencies: 00013
	# Avg Latencies: 16.879997
	# Max Latencies: 00043

   MONO clock:
	# Total: 000083988
	# Min Latencies: 00012
	# Avg Latencies: 2814377.000000
	# Max Latencies: 00041

Hardware env:
1. Rock PI X V1.4.
2. GPIO loopback connection between GPIO 334 and 335.

Hongzhan Chen (3):
  drivers/gpio: core: Move out of OF config conditional compilation
  drivers/gpio: core: Introduce helper to find gpiochip
  driver/gpio: Add Intel Cherryview pinctrl driver

 include/cobalt/kernel/rtdm/gpio.h     | 12 +++++--
 kernel/drivers/gpio/Kconfig           |  7 ++++
 kernel/drivers/gpio/Makefile          |  2 ++
 kernel/drivers/gpio/gpio-bcm2835.c    |  2 +-
 kernel/drivers/gpio/gpio-cherryview.c | 42 ++++++++++++++++++++++
 kernel/drivers/gpio/gpio-core.c       | 50 ++++++++++++++++++++++++---
 kernel/drivers/gpio/gpio-mxc.c        |  2 +-
 kernel/drivers/gpio/gpio-omap.c       |  2 +-
 kernel/drivers/gpio/gpio-sun8i-h3.c   |  2 +-
 kernel/drivers/gpio/gpio-xilinx.c     |  2 +-
 kernel/drivers/gpio/gpio-zynq7000.c   |  2 +-
 11 files changed, 112 insertions(+), 13 deletions(-)
 create mode 100644 kernel/drivers/gpio/gpio-cherryview.c

-- 
2.17.1

[PATCH 2/3] drivers/gpio: core: Introduce helper to find gpiochip

[Hongzhan Chen]

To find gpiochip for non-OF platforms like x86

Signed-off-by: Hongzhan Chen <hongzhan.chen@intel.com>
---
 include/cobalt/kernel/rtdm/gpio.h |  7 ++++++
 kernel/drivers/gpio/gpio-core.c   | 42 +++++++++++++++++++++++++++++++
 2 files changed, 49 insertions(+)

diff --git a/include/cobalt/kernel/rtdm/gpio.h b/include/cobalt/kernel/rtdm/gpio.h
index 29f26d6c6..98279e242 100644
--- a/include/cobalt/kernel/rtdm/gpio.h
+++ b/include/cobalt/kernel/rtdm/gpio.h
@@ -62,6 +62,13 @@ 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,
+			  const char *label, int type);
+
+int rtdm_gpiochip_array_find(struct device_node *from,
+				const char *compat[],
+				int nentries, 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 ccda67bd1..ce80e8d73 100644
--- a/kernel/drivers/gpio/gpio-core.c
+++ b/kernel/drivers/gpio/gpio-core.c
@@ -528,6 +528,48 @@ int rtdm_gpiochip_add_by_name(struct rtdm_gpio_chip *rgc,
 }
 EXPORT_SYMBOL_GPL(rtdm_gpiochip_add_by_name);
 
+int rtdm_gpiochip_find(struct device_node *from, const char *label,
+			  int type)
+{
+	struct rtdm_gpio_chip *rgc;
+	struct gpio_chip *chip;
+	int ret = -ENODEV;
+
+	if (!rtdm_available())
+		return -ENOSYS;
+
+	chip = find_chip_by_name(label);
+	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_array_find(struct device_node *from,
+				const char *compat[],
+				int nentries, int type)
+{
+	int ret = -ENODEV, _ret, n;
+
+	for (n = 0; n < nentries; n++) {
+		_ret = rtdm_gpiochip_find(from, compat[n], type);
+		if (_ret) {
+			if (_ret != -ENODEV)
+				return _ret;
+		} else
+			ret = 0;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(rtdm_gpiochip_array_find);
+
 #ifdef CONFIG_OF
 
 #include <linux/of_platform.h>
-- 
2.17.1

Re: [PATCH 2/3] drivers/gpio: core: Introduce helper to find gpiochip

[Jan Kiszka]

On 31.08.21 07:07, Hongzhan Chen via Xenomai wrote:
> To find gpiochip for non-OF platforms like x86
> 
> Signed-off-by: Hongzhan Chen <hongzhan.chen@intel.com>
> ---
>  include/cobalt/kernel/rtdm/gpio.h |  7 ++++++
>  kernel/drivers/gpio/gpio-core.c   | 42 +++++++++++++++++++++++++++++++
>  2 files changed, 49 insertions(+)
> 
> diff --git a/include/cobalt/kernel/rtdm/gpio.h b/include/cobalt/kernel/rtdm/gpio.h
> index 29f26d6c6..98279e242 100644
> --- a/include/cobalt/kernel/rtdm/gpio.h
> +++ b/include/cobalt/kernel/rtdm/gpio.h
> @@ -62,6 +62,13 @@ 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,
> +			  const char *label, int type);
> +
> +int rtdm_gpiochip_array_find(struct device_node *from,
> +				const char *compat[],
> +				int nentries, 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 ccda67bd1..ce80e8d73 100644
> --- a/kernel/drivers/gpio/gpio-core.c
> +++ b/kernel/drivers/gpio/gpio-core.c
> @@ -528,6 +528,48 @@ int rtdm_gpiochip_add_by_name(struct rtdm_gpio_chip *rgc,
>  }
>  EXPORT_SYMBOL_GPL(rtdm_gpiochip_add_by_name);
>  
> +int rtdm_gpiochip_find(struct device_node *from, const char *label,
> +			  int type)
> +{
> +	struct rtdm_gpio_chip *rgc;
> +	struct gpio_chip *chip;
> +	int ret = -ENODEV;
> +
> +	if (!rtdm_available())
> +		return -ENOSYS;
> +
> +	chip = find_chip_by_name(label);
> +	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_array_find(struct device_node *from,
> +				const char *compat[],

Is "compat" the right argument name here? We are searching by name
(label), no?

Jan

> +				int nentries, int type)
> +{
> +	int ret = -ENODEV, _ret, n;
> +
> +	for (n = 0; n < nentries; n++) {
> +		_ret = rtdm_gpiochip_find(from, compat[n], type);
> +		if (_ret) {
> +			if (_ret != -ENODEV)
> +				return _ret;
> +		} else
> +			ret = 0;
> +	}
> +
> +	return ret;
> +}
> +EXPORT_SYMBOL_GPL(rtdm_gpiochip_array_find);
> +
>  #ifdef CONFIG_OF
>  
>  #include <linux/of_platform.h>
> 

-- 
Siemens AG, T RDA IOT
Corporate Competence Center Embedded Linux

RE: [PATCH 2/3] drivers/gpio: core: Introduce helper to find gpiochip

[Chen, Hongzhan]

 -----Original Message-----
>From: Jan Kiszka <jan.kiszka@siemens.com> 
>Sent: Tuesday, August 31, 2021 2:48 PM
>To: Chen, Hongzhan <hongzhan.chen@intel.com>; xenomai@xenomai.org
>Subject: Re: [PATCH 2/3] drivers/gpio: core: Introduce helper to find gpiochip
>
>On 31.08.21 07:07, Hongzhan Chen via Xenomai wrote:
>> To find gpiochip for non-OF platforms like x86
>> 
>> Signed-off-by: Hongzhan Chen <hongzhan.chen@intel.com>
>> ---
>>  include/cobalt/kernel/rtdm/gpio.h |  7 ++++++
>>  kernel/drivers/gpio/gpio-core.c   | 42 +++++++++++++++++++++++++++++++
>>  2 files changed, 49 insertions(+)
>> 
>> diff --git a/include/cobalt/kernel/rtdm/gpio.h b/include/cobalt/kernel/rtdm/gpio.h
>> index 29f26d6c6..98279e242 100644
>> --- a/include/cobalt/kernel/rtdm/gpio.h
>> +++ b/include/cobalt/kernel/rtdm/gpio.h
>> @@ -62,6 +62,13 @@ 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,
>> +			  const char *label, int type);
>> +
>> +int rtdm_gpiochip_array_find(struct device_node *from,
>> +				const char *compat[],
>> +				int nentries, 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 ccda67bd1..ce80e8d73 100644
>> --- a/kernel/drivers/gpio/gpio-core.c
>> +++ b/kernel/drivers/gpio/gpio-core.c
>> @@ -528,6 +528,48 @@ int rtdm_gpiochip_add_by_name(struct rtdm_gpio_chip *rgc,
>>  }
>>  EXPORT_SYMBOL_GPL(rtdm_gpiochip_add_by_name);
>>  
>> +int rtdm_gpiochip_find(struct device_node *from, const char *label,
>> +			  int type)
>> +{
>> +	struct rtdm_gpio_chip *rgc;
>> +	struct gpio_chip *chip;
>> +	int ret = -ENODEV;
>> +
>> +	if (!rtdm_available())
>> +		return -ENOSYS;
>> +
>> +	chip = find_chip_by_name(label);
>> +	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_array_find(struct device_node *from,
>> +				const char *compat[],
>
>Is "compat" the right argument name here? We are searching by name
>(label), no?

Yes , it is label. Let me modify it. So patch 1/3 is accepted ?

Regards

Hongzhan chen
>
>Jan
>
>> +				int nentries, int type)
>> +{
>> +	int ret = -ENODEV, _ret, n;
>> +
>> +	for (n = 0; n < nentries; n++) {
>> +		_ret = rtdm_gpiochip_find(from, compat[n], type);
>> +		if (_ret) {
>> +			if (_ret != -ENODEV)
>> +				return _ret;
>> +		} else
>> +			ret = 0;
>> +	}
>> +
>> +	return ret;
>> +}
>> +EXPORT_SYMBOL_GPL(rtdm_gpiochip_array_find);
>> +
>>  #ifdef CONFIG_OF
>>  
>>  #include <linux/of_platform.h>
>> 
>
>-- 
>Siemens AG, T RDA IOT
>Corporate Competence Center Embedded Linux
>
>

Re: [PATCH 2/3] drivers/gpio: core: Introduce helper to find gpiochip

[Jan Kiszka]

On 31.08.21 09:07, Chen, Hongzhan wrote:
> 
> 
>  -----Original Message-----
>> From: Jan Kiszka <jan.kiszka@siemens.com> 
>> Sent: Tuesday, August 31, 2021 2:48 PM
>> To: Chen, Hongzhan <hongzhan.chen@intel.com>; xenomai@xenomai.org
>> Subject: Re: [PATCH 2/3] drivers/gpio: core: Introduce helper to find gpiochip
>>
>> On 31.08.21 07:07, Hongzhan Chen via Xenomai wrote:
>>> To find gpiochip for non-OF platforms like x86
>>>
>>> Signed-off-by: Hongzhan Chen <hongzhan.chen@intel.com>
>>> ---
>>>  include/cobalt/kernel/rtdm/gpio.h |  7 ++++++
>>>  kernel/drivers/gpio/gpio-core.c   | 42 +++++++++++++++++++++++++++++++
>>>  2 files changed, 49 insertions(+)
>>>
>>> diff --git a/include/cobalt/kernel/rtdm/gpio.h b/include/cobalt/kernel/rtdm/gpio.h
>>> index 29f26d6c6..98279e242 100644
>>> --- a/include/cobalt/kernel/rtdm/gpio.h
>>> +++ b/include/cobalt/kernel/rtdm/gpio.h
>>> @@ -62,6 +62,13 @@ 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,
>>> +			  const char *label, int type);
>>> +
>>> +int rtdm_gpiochip_array_find(struct device_node *from,
>>> +				const char *compat[],
>>> +				int nentries, 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 ccda67bd1..ce80e8d73 100644
>>> --- a/kernel/drivers/gpio/gpio-core.c
>>> +++ b/kernel/drivers/gpio/gpio-core.c
>>> @@ -528,6 +528,48 @@ int rtdm_gpiochip_add_by_name(struct rtdm_gpio_chip *rgc,
>>>  }
>>>  EXPORT_SYMBOL_GPL(rtdm_gpiochip_add_by_name);
>>>  
>>> +int rtdm_gpiochip_find(struct device_node *from, const char *label,
>>> +			  int type)
>>> +{
>>> +	struct rtdm_gpio_chip *rgc;
>>> +	struct gpio_chip *chip;
>>> +	int ret = -ENODEV;
>>> +
>>> +	if (!rtdm_available())
>>> +		return -ENOSYS;
>>> +
>>> +	chip = find_chip_by_name(label);
>>> +	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_array_find(struct device_node *from,
>>> +				const char *compat[],
>>
>> Is "compat" the right argument name here? We are searching by name
>> (label), no?
> 
> Yes , it is label. Let me modify it. So patch 1/3 is accepted ?
> 

Already done that locally while merging. Also just renamed compat_array
to label_array in patch 3. Let me push this then.

Jan

-- 
Siemens AG, T RDA IOT
Corporate Competence Center Embedded Linux