Commit fixing that threads not cleaning up after themselves.
[can-benchmark.git] / 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 }