rtems/gw/cangw/gw.c

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