X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/blobdiff_plain/ed81f09538552b857c45ec9086a99b010ceec38a..0c60e90bdf1a75402f9cc86eb300c5619dd895bc:/rtems/gw/cangw/gw.c

diff --git a/rtems/gw/cangw/gw.c b/rtems/gw/cangw/gw.c
new file mode 100644
index 0000000..87dc756
--- /dev/null
+++ b/rtems/gw/cangw/gw.c
@@ -0,0 +1,236 @@
+#include <system_def.h>
+#include "system.h"
+#include "app_def.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
+
+/* 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.
+* 
+*/
+void* CAN_GW_thread(void* arg){
+    int fd_in, fd_out;
+    int res;
+    unsigned long int *total, *succ, *err;
+    
+    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);
+        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);
+        total = &total_2;
+        succ = &succ_2;
+        err = &err_2;
+    } else {
+        /* Invalid argument, terminates itself. */
+        rtems_task_delete(RTEMS_SELF);
+    }
+    
+    /* 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) {
+                /* Retry? Decide later. */
+                (*err)++;
+            } else {
+                (*succ)++;
+            }
+            (*total)++;
+        }
+    }
+    return NULL;
+}
+
+
+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);
+    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);
+    if (res != 0){
+        /* First thread needs to be aborted before return. */
+        pthread_cancel(CAN_A_to_B_thread);
+        return 1;
+    }
+    
+//    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 %u 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