Changed all (I think) files using CRLF to use LF.
[can-benchmark.git] / rtems / gw / cangw / gw.c
index 2a08a0c..1b8a80d 100644 (file)
-#include <system_def.h>\r
-#include "app_def.h"\r
-#include "system.h"\r
-#include <stdio.h>\r
-#include <stdlib.h>\r
-#include <rtems/untar.h>\r
-#include <rtems/error.h>\r
-#include <rtems/mw_uid.h>\r
-#include <unistd.h>\r
-#include <fcntl.h>\r
-#include <errno.h>\r
-\r
-#include <bsp/mscan.h>\r
-#include <bsp/mscan-base.h>\r
-#include <pthread.h>\r
-\r
-#include "gw.h"\r
-\r
-/* Local defines. */\r
-#define CAN_GW_A_TO_B_MODE 1\r
-#define CAN_GW_B_TO_A_MODE 2\r
-#define CAN_GW_BAUDRATE 1000000\r
-#define CAN_GW_TASK_HIGH_PRIO 250 /* POSIX API actually has priorities the "right way around" that is, higher is indeed more important */\r
-\r
-/* counters for informational purposes */\r
-unsigned long int total_1 = 0, total_2 = 0;\r
-unsigned long int succ_1 = 0, succ_2 = 0;   \r
-unsigned long int err_1 = 0, err_2 = 0;\r
-\r
-/* Pthread handles for the GW */\r
-static pthread_t CAN_A_to_B_thread, CAN_B_to_A_thread;\r
-/* Dummy FDs for setting baudrate */\r
-static int fd1, fd2;\r
-\r
-/* Prototypes for local functions. */\r
-static void* CAN_GW_thread(void* arg);\r
-static void fd_closer(void* arg);\r
-static int start_tasks();\r
-static int end_tasks();\r
-static int set_baudrate(int baudrate, int* fd1, int* fd2);\r
-\r
-/*\r
-* Simple cleanup handler for GW threads. Closes opened FD.\r
-*/\r
-static void fd_closer(void* arg){\r
-    close( (*(int*)arg) );\r
-}\r
-\r
-\r
-\r
-/*\r
-* This function implements the main thread loop and enough decision logic so that it knows which device to take messages from and which device to send the messages to.\r
-* \r
-*/\r
-static void* CAN_GW_thread(void* arg){\r
-    int fd_in, fd_out;\r
-    int res;\r
-#ifndef BENCH_BUILD\r
-    unsigned long int *total = NULL, *succ = NULL, *err = NULL; /* Makes compiler shut up about warnings. */\r
-#endif\r
-    \r
-    struct can_message canmsg;\r
-    struct mscan_rx_parms rxparms;\r
-    struct mscan_tx_parms txparms;\r
-    memset(&canmsg, 0, sizeof(canmsg));\r
-    memset(&rxparms, 0, sizeof(rxparms));\r
-    memset(&txparms, 0, sizeof(txparms));\r
-\r
-    /* These remain constant during the loop. */\r
-    rxparms.rx_mess = &canmsg;\r
-    rxparms.rx_timeout = rtems_clock_get_ticks_per_second(); /* 1s timeout */\r
-    txparms.tx_mess = &canmsg;\r
-    \r
-    \r
-    /* Decide in which direction this task should work. */\r
-    if (((int)arg) == CAN_GW_A_TO_B_MODE){\r
-        fd_in = open(MSCAN_A_DEV_NAME, O_RDWR);\r
-        fd_out = open(MSCAN_B_DEV_NAME, O_RDWR);\r
-    } else if (((int)arg) == CAN_GW_B_TO_A_MODE){\r
-        fd_in = open(MSCAN_B_DEV_NAME, O_RDWR);\r
-        fd_out = open(MSCAN_A_DEV_NAME, O_RDWR);\r
-    } else {\r
-        /* Invalid argument, terminate self. */\r
-        return NULL;\r
-    }\r
-\r
-#ifndef BENCH_BUILD /* Overhead decrease for benching builds.*/\r
-    if (((int)arg) == CAN_GW_A_TO_B_MODE){\r
-        total = &total_1;\r
-        succ = &succ_1;\r
-        err = &err_1;\r
-    } else {\r
-        total = &total_2;\r
-        succ = &succ_2;\r
-        err = &err_2;\r
-    }\r
-#endif\r
-    \r
-    /* CANs are set to proper baudrate outside of these task due to potential race condition. */\r
-\r
-    \r
-    /* Prepare cleanup for both opened FDs. */\r
-    pthread_cleanup_push(fd_closer, (void*)&fd_in);\r
-    pthread_cleanup_push(fd_closer, (void*)&fd_out);\r
-\r
-    while (true){\r
-        /* RTEMS doesn't implement cancellation point inside read(), violating the API's contract. Luckily, pthread_testcancel() works. */\r
-        pthread_testcancel();\r
-        res = read(fd_in, &rxparms, sizeof(rxparms));\r
-        if (res < 0){\r
-            /* Read error, doesn't really do anything. (Currently) */\r
-        } else {\r
-            /* Message was read, now we should resend it. */\r
-            txparms.tx_idx = canmsg.mess_id;\r
-            res = write(fd_out, &txparms, sizeof(txparms));\r
-            if (res < 0) {\r
-#ifndef BENCH_BUILD /* This is the simplest way to remove all the counting */\r
-                /* Retry? Decide later. */\r
-                (*err)++;\r
-            } else {\r
-                (*succ)++;\r
-            }\r
-            (*total)++;\r
-#else\r
-            } /* Turns it into empty loop. */\r
-#endif\r
-\r
-        }\r
-    }\r
-    return NULL;\r
-}\r
-\r
-\r
-static int start_tasks(){\r
-    int res;\r
-\r
-    \r
-    pthread_attr_t attributes;\r
-    struct sched_param parm;    \r
-    parm.sched_priority = CAN_GW_TASK_HIGH_PRIO;\r
-    pthread_attr_init(&attributes);\r
-#ifdef HIGH_PRIO /* Without setting PTHREAD_EXPLICIT_SCHED, thread is created with parameters inherited from this thread. */\r
-    res = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);\r
-#endif\r
-    pthread_attr_setschedparam(&attributes, &parm);\r
-\r
-    res = pthread_create(&CAN_A_to_B_thread, &attributes, CAN_GW_thread, (void*)CAN_GW_A_TO_B_MODE);\r
-    if (res != 0){\r
-        /* No cleanup is needed here. */\r
-        return 1;\r
-    }\r
-\r
-    res = pthread_create(&CAN_B_to_A_thread, &attributes, CAN_GW_thread, (void*)CAN_GW_B_TO_A_MODE);\r
-    if (res != 0){\r
-        /* First thread needs to be aborted before return. */\r
-        pthread_cancel(CAN_A_to_B_thread);\r
-        return 1;\r
-    }\r
-    \r
-    pthread_attr_destroy(&attributes);\r
-    \r
-    /* detach is needed so that the threads call clean up handlers automatically. */\r
-    pthread_detach(CAN_B_to_A_thread);\r
-    pthread_detach(CAN_A_to_B_thread);\r
-\r
-    /* Threads are started and running at this point. */\r
-    return 0;\r
-}\r
-\r
-/*\r
-* This function starts the GW.\r
-*\r
-* It opens two dummy file descriptors to set CAN baudrate (avoiding need of synchronized thread start), starts the forwarding threads.\r
-*\r
-* Currently has only simple error handling. \r
-*/\r
-int start_GW(){\r
-    /* Baudrate is set for dummy FD, it should be remembered by the device. */\r
-    int res;\r
-    res = set_baudrate(CAN_GW_BAUDRATE, &fd1, &fd2);\r
-    printf("Baudrate set.\n");\r
-    if (res == 0){\r
-        res = start_tasks();\r
-        printf("tasks started\n");\r
-    }\r
-    return res;\r
-}\r
-\r
-\r
-/*\r
-* This function stops threads implementing GW's forwarding. It tries to stop the threads "politely" via pthread_cancel.\r
-*\r
-* Error handling is not yet implemented, so it always returns 0 (success).\r
-*/\r
-static int end_tasks(){\r
-    int res;\r
-    printf("Attempting to stop thread 1\n");\r
-    res = pthread_cancel(CAN_A_to_B_thread);\r
-    if (res != 0){\r
-        printf("Failed.\n");\r
-        /* Not sure what to do with error here, will have to figure out later. */\r
-    }\r
-    printf("Attempting to stop thread 2\n");\r
-    res = pthread_cancel(CAN_B_to_A_thread);\r
-    if (res != 0){\r
-        printf("Failed.\n");\r
-        /* Not sure what to do with error here, will have to figure out later. */\r
-    }\r
-    sleep(1);\r
-    printf("Both threads should now be stopped.\n");\r
-    return 0;\r
-}\r
-\r
-\r
-/*\r
-* Sets baudrate to the devices via dummy fd.\r
-*/\r
-static int set_baudrate(int baudrate, int* fd1, int* fd2){\r
-    int fd, res;\r
-    struct mscan_ctrl_parms ctrl_parms;\r
-    memset(&ctrl_parms, 0, sizeof(ctrl_parms));\r
-    ctrl_parms.ctrl_can_bitrate = baudrate;\r
-\r
-    printf("Attempting to set bitrate %"PRIu32" for fd.\n",  ctrl_parms.ctrl_can_bitrate);\r
-    \r
-    fd = open(MSCAN_A_DEV_NAME, O_RDWR);\r
-    if (fd < 0){\r
-        return 1;\r
-    }\r
-    res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);\r
-    if (res < 0) {\r
-        printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));\r
-        return 1;\r
-    }\r
-    (*fd1) = fd;\r
-\r
-    fd = open(MSCAN_B_DEV_NAME, O_RDWR);\r
-    if (fd < 0){\r
-        /* Needs to cleanup by closing first fd. */\r
-        close( (*fd1) );\r
-        return 1;\r
-    }\r
-    res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);\r
-    if (res < 0) {\r
-        printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));\r
-        return 1;\r
-    }\r
-    \r
-    (*fd2) = fd;\r
-    \r
-    return 0;\r
-}\r
-\r
-/*\r
-* Wrapper function and entry point for stopping the GW.\r
-*/\r
-int end_GW(){\r
-    return end_tasks();\r
-    /* Clean up of dummy FDs. */\r
-    close(fd1);\r
-    close(fd2);\r
-\r
+#include <system_def.h>
+#include "app_def.h"
+#include "system.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <rtems/untar.h>
+#include <rtems/error.h>
+#include <rtems/mw_uid.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include <bsp/mscan.h>
+#include <bsp/mscan-base.h>
+#include <pthread.h>
+
+#include "gw.h"
+
+/* Local defines. */
+#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;
+unsigned long int succ_1 = 0, succ_2 = 0;   
+unsigned long int err_1 = 0, err_2 = 0;
+
+/* Pthread handles for the GW */
+static pthread_t CAN_A_to_B_thread, CAN_B_to_A_thread;
+/* Dummy FDs for setting baudrate */
+static int fd1, fd2;
+
+/* Prototypes for local functions. */
+static void* CAN_GW_thread(void* arg);
+static void fd_closer(void* arg);
+static int start_tasks();
+static int end_tasks();
+static int set_baudrate(int baudrate, int* fd1, int* fd2);
+
+/*
+* Simple cleanup handler for GW threads. Closes opened FD.
+*/
+static void fd_closer(void* arg){
+    close( (*(int*)arg) );
+}
+
+
+
+/*
+* This function implements the main thread loop and enough decision logic so that it knows which device to take messages from and which device to send the messages to.
+* 
+*/
+static void* CAN_GW_thread(void* arg){
+    int fd_in, fd_out;
+    int res;
+#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;
+    struct mscan_tx_parms txparms;
+    memset(&canmsg, 0, sizeof(canmsg));
+    memset(&rxparms, 0, sizeof(rxparms));
+    memset(&txparms, 0, sizeof(txparms));
+
+    /* These remain constant during the loop. */
+    rxparms.rx_mess = &canmsg;
+    rxparms.rx_timeout = rtems_clock_get_ticks_per_second(); /* 1s timeout */
+    txparms.tx_mess = &canmsg;
+    
+    
+    /* Decide in which direction this task should work. */
+    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);
+    } 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;
+    }
+#endif
+    
+    /* CANs are set to proper baudrate outside of these task due to potential race condition. */
+
+    
+    /* Prepare cleanup for both opened FDs. */
+    pthread_cleanup_push(fd_closer, (void*)&fd_in);
+    pthread_cleanup_push(fd_closer, (void*)&fd_out);
+
+    while (true){
+        /* RTEMS doesn't implement cancellation point inside read(), violating the API's contract. Luckily, pthread_testcancel() works. */
+        pthread_testcancel();
+        res = read(fd_in, &rxparms, sizeof(rxparms));
+        if (res < 0){
+            /* Read error, doesn't really do anything. (Currently) */
+        } else {
+            /* Message was read, now we should resend it. */
+            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;
+}
+
+
+static int start_tasks(){
+    int res;
+
+    
+    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, &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);
+
+    /* Threads are started and running at this point. */
+    return 0;
+}
+
+/*
+* This function starts the GW.
+*
+* It opens two dummy file descriptors to set CAN baudrate (avoiding need of synchronized thread start), starts the forwarding threads.
+*
+* Currently has only simple error handling. 
+*/
+int start_GW(){
+    /* Baudrate is set for dummy FD, it should be remembered by the device. */
+    int res;
+    res = set_baudrate(CAN_GW_BAUDRATE, &fd1, &fd2);
+    printf("Baudrate set.\n");
+    if (res == 0){
+        res = start_tasks();
+        printf("tasks started\n");
+    }
+    return res;
+}
+
+
+/*
+* This function stops threads implementing GW's forwarding. It tries to stop the threads "politely" via pthread_cancel.
+*
+* Error handling is not yet implemented, so it always returns 0 (success).
+*/
+static int end_tasks(){
+    int res;
+    printf("Attempting to stop thread 1\n");
+    res = pthread_cancel(CAN_A_to_B_thread);
+    if (res != 0){
+        printf("Failed.\n");
+        /* Not sure what to do with error here, will have to figure out later. */
+    }
+    printf("Attempting to stop thread 2\n");
+    res = pthread_cancel(CAN_B_to_A_thread);
+    if (res != 0){
+        printf("Failed.\n");
+        /* Not sure what to do with error here, will have to figure out later. */
+    }
+    sleep(1);
+    printf("Both threads should now be stopped.\n");
+    return 0;
+}
+
+
+/*
+* Sets baudrate to the devices via dummy fd.
+*/
+static int set_baudrate(int baudrate, int* fd1, int* fd2){
+    int fd, res;
+    struct mscan_ctrl_parms ctrl_parms;
+    memset(&ctrl_parms, 0, sizeof(ctrl_parms));
+    ctrl_parms.ctrl_can_bitrate = baudrate;
+
+    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){
+        return 1;
+    }
+    res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);
+    if (res < 0) {
+        printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));
+        return 1;
+    }
+    (*fd1) = fd;
+
+    fd = open(MSCAN_B_DEV_NAME, O_RDWR);
+    if (fd < 0){
+        /* Needs to cleanup by closing first fd. */
+        close( (*fd1) );
+        return 1;
+    }
+    res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);
+    if (res < 0) {
+        printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));
+        return 1;
+    }
+    
+    (*fd2) = fd;
+    
+    return 0;
+}
+
+/*
+* Wrapper function and entry point for stopping the GW.
+*/
+int end_GW(){
+    return end_tasks();
+    /* Clean up of dummy FDs. */
+    close(fd1);
+    close(fd2);
+
 }
\ No newline at end of file