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 //    pthread_detach(CAN_B_to_A_thread);\r
 137 //    pthread_detach(CAN_A_to_B_thread);\r
 138 \r
 139     /* Threads are started and running at this point. */\r
 140     return 0;\r
 141 }\r
 142 \r
 143 /*\r
 144 * This function starts the GW.\r
 145 *\r
 146 * It opens two dummy file descriptors to set CAN baudrate (avoiding need of synchronized thread start), starts the forwarding threads.\r
 147 *\r
 148 * Currently has only simple error handling. \r
 149 */\r
 150 int start_GW(){\r
 151     /* Baudrate is set for dummy FD, it should be remembered by the device. */\r
 152     int res;\r
 153     res = set_baudrate(CAN_GW_BAUDRATE, &fd1, &fd2);\r
 154     printf("Baudrate set.\n");\r
 155     if (res == 0){\r
 156         res = start_tasks();\r
 157         printf("tasks started\n");\r
 158     }\r
 159     return res;\r
 160 }\r
 161 \r
 162 \r
 163 /*\r
 164 * This function stops threads implementing GW's forwarding. It tries to stop the threads "politely" via pthread_cancel.\r
 165 *\r
 166 * Error handling is not yet implemented, so it always returns 0 (success).\r
 167 */\r
 168 static int end_tasks(){\r
 169     int res;\r
 170     printf("Attempting to stop thread 1\n");\r
 171     res = pthread_cancel(CAN_A_to_B_thread);\r
 172     if (res != 0){\r
 173         printf("Failed./n");\r
 174         /* Not sure what to do with error here, will have to figure out later. */\r
 175     }\r
 176     printf("Attempting to stop thread 2\n");\r
 177     res = pthread_cancel(CAN_B_to_A_thread);\r
 178     if (res != 0){\r
 179         printf("Failed./n");\r
 180         /* Not sure what to do with error here, will have to figure out later. */\r
 181     }\r
 182     sleep(1);\r
 183     printf("Both threads should now be stopped.\n");\r
 184     return 0;\r
 185 }\r
 186 \r
 187 \r
 188 /*\r
 189 * Sets baudrate to the devices via dummy fd.\r
 190 */\r
 191 static int set_baudrate(int baudrate, int* fd1, int* fd2){\r
 192     int fd, res;\r
 193     struct mscan_ctrl_parms ctrl_parms;\r
 194     memset(&ctrl_parms, 0, sizeof(ctrl_parms));\r
 195     ctrl_parms.ctrl_can_bitrate = baudrate;\r
 196 \r
 197     printf("Attempting to set bitrate %u for fd.\n",  ctrl_parms.ctrl_can_bitrate);\r
 198     \r
 199     fd = open(MSCAN_A_DEV_NAME, O_RDWR);\r
 200     if (fd < 0){\r
 201         return 1;\r
 202     }\r
 203     res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);\r
 204     if (res < 0) {\r
 205         printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));\r
 206         return 1;\r
 207     }\r
 208     (*fd1) = fd;\r
 209 \r
 210     fd = open(MSCAN_B_DEV_NAME, O_RDWR);\r
 211     if (fd < 0){\r
 212         /* Needs to cleanup by closing first fd. */\r
 213         close( (*fd1) );\r
 214         return 1;\r
 215     }\r
 216     res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);\r
 217     if (res < 0) {\r
 218         printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));\r
 219         return 1;\r
 220     }\r
 221     \r
 222     (*fd2) = fd;\r
 223     \r
 224     return 0;\r
 225 }\r
 226 \r
 227 /*\r
 228 * Wrapper function and entry point for stopping the GW.\r
 229 */\r
 230 int end_GW(){\r
 231     return end_tasks();\r
 232     /* Clean up of dummy FDs. */\r
 233     close(fd1);\r
 234     close(fd2);\r
 235 \r
 236 }