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