rtems/gw/cangw/gw.c

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