+#include <system_def.h>\r
+#include "system.h"\r
+#include "app_def.h"\r
+#include <stdio.h>\r
+#include <stdlib.h>\r
+#include <rtems/untar.h>\r
+#include <rtems/error.h>\r
+#include <rtems/mw_uid.h>\r
+#include <unistd.h>\r
+#include <fcntl.h>\r
+#include <errno.h>\r
+\r
+#include <bsp/mscan.h>\r
+#include <bsp/mscan-base.h>\r
+#include <pthread.h>\r
+\r
+#include "gw.h"\r
+\r
+/* Local defines. */\r
+#define CAN_GW_A_TO_B_MODE 1\r
+#define CAN_GW_B_TO_A_MODE 2\r
+#define CAN_GW_BAUDRATE 1000000\r
+\r
+/* counters for informational purposes */\r
+unsigned long int total_1 = 0, total_2 = 0;\r
+unsigned long int succ_1 = 0, succ_2 = 0; \r
+unsigned long int err_1 = 0, err_2 = 0;\r
+\r
+/* Pthread handles for the GW */\r
+static pthread_t CAN_A_to_B_thread, CAN_B_to_A_thread;\r
+/* Dummy FDs for setting baudrate */\r
+static int fd1, fd2;\r
+\r
+/* Prototypes for local functions. */\r
+static void* CAN_GW_thread(void* arg);\r
+static void fd_closer(void* arg);\r
+static int start_tasks();\r
+static int end_tasks();\r
+static int set_baudrate(int baudrate, int* fd1, int* fd2);\r
+\r
+/*\r
+* Simple cleanup handler for GW threads. Closes opened FD.\r
+*/\r
+static void fd_closer(void* arg){\r
+ close( (*(int*)arg) );\r
+}\r
+\r
+\r
+\r
+/*\r
+* 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
+* \r
+*/\r
+void* CAN_GW_thread(void* arg){\r
+ int fd_in, fd_out;\r
+ int res;\r
+ unsigned long int *total, *succ, *err;\r
+ \r
+ struct can_message canmsg;\r
+ struct mscan_rx_parms rxparms;\r
+ struct mscan_tx_parms txparms;\r
+ memset(&canmsg, 0, sizeof(canmsg));\r
+ memset(&rxparms, 0, sizeof(rxparms));\r
+ memset(&txparms, 0, sizeof(txparms));\r
+\r
+ /* These remain constant during the loop. */\r
+ rxparms.rx_mess = &canmsg;\r
+ rxparms.rx_timeout = rtems_clock_get_ticks_per_second(); /* 1s timeout */\r
+ txparms.tx_mess = &canmsg;\r
+ \r
+ \r
+ /* Decide in which direction this task should work. */\r
+ if (((int)arg) == CAN_GW_A_TO_B_MODE){\r
+ fd_in = open(MSCAN_A_DEV_NAME, O_RDWR);\r
+ fd_out = open(MSCAN_B_DEV_NAME, O_RDWR);\r
+ total = &total_1;\r
+ succ = &succ_1;\r
+ err = &err_1;\r
+ } else if (((int)arg) == CAN_GW_B_TO_A_MODE){\r
+ fd_in = open(MSCAN_B_DEV_NAME, O_RDWR);\r
+ fd_out = open(MSCAN_A_DEV_NAME, O_RDWR);\r
+ total = &total_2;\r
+ succ = &succ_2;\r
+ err = &err_2;\r
+ } else {\r
+ /* Invalid argument, terminates itself. */\r
+ rtems_task_delete(RTEMS_SELF);\r
+ }\r
+ \r
+ /* CANs are set to proper baudrate outside of these task due to potential race condition. */\r
+\r
+ \r
+ /* Prepare cleanup for both opened FDs. */\r
+ pthread_cleanup_push(fd_closer, (void*)&fd_in);\r
+ pthread_cleanup_push(fd_closer, (void*)&fd_out);\r
+\r
+ while (true){\r
+ /* RTEMS doesn't implement cancellation point inside read(), violating the API's contract. Luckily, pthread_testcancel() works. */\r
+ pthread_testcancel();\r
+ res = read(fd_in, &rxparms, sizeof(rxparms));\r
+ if (res < 0){\r
+ /* Read error, doesn't really do anything. (Currently) */\r
+ } else {\r
+ /* Message was read, now we should resend it. */\r
+ txparms.tx_idx = canmsg.mess_id;\r
+ res = write(fd_out, &txparms, sizeof(txparms));\r
+ if (res < 0) {\r
+ /* Retry? Decide later. */\r
+ (*err)++;\r
+ } else {\r
+ (*succ)++;\r
+ }\r
+ (*total)++;\r
+ }\r
+ }\r
+ return NULL;\r
+}\r
+\r
+\r
+static int start_tasks(){\r
+ int res;\r
+ \r
+ res = pthread_create(&CAN_A_to_B_thread, NULL, CAN_GW_thread, (void*)CAN_GW_A_TO_B_MODE);\r
+ if (res != 0){\r
+ /* No cleanup is needed here. */\r
+ return 1;\r
+ }\r
+\r
+ res = pthread_create(&CAN_B_to_A_thread, NULL, CAN_GW_thread, (void*)CAN_GW_B_TO_A_MODE);\r
+ if (res != 0){\r
+ /* First thread needs to be aborted before return. */\r
+ pthread_cancel(CAN_A_to_B_thread);\r
+ return 1;\r
+ }\r
+ \r
+// pthread_detach(CAN_B_to_A_thread);\r
+// pthread_detach(CAN_A_to_B_thread);\r
+\r
+ /* Threads are started and running at this point. */\r
+ return 0;\r
+}\r
+\r
+/*\r
+* This function starts the GW.\r
+*\r
+* It opens two dummy file descriptors to set CAN baudrate (avoiding need of synchronized thread start), starts the forwarding threads.\r
+*\r
+* Currently has only simple error handling. \r
+*/\r
+int start_GW(){\r
+ /* Baudrate is set for dummy FD, it should be remembered by the device. */\r
+ int res;\r
+ res = set_baudrate(CAN_GW_BAUDRATE, &fd1, &fd2);\r
+ printf("Baudrate set.\n");\r
+ if (res == 0){\r
+ res = start_tasks();\r
+ printf("tasks started\n");\r
+ }\r
+ return res;\r
+}\r
+\r
+\r
+/*\r
+* This function stops threads implementing GW's forwarding. It tries to stop the threads "politely" via pthread_cancel.\r
+*\r
+* Error handling is not yet implemented, so it always returns 0 (success).\r
+*/\r
+static int end_tasks(){\r
+ int res;\r
+ printf("Attempting to stop thread 1\n");\r
+ res = pthread_cancel(CAN_A_to_B_thread);\r
+ if (res != 0){\r
+ printf("Failed./n");\r
+ /* Not sure what to do with error here, will have to figure out later. */\r
+ }\r
+ printf("Attempting to stop thread 2\n");\r
+ res = pthread_cancel(CAN_B_to_A_thread);\r
+ if (res != 0){\r
+ printf("Failed./n");\r
+ /* Not sure what to do with error here, will have to figure out later. */\r
+ }\r
+ sleep(1);\r
+ printf("Both threads should now be stopped.\n");\r
+ return 0;\r
+}\r
+\r
+\r
+/*\r
+* Sets baudrate to the devices via dummy fd.\r
+*/\r
+static int set_baudrate(int baudrate, int* fd1, int* fd2){\r
+ int fd, res;\r
+ struct mscan_ctrl_parms ctrl_parms;\r
+ memset(&ctrl_parms, 0, sizeof(ctrl_parms));\r
+ ctrl_parms.ctrl_can_bitrate = baudrate;\r
+\r
+ printf("Attempting to set bitrate %u for fd.\n", ctrl_parms.ctrl_can_bitrate);\r
+ \r
+ fd = open(MSCAN_A_DEV_NAME, O_RDWR);\r
+ if (fd < 0){\r
+ return 1;\r
+ }\r
+ res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);\r
+ if (res < 0) {\r
+ printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));\r
+ return 1;\r
+ }\r
+ (*fd1) = fd;\r
+\r
+ fd = open(MSCAN_B_DEV_NAME, O_RDWR);\r
+ if (fd < 0){\r
+ /* Needs to cleanup by closing first fd. */\r
+ close( (*fd1) );\r
+ return 1;\r
+ }\r
+ res = ioctl(fd, MSCAN_SET_BAUDRATE, &ctrl_parms);\r
+ if (res < 0) {\r
+ printf("fd - MSCAN_SET_BAUDRATE error (%x:%x) %s\n", res, errno, strerror(errno));\r
+ return 1;\r
+ }\r
+ \r
+ (*fd2) = fd;\r
+ \r
+ return 0;\r
+}\r
+\r
+/*\r
+* Wrapper function and entry point for stopping the GW.\r
+*/\r
+int end_GW(){\r
+ return end_tasks();\r
+ /* Clean up of dummy FDs. */\r
+ close(fd1);\r
+ close(fd2);\r
+\r
+}
\ No newline at end of file
projects / can-benchmark.git / blobdiff