+void *Periodic_Thread(void *Unused __attribute__((unused)))
+{
+ int i;
+
+ for( ;; )
+ {
+ sleep(10); // Sleep for a while
+// printf("Periodic firing\n");
+ for( i = 0; i < ciMaxPeriodics; i ++ )
+ {
+ if( gaPeriodicCalls[i].Function )
+ gaPeriodicCalls[i].Function();
+ }
+ }
+ return NULL;
+}
+
+void StartPeriodicThread(void)
+{
+ pthread_create( &gTimerThread, NULL, Periodic_Thread, NULL );
+}
+
+void AddPeriodicFunction(void (*Fcn)(void))
+{
+ int i;
+ for( i = 0; i < ciMaxPeriodics; i ++ )
+ {
+ if( gaPeriodicCalls[i].Function ) continue;
+ gaPeriodicCalls[i].Function = Fcn;
+ return ;
+ }
+
+ fprintf(stderr, "Error: No space for %p in periodic list\n", Fcn);
+}
+
+int RunRegex(regex_t *regex, const char *string, int nMatches, regmatch_t *matches, const char *errorMessage)
+{
+ int ret;
+
+ ret = regexec(regex, string, nMatches, matches, 0);
+ if( ret == REG_NOMATCH ) {
+ return -1;
+ }
+ if( ret ) {
+ size_t len = regerror(ret, regex, NULL, 0);
+ char errorStr[len];
+ regerror(ret, regex, errorStr, len);
+ printf("string = '%s'\n", string);
+ fprintf(stderr, "%s\n%s", errorMessage, errorStr);
+ exit(-1);
+ }
+
+ return ret;
+}
+
+void CompileRegex(regex_t *regex, const char *pattern, int flags)
+{
+ int ret = regcomp(regex, pattern, flags);
+ if( ret ) {
+ size_t len = regerror(ret, regex, NULL, 0);
+ char errorStr[len];
+ regerror(ret, regex, errorStr, len);
+ fprintf(stderr, "Regex compilation failed - %s\n%s\n", errorStr, pattern);
+ exit(-1);
+ }
+}
+
+// Serial helper
+int InitSerial(const char *File, int BaudRate)
+{
+ struct termios info;
+ int baud;
+ int fd;
+
+ fd = open(File, O_RDWR | O_NOCTTY | O_NONBLOCK);
+ if( fd == -1 ) return -1;
+
+ switch(BaudRate)
+ {
+ case 1200: baud = B1200; break;
+ case 9600: baud = B9600; break;
+ case 115200: baud = B115200; break;
+ default: close(fd); return -1;
+ }
+
+ info.c_lflag = 0; // Non-Canoical, No Echo
+ info.c_cflag = baud | CS8 | CLOCAL | CREAD; // baud, 8N1
+ info.c_iflag = IGNCR; // Ignore \r
+ info.c_oflag = 0; // ???
+ cfsetspeed(&info, baud);
+ info.c_cc[VTIME] = 0; // No time limit
+ info.c_cc[VMIN] = 1; // Block until 1 char
+
+ tcflush(fd, TCIFLUSH);
+ tcsetattr(fd, TCSANOW, &info);
+
+ return fd;
+}
+
+
+/**
+ * \brief Create a formatted heap string
+ */
+char *mkstr(const char *Format, ...)
+{
+ va_list args;
+ int len;
+ char *ret;
+
+ va_start(args, Format);
+ len = vsnprintf(NULL, 0, Format, args);
+ va_end(args);
+
+ ret = malloc( len + 1 );
+ if(!ret) return NULL;
+
+ va_start(args, Format);
+ vsprintf(ret, Format, args);
+ va_end(args);
+
+ return ret;
+}
+