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]; 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 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 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 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 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) 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 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 279 void sigint_callback_handler(int signum) 280 { 281 fprintf(stdout, "Caught signal %d\n", signum); 282 do_exit = true; 283 } 284 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 ret = 0; 292 293 if (argc!=2) { 294 printf("Wrong input!\n"); 295 print_usage(); 296 return(ret); 297 } 298 299 ret = parse_para_string(argv[1], &action_flag, ®_type, ®_idx, ®_val, &interval_ms); 300 printf("parse: ret %d\n", ret); 301 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); 302 if (ret<0) { 303 printf("Wrong input!\n"); 304 print_usage(); 305 return(ret); 306 } 307 308 // if (signal(SIGINT, &sigint_callback_handler)==SIG_ERR || 309 // signal(SIGILL, &sigint_callback_handler)==SIG_ERR || 310 // signal(SIGFPE, &sigint_callback_handler)==SIG_ERR || 311 // signal(SIGSEGV, &sigint_callback_handler)==SIG_ERR || 312 // signal(SIGTERM, &sigint_callback_handler)==SIG_ERR || 313 // signal(SIGABRT, &sigint_callback_handler)==SIG_ERR) { 314 if (signal(SIGINT, &sigint_callback_handler)==SIG_ERR) { 315 printf("SIG_ERR!\n"); 316 return(ret); 317 } 318 319 sock_fd=socket(PF_NETLINK, SOCK_RAW, NETLINK_USERSOCK); 320 if(sock_fd<0) { 321 printf("sock_fd %d\n", sock_fd); 322 return -1; 323 } 324 325 memset(&src_addr, 0, sizeof(src_addr)); 326 src_addr.nl_family = AF_NETLINK; 327 src_addr.nl_pid = getpid(); /* self pid */ 328 329 bind(sock_fd, (struct sockaddr*)&src_addr, sizeof(src_addr)); 330 331 memset(&dest_addr, 0, sizeof(dest_addr)); 332 memset(&dest_addr, 0, sizeof(dest_addr)); 333 dest_addr.nl_family = AF_NETLINK; 334 dest_addr.nl_pid = 0; /* For Linux Kernel */ 335 dest_addr.nl_groups = 0; /* unicast */ 336 337 nlh = (struct nlmsghdr *)malloc(NLMSG_SPACE(MAX_PAYLOAD)); 338 // memset(nlh, 0, NLMSG_SPACE(MAX_PAYLOAD)); 339 340 // nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD); 341 342 // strcpy(NLMSG_DATA(nlh), "Hello"); 343 344 // udp socket setup 345 port = htons(4000); // port 4000 at remote server 346 if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0){ 347 printf("socket() error! Will not send info to remote.\n"); 348 socket_ok = 0; 349 } 350 server.sin_family = AF_INET; /* Internet Domain */ 351 server.sin_port = port; /* Server Port */ 352 server.sin_addr.s_addr = inet_addr("192.168.10.1"); /* Server's Address */ 353 354 while(do_exit==false) { 355 nlh->nlmsg_len = NLMSG_SPACE(4*4); 356 nlh->nlmsg_pid = getpid(); 357 nlh->nlmsg_flags = 0; 358 359 cmd_buf = (unsigned int*)NLMSG_DATA(nlh); 360 cmd_buf[0] = action_flag; 361 cmd_buf[1] = reg_type; 362 cmd_buf[2] = reg_idx; 363 cmd_buf[3] = reg_val; 364 365 iov.iov_base = (void *)nlh; 366 iov.iov_len = nlh->nlmsg_len; 367 msg.msg_name = (void *)&dest_addr; 368 msg.msg_namelen = sizeof(dest_addr); 369 msg.msg_iov = &iov; 370 msg.msg_iovlen = 1; 371 372 // printf("Sending message to kernel\n"); 373 sendmsg(sock_fd,&msg,0); 374 // printf("Waiting for message from kernel\n"); 375 376 nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD); 377 iov.iov_len = nlh->nlmsg_len; 378 /* Read message from kernel */ 379 recvmsg(sock_fd, &msg, 0); 380 // printf("Received message payload: %s\n", (char *)NLMSG_DATA(nlh)); 381 382 side_info_size = nlh->nlmsg_len-NLMSG_HDRLEN; 383 // printf("num_dma_symbol %d\n", side_info_size/8); 384 385 if (action_flag!=ACTION_SIDE_INFO_GET) { 386 printf(" rx: size %d val %d 0x%08x\n", side_info_size, cmd_buf[0], cmd_buf[0]); 387 break; 388 } 389 390 if (socket_ok && (side_info_size >= ((CSI_LEN+0*EQUALIZER_LEN+HEADER_LEN)*8))) 391 if (sendto(s, cmd_buf, side_info_size, 0, (struct sockaddr *)&server, sizeof(server)) < 0) 392 printf("sendto() error!\n"); 393 394 side_info_count = side_info_count + (side_info_size>4); 395 loop_count++; 396 if ((loop_count%64) == 0) 397 printf("loop %d side info count %d\n", loop_count, side_info_count); 398 399 usleep(interval_ms*1000); 400 } 401 402 close(s); 403 close(sock_fd); 404 return(ret); 405 } 406