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