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