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