1 /* Industrialio buffer test code. 2 * 3 * Copyright (c) 2012 Invensense Inc. 4 * 5 * This program is free software; you can redistribute it and/or modify it 6 * under the terms of the GNU General Public License version 2 as published by 7 * the Free Software Foundation. 8 * 9 * Command line parameters 10 * stress_iio -d time1 -e time2 11 */ 12 13 #include <unistd.h> 14 #include <dirent.h> 15 #include <fcntl.h> 16 #include <stdio.h> 17 #include <errno.h> 18 #include <sys/stat.h> 19 #include <dirent.h> 20 #include <linux/types.h> 21 #include <string.h> 22 #include <poll.h> 23 #include <pthread.h> 24 #include "iio_utils.h" 25 #include "ml_load_dmp.h" 26 #include "ml_sysfs_helper.h" 27 #include "authenticate.h" 28 29 pthread_mutex_t data_switch_lock = PTHREAD_MUTEX_INITIALIZER; 30 31 static int has_compass = 0; 32 static int has_pressure = 0; 33 static int enable_random_delay = 0; 34 static int enable_delay = 10; 35 static int disable_delay = 10; 36 static int enable_motion_on = 0; 37 static int final_output_rate; 38 static int first_flag; 39 static char dmp_path[200]; 40 41 static int dev_num; 42 static char *dev_dir_name; 43 static char *buf_dir_name; 44 static char *scan_el_dir; 45 static int gyro_data_is_enabled, accel_data_is_enabled, compass_data_is_enabled, quaternion_data_is_enabled; 46 static int accel_engine_is_on; 47 48 struct dmp_struct { 49 char fname[100]; 50 void (*action)(struct dmp_struct *, int); 51 }; 52 53 static void HandleTap(struct dmp_struct *dmp, int tap); 54 static void sipmle_print(struct dmp_struct *dmp, int d){ 55 printf("%s:%d\n", dmp->fname, d); 56 } 57 58 static void handle_smd() { 59 printf("write wake lock for SMD\n"); 60 //write_sysfs_string_and_verify("wake_lock", "/sys/power/", "hack"); 61 } 62 63 static void pedo_print() 64 { 65 struct timespec aa; 66 unsigned long long t; 67 68 clock_gettime(CLOCK_REALTIME, &aa); 69 t = (unsigned long long)aa.tv_sec * 1000000000 + aa.tv_nsec; 70 printf("steps=%lld, time=%lld, system=%lld\n", 71 read_sysfs_poslonglong("pedometer_steps", dev_dir_name), 72 read_sysfs_poslonglong("pedometer_time", dev_dir_name), 73 t); 74 } 75 76 struct dmp_struct event_file[] = { 77 #if 1 78 { 79 .fname = "event_tap", 80 .action = HandleTap, 81 }, 82 #endif 83 { 84 .fname = "event_smd", 85 .action = handle_smd, 86 }, 87 { 88 .fname = "event_accel_motion", 89 .action = sipmle_print, 90 }, 91 { 92 .fname = "event_pedometer", 93 .action = pedo_print, 94 }, 95 }; 96 97 static void HandleTap(struct dmp_struct *dmp, int tap) 98 { 99 int tap_dir = tap/8; 100 int tap_num = tap%8 + 1; 101 102 switch (tap_dir) { 103 case 1: 104 printf("INV_TAP_AXIS_X_POS\n"); 105 break; 106 case 2: 107 printf("INV_TAP_AXIS_X_NEG\n"); 108 break; 109 case 3: 110 printf("INV_TAP_AXIS_Y_POS\n"); 111 break; 112 case 4: 113 printf("INV_TAP_AXIS_Y_NEG\n"); 114 break; 115 case 5: 116 printf("INV_TAP_AXIS_Z_POS\n"); 117 break; 118 case 6: 119 printf("INV_TAP_AXIS_Z_NEG\n"); 120 break; 121 default: 122 break; 123 } 124 printf("Tap number: %d\n", tap_num); 125 } 126 #define DMP_CODE_SIZE 2799 127 static char dmp_img[DMP_CODE_SIZE]; 128 static void verify_img(){ 129 FILE *fp; 130 int i; 131 char dmp_path[] = "/sys/bus/iio/devices/iio:device0/dmp_firmware"; 132 133 printf("saving image\n"); 134 if ((fp = fopen(dmp_path, "rb")) < 0 ) { 135 perror("dmp fail"); 136 } 137 138 i = fread(dmp_img, 1, DMP_CODE_SIZE, fp); 139 printf("Result=%d\n", i); 140 fclose(fp); 141 fp = fopen("/dev/read_img.h", "wt"); 142 fprintf(fp, "unsigned char rec[]={\n"); 143 for(i=0; i<DMP_CODE_SIZE; i++) { 144 fprintf(fp, "0x%02x, ", dmp_img[i]); 145 //printf( "0x%02x, ", dmp_img[i]); 146 if(((i+1)%16) == 0) { 147 fprintf(fp, "\n"); 148 //printf("\n"); 149 } 150 } 151 fprintf(fp, "};\n "); 152 fclose(fp); 153 printf("saving image Done\n"); 154 } 155 156 static void inv_set_rate() 157 { 158 int ret; 159 160 printf("set rate \n"); 161 ret = write_sysfs_int_and_verify("accel_rate", dev_dir_name, 5); 162 ret = write_sysfs_int_and_verify("gyro_rate", dev_dir_name, 5); 163 if (has_compass) 164 ret = write_sysfs_int_and_verify("compass_rate", dev_dir_name, 10); 165 if (has_pressure) 166 ret = write_sysfs_int_and_verify("pressure_rate", dev_dir_name, 30); 167 ret = write_sysfs_int_and_verify("ped_q_rate", dev_dir_name, 5); 168 ret = write_sysfs_int_and_verify("six_axes_q_rate", dev_dir_name, 5); 169 ret = write_sysfs_int_and_verify("three_axes_q_rate", dev_dir_name, 5); 170 } 171 172 173 static int setup_offset_and_bias() 174 { 175 int ret; 176 177 ret = write_sysfs_int_and_verify("in_accel_x_offset", dev_dir_name, 0); 178 if (ret < 0) 179 printf("write accel x offset failed.\n"); 180 ret = write_sysfs_int_and_verify("in_accel_y_offset", dev_dir_name, 0); 181 if (ret < 0) 182 printf("write accel y offset failed.\n"); 183 ret = write_sysfs_int_and_verify("in_accel_z_offset", dev_dir_name, 0); 184 if (ret < 0) 185 printf("write accel z offset failed.\n"); 186 187 ret = write_sysfs_int_and_verify("in_anglvel_x_offset", dev_dir_name, 0); 188 if (ret < 0) 189 printf("write accel x offset failed.\n"); 190 ret = write_sysfs_int_and_verify("in_anglvel_y_offset", dev_dir_name, 0); 191 if (ret < 0) 192 printf("write accel y offset failed.\n"); 193 ret = write_sysfs_int_and_verify("in_anglvel_z_offset", dev_dir_name, 0); 194 if (ret < 0) 195 printf("write accel z offset failed.\n"); 196 197 ret = write_sysfs_int_and_verify("in_accel_x_dmp_bias", dev_dir_name, 0); 198 if (ret < 0) 199 printf("write accel x offset failed.\n"); 200 ret = write_sysfs_int_and_verify("in_accel_y_dmp_bias", dev_dir_name, 0); 201 if (ret < 0) 202 printf("write accel y offset failed.\n"); 203 ret = write_sysfs_int_and_verify("in_accel_z_dmp_bias", dev_dir_name, 0); 204 if (ret < 0) 205 printf("write accel z offset failed.\n"); 206 207 ret = write_sysfs_int_and_verify("in_anglvel_x_dmp_bias", dev_dir_name, 0); 208 if (ret < 0) 209 printf("write gyro x offset failed.\n"); 210 ret = write_sysfs_int_and_verify("in_anglvel_y_dmp_bias", dev_dir_name, 0); 211 if (ret < 0) 212 printf("write gyro y offset failed.\n"); 213 ret = write_sysfs_int_and_verify("in_anglvel_z_dmp_bias", dev_dir_name, 0); 214 if (ret < 0) 215 printf("write gyro z offset failed.\n"); 216 217 return 0; 218 } 219 220 static void setup_dmp(char *dev_path){ 221 char sysfs_path[200]; 222 int ret; 223 FILE *fd; 224 sprintf(sysfs_path, "%s", dev_path); 225 printf("sysfs: %s\n", sysfs_path); 226 ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1); 227 if (ret < 0) 228 return; 229 230 ret = write_sysfs_int("in_accel_scale", dev_path, 0); 231 if (ret < 0) 232 return; 233 ret = write_sysfs_int("in_anglvel_scale", dev_path, 3); 234 if (ret < 0) 235 return; 236 ret = write_sysfs_int("sampling_frequency", sysfs_path, 200); 237 if (ret < 0) 238 return; 239 ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0); 240 if (ret < 0) 241 return; 242 sprintf(dmp_path, "%s/dmp_firmware", dev_path); 243 if ((fd = fopen(dmp_path, "wb")) < 0 ) { 244 perror("dmp fail"); 245 } 246 inv_load_dmp(fd); 247 fclose(fd); 248 printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path)); 249 ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1); 250 if (ret < 0) 251 return; 252 ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1); 253 if (ret < 0) 254 return; 255 /* selelct which event to enable and interrupt on/off here */ 256 //enable_glu(sysfs_path, 0); 257 // ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 0); 258 // if (ret < 0) 259 // return; 260 ret = write_sysfs_int_and_verify("pedometer_int_on", sysfs_path, 1); 261 ret = write_sysfs_int_and_verify("pedometer_on", sysfs_path, 1); 262 263 ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, 1); 264 write_sysfs_int64("pedometer_steps", sysfs_path, 0); 265 write_sysfs_int64("pedometer_time", sysfs_path, 0); 266 if (ret < 0) 267 return; 268 269 ret = setup_offset_and_bias(); 270 271 return; 272 } 273 274 #if 0 275 static char reg_dump_arr[2000]; 276 static int inv_do_reg_dump(void) 277 { 278 char reg_dump_name[100]; 279 int fd, i; 280 281 sprintf(reg_dump_name, "%s/reg_dump", dev_dir_name); 282 printf("%s\n", reg_dump_name); 283 fd = open(reg_dump_name, O_RDONLY); 284 pread(fd, reg_dump_arr, 2000, 0); 285 close(fd); 286 for ( i = 0; i < 2000; i++) { 287 printf("%c", reg_dump_arr[i]); 288 //if((i+1)%16 == 0) 289 //printf("\n"); 290 } 291 return 0; 292 } 293 #endif 294 295 static void *get_dmp_event(void *param) { 296 char file_name[100]; 297 int i; 298 int data; 299 char d[4]; 300 FILE *fp; 301 struct pollfd pfd[ARRAY_SIZE(event_file)]; 302 303 printf("get DMP event: %s\n", dev_dir_name); 304 while(1) { 305 for (i = 0; i < ARRAY_SIZE(event_file); i++) { 306 sprintf(file_name, "%s/%s", dev_dir_name, event_file[i].fname); 307 pfd[i].fd = open(file_name, O_RDONLY | O_NONBLOCK); 308 pfd[i].events = POLLPRI|POLLERR; 309 pfd[i].revents = 0; 310 read(pfd[i].fd, d, 4); 311 } 312 313 poll(pfd, ARRAY_SIZE(event_file), -1); 314 for (i = 0; i < ARRAY_SIZE(event_file); i++) { 315 close(pfd[i].fd); 316 } 317 318 for (i=0; i< ARRAY_SIZE(pfd); i++) { 319 if(pfd[i].revents != 0) { 320 sprintf(file_name, "%s/%s", dev_dir_name, event_file[i].fname); 321 fp = fopen(file_name, "rt"); 322 fscanf(fp, "%d\n", &data); 323 event_file[i].action(&event_file[i], data); 324 } 325 } 326 } 327 328 return 0; 329 } 330 331 static int enable_gyro(int on){ 332 int ret; 333 ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, on); 334 if (ret < 0) 335 printf("write gyro_enable failed\n"); 336 337 return ret; 338 } 339 340 static int enable_gyro_output(int on){ 341 int ret; 342 gyro_data_is_enabled = on; 343 ret = write_sysfs_int_and_verify("gyro_fifo_enable", dev_dir_name, on); 344 if (ret < 0) 345 printf("write gyro_fifo_enable failed\n"); 346 347 return ret; 348 } 349 350 static int enable_compass(int on){ 351 int ret; 352 353 compass_data_is_enabled = on; 354 ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, on); 355 if (ret < 0) 356 printf("write gyro_enable failed\n"); 357 358 return ret; 359 } 360 361 static int enable_pressure(int on){ 362 int ret; 363 364 ret = write_sysfs_int_and_verify("pressure_enable", dev_dir_name, on); 365 if (ret < 0) 366 printf("write pressure_enable failed\n"); 367 368 return ret; 369 } 370 371 static int enable_quaternion(int on) { 372 int ret; 373 ret = write_sysfs_int_and_verify("ped_q_on", dev_dir_name, on); 374 if (ret < 0) 375 printf("write quaternion_on failed\n"); 376 ret = write_sysfs_int_and_verify("six_axes_q_on", dev_dir_name, on); 377 if (ret < 0) 378 printf("write quaternion_on failed\n"); 379 ret = write_sysfs_int_and_verify("three_axes_q_on", dev_dir_name, on); 380 if (ret < 0) 381 printf("write quaternion_on failed\n"); 382 383 return ret; 384 } 385 static int enable_step_detector(int on) { 386 int ret; 387 388 ret = write_sysfs_int_and_verify("step_detector_on", dev_dir_name, on); 389 if (ret < 0) 390 printf("write step detector on failed\n"); 391 } 392 static int enable_step_indicator(int on) { 393 int ret; 394 395 ret = write_sysfs_int_and_verify("step_indicator_on", dev_dir_name, on); 396 if (ret < 0) 397 printf("write step indicator on failed\n"); 398 } 399 400 static int enable_accel(int on){ 401 int ret; 402 accel_data_is_enabled = on; 403 accel_engine_is_on = on; 404 ret = write_sysfs_int_and_verify("accel_enable", dev_dir_name, on); 405 if (ret < 0) 406 printf("write accel_enable failed\n"); 407 ret = write_sysfs_int_and_verify("accel_fifo_enable", dev_dir_name, on); 408 if (ret < 0) 409 printf("write accel_fifo_enable failed\n"); 410 411 return ret; 412 } 413 static int enable_accel_output(int on) { 414 int ret; 415 accel_data_is_enabled = on; 416 417 ret = write_sysfs_int_and_verify("accel_fifo_enable", dev_dir_name, on); 418 if (ret < 0) 419 printf("write accel_fifo_enable failed\n"); 420 421 return ret; 422 } 423 424 static int enable_enable(int on){ 425 int ret; 426 427 if (0 == on) { 428 //pthread_mutex_lock(&data_switch_lock); 429 } 430 ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, on); 431 if (ret < 0) 432 printf("write enable failed\n"); 433 434 if (on) { 435 //pthread_mutex_unlock(&data_switch_lock); 436 } 437 438 return 0; 439 } 440 static int write_dmp_event(int on) { 441 int ret; 442 ret = write_sysfs_int_and_verify("dmp_event_int_on", dev_dir_name, on); 443 if (ret < 0) 444 printf("write dmp_event_int_on failed\n"); 445 return 0; 446 } 447 448 static void random_delay(){ 449 int i; 450 float bb; 451 452 i = rand(); 453 bb = i * 200.0; 454 bb = i * 10.0; 455 i = 1 + (unsigned int)(bb/(RAND_MAX + 1.0)); 456 i *= 2; 457 if (i%2) { 458 printf("sleep %d ms\n", i); 459 usleep(i*1000); 460 } else { 461 printf("sleep %d s\n", i); 462 sleep(i); 463 } 464 465 } 466 static void dmp_event_control(on){ 467 int ret; 468 469 ret = 0; 470 471 // ret = write_sysfs_int_and_verify("tap_on", dev_dir_name, on); 472 ret = write_sysfs_int_and_verify("smd_enable", dev_dir_name, 1); 473 if (ret < 0) 474 return; 475 inv_set_rate(); 476 477 //ret = write_sysfs_int_and_verify("batchmode_wake_fifo_full_on", dev_dir_name, 1); 478 ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 10000); 479 ret = write_sysfs_int_and_verify("batchmode_timeout", dev_dir_name, 0); 480 //ret = write_sysfs_int_and_verify("smd_delay_threshold", dev_dir_name, 10); 481 if (ret < 0) 482 return; 483 //ret = write_sysfs_int_and_verify("smd_threshold", dev_dir_name, 5000); 484 if (ret < 0) 485 return; 486 //write_sysfs_int_and_verify("motion_lpa_duration", dev_dir_name, 1000); 487 //write_sysfs_int_and_verify("motion_lpa_threshold", dev_dir_name, 200); 488 write_sysfs_int_and_verify("dmp_on", dev_dir_name, 1); 489 ret = write_sysfs_int_and_verify("sampling_frequency", dev_dir_name, 200); 490 //write_sysfs_int_and_verify("motion_lpa_freq", dev_dir_name, 3); 491 492 } 493 void enable_motion(int on) { 494 int ret; 495 496 ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, on); 497 if (on) { 498 gyro_data_is_enabled = 0; 499 compass_data_is_enabled = 0; 500 quaternion_data_is_enabled = 0; 501 } 502 } 503 bool g, a; 504 static int counter = 0; 505 static unsigned char data_rate[] = {5, 10, 15, 50, 100, 200}; 506 static int run_enable_sequence() 507 { 508 bool g, a, out; 509 510 counter++; 511 g = rand()%2; 512 a = rand()%2; 513 if (!g && !a) 514 a = true; 515 516 g = true; 517 // g = false; 518 a = true; 519 // a = false; 520 /*disable the master enable */ 521 enable_enable(0); 522 if(g) { 523 enable_gyro(1); 524 if (rand()%2) { 525 out = rand()%2; 526 enable_quaternion(out); 527 enable_gyro_output(!out); 528 } else { 529 enable_quaternion(1); 530 enable_gyro_output(1); 531 } 532 enable_quaternion(1); 533 enable_gyro_output(0); 534 535 } else { 536 enable_gyro(0); 537 enable_gyro_output(0); 538 enable_quaternion(0); 539 } 540 if(a) { 541 enable_accel(1); 542 enable_accel_output(0); 543 } else { 544 enable_accel(0); 545 enable_accel_output(0); 546 } 547 if (has_compass) { 548 if(rand()%2) 549 enable_compass(1); 550 else 551 enable_compass(0); 552 enable_compass(counter%2); 553 enable_compass(0); 554 } 555 if (has_pressure) { 556 if(rand()%2) 557 enable_pressure(1); 558 else 559 enable_pressure(0); 560 enable_pressure(counter%3); 561 enable_pressure(0); 562 } 563 enable_step_detector(1); 564 enable_step_indicator(1); 565 enable_step_detector(0); 566 enable_step_indicator(0); 567 568 write_dmp_event(0); 569 570 enable_motion(0); 571 if (accel_engine_is_on) 572 dmp_event_control(1); 573 else 574 dmp_event_control(0); 575 first_flag = 1; 576 /*enable the master enable */ 577 enable_enable(1); 578 //enable_enable(0); 579 //verify_img(); 580 //while(1); 581 //write_sysfs_string_and_verify("wake_unlock", "/sys/power/", "hack"); 582 if (enable_random_delay) 583 random_delay(); 584 else { 585 printf("sleep %ds\n", enable_delay); 586 sleep(enable_delay); 587 } 588 589 return 0; 590 } 591 592 static int run_disable_sequence() { 593 enable_enable(0); 594 595 enable_gyro(0); 596 enable_accel(1); 597 enable_quaternion(0); 598 enable_accel_output(0); 599 write_dmp_event(1); 600 enable_motion(enable_motion_on); 601 if (accel_engine_is_on) 602 dmp_event_control(1); 603 else 604 dmp_event_control(0); 605 606 enable_enable(1); 607 if (enable_random_delay) 608 random_delay(); 609 else { 610 printf("sleep %ds\n", disable_delay); 611 sleep(disable_delay); 612 } 613 614 if (has_pressure) { 615 if(rand()%2) 616 enable_pressure(1); 617 else 618 enable_pressure(0); 619 enable_pressure(counter%3); 620 enable_pressure(1); 621 } 622 623 return 0; 624 } 625 static int run_dmp_off() { 626 bool g, a, out; 627 628 counter++; 629 g = rand()%2; 630 a = rand()%2; 631 if (!g && !a) 632 a = true; 633 634 g = true; 635 a = true; 636 a = false; 637 // g = false; 638 /*disable the master enable */ 639 enable_enable(0); 640 if(g) { 641 enable_gyro(1); 642 if (rand()%2) { 643 enable_gyro_output(!out); 644 } else { 645 enable_gyro_output(1); 646 } 647 enable_gyro_output(1); 648 649 } else { 650 enable_gyro(0); 651 enable_gyro_output(0); 652 } 653 if(a) { 654 enable_accel(1); 655 enable_accel_output(1); 656 // enable_accel_output(0); 657 } else { 658 enable_accel(0); 659 enable_accel_output(0); 660 } 661 if (has_compass) { 662 if(rand()%2) 663 enable_compass(1); 664 else 665 enable_compass(0); 666 enable_compass(counter%2); 667 enable_compass(0); 668 } 669 if (has_pressure) { 670 if(rand()%2) 671 enable_pressure(1); 672 else 673 enable_pressure(0); 674 enable_pressure(counter%3); 675 enable_pressure(1); 676 } 677 printf("111111111111111\n"); 678 679 write_sysfs_int_and_verify("sampling_frequency", dev_dir_name,15); 680 write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0); 681 first_flag = 1; 682 /*enable the master enable */ 683 enable_enable(1); 684 //sleep(2); 685 686 return 0; 687 } 688 static void *control_switch(void *param) 689 { 690 while(1) { 691 run_enable_sequence(); 692 //run_dmp_off(); 693 printf("sleeping\n"); 694 //sleep(1000); 695 run_disable_sequence(); 696 } 697 return 0; 698 } 699 700 void get_sensor_data(char *d, short *sensor) 701 { 702 int i; 703 704 for (i = 0; i < 3; i++) 705 sensor[i] = *(short *)(d + 2 + i * 2); 706 } 707 708 static void *read_data(void *param) 709 { 710 char *buffer_access; 711 char data[2048], *dptr, tmp[24]; 712 short sensor[3]; 713 int q[3], i, ind, left_over_size, buf_size; 714 int ret, fp,read_size; 715 unsigned short hdr; 716 bool done_flag; 717 struct timespec ts_1; 718 unsigned long long t0, t1; 719 int g_count, sq_count; 720 721 #define PRESSURE_HDR 0x8000 722 #define ACCEL_HDR 0x4000 723 #define GYRO_HDR 0x2000 724 #define COMPASS_HDR 0x1000 725 #define COMPASS_HDR_2 0x1800 726 #define LPQUAT_HDR 0x0800 727 #define SIXQUAT_HDR 0x0400 728 #define PEDQUAT_HDR 0x0200 729 #define STEP_DETECTOR_HDR 0x0100 730 #define STEP_INDICATOR_HDR 0x0001 731 #define END_MARKER 0x0010 732 #define EMPTY_MARKER 0x0020 733 734 printf("read_data Thread: %s\n", dev_dir_name); 735 ret = asprintf(&scan_el_dir, FORMAT_SCAN_ELEMENTS_DIR, dev_dir_name); 736 if (ret < 0) 737 goto error_alloc_scan_el_dir; 738 ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num); 739 if (ret < 0) 740 goto error_alloc_buffer_access; 741 742 fp = open(buffer_access, O_RDONLY | O_NONBLOCK); 743 if (fp == -1) { /*If it isn't there make the node */ 744 printf("Failed to open %s\n", buffer_access); 745 ret = -errno; 746 goto error_open_buffer_access; 747 } 748 ind = 0; 749 750 clock_gettime(CLOCK_REALTIME, &ts_1); 751 t0 = (unsigned long long)ts_1.tv_sec * 1000000000 + ts_1.tv_nsec; 752 while(1) { 753 754 clock_gettime(CLOCK_REALTIME, &ts_1); 755 t1 = (unsigned long long)ts_1.tv_sec * 1000000000 + ts_1.tv_nsec; 756 //printf("diff=%lld, a_count=%d, sq_count=%d\n", (t1-t0), g_count, sq_count); 757 g_count = 0; 758 sq_count = 0; 759 t0 = t1; 760 761 struct pollfd pfd = { 762 .fd = fp, 763 .events = POLLIN, 764 }; 765 poll(&pfd, 1, -1); 766 767 if (left_over_size > 0) 768 memcpy(data, tmp, left_over_size); 769 dptr = data + left_over_size; 770 read_size = read(fp, dptr, 2000); 771 printf("readsize=%d, left_over_size=%d\n", read_size, left_over_size); 772 if (read_size <= 0) { 773 printf("Wrong size=%d\n", read_size); 774 pthread_mutex_unlock(&data_switch_lock); 775 continue; 776 } 777 ind = read_size + left_over_size; 778 dptr = data; 779 //printf("ind=%d\n", ind); 780 buf_size = ind - (dptr - data); 781 done_flag = false; 782 783 while ((buf_size > 0) && (!done_flag)) { 784 hdr = *((short *)(dptr)); 785 if ((hdr & 0xf) && (hdr != STEP_INDICATOR_HDR)) 786 printf("STEP$$$$$$$$$$$$$$$=%x ", hdr); 787 switch (hdr & (~0xf)) { 788 case PRESSURE_HDR: 789 if (buf_size >= 16) { 790 get_sensor_data(dptr, sensor); 791 dptr += 8; 792 printf("PRESSURE:%d, %lld\n", (sensor[1] << 16) + (unsigned short)sensor[2], *(long long *)dptr); 793 } else 794 done_flag = true; 795 break; 796 case ACCEL_HDR: 797 if (buf_size >= 16) { 798 get_sensor_data(dptr, sensor); 799 dptr += 8; 800 printf("A:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); 801 } else 802 done_flag = true; 803 break; 804 case GYRO_HDR: 805 if (buf_size >= 16) { 806 get_sensor_data(dptr, sensor); 807 dptr += 8; 808 g_count++; 809 printf("G:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); 810 } else 811 done_flag = true; 812 break; 813 case COMPASS_HDR: 814 if (buf_size >= 16) { 815 get_sensor_data(dptr, sensor); 816 dptr += 8; 817 printf("M:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); 818 } else 819 done_flag = true; 820 break; 821 case PEDQUAT_HDR: 822 if (buf_size >= 16) { 823 get_sensor_data(dptr, sensor); 824 dptr += 8; 825 printf("PED:%d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr); 826 } else 827 done_flag = true; 828 break; 829 case LPQUAT_HDR: 830 if (buf_size >= 24) { 831 q[0] = *(int *)(dptr + 4); 832 dptr += 8; 833 q[1] = *(int *)(dptr); 834 q[2] = *(int *)(dptr + 4); 835 dptr += 8; 836 printf("LPQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); 837 } else 838 done_flag = true; 839 break; 840 case SIXQUAT_HDR: 841 if (buf_size >= 24) { 842 q[0] = *(int *)(dptr + 4); 843 dptr += 8; 844 q[1] = *(int *)(dptr); 845 q[2] = *(int *)(dptr + 4); 846 dptr += 8; 847 sq_count++; 848 printf("SIXQ:%d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr); 849 } else 850 done_flag = true; 851 break; 852 case STEP_DETECTOR_HDR: 853 if (buf_size >= 16) { 854 printf("STEP DETECTOR "); 855 dptr += 8; 856 printf(" %lld\n", *(long long *)dptr); 857 } else 858 done_flag = true; 859 860 break; 861 default: 862 if (hdr == EMPTY_MARKER) { 863 printf("emptry marker !!!!!!!!!!!\n"); 864 } else if (hdr == END_MARKER) { 865 printf("end marker !!!!!\n"); 866 } else if (hdr == COMPASS_HDR_2) { 867 printf("bad compass data\n"); 868 } else { 869 dptr +=8; 870 printf("%lld\n", *(long long *)dptr); 871 } 872 break; 873 } 874 if (!done_flag) 875 dptr += 8; 876 buf_size = ind - (dptr - data); 877 } 878 if (ind - (dptr - data) > 0) 879 memcpy(tmp, dptr, ind - (dptr - data)); 880 left_over_size = ind - (dptr - data); 881 } 882 close(fp); 883 884 error_open_buffer_access: 885 free(buffer_access); 886 error_alloc_buffer_access: 887 free(scan_el_dir); 888 error_alloc_scan_el_dir: 889 890 return 0; 891 } 892 893 static void inv_create_thread() { 894 pthread_t thread_dmp_event, thread_read_data, thread_control; 895 896 pthread_create(&thread_dmp_event, NULL, &get_dmp_event, (void *)dev_dir_name); 897 pthread_create(&thread_read_data, NULL, &read_data, (void *)dev_dir_name); 898 pthread_create(&thread_control, NULL, &control_switch, (void *)dev_dir_name); 899 900 pthread_join(thread_dmp_event, NULL); 901 pthread_join(thread_read_data, NULL); 902 pthread_join(thread_control, NULL); 903 } 904 905 static int enable_enable_main(int on){ 906 int ret; 907 908 printf("enable_enable: %s=%d\n", dev_dir_name, on); 909 ret = write_sysfs_int_and_verify("enable", buf_dir_name, on); 910 if (ret < 0) 911 printf("write enable failed\n"); 912 913 return 0; 914 } 915 916 int main(int argc, char **argv) 917 { 918 unsigned long buf_len = 240; 919 920 int ret, c, i; 921 922 char *trigger_name = NULL; 923 924 int datardytrigger = 1; 925 int trig_num; 926 char *dummy; 927 char chip_name[10]; 928 char device_name[10]; 929 char sysfs[100]; 930 931 gyro_data_is_enabled = 0; 932 accel_data_is_enabled = 0; 933 compass_data_is_enabled = 0; 934 quaternion_data_is_enabled = 0; 935 936 while ((c = getopt(argc, argv, "lcd:e:rmp")) != -1) { 937 switch (c) { 938 case 'c': 939 has_compass = 1; 940 break; 941 case 'p': 942 has_pressure = 1; 943 break; 944 case 'd': 945 disable_delay = strtoul(optarg, &dummy, 10); 946 break; 947 case 'e': 948 enable_delay = strtoul(optarg, &dummy, 10); 949 break; 950 case 'r': 951 enable_random_delay = 1; 952 break; 953 case 'm': 954 enable_motion_on = 1; 955 break; 956 case '?': 957 return -1; 958 } 959 } 960 961 inv_get_sysfs_path(sysfs); 962 printf("sss:::%s\n", sysfs); 963 if (inv_get_chip_name(chip_name) != INV_SUCCESS) { 964 printf("get chip name fail\n"); 965 exit(0); 966 } 967 printf("chip_name=%s\n", chip_name); 968 if (INV_SUCCESS != inv_check_key()) 969 printf("key check fail\n"); 970 else 971 printf("key authenticated\n"); 972 973 for (i=0; i<strlen(chip_name); i++) { 974 device_name[i] = tolower(chip_name[i]); 975 } 976 device_name[strlen(chip_name)] = '\0'; 977 printf("device name: %s\n", device_name); 978 979 /* Find the device requested */ 980 dev_num = find_type_by_name(device_name, "iio:device"); 981 if (dev_num < 0) { 982 printf("Failed to find the %s\n", device_name); 983 ret = -ENODEV; 984 goto error_ret; 985 } 986 printf("iio device number being used is %d\n", dev_num); 987 asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num); 988 printf("allco=%x\n", (int)dev_dir_name); 989 if (trigger_name == NULL) { 990 /* 991 * Build the trigger name. If it is device associated it's 992 * name is <device_name>_dev[n] where n matches the device 993 * number found above 994 */ 995 ret = asprintf(&trigger_name, 996 "%s-dev%d", device_name, dev_num); 997 if (ret < 0) { 998 ret = -ENOMEM; 999 goto error_ret; 1000 } 1001 } 1002 /* Verify the trigger exists */ 1003 trig_num = find_type_by_name(trigger_name, "trigger"); 1004 if (trig_num < 0) { 1005 printf("Failed to find the trigger %s\n", trigger_name); 1006 ret = -ENODEV; 1007 goto error_free_triggername; 1008 } 1009 printf("iio trigger number being used is %d\n", trig_num); 1010 ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num); 1011 if (ret < 0) { 1012 ret = -ENOMEM; 1013 goto error_free_triggername; 1014 } 1015 enable_enable_main(0); 1016 ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1); 1017 /* 1018 * Parse the files in scan_elements to identify what channels are 1019 * present 1020 */ 1021 ret = 0; 1022 setup_dmp(dev_dir_name); 1023 1024 printf("%s %s\n", dev_dir_name, trigger_name); 1025 1026 /* Set the device trigger to be the data rdy trigger found above */ 1027 ret = write_sysfs_string_and_verify("trigger/current_trigger", 1028 dev_dir_name, 1029 trigger_name); 1030 if (ret < 0) { 1031 printf("Failed to write current_trigger file\n"); 1032 goto error_free_buf_dir_name; 1033 } 1034 /* Setup ring buffer parameters */ 1035 /* length must be even number because iio_store_to_sw_ring is expecting 1036 half pointer to be equal to the read pointer, which is impossible 1037 when buflen is odd number. This is actually a bug in the code */ 1038 ret = write_sysfs_int("length", buf_dir_name, buf_len*2); 1039 if (ret < 0) 1040 goto exit_here; 1041 enable_enable_main(1); 1042 inv_create_thread(); 1043 exit_here: 1044 error_free_buf_dir_name: 1045 free(buf_dir_name); 1046 error_free_triggername: 1047 if (datardytrigger) 1048 free(trigger_name); 1049 error_ret: 1050 return ret; 1051 } 1052
