1 /*
2 * openwifi side channel user space program
3 * Author: Xianjun Jiao
4 * SPDX-FileCopyrightText: 2019 UGent
5 * SPDX-License-Identifier: AGPL-3.0-or-later
6 */
7
8 #include <sys/socket.h>
9 #include <linux/netlink.h>
10 #include <stdlib.h>
11 #include <string.h>
12 #include <stdio.h>
13 #include <signal.h>
14 #include <stdbool.h>
15 #include <sys/types.h>
16 #include <unistd.h>
17 #include <sys/socket.h>
18 #include <netinet/in.h>
19 #include <netdb.h>
20 #include <arpa/inet.h>
21
22 // #define NETLINK_USER 31
23 #define MAX_NUM_DMA_SYMBOL 8192 //align with side_ch.v side_ch.h
24
25 #define MAX_PAYLOAD (8*MAX_NUM_DMA_SYMBOL) /* maximum payload size*/
26 struct sockaddr_nl src_addr, dest_addr;
27 struct nlmsghdr *nlh = NULL;
28 struct iovec iov;
29 int sock_fd;
30 struct msghdr msg;
31
32 //align with side_ch_control.v and all related user space, remote files
33 #define CSI_LEN 56 // length of single CSI
34 #define EQUALIZER_LEN (56-4) // for non HT, four {32767,32767} will be padded to achieve 52 (non HT should have 48)
35 #define HEADER_LEN 2 //timestamp and frequency offset
36
37 #define ACTION_INVALID 0
38 #define ACTION_REG_WRITE 1
39 #define ACTION_REG_READ 2
40 #define ACTION_SIDE_INFO_GET 3
41
42 #define REG_TYPE_INVALID 0
43 #define REG_TYPE_HARDWARE 1
44 #define REG_TYPE_SOFTWARE 2
45
46 #define MAX_PARA_STRING_LEN 31
47 char tmp_str[MAX_PARA_STRING_LEN+1];
take_reg_idx_string_for_write(char * para)48 int take_reg_idx_string_for_write(char *para) { // return into tmp_str till 'd' 'D' 'h' 'H' or 0
49 int i = 0;
50
51 // while (para[i] != 'd' && para[i] != 'D' && para[i] != 'h' && para[i] != 'H' && para[i] != 0) {
52 while (para[i] != 'd' && para[i] != 'h' && para[i] != 0) {
53 tmp_str[i] = para[i];
54 i++;
55 }
56
57 if (i==0)
58 return(-1);
59
60 if (para[i-1] == 0) // we expect d D h H, not 0!
61 return(-2);
62
63 tmp_str[i] = 0;
64
65 return(i);
66 }
67
take_reg_val_string_for_write(char * para)68 int take_reg_val_string_for_write(char *para) {
69 int i = 0;
70
71 while (para[i] != 0) {
72 tmp_str[i] = para[i];
73 i++;
74 }
75
76 if (i==0)
77 return(-1);
78
79 tmp_str[i] = 0;
80
81 return(i);
82 }
83
all_zero_in_string(char * para)84 int all_zero_in_string(char *para) {
85 int i;
86 int check_len = strlen(para);
87
88 if (check_len == 0)
89 return(-1);
90
91 i = 0;
92 while (para[i] == '0')
93 i++;
94
95 if (i == check_len)
96 return(1);
97
98 return(0);
99 }
100
atoi_my(char * para)101 long int atoi_my(char *para) {
102 long int ret = all_zero_in_string(para);
103
104 if (ret<0)
105 return(-1);
106
107 if (ret==1)
108 return(0);
109
110 ret = atol(para);
111
112 if (ret==0)
113 return(-1);
114
115 return(ret);
116 }
117
hextoi_my(char * para)118 long int hextoi_my(char *para) {
119 long int ret = all_zero_in_string(para);
120
121 if (ret<0)
122 return(-1);
123
124 if (ret==1)
125 return(0);
126
127 ret = strtoul(para, NULL, 16);
128
129 if (ret==0)
130 return(-1);
131
132 return(ret);
133 }
134
135 // parameter_string format:
136 // write 987 to hardware register 3: wh3d987 (w--write; h--hardware; 3 --register idx; d--decimal; 987--value)
137 // write 0x3db to software register 19: ws19h3db (w--write; s--software; 19--register idx; h--hex; 3db--value 0x3db)
138 // read software register 23: rs23 (r-- read; s--software; 23--register idx)
139 // get csi and equalizer output: g400 (g-- get; 400--every 400ms; no/wrong input means default 100ms)
parse_para_string(char * para,int * action_flag,int * reg_type,int * reg_idx,unsigned int * reg_val,int * interval_ms)140 int parse_para_string(char *para, int *action_flag, int *reg_type, int *reg_idx, unsigned int *reg_val, int *interval_ms) {
141 int i, para_string_len, num_char_reg_idx, num_char_reg_val, hex_flag;
142
143 para_string_len = strlen(para);
144
145 if (para_string_len == 0 || para_string_len>MAX_PARA_STRING_LEN) {
146 printf("Parameter string is too short/long!\n");
147 return(-1);
148 }
149
150 // process the csi/equalizer get command
151 if ( para[0] == 'g'){// || para[0] == 'G' ) {
152 (*action_flag) = ACTION_SIDE_INFO_GET;
153
154 if (para_string_len == 1) { // no explicit input
155 (*interval_ms) = 100;
156 printf("The default 100ms side info getting period is taken!\n");
157 return(0);
158 }
159
160 // there is something input
161 (*interval_ms) = atoi_my(para+1);
162 if ( (*interval_ms)<0 ) { // for invalid input, we set it to the default 100ms
163 (*interval_ms) = 100;
164 printf("Invalid side info getting period!\n");
165 printf("The default 100ms side info getting period is taken!\n");
166 }
167
168 return(0);
169 }
170
171 if (para_string_len == 2) {// this is invalid, for read and write, the length should be > 2
172 printf("Lack of input (register index/value) for read/write action\n");
173 return(-2);
174 }
175
176 // process the register read command
177 if ( para[0] == 'r'){// || para[0] == 'R' ) {
178 (*action_flag) = ACTION_REG_READ;
179
180 if ( para[1] == 'h')// || para[1] == 'H' )
181 (*reg_type) = REG_TYPE_HARDWARE;
182 else if ( para[1] == 's')// || para[1] == 'S' )
183 (*reg_type) = REG_TYPE_SOFTWARE;
184 else {
185 (*reg_type) = REG_TYPE_INVALID;
186 printf("Invalid register type (s/h is expected)!\n");
187 return(-3);
188 }
189
190 (*reg_idx) = atoi_my(para+2);
191 if ( (*reg_idx)<0 || (*reg_idx)>31) {
192 printf("Invalid register index (should be 0~31)!\n");
193 return(-4);
194 }
195
196 return(0);
197 }
198
199 if (para_string_len < 5) { // this is invalid, for write, the length should be >= 5. example wh3d9
200 printf("Lack of input (register value/etc) for write action\n");
201 return(-5);
202 }
203
204 // process the register write command
205 if ( para[0] == 'w'){// || para[0] == 'W' ) {
206 (*action_flag) = ACTION_REG_WRITE;
207
208 if ( para[1] == 'h')// || para[1] == 'H' )
209 (*reg_type) = REG_TYPE_HARDWARE;
210 else if ( para[1] == 's')// || para[1] == 'S' )
211 (*reg_type) = REG_TYPE_SOFTWARE;
212 else {
213 (*reg_type) = REG_TYPE_INVALID;
214 printf("Invalid register type (s/h is expected)!\n");
215 return(-6);
216 }
217
218 num_char_reg_idx = take_reg_idx_string_for_write(para+2);
219 if ( num_char_reg_idx<0 ) {
220 printf("Invalid register index input!\n");
221 return(-7);
222 }
223
224 // if ((num_char_reg_idx+2)==para_string_len) //consume all string already
225 // return(-8);
226
227 (*reg_idx) = atoi_my(tmp_str);
228 if ( (*reg_idx)<0 || (*reg_idx)>31 ) {
229 printf("Invalid register index (should be 0~31)!\n");
230 return(-9);
231 }
232
233 if (para[2+num_char_reg_idx] == 'd')// || para[2+num_char_reg_idx] == 'D')
234 hex_flag=0;
235 else if (para[2+num_char_reg_idx] == 'h')// || para[2+num_char_reg_idx] == 'H')
236 hex_flag=1;
237 else {
238 printf("Invalid hex/decimal flag (d/h is expected)!\n");
239 return(-10);
240 }
241
242 num_char_reg_val = take_reg_val_string_for_write(para+2+num_char_reg_idx+1);
243 if ( num_char_reg_val<0 ) {
244 printf("Invalid register value input!\n");
245 return(-11);
246 }
247
248 if (hex_flag==0) {
249 (*reg_val) = atoi_my(tmp_str);
250 if ( (*reg_val)<0 ) {
251 printf("Invalid register value input of decimal number!\n");
252 return(-12);
253 }
254 } else {
255 (*reg_val) = hextoi_my(tmp_str);
256 // printf("%u %s\n", (*reg_val), tmp_str);
257 if ( (*reg_val)<0 ) {
258 printf("Invalid register value input of hex number!\n");
259 return(-13);
260 }
261 }
262 return(0);
263 }
264
265 return(-14);
266 }
267
print_usage(void)268 void print_usage(void) {
269 printf("Usage: side_ch_ctl parameter_string\n");
270 printf("Example:\n");
271 printf("write 987 to hardware register 3: wh3d987 (w--write; h--hardware; 3 --register idx; d--decimal; 987--value)\n");
272 printf("write 0x3db to software register 19: ws19h3db (w--write; s--software; 19--register idx; h--hex; 3db--value 0x3db)\n");
273 printf(" read software register 23: rs23 (r-- read; s--software; 23--register idx)\n");
274 printf(" get csi and equalizer output: g400 (g-- get; 400--every 400ms; no/wrong input means default 100ms)\n");
275 }
276
277 volatile bool do_exit = false;
278
sigint_callback_handler(int signum)279 void sigint_callback_handler(int signum)
280 {
281 fprintf(stdout, "Caught signal %d\n", signum);
282 do_exit = true;
283 }
284
main(const int argc,char * const argv[])285 int main(const int argc, char * const argv[])
286 {
287 int action_flag, reg_type, reg_idx, interval_ms, s, side_info_size, socket_ok = 1, loop_count=0, side_info_count=0;
288 unsigned int reg_val, *cmd_buf;
289 unsigned short port;
290 struct sockaddr_in server;
291 int value_only_flag = 0;
292 int ret = 0;
293
294 if (argc<2) {
295 printf("1 argument is needed!\n");
296 print_usage();
297 return(ret);
298 }
299
300 value_only_flag = (argc>2?1:0);
301
302 ret = parse_para_string(argv[1], &action_flag, ®_type, ®_idx, ®_val, &interval_ms);
303 if (value_only_flag==0) {
304 printf("parse: ret %d\n", ret);
305 printf(" tx: action_flag %d reg_type %d reg_idx %d reg_val %u interval_ms %d\n", action_flag, reg_type, reg_idx, reg_val, interval_ms);
306 }
307 if (ret<0) {
308 printf("Wrong input!\n");
309 print_usage();
310 return(ret);
311 }
312
313 // if (signal(SIGINT, &sigint_callback_handler)==SIG_ERR ||
314 // signal(SIGILL, &sigint_callback_handler)==SIG_ERR ||
315 // signal(SIGFPE, &sigint_callback_handler)==SIG_ERR ||
316 // signal(SIGSEGV, &sigint_callback_handler)==SIG_ERR ||
317 // signal(SIGTERM, &sigint_callback_handler)==SIG_ERR ||
318 // signal(SIGABRT, &sigint_callback_handler)==SIG_ERR) {
319 if (signal(SIGINT, &sigint_callback_handler)==SIG_ERR) {
320 printf("SIG_ERR!\n");
321 return(ret);
322 }
323
324 sock_fd=socket(PF_NETLINK, SOCK_RAW, NETLINK_USERSOCK);
325 if(sock_fd<0) {
326 printf("sock_fd %d\n", sock_fd);
327 return -1;
328 }
329
330 memset(&src_addr, 0, sizeof(src_addr));
331 src_addr.nl_family = AF_NETLINK;
332 src_addr.nl_pid = getpid(); /* self pid */
333
334 bind(sock_fd, (struct sockaddr*)&src_addr, sizeof(src_addr));
335
336 memset(&dest_addr, 0, sizeof(dest_addr));
337 memset(&dest_addr, 0, sizeof(dest_addr));
338 dest_addr.nl_family = AF_NETLINK;
339 dest_addr.nl_pid = 0; /* For Linux Kernel */
340 dest_addr.nl_groups = 0; /* unicast */
341
342 nlh = (struct nlmsghdr *)malloc(NLMSG_SPACE(MAX_PAYLOAD));
343 // memset(nlh, 0, NLMSG_SPACE(MAX_PAYLOAD));
344
345 // nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD);
346
347 // strcpy(NLMSG_DATA(nlh), "Hello");
348
349 // udp socket setup
350 port = htons(4000); // port 4000 at remote server
351 if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0){
352 printf("socket() error! Will not send info to remote.\n");
353 socket_ok = 0;
354 }
355 server.sin_family = AF_INET; /* Internet Domain */
356 server.sin_port = port; /* Server Port */
357 server.sin_addr.s_addr = inet_addr("192.168.10.1"); /* Server's Address */
358
359 while(do_exit==false) {
360 nlh->nlmsg_len = NLMSG_SPACE(4*4);
361 nlh->nlmsg_pid = getpid();
362 nlh->nlmsg_flags = 0;
363
364 cmd_buf = (unsigned int*)NLMSG_DATA(nlh);
365 cmd_buf[0] = action_flag;
366 cmd_buf[1] = reg_type;
367 cmd_buf[2] = reg_idx;
368 cmd_buf[3] = reg_val;
369
370 iov.iov_base = (void *)nlh;
371 iov.iov_len = nlh->nlmsg_len;
372 msg.msg_name = (void *)&dest_addr;
373 msg.msg_namelen = sizeof(dest_addr);
374 msg.msg_iov = &iov;
375 msg.msg_iovlen = 1;
376
377 // printf("Sending message to kernel\n");
378 sendmsg(sock_fd,&msg,0);
379 // printf("Waiting for message from kernel\n");
380
381 nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD);
382 iov.iov_len = nlh->nlmsg_len;
383 /* Read message from kernel */
384 recvmsg(sock_fd, &msg, 0);
385 // printf("Received message payload: %s\n", (char *)NLMSG_DATA(nlh));
386
387 side_info_size = nlh->nlmsg_len-NLMSG_HDRLEN;
388 // printf("num_dma_symbol %d\n", side_info_size/8);
389
390 if (action_flag!=ACTION_SIDE_INFO_GET) {
391 if (value_only_flag==0)
392 printf(" rx: size %d val %d 0x%08x\n", side_info_size, cmd_buf[0], cmd_buf[0]);
393 else
394 printf("%u\n", cmd_buf[0]);
395 break;
396 }
397
398 if (socket_ok && (side_info_size >= ((CSI_LEN+0*EQUALIZER_LEN+HEADER_LEN)*8)))
399 if (sendto(s, cmd_buf, side_info_size, 0, (struct sockaddr *)&server, sizeof(server)) < 0)
400 printf("sendto() error!\n");
401
402 side_info_count = side_info_count + (side_info_size>4);
403 loop_count++;
404 if ((loop_count%64) == 0)
405 printf("loop %d side info count %d\n", loop_count, side_info_count);
406
407 usleep(interval_ms*1000);
408 }
409
410 close(s);
411 close(sock_fd);
412 return(ret);
413 }
414