1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <stdint.h>
4 #include <unistd.h>
5 #include <string.h>
6 #include <signal.h>
7 #include <poll.h>
8 #include <ctype.h>
9 #include <libgen.h>
10 #include <time.h>
11 #include <errno.h>
12
13 #include <sys/time.h>
14 #include <sys/types.h>
15 #include <sys/socket.h>
16 #include <sys/ioctl.h>
17 #include <sys/uio.h>
18 #include <net/if.h>
19 #include <pthread.h>
20
21 #include <linux/can.h>
22 #include <linux/can/raw.h>
23
24 #define DEFAULT_GAP 200 /* ms */
25
26 #define MODE_RANDOM 0
27 #define MODE_INCREMENT 1
28 #define MODE_FIX 2
29
30 static int write_count = 0;
31 static int read_count = 0;
32 static int err_count = 0;
33 static const unsigned char dlc2len[] = {0, 1, 2, 3, 4, 5, 6, 7,
34 8, 12, 16, 20, 24, 32, 48, 64};
35
36 /* get data length from can_dlc with sanitized can_dlc */
can_dlc2len(unsigned char can_dlc)37 unsigned char can_dlc2len(unsigned char can_dlc)
38 {
39 return dlc2len[can_dlc & 0x0F];
40 }
41
42 static const unsigned char len2dlc[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, /* 0 - 8 */
43 9, 9, 9, 9, /* 9 - 12 */
44 10, 10, 10, 10, /* 13 - 16 */
45 11, 11, 11, 11, /* 17 - 20 */
46 12, 12, 12, 12, /* 21 - 24 */
47 13, 13, 13, 13, 13, 13, 13, 13, /* 25 - 32 */
48 14, 14, 14, 14, 14, 14, 14, 14, /* 33 - 40 */
49 14, 14, 14, 14, 14, 14, 14, 14, /* 41 - 48 */
50 15, 15, 15, 15, 15, 15, 15, 15, /* 49 - 56 */
51 15, 15, 15, 15, 15, 15, 15, 15}; /* 57 - 64 */
52
53 /* map the sanitized data length to an appropriate data length code */
can_len2dlc(unsigned char len)54 unsigned char can_len2dlc(unsigned char len)
55 {
56 if (len > 64)
57 return 0xF;
58
59 return len2dlc[len];
60 }
61
62
63 static volatile int running = 1;
64 static unsigned long long enobufs_count;
65
sigterm(int signo)66 void sigterm(int signo)
67 {
68 running = 0;
69 printf("\nCounted %llu ENOBUFS return values on write().\n\n",
70 enobufs_count);
71 printf("write_count = %d read_count = %d err_count = %d\n", write_count, read_count, err_count);
72 exit(0);
73 }
74
canfd_recv(void * argv)75 void *canfd_recv(void *argv)
76 {
77 int mtu, maxdlen;
78 int opt;
79 int s; /* socket */
80
81 struct sockaddr_can addr;
82 static struct canfd_frame frame;
83 int nbytes;
84 int i;
85 struct ifreq ifr;
86
87 struct timeval now;
88
89 /* set seed value for pseudo random numbers */
90 gettimeofday(&now, NULL);
91 srandom(now.tv_usec);
92
93 if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
94 perror("socket");
95 }
96
97 addr.can_family = AF_CAN;
98
99 strcpy(ifr.ifr_name, "can1");
100 if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
101 perror("SIOCGIFINDEX");
102 }
103 addr.can_ifindex = ifr.ifr_ifindex;
104
105 // setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
106
107 int enable_canfd = 1;
108
109 /* check if the frame fits into the CAN netdevice */
110 if (ioctl(s, SIOCGIFMTU, &ifr) < 0) {
111 perror("SIOCGIFMTU");
112 }
113
114 if (ifr.ifr_mtu != CANFD_MTU) {
115 printf("CAN interface ist not CAN FD capable - sorry.\n");
116 }
117
118 /* interface is ok - try to switch the socket into CAN FD mode */
119 if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd))){
120 printf("error when enabling CAN FD support\n");
121 }
122
123 /* ensure discrete CAN FD length values 0..8, 12, 16, 20, 24, 32, 64 */
124 frame.len = can_dlc2len(can_len2dlc(frame.len));
125
126 if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
127 perror("bind");
128 }
129
130 while (1) {
131 int nbytes = read(s, &frame, sizeof(frame));
132 if (nbytes > 0) {
133 printf("nbytes = %d\n", nbytes);
134 printf("ID=0x%x\n", frame.can_id);
135 for (uint8_t i = 0; i < sizeof(frame.data); i++) {
136 if (frame.data[i] == (i + 0x10))
137 printf("%02x ", frame.data[i]);
138 else
139 err_count++;
140 }
141 printf("\n");
142 ++read_count;
143 }
144 }
145
146 }
147
main(int argc,char ** argv)148 int main(int argc, char **argv)
149 {
150 unsigned char canfd = 0;
151 unsigned char mix = 0;
152 int mtu, maxdlen;
153
154 int opt;
155 int s; /* socket */
156 struct pollfd fds;
157
158 struct sockaddr_can addr;
159 static struct canfd_frame frame;
160 int nbytes;
161 int i;
162 struct ifreq ifr;
163
164 struct timeval now;
165
166 system("ip link set can0 up type can bitrate 500000 sample-point 0.75 dbitrate 4000000 dsample-point 0.8 fd on");
167 system("ip link set can1 up type can bitrate 500000 sample-point 0.75 dbitrate 4000000 dsample-point 0.8 fd on");
168 sleep(1);
169
170 /* set seed value for pseudo random numbers */
171 gettimeofday(&now, NULL);
172 srandom(now.tv_usec);
173
174 signal(SIGTERM, sigterm);
175 signal(SIGHUP, sigterm);
176 signal(SIGINT, sigterm);
177
178 mix = 1;
179 canfd = 1; /* to switch the socket into CAN FD mode */
180
181 if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
182 perror("socket");
183 return 1;
184 }
185
186 addr.can_family = AF_CAN;
187
188 strcpy(ifr.ifr_name, "can1");
189 if (ioctl(s, SIOCGIFINDEX, &ifr) < 0) {
190 perror("SIOCGIFINDEX");
191 return 1;
192 }
193 addr.can_ifindex = ifr.ifr_ifindex;
194
195 /* disable default receive filter on this RAW socket */
196 /* This is obsolete as we do not read from the socket at all, but for */
197 /* this reason we can remove the receive list in the Kernel to save a */
198 /* little (really a very little!) CPU usage. */
199 setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
200
201 int enable_canfd = 1;
202
203 /* check if the frame fits into the CAN netdevice */
204 if (ioctl(s, SIOCGIFMTU, &ifr) < 0) {
205 perror("SIOCGIFMTU");
206 return 1;
207 }
208
209 if (ifr.ifr_mtu != CANFD_MTU) {
210 printf("CAN interface ist not CAN FD capable - sorry.\n");
211 return 1;
212 }
213
214 /* interface is ok - try to switch the socket into CAN FD mode */
215 if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd))){
216 printf("error when enabling CAN FD support\n");
217 return 1;
218 }
219
220 /* ensure discrete CAN FD length values 0..8, 12, 16, 20, 24, 32, 64 */
221 frame.len = can_dlc2len(can_len2dlc(frame.len));
222
223
224 if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
225 perror("bind");
226 return 1;
227 }
228
229 pthread_t tid;
230 pthread_create(&tid, NULL, canfd_recv, NULL);
231
232 while (1) {
233 frame.flags = 0;
234
235 mtu = CANFD_MTU;
236
237 frame.can_id = random();
238 frame.can_id &= CAN_SFF_MASK;
239 frame.len = can_dlc2len(0xF);
240
241 for(uint8_t i = 0x0; i < frame.len; i++) {
242 frame.data[i] = 0x10 + i;
243 }
244
245 resend:
246 nbytes = write(s, &frame, mtu);
247 if (nbytes < 0) {
248 if (errno != ENOBUFS) {
249 perror("write");
250 }
251 enobufs_count++;
252
253 } else if (nbytes < mtu) {
254 fprintf(stderr, "write: incomplete CAN frame\n");
255 }
256
257 write_count++;
258
259 frame.can_id++;
260
261 if (mix) {
262 i = random();
263 canfd = i&2;
264 }
265 usleep(100 * 1000);
266 }
267
268 close(s);
269
270 return 0;
271 }
272