From 2ccacdf2282e666bff0d5fa0fe0da4367251ffaf Mon Sep 17 00:00:00 2001
From: Daniel Borkmann <borkmann@gnumaniacs.org>
Date: Fri, 8 Apr 2011 19:52:12 +0200
Subject: [PATCH] added process scheduling routines

---
 src/Makefile |  2 +-
 src/all.h    |  1 +
 src/psched.c | 73 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/psched.h | 42 ++++++++++++++++++++++++++++++++++
 4 files changed, 117 insertions(+), 1 deletion(-)

diff --git a/src/Makefile b/src/Makefile
index f6b2ccd2..2f97fd7a 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -12,7 +12,7 @@ CFLAGS  += -g -O2 -Iinclude/nao/
 target   = libnaohtwk
 
 OBJS     = tlsf.o strlcpy.o xmalloc.o mmap_v4l.o notifier.o urcu.o \
-	   urcu-asm.o rbtree.o i2c_cam.o
+	   urcu-asm.o rbtree.o i2c_cam.o psched.o
 
 all: $(OBJS)
 	$(CC) -shared -Wl,-soname,$(target).so.1 -o $(target).so.1.0.1 $(OBJS)
diff --git a/src/all.h b/src/all.h
index ce08dc37..1927d681 100644
--- a/src/all.h
+++ b/src/all.h
@@ -25,6 +25,7 @@
 #include "mmap_v4l.h"
 #include "die.h"
 #include "sort.h"
+#include "psched.h"
 #include "i2c.h"
 #include "rbtree.h"
 #include "mtrand.h"
diff --git a/src/psched.c b/src/psched.c
index e69de29b..aae941bf 100644
--- a/src/psched.c
+++ b/src/psched.c
@@ -0,0 +1,73 @@
+/*
+ * nao-ulib
+ * Copyright 2011 Daniel Borkmann <dborkma@tik.ee.ethz.ch>
+ * Subject to the GPL.
+ * Nao-Team HTWK,
+ * Faculty of Computer Science, Mathematics and Natural Sciences,
+ * Leipzig University of Applied Sciences (HTWK Leipzig)
+ */
+
+#define _GNU_SOURCE
+#include <sys/types.h>
+#include <unistd.h>
+#include <sched.h>
+
+#include "die.h"
+#include "psched.h"
+
+void run_on_cpu(int cpu)
+{
+	cpu_set_t mask;
+
+	CPU_ZERO(&mask);
+	CPU_SET(cpu, &mask);
+
+	sched_setaffinity(0, sizeof(mask), &mask);
+}
+
+int set_proc_prio(int priority)
+{
+	/*
+	 * setpriority() is clever, even if you put a nice value which 
+	 * is out of range it corrects it to the closest valid nice value
+	 */
+	int ret = setpriority(PRIO_PROCESS, getpid(), priority);
+	if (ret)
+		panic("Can't set nice val to %i!\n", priority);
+
+	return 0;
+}
+
+int set_sched_status(int policy, int priority)
+{
+	int ret, min_prio, max_prio;
+	struct sched_param sp;
+
+	max_prio = sched_get_priority_max(policy);
+	min_prio = sched_get_priority_min(policy);
+
+	if (max_prio == -1 || min_prio == -1)
+		whine("Cannot determine scheduler prio limits!\n");
+	else if (priority < min_prio)
+		priority = min_prio;
+	else if (priority > max_prio)
+		priority = max_prio;
+
+	memset(&sp, 0, sizeof(sp));
+	sp.sched_priority = priority;
+
+	ret = sched_setscheduler(getpid(), policy, &sp);
+	if (ret) {
+		whine("Cannot set scheduler policy!\n");
+		return -EINVAL;
+	}
+
+	ret = sched_setparam(getpid(), &sp);
+	if (ret) {
+		whine("Cannot set scheduler prio!\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
diff --git a/src/psched.h b/src/psched.h
index e69de29b..cfbc79d7 100644
--- a/src/psched.h
+++ b/src/psched.h
@@ -0,0 +1,42 @@
+/*
+ * nao-ulib
+ * Copyright 2011 Daniel Borkmann <dborkma@tik.ee.ethz.ch>
+ * Subject to the GPL.
+ * Nao-Team HTWK,
+ * Faculty of Computer Science, Mathematics and Natural Sciences,
+ * Leipzig University of Applied Sciences (HTWK Leipzig)
+ */
+
+#ifndef PSCHED_H
+#define PSCHED_H
+
+#define _GNU_SOURCE
+#include <unistd.h>
+#include <sched.h>
+#include <sys/resource.h>
+
+extern void run_on_cpu(int cpu);
+extern int set_proc_prio(int priority);
+extern int set_sched_status(int policy, int priority);
+
+static inline int get_default_sched_policy(void)
+{
+	return SCHED_FIFO;
+}
+
+static inline int get_default_sched_prio(void)
+{
+	return sched_get_priority_max(get_default_sched_policy());
+}
+
+static inline int get_number_cpus(void)
+{
+	return sysconf(_SC_NPROCESSORS_CONF);
+}
+
+static inline int get_number_cpus_online(void)
+{
+	return sysconf(_SC_NPROCESSORS_ONLN);
+}
+
+#endif /* PSCHED_H */
-- 
2.11.4.GIT