Initial commit of simple (just forwarding from CAN0 to CAN1 and back) CAN gateway...
[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 //    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 }