From f069d670ca5ce613eb8f9c87cf9f2ae88c154fc3 Mon Sep 17 00:00:00 2001
From: =?utf8?q?Martin=20Ho=C5=99e=C5=88ovsk=C3=BD?=
 <Martin.Horenovsky@gmail.com>
Date: Mon, 16 Sep 2013 16:45:36 +0200
Subject: [PATCH] Enables conditional compilation for gateway itself.

Added highprio option -- posix thread that works as the GW receives higher priority
than other "userspace" thread/tasks.
Also closes off various debug counters behind conditional compilation if benchbuild
is enabled.
---
 rtems/gw/cangw/gw.c | 48 ++++++++++++++++++++++++++++++++++-----------
 1 file changed, 37 insertions(+), 11 deletions(-)

diff --git a/rtems/gw/cangw/gw.c b/rtems/gw/cangw/gw.c
index 24d4087..2a08a0c 100644
--- a/rtems/gw/cangw/gw.c
+++ b/rtems/gw/cangw/gw.c
@@ -1,6 +1,6 @@
 #include <system_def.h>
-#include "system.h"
 #include "app_def.h"
+#include "system.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <rtems/untar.h>
@@ -20,6 +20,7 @@
 #define CAN_GW_A_TO_B_MODE 1
 #define CAN_GW_B_TO_A_MODE 2
 #define CAN_GW_BAUDRATE 1000000
+#define CAN_GW_TASK_HIGH_PRIO 250 /* POSIX API actually has priorities the "right way around" that is, higher is indeed more important */
 
 /* counters for informational purposes */
 unsigned long int total_1 = 0, total_2 = 0;
@@ -54,7 +55,9 @@ static void fd_closer(void* arg){
 static void* CAN_GW_thread(void* arg){
     int fd_in, fd_out;
     int res;
-    unsigned long int *total, *succ, *err;
+#ifndef BENCH_BUILD
+    unsigned long int *total = NULL, *succ = NULL, *err = NULL; /* Makes compiler shut up about warnings. */
+#endif
     
     struct can_message canmsg;
     struct mscan_rx_parms rxparms;
@@ -73,19 +76,25 @@ static void* CAN_GW_thread(void* arg){
     if (((int)arg) == CAN_GW_A_TO_B_MODE){
         fd_in = open(MSCAN_A_DEV_NAME, O_RDWR);
         fd_out = open(MSCAN_B_DEV_NAME, O_RDWR);
-        total = &total_1;
-        succ = &succ_1;
-        err = &err_1;
     } else if (((int)arg) == CAN_GW_B_TO_A_MODE){
         fd_in = open(MSCAN_B_DEV_NAME, O_RDWR);
         fd_out = open(MSCAN_A_DEV_NAME, O_RDWR);
+    } else {
+        /* Invalid argument, terminate self. */
+        return NULL;
+    }
+
+#ifndef BENCH_BUILD /* Overhead decrease for benching builds.*/
+    if (((int)arg) == CAN_GW_A_TO_B_MODE){
+        total = &total_1;
+        succ = &succ_1;
+        err = &err_1;
+    } else {
         total = &total_2;
         succ = &succ_2;
         err = &err_2;
-    } else {
-        /* Invalid argument, terminates itself. */
-        rtems_task_delete(RTEMS_SELF);
     }
+#endif
     
     /* CANs are set to proper baudrate outside of these task due to potential race condition. */
 
@@ -105,12 +114,17 @@ static void* CAN_GW_thread(void* arg){
             txparms.tx_idx = canmsg.mess_id;
             res = write(fd_out, &txparms, sizeof(txparms));
             if (res < 0) {
+#ifndef BENCH_BUILD /* This is the simplest way to remove all the counting */
                 /* Retry? Decide later. */
                 (*err)++;
             } else {
                 (*succ)++;
             }
             (*total)++;
+#else
+            } /* Turns it into empty loop. */
+#endif
+
         }
     }
     return NULL;
@@ -119,20 +133,32 @@ static void* CAN_GW_thread(void* arg){
 
 static int start_tasks(){
     int res;
+
     
-    res = pthread_create(&CAN_A_to_B_thread, NULL, CAN_GW_thread, (void*)CAN_GW_A_TO_B_MODE);
+    pthread_attr_t attributes;
+    struct sched_param parm;    
+    parm.sched_priority = CAN_GW_TASK_HIGH_PRIO;
+    pthread_attr_init(&attributes);
+#ifdef HIGH_PRIO /* Without setting PTHREAD_EXPLICIT_SCHED, thread is created with parameters inherited from this thread. */
+    res = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);
+#endif
+    pthread_attr_setschedparam(&attributes, &parm);
+
+    res = pthread_create(&CAN_A_to_B_thread, &attributes, CAN_GW_thread, (void*)CAN_GW_A_TO_B_MODE);
     if (res != 0){
         /* No cleanup is needed here. */
         return 1;
     }
 
-    res = pthread_create(&CAN_B_to_A_thread, NULL, CAN_GW_thread, (void*)CAN_GW_B_TO_A_MODE);
+    res = pthread_create(&CAN_B_to_A_thread, &attributes, CAN_GW_thread, (void*)CAN_GW_B_TO_A_MODE);
     if (res != 0){
         /* First thread needs to be aborted before return. */
         pthread_cancel(CAN_A_to_B_thread);
         return 1;
     }
     
+    pthread_attr_destroy(&attributes);
+    
     /* detach is needed so that the threads call clean up handlers automatically. */
     pthread_detach(CAN_B_to_A_thread);
     pthread_detach(CAN_A_to_B_thread);
@@ -195,7 +221,7 @@ static int set_baudrate(int baudrate, int* fd1, int* fd2){
     memset(&ctrl_parms, 0, sizeof(ctrl_parms));
     ctrl_parms.ctrl_can_bitrate = baudrate;
 
-    printf("Attempting to set bitrate %u for fd.\n",  ctrl_parms.ctrl_can_bitrate);
+    printf("Attempting to set bitrate %"PRIu32" for fd.\n",  ctrl_parms.ctrl_can_bitrate);
     
     fd = open(MSCAN_A_DEV_NAME, O_RDWR);
     if (fd < 0){
-- 
2.39.2