From a44dc75609448e871feb189bb81f84411256f52b Mon Sep 17 00:00:00 2001
From: Sam Moore <matches@ucc.asn.au>
Date: Sat, 19 Oct 2013 23:35:36 +0800
Subject: [PATCH] Add define for compiling under RTLinux

Tested on amd64; doesn't really improve sampling consistency over vanilla.
But whatever.
---
 server/common.h |  4 ++-
 server/main.c   | 66 ++++++++++++++++++++++++++++++++++++++++++++++++-
 server/sensor.c | 20 ++++++++++++---
 3 files changed, 84 insertions(+), 6 deletions(-)

diff --git a/server/common.h b/server/common.h
index cc2ab3f..637a813 100644
--- a/server/common.h
+++ b/server/common.h
@@ -10,7 +10,7 @@
 #define _POSIX_C_SOURCE 200809L
 //#define _BSD_SOURCE
 #define _XOPEN_SOURCE 600
-
+#define _GNU_SOURCE
 /** Determine if we're running on the BBB **/
 #ifdef __arm__
 	#define _BBB
@@ -21,6 +21,7 @@
 /** The current API version **/
 #define API_VERSION 0
 
+//#define REALTIME_VERSION
 
 
 
@@ -34,6 +35,7 @@
 #include <assert.h>
 #include <sys/time.h>
 #include <time.h>
+#include <string.h>
 
 #include "log.h"
 #include "fastcgi.h"
diff --git a/server/main.c b/server/main.c
index 0a39ae9..5aef29f 100644
--- a/server/main.c
+++ b/server/main.c
@@ -17,6 +17,13 @@
 #include <signal.h> // for signal handling
 
 
+#ifdef REALTIME_VERSION
+#include <time.h>
+#include <sched.h>
+#include <sys/mman.h>
+#include <sys/utsname.h>
+#endif //REALTIME_VERSION
+
 // --- Variable definitions --- //
 Options g_options; // options passed to program through command line arguments
 
@@ -113,6 +120,38 @@ void Cleanup()
 	Log(LOGDEBUG, "Finish cleanup.");
 }
 
+
+#ifdef REALTIME_VERSION
+
+#define MAX_SAFE_STACK (8*1024)
+#define NSEC_PER_SEC (1000000000)
+
+void stack_prefault()
+{
+	unsigned char dummy[MAX_SAFE_STACK];
+	memset(dummy, 0, MAX_SAFE_STACK);
+	return;
+}
+
+bool is_realtime()
+{
+	struct utsname u;
+	bool crit1 = 0;
+	bool crit2 = 0;
+	FILE * f;
+	uname(&u);
+	crit1 = (strcasestr(u.version, "PREEMPT RT") != NULL);
+	if (crit1 && ((f = fopen("/sys/kernel/realtime", "r")) != NULL))
+	{
+		int flag;
+		crit2 = ((fscanf(f, "%d", &flag) == 1) && (flag == 1));
+		fclose(f);
+	}
+	return (crit1 && crit2);
+}
+
+#endif //REALTIME_VERSION
+
 /**
  * Main entry point; start worker threads, setup signal handling, wait for threads to exit, exit
  * @param argc - Num args
@@ -122,12 +161,37 @@ void Cleanup()
  */
 int main(int argc, char ** argv)
 {
+
 	// Open log before calling ParseArguments (since ParseArguments may call the Log functions)
 	openlog("mctxserv", LOG_PID | LOG_PERROR, LOG_USER);
-	Log(LOGINFO, "Server started");
 
 	ParseArguments(argc, argv); // Setup the g_options structure from program arguments
 
+	Log(LOGINFO, "Server started");
+
+
+	
+	#ifdef REALTIME_VERSION
+	
+	if (is_realtime())
+	{
+		Log(LOGDEBUG, "Running under realtime kernel");
+	}
+	else
+	{
+		Fatal("Not running under realtime kernel");
+	}
+	struct sched_param param;
+	param.sched_priority = 49;
+	if (sched_setscheduler(0, SCHED_FIFO, &param) < 0)
+		Fatal("sched_setscheduler failed - %s", strerror(errno));
+	if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
+		Fatal("mlockall failed - %s", strerror(errno));
+	stack_prefault();
+	#endif //REALTIME_VERSION
+
+	
+
 	Sensor_Init();
 	Actuator_Init();
 	Pin_Init();
diff --git a/server/sensor.c b/server/sensor.c
index 8cc8c82..816a895 100644
--- a/server/sensor.c
+++ b/server/sensor.c
@@ -47,9 +47,9 @@ int Sensor_Add(const char * name, int user_id, ReadFn read, InitFn init, CleanFn
 	s->init = init; // Set init function
 
 	// Start by averaging values taken over a second
-	s->sample_time.tv_sec = 1;
-	s->sample_time.tv_nsec = 0;
+	DOUBLE_TO_TIMEVAL(1e-4, &(s->sample_time));
 	s->averages = 1;
+	s->num_read = 0;
 
 	// Set sanity function
 	s->sanity = sanity;
@@ -60,7 +60,8 @@ int Sensor_Add(const char * name, int user_id, ReadFn read, InitFn init, CleanFn
 			Fatal("Couldn't init sensor %s", name);
 	}
 
-
+	s->current_data.time_stamp = 0;
+	s->current_data.value = 0;
 	return g_num_sensors;
 }
 
@@ -213,7 +214,18 @@ void * Sensor_Loop(void * arg)
 					Fatal("Sensor %s (%d,%d) reads unsafe value", s->name, s->id, s->user_id);
 				}
 			}
-			Data_Save(&(s->data_file), &d, 1); // Record it
+			s->current_data.time_stamp += d.time_stamp;
+			s->current_data.value += d.value;
+			
+			if (++(s->num_read) >= s->averages)
+			{
+				s->current_data.time_stamp /= s->averages;
+				s->current_data.value /= s->averages;
+				Data_Save(&(s->data_file), &(s->current_data), 1); // Record it
+				s->num_read = 0;
+				s->current_data.time_stamp = 0;
+				s->current_data.value = 0;
+			}
 		}
 		else
 			Log(LOGWARN, "Failed to read sensor %s (%d,%d)", s->name, s->id,s->user_id);
-- 
2.20.1