rtems/gw/cangw/gw.c

   1 #include <system_def.h>
   2 #include "app_def.h"
   3 #include "system.h"
   4 #include <stdio.h>
   5 #include <stdlib.h>
   6 #include <rtems/untar.h>
   7 #include <rtems/error.h>
   8 #include <rtems/mw_uid.h>
   9 #include <unistd.h>
  10 #include <fcntl.h>
  11 #include <errno.h>
  12
  13 #include <bsp/mscan.h>
  14 #include <bsp/mscan-base.h>
  15 #include <pthread.h>
  16
  17 #include "gw.h"
  18
  19 /* Local defines. */
  20 #define CAN_GW_A_TO_B_MODE 1
  21 #define CAN_GW_B_TO_A_MODE 2
  22 #define CAN_GW_BAUDRATE 1000000
  23 #define CAN_GW_TASK_HIGH_PRIO 250 /* POSIX API actually has priorities the "right way around" that is, higher is indeed more important */
  24
  25 /* counters for informational purposes */
  26 unsigned long int total_1 = 0, total_2 = 0;
  27 unsigned long int succ_1 = 0, succ_2 = 0;
  28 unsigned long int err_1 = 0, err_2 = 0;
  29
  30 /* Pthread handles for the GW */
  31 static pthread_t CAN_A_to_B_thread, CAN_B_to_A_thread;
  32 /* Dummy FDs for setting baudrate */
  33 static int fd1, fd2;
  34
  35 /* Prototypes for local functions. */
  36 static void* CAN_GW_thread(void* arg);
  37 static void fd_closer(void* arg);
  38 static int start_tasks();
  39 static int end_tasks();
  40 static int set_baudrate(int baudrate, int* fd1, int* fd2);
  41
  42 /*
  43 * Simple cleanup handler for GW threads. Closes opened FD.
  44 */
  45 static void fd_closer(void* arg){
  46     close( (*(int*)arg) );
  47 }
  48
  49
  50
  51 /*
  52 * 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.
  53 *
  54 */
  55 static void* CAN_GW_thread(void* arg){
  56     int fd_in, fd_out;
  57     int res;
  58 #ifndef BENCH_BUILD
  59     unsigned long int *total = NULL, *succ = NULL, *err = NULL; /* Makes compiler shut up about warnings. */
  60 #endif
  61
  62     struct can_message canmsg;
  63     struct mscan_rx_parms rxparms;
  64     struct mscan_tx_parms txparms;
  65     memset(&canmsg, 0, sizeof(canmsg));
  66     memset(&rxparms, 0, sizeof(rxparms));
  67     memset(&txparms, 0, sizeof(txparms));
  68
  69     /* These remain constant during the loop. */
  70     rxparms.rx_mess = &canmsg;
  71     rxparms.rx_timeout = rtems_clock_get_ticks_per_second(); /* 1s timeout */
  72     txparms.tx_mess = &canmsg;
  73
  74
  75     /* Decide in which direction this task should work. */
  76     if (((int)arg) == CAN_GW_A_TO_B_MODE){
  77         fd_in = open(MSCAN_A_DEV_NAME, O_RDWR);
  78         fd_out = open(MSCAN_B_DEV_NAME, O_RDWR);
  79     } else if (((int)arg) == CAN_GW_B_TO_A_MODE){
  80         fd_in = open(MSCAN_B_DEV_NAME, O_RDWR);
  81         fd_out = open(MSCAN_A_DEV_NAME, O_RDWR);
  82     } else {
  83         /* Invalid argument, terminate self. */
  84         return NULL;
  85     }
  86
  87 #ifndef BENCH_BUILD /* Overhead decrease for benching builds.*/
  88     if (((int)arg) == CAN_GW_A_TO_B_MODE){
  89         total = &total_1;
  90         succ = &succ_1;
  91         err = &err_1;
  92     } else {
  93         total = &total_2;
  94         succ = &succ_2;
  95         err = &err_2;
  96     }
  97 #endif
  98
  99     /* CANs are set to proper baudrate outside of these task due to potential race condition. */
 100
 101
 102     /* Prepare cleanup for both opened FDs. */
 103     pthread_cleanup_push(fd_closer, (void*)&fd_in);
 104     pthread_cleanup_push(fd_closer, (void*)&fd_out);
 105
 106     while (true){
 107         /* RTEMS doesn't implement cancellation point inside read(), violating the API's contract. Luckily, pthread_testcancel() works. */
 108         pthread_testcancel();
 109         res = read(fd_in, &rxparms, sizeof(rxparms));
 110         if (res < 0){
 111             /* Read error, doesn't really do anything. (Currently) */
 112         } else {
 113             /* Message was read, now we should resend it. */
 114             txparms.tx_idx = canmsg.mess_id;
 115             res = write(fd_out, &txparms, sizeof(txparms));
 116             if (res < 0) {
 117 #ifndef BENCH_BUILD /* This is the simplest way to remove all the counting */
 118                 /* Retry? Decide later. */
 119                 (*err)++;
 120             } else {
 121                 (*succ)++;
 122             }
 123             (*total)++;
 124 #else
 125             } /* Turns it into empty loop. */
 126 #endif
 127
 128         }
 129     }
 130     return NULL;
 131 }
 132
 133
 134 static int start_tasks(){
 135     int res;
 136
 137
 138     pthread_attr_t attributes;
 139     struct sched_param parm;
 140     parm.sched_priority = CAN_GW_TASK_HIGH_PRIO;
 141     pthread_attr_init(&attributes);
 142 #ifdef HIGH_PRIO /* Without setting PTHREAD_EXPLICIT_SCHED, thread is created with parameters inherited from this thread. */
 143     res = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);
 144 #endif
 145     pthread_attr_setschedparam(&attributes, &parm);
 146
 147     res = pthread_create(&CAN_A_to_B_thread, &attributes, CAN_GW_thread, (void*)CAN_GW_A_TO_B_MODE);
 148     if (res != 0){
 149         /* No cleanup is needed here. */
 150         return 1;
 151     }
 152
 153     res = pthread_create(&CAN_B_to_A_thread, &attributes, CAN_GW_thread, (void*)CAN_GW_B_TO_A_MODE);
 154     if (res != 0){
 155         /* First thread needs to be aborted before return. */
 156         pthread_cancel(CAN_A_to_B_thread);
 157         return 1;
 158     }
 159
 160     pthread_attr_destroy(&attributes);
 161
 162     /* detach is needed so that the threads call clean up handlers automatically. */
 163     pthread_detach(CAN_B_to_A_thread);
 164     pthread_detach(CAN_A_to_B_thread);
 165
 166     /* Threads are started and running at this point. */
 167     return 0;
 168 }
 169
 170 /*
 171 * This function starts the GW.
 172 *
 173 * It opens two dummy file descriptors to set CAN baudrate (avoiding need of synchronized thread start), starts the forwarding threads.
 174 *
 175 * Currently has only simple error handling.
 176 */
 177 int start_GW(){
 178     /* Baudrate is set for dummy FD, it should be remembered by the device. */
 179     int res;
 180     res = set_baudrate(CAN_GW_BAUDRATE, &fd1, &fd2);
 181     printf("Baudrate set.\n");
 182     if (res == 0){
 183         res = start_tasks();
 184         printf("tasks started\n");
 185     }
 186     return res;
 187 }
 188
 189
 190 /*
 191 * This function stops threads implementing GW's forwarding. It tries to stop the threads "politely" via pthread_cancel.
 192 *
 193 * Error handling is not yet implemented, so it always returns 0 (success).
 194 */
 195 static int end_tasks(){
 196     int res;
 197     printf("Attempting to stop thread 1\n");
 198     res = pthread_cancel(CAN_A_to_B_thread);
 199     if (res != 0){
 200         printf("Failed.\n");
 201         /* Not sure what to do with error here, will have to figure out later. */
 202     }
 203     printf("Attempting to stop thread 2\n");
 204     res = pthread_cancel(CAN_B_to_A_thread);
 205     if (res != 0){
 206         printf("Failed.\n");
 207         /* Not sure what to do with error here, will have to figure out later. */
 208     }
 209     sleep(1);
 210     printf("Both threads should now be stopped.\n");
 211     return 0;
 212 }
 213
 214
 215 /*
 216 * Sets baudrate to the devices via dummy fd.
 217 */
 218 static int set_baudrate(int baudrate, int* fd1, int* fd2){
 219     int fd, res;
 220     struct mscan_ctrl_parms ctrl_parms;
 221     memset(&ctrl_parms, 0, sizeof(ctrl_parms));
 222     ctrl_parms.ctrl_can_bitrate = baudrate;
 223
 224     printf("Attempting to set bitrate %"PRIu32" for fd.\n",  ctrl_parms.ctrl_can_bitrate);
 225
 226     fd = open(MSCAN_A_DEV_NAME, O_RDWR);
 227     if (fd < 0){
 228         return 1;
 229     }
 230     res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);
 231     if (res < 0) {
 232         printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));
 233         return 1;
 234     }
 235     (*fd1) = fd;
 236
 237     fd = open(MSCAN_B_DEV_NAME, O_RDWR);
 238     if (fd < 0){
 239         /* Needs to cleanup by closing first fd. */
 240         close( (*fd1) );
 241         return 1;
 242     }
 243     res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);
 244     if (res < 0) {
 245         printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));
 246         return 1;
 247     }
 248
 249     (*fd2) = fd;
 250
 251     return 0;
 252 }
 253
 254 /*
 255 * Wrapper function and entry point for stopping the GW.
 256 */
 257 int end_GW(){
 258     return end_tasks();
 259     /* Clean up of dummy FDs. */
 260     close(fd1);
 261     close(fd2);
 262
 263 }