X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/8eef03acb9747a40aaf92175c49e43397b872404..ddc40c70cfce8c93b228bc28a3144da45acd6708:/rtems/gw/cangw/gw.c

diff --git a/rtems/gw/cangw/gw.c b/rtems/gw/cangw/gw.c
index 2a08a0c..1b8a80d 100644
--- a/rtems/gw/cangw/gw.c
+++ b/rtems/gw/cangw/gw.c
@@ -1,263 +1,263 @@
-#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);
-
+#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