1 /* 2 * Copyright (c) Invensense Inc. 2012 3 * 4 * This program is free software; you can redistribute it and/or modify it 5 * under the terms of the GNU General Public License version 2 as published by 6 * the Free Software Foundation. 7 */ 8 9 #include <unistd.h> 10 #include <dirent.h> 11 #include <fcntl.h> 12 #include <stdio.h> 13 #include <errno.h> 14 #include <sys/stat.h> 15 #include <dirent.h> 16 #include <linux/types.h> 17 #include <string.h> 18 #include <poll.h> 19 #include <termios.h> 20 21 #include "iio_utils.h" 22 #include "ml_load_dmp.h" 23 #include "ml_sysfs_helper.h" 24 #include "authenticate.h" 25 #include "mlos.h" 26 27 #define DMP_CODE_SIZE (3060) 28 #define POLL_TIME (2000) // 2sec 29 30 // settings 31 static int accel_only = false; 32 static int test_motion = false; 33 static int test_flick = false; 34 static int test_pedometer = false; 35 static int test_orientation = false; 36 int verbose = false; 37 38 // paths 39 char *dev_dir_name, *buf_dir_name; 40 41 // all the DMP features supported 42 enum { 43 FEAT_TAP = 0, 44 FEAT_ORIENTATION, 45 FEAT_DISPLAY_ORIENTATION, 46 FEAT_MOTION, 47 FEAT_FLICK, 48 49 FEAT_NUM, 50 } features; 51 52 typedef void (*handler_t) (int data); 53 54 struct dmp_feat_t { 55 int enabled; 56 struct pollfd *pollfd; 57 char *sysfs_name; 58 handler_t phandler; 59 }; 60 61 static struct dmp_feat_t dmp_feat[FEAT_NUM] = {{0}}; 62 static struct pollfd pollfds[FEAT_NUM]; 63 static int pollfds_used = 0; 64 65 /************************************************** 66 This _kbhit() function is courtesy of the web 67 ***************************************************/ 68 int _kbhit(void) 69 { 70 static const int STDIN = 0; 71 static bool initialized = false; 72 73 if (!initialized) { 74 // Use termios to turn off line buffering 75 struct termios term; 76 tcgetattr(STDIN, &term); 77 term.c_lflag &= ~ICANON; 78 tcsetattr(STDIN, TCSANOW, &term); 79 setbuf(stdin, NULL); 80 initialized = true; 81 } 82 83 int bytesWaiting; 84 ioctl(STDIN, FIONREAD, &bytesWaiting); 85 return bytesWaiting; 86 } 87 88 /** 89 * size_from_channelarray() - calculate the storage size of a scan 90 * @channels: the channel info array 91 * @num_channels: size of the channel info array 92 * 93 * Has the side effect of filling the channels[i].location values used 94 * in processing the buffer output. 95 */ 96 int size_from_channelarray(struct iio_channel_info *channels, int num_channels) 97 { 98 int bytes = 0; 99 int i = 0; 100 while (i < num_channels) { 101 if (bytes % channels[i].bytes == 0) 102 channels[i].location = bytes; 103 else 104 channels[i].location = bytes - bytes%channels[i].bytes 105 + channels[i].bytes; 106 bytes = channels[i].location + channels[i].bytes; 107 i++; 108 } 109 return bytes; 110 } 111 112 void print2byte(int input, struct iio_channel_info *info) 113 { 114 /* shift before conversion to avoid sign extension 115 of left aligned data */ 116 input = input >> info->shift; 117 if (info->is_signed) { 118 int16_t val = input; 119 val &= (1 << info->bits_used) - 1; 120 val = (int16_t)(val << (16 - info->bits_used)) >> 121 (16 - info->bits_used); 122 /*printf("%d, %05f, scale=%05f", val, 123 (float)(val + info->offset)*info->scale, info->scale);*/ 124 printf("%d, ", val); 125 126 } else { 127 uint16_t val = input; 128 val &= (1 << info->bits_used) - 1; 129 printf("%05f ", ((float)val + info->offset)*info->scale); 130 } 131 } 132 133 /** 134 * process_scan() - print out the values in SI units 135 * @data: pointer to the start of the scan 136 * @infoarray: information about the channels. Note 137 * size_from_channelarray must have been called first to fill the 138 * location offsets. 139 * @num_channels: the number of active channels 140 */ 141 void process_scan(char *data, struct iio_channel_info *infoarray, 142 int num_channels) 143 { 144 int k; 145 //char *tmp; 146 for (k = 0; k < num_channels; k++) { 147 switch (infoarray[k].bytes) { 148 /* only a few cases implemented so far */ 149 case 2: 150 print2byte(*(uint16_t *)(data + infoarray[k].location), 151 &infoarray[k]); 152 //tmp = data + infoarray[k].location; 153 break; 154 case 4: 155 if (infoarray[k].is_signed) { 156 int32_t val = *(int32_t *)(data + infoarray[k].location); 157 if ((val >> infoarray[k].bits_used) & 1) 158 val = (val & infoarray[k].mask) | ~infoarray[k].mask; 159 /* special case for timestamp */ 160 printf(" %d ", val); 161 } 162 break; 163 case 8: 164 if (infoarray[k].is_signed) { 165 int64_t val = *(int64_t *)(data + infoarray[k].location); 166 if ((val >> infoarray[k].bits_used) & 1) 167 val = (val & infoarray[k].mask) | ~infoarray[k].mask; 168 /* special case for timestamp */ 169 if (infoarray[k].scale == 1.0f && 170 infoarray[k].offset == 0.0f) 171 printf(" %lld", val); 172 else 173 printf("%05f ", ((float)val + infoarray[k].offset) 174 * infoarray[k].scale); 175 } 176 break; 177 default: 178 break; 179 } 180 } 181 printf("\n"); 182 } 183 184 /* 185 Enablers for the gestures 186 */ 187 188 int enable_flick(char *p, int on) 189 { 190 int ret; 191 printf("flick:%s\n", p); 192 ret = write_sysfs_int_and_verify("flick_int_on", p, on); 193 if (ret < 0) 194 return ret; 195 ret = write_sysfs_int_and_verify("flick_upper", p, 3147790); 196 if (ret < 0) 197 return ret; 198 ret = write_sysfs_int_and_verify("flick_lower", p, -3147790); 199 if (ret < 0) 200 return ret; 201 ret = write_sysfs_int_and_verify("flick_counter", p, 50); 202 if (ret < 0) 203 return ret; 204 ret = write_sysfs_int_and_verify("flick_message_on", p, 0); 205 if (ret < 0) 206 return ret; 207 ret = write_sysfs_int_and_verify("flick_axis", p, 0); 208 if (ret < 0) 209 return ret; 210 211 return 0; 212 } 213 214 void verify_img(char *dmp_path) 215 { 216 FILE *fp; 217 int i; 218 char dmp_img[DMP_CODE_SIZE]; 219 220 if ((fp = fopen(dmp_path, "rb")) < 0) { 221 perror("dmp fail"); 222 } 223 i = fread(dmp_img, 1, DMP_CODE_SIZE, fp); 224 printf("Result=%d\n", i); 225 fclose(fp); 226 fp = fopen("/dev/read_img.h", "wt"); 227 fprintf(fp, "char rec[]={\n"); 228 for(i = 0; i < DMP_CODE_SIZE; i++) { 229 fprintf(fp, "0x%02x, ", dmp_img[i]); 230 if(((i + 1) % 16) == 0) { 231 fprintf(fp, "\n"); 232 } 233 } 234 fprintf(fp, "};\n "); 235 fclose(fp); 236 } 237 238 int setup_dmp(char *dev_path, int p_event) 239 { 240 char dmp_path[100]; 241 int ret; 242 FILE *fd; 243 244 printf("INFO: sysfs path=%s\n", dev_path); 245 246 ret = write_sysfs_int_and_verify("power_state", dev_path, 1); 247 if (ret < 0) 248 return ret; 249 250 ret = write_sysfs_int("in_accel_scale", dev_path, 0); 251 if (ret < 0) 252 return ret; 253 ret = write_sysfs_int("in_anglvel_scale", dev_path, 3); 254 if (ret < 0) 255 return ret; 256 ret = write_sysfs_int("sampling_frequency", dev_path, 200); 257 if (ret < 0) 258 return ret; 259 ret = write_sysfs_int_and_verify("firmware_loaded", dev_path, 0); 260 if (ret < 0) 261 return ret; 262 263 sprintf(dmp_path, "%s/dmp_firmware", dev_path); 264 if ((fd = fopen(dmp_path, "wb")) < 0 ) { 265 perror("dmp fail"); 266 } 267 inv_load_dmp(fd); 268 fclose(fd); 269 270 printf("INFO: firmware_loaded=%d\n", 271 read_sysfs_posint("firmware_loaded", dev_path)); 272 273 // set accel offsets 274 //ret = write_sysfs_int_and_verify("in_accel_x_offset", 275 // dev_path, 0xabcd0000); 276 //if (ret < 0) 277 // return ret; 278 //ret = write_sysfs_int_and_verify("in_accel_y_offset", 279 // dev_path, 0xffff0000); 280 //if (ret < 0) 281 // return ret; 282 //ret = write_sysfs_int_and_verify("in_accel_z_offset", 283 // dev_path, 0xcdef0000); 284 //if (ret < 0) 285 // return ret; 286 287 ret = write_sysfs_int_and_verify("dmp_on", dev_path, 1); 288 if (ret < 0) 289 return ret; 290 ret = write_sysfs_int_and_verify("dmp_int_on", dev_path, 1); 291 if (ret < 0) 292 return ret; 293 294 /* select which event to enable and interrupt on/off here */ 295 if (test_flick) { 296 ret = enable_flick(dev_path, 1); 297 if (ret < 0) 298 return ret; 299 } 300 301 /* 302 ret = write_sysfs_int_and_verify("tap_on", dev_path, 1); 303 if (ret < 0) 304 return ret; 305 */ 306 307 /*ret = write_sysfs_int_and_verify("display_orientation_on", 308 dev_path, 1); 309 if (ret < 0) 310 return ret;*/ 311 if (test_orientation) { 312 ret = write_sysfs_int_and_verify("orientation_on", dev_path, 1); 313 if (ret < 0) 314 return ret; 315 } 316 /* ret = write_sysfs_int_and_verify("dmp_output_rate", dev_path, 25); 317 if (ret < 0) 318 return ret;*/ 319 ret = write_sysfs_int_and_verify("dmp_event_int_on", dev_path, p_event); 320 if (ret < 0) 321 return ret; 322 323 //verify_img(dmp_path); 324 return 0; 325 } 326 327 /* 328 Handlers for the gestures 329 */ 330 331 void handle_flick(int flick) 332 { 333 printf("flick=%x\n", flick); 334 } 335 336 void handle_display_orientation(int orient) 337 { 338 printf("display_orientation=%x\n", orient); 339 } 340 341 void handle_motion(int motion) 342 { 343 printf("motion=%x\n", motion); 344 } 345 346 void handle_orientation(int orient) 347 { 348 printf("orientation="); 349 if (orient & 0x01) 350 printf("+X, "); 351 if (orient & 0x02) 352 printf("-X, "); 353 if (orient & 0x04) 354 printf("+Y, "); 355 if (orient & 0x08) 356 printf("-Y, "); 357 if (orient & 0x10) 358 printf("+Z, "); 359 if (orient & 0x20) 360 printf("-Z, "); 361 if (orient & 0x40) 362 printf("flip"); 363 printf("\n"); 364 } 365 366 void handle_tap(int tap) 367 { 368 int tap_dir = tap / 8; 369 int tap_num = tap % 8 + 1; 370 371 printf("tap="); 372 switch (tap_dir) { 373 case 1: 374 printf("+X, "); 375 break; 376 case 2: 377 printf("-X, "); 378 break; 379 case 3: 380 printf("+Y, "); 381 break; 382 case 4: 383 printf("-Y, "); 384 break; 385 case 5: 386 printf("+Z, "); 387 break; 388 case 6: 389 printf("-Z, "); 390 break; 391 default: 392 break; 393 } 394 printf("#%d\n", tap_num); 395 } 396 397 int handle_pedometer(int *got_event) 398 { 399 static int last_pedometer_steps = -1; 400 static long last_pedometer_time = -1; 401 static unsigned long last_pedometer_poll = 0L; 402 static unsigned long pedometer_poll_timeout = 500L; // .5 second 403 404 unsigned long now; 405 int pedometer_steps; 406 long pedometer_time; 407 408 #ifdef DEBUG_PRINT 409 printf("GT:Pedometer Handler\n"); 410 #endif 411 412 if ((now = inv_get_tick_count()) - last_pedometer_poll 413 < pedometer_poll_timeout) { 414 return 0; 415 } 416 last_pedometer_poll = now; 417 418 pedometer_steps = read_sysfs_posint("pedometer_steps", dev_dir_name); 419 pedometer_time = read_sysfs_posint("pedometer_time", dev_dir_name); 420 421 if (last_pedometer_steps == -1 && last_pedometer_time == -1) { 422 if (!*got_event) 423 printf("\n"); 424 printf("p> pedometer=%d, %ld ", 425 pedometer_steps, pedometer_time); 426 if (pedometer_steps > 10 427 || pedometer_time > (pedometer_poll_timeout * 2)) 428 printf("(resumed)\n"); 429 else 430 printf("\n"); 431 *got_event = true; 432 } else if (last_pedometer_steps != pedometer_steps 433 || last_pedometer_time != pedometer_time) { 434 if (!*got_event) 435 printf("\n"); 436 printf("p> pedometer=%d, %ld\n", 437 pedometer_steps, pedometer_time); 438 *got_event = true; 439 } 440 441 last_pedometer_steps = pedometer_steps; 442 last_pedometer_time = pedometer_time; 443 444 return 0; 445 } 446 447 /* 448 Main processing functions 449 */ 450 451 void dump_dmp_event_struct(void) 452 { 453 #define VARVAL(f, v) printf("\t%s : " f "\n", #v, v); 454 int i; 455 456 printf("dmp_feat structure content:\n"); 457 for (i = 0; i < FEAT_NUM; i++) { 458 printf("%d - ", i); 459 VARVAL("%d", dmp_feat[i].enabled); 460 VARVAL("%s", dmp_feat[i].sysfs_name); 461 VARVAL("%p", dmp_feat[i].phandler); 462 VARVAL("%p", dmp_feat[i].pollfd); 463 if (dmp_feat[i].pollfd) { 464 VARVAL("%d", dmp_feat[i].pollfd->events); 465 VARVAL("%d", dmp_feat[i].pollfd->revents); 466 VARVAL("%d", dmp_feat[i].pollfd->fd); 467 } 468 } 469 printf("dmp_feat structure content:\n"); 470 for (i = 0; i < FEAT_NUM; i++) { 471 printf("%d - ", i); 472 VARVAL("%d", pollfds[i].fd); 473 VARVAL("%d", pollfds[i].events); 474 VARVAL("%d", pollfds[i].revents); 475 } 476 printf("end.\n"); 477 } 478 479 void init_dmp_event_fds(void) 480 { 481 int i, j = 0; 482 char file_name[100]; 483 484 for (i = 0; i < FEAT_NUM; i++) { 485 if (!dmp_feat[i].enabled) 486 continue; 487 sprintf(file_name, "%s/%s", dev_dir_name, dmp_feat[i].sysfs_name); 488 pollfds[j].fd = open(file_name, O_RDONLY | O_NONBLOCK); 489 if (pollfds[j].fd < 0) { 490 printf("Err: cannot open requested event file '%s'\n", file_name); 491 } else { 492 printf("INFO: opened event node '%s'\n", file_name); 493 } 494 pollfds[j].events = POLLPRI | POLLERR; 495 pollfds[j].revents = 0; 496 497 dmp_feat[i].pollfd = &pollfds[j]; 498 j++; 499 } 500 } 501 502 void close_dmp_event_fds(void) 503 { 504 int i; 505 for (i = 0; i < pollfds_used; i++) 506 close(pollfds[i].fd); 507 } 508 509 void poll_dmp_event_fds(void) 510 { 511 int i; 512 char d[4]; 513 static int got_event = 1; 514 515 // read the pollable fds 516 for (i = 0; i < pollfds_used; i++) 517 read(pollfds[i].fd, d, 4); 518 519 // poll 520 if (got_event) 521 printf("e> "); 522 got_event = false; 523 poll(pollfds, pollfds_used, POLL_TIME); 524 525 for (i = 0; i < FEAT_NUM; i++) { 526 if (!dmp_feat[i].enabled) 527 continue; 528 529 if (dmp_feat[i].pollfd->revents != 0) { 530 char file_name[200]; 531 int data; 532 533 sprintf(file_name, "%s/%s", 534 dev_dir_name, dmp_feat[i].sysfs_name); 535 FILE *fp = fopen(file_name, "rt"); 536 if (!fp) { 537 printf("Err:cannot open requested event file '%s'\n", 538 dmp_feat[i].sysfs_name); 539 continue; 540 } 541 fscanf(fp, "%d\n", &data); 542 fclose(fp); 543 dmp_feat[i].pollfd->revents = 0; 544 545 dmp_feat[i].phandler(data); 546 got_event = true; 547 } 548 } 549 550 if (test_pedometer) { 551 /* pedometer is not event based, therefore we poll using a timer every 552 pedometer_poll_timeout milliseconds */ 553 handle_pedometer(&got_event); 554 } 555 } 556 557 /* 558 Main 559 */ 560 561 int main(int argc, char **argv) 562 { 563 unsigned long num_loops = 2; 564 unsigned long timedelay = 100000; 565 unsigned long buf_len = 128; 566 567 int ret, c, i, j, toread; 568 int fp; 569 570 int num_channels; 571 char *trigger_name = NULL; 572 573 int datardytrigger = 1; 574 char *data; 575 int read_size; 576 int dev_num, trig_num; 577 char *buffer_access; 578 int scan_size; 579 int noevents = 0; 580 int p_event = 0, nodmp = 0; 581 char *dummy; 582 char chip_name[10]; 583 char device_name[10]; 584 char sysfs[100]; 585 586 struct iio_channel_info *infoarray; 587 588 // all output to stdout must be delivered immediately, no buffering 589 setvbuf(stdout, NULL, _IONBF, 0); 590 591 // get info about the device and driver 592 inv_get_sysfs_path(sysfs); 593 if (inv_get_chip_name(chip_name) != INV_SUCCESS) { 594 printf("get chip name fail\n"); 595 exit(0); 596 } 597 printf("INFO: chip_name=%s\n", chip_name); 598 599 for (i = 0; i < strlen(chip_name); i++) 600 device_name[i] = tolower(chip_name[i]); 601 device_name[strlen(chip_name)] = '\0'; 602 printf("INFO: device name=%s\n", device_name); 603 604 /* parse the command line parameters 605 -r means no DMP is enabled (raw) -> should be used for mpu3050. 606 -p means no print of data 607 when using -p, 1 means orientation, 2 means tap, 3 means flick */ 608 while ((c = getopt(argc, argv, "l:w:c:premavt:")) != -1) { 609 switch (c) { 610 case 't': 611 trigger_name = optarg; 612 datardytrigger = 0; 613 break; 614 case 'e': 615 noevents = 1; 616 break; 617 case 'p': 618 p_event = 1; 619 break; 620 case 'r': 621 nodmp = 1; 622 break; 623 case 'c': 624 num_loops = strtoul(optarg, &dummy, 10); 625 break; 626 case 'w': 627 timedelay = strtoul(optarg, &dummy, 10); 628 break; 629 case 'l': 630 buf_len = strtoul(optarg, &dummy, 10); 631 break; 632 case 'm': 633 test_motion = true; 634 break; 635 case 'a': 636 accel_only = true; 637 break; 638 case 'v': 639 verbose = true; 640 break; 641 case '?': 642 return -1; 643 } 644 } 645 646 pollfds_used = 0; 647 648 // comment out/remove/if(0) the block corresponding to the feature 649 // that you want to disable 650 651 if (0) { 652 struct dmp_feat_t f = { 653 true, 654 NULL, 655 "event_tap", 656 handle_tap 657 }; 658 dmp_feat[pollfds_used] = f; 659 pollfds_used++; 660 } 661 if (test_orientation) { 662 struct dmp_feat_t f = { 663 true, 664 NULL, 665 "event_orientation", 666 handle_orientation 667 }; 668 dmp_feat[pollfds_used] = f; 669 pollfds_used++; 670 } 671 /*if (1) { 672 struct dmp_feat_t f = { 673 true, 674 NULL, 675 "event_display_orientation", 676 handle_display_orientation 677 }; 678 dmp_feat[pollfds_used] = f; 679 pollfds_used++; 680 }*/ 681 if (test_motion) { 682 struct dmp_feat_t f = { 683 true, 684 NULL, 685 "event_accel_motion", 686 handle_motion 687 }; 688 dmp_feat[pollfds_used] = f; 689 pollfds_used++; 690 } 691 if (test_flick) { 692 struct dmp_feat_t f = { 693 true, 694 NULL, 695 "event_flick", 696 handle_flick 697 }; 698 dmp_feat[pollfds_used] = f; 699 pollfds_used++; 700 } 701 702 // debug 703 printf("INFO\n"); 704 printf("INFO: Configured features:\n"); 705 for (i = 0; i < pollfds_used; i++) 706 printf("INFO: %d -> %s\n", i, dmp_feat[i].sysfs_name); 707 printf("INFO\n"); 708 709 /* Find the device requested */ 710 dev_num = find_type_by_name(device_name, "iio:device"); 711 if (dev_num < 0) { 712 printf("Failed to find the %s\n", device_name); 713 ret = -ENODEV; 714 goto error_ret; 715 } 716 printf("INFO: iio device number=%d\n", dev_num); 717 asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num); 718 if (trigger_name == NULL) { 719 /* 720 * Build the trigger name. If it is device associated it's 721 * name is <device_name>_dev[n] where n matches the device 722 * number found above 723 */ 724 ret = asprintf(&trigger_name, "%s-dev%d", device_name, dev_num); 725 if (ret < 0) { 726 ret = -ENOMEM; 727 goto error_ret; 728 } 729 } 730 731 ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, 0); 732 if (ret < 0) 733 return ret; 734 ret = write_sysfs_int_and_verify("buffer/enable", dev_dir_name, 0); 735 if (ret < 0) 736 return ret; 737 ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1); 738 739 // 740 // motion interrupt in low power accel mode 741 // 742 if (test_motion) { 743 ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, 1); 744 if (ret < 0) 745 return ret; 746 // magnitude threshold - range [0, 1020] in 32 mg increments 747 ret = write_sysfs_int_and_verify("motion_lpa_threshold", dev_dir_name, 748 3 * 32); 749 if (ret < 0) 750 return ret; 751 // duration in ms up to 2^16 752 // ret = write_sysfs_int_and_verify("motion_lpa_dur", dev_dir_name, 753 // 200 * 1); 754 //if (ret < 0) 755 // return ret; 756 // motion_lpa_freq: 0 for 1.25, 1 for 5, 2 for 20, 3 for 40 Hz update rate 757 // of the low power accel mode. 758 // The higher the rate, the better responsiveness of the motion interrupt. 759 ret = write_sysfs_int("motion_lpa_freq", dev_dir_name, 2); 760 if (ret < 0) 761 return ret; 762 } else { 763 ret = write_sysfs_int_and_verify("motion_lpa_on", dev_dir_name, 0); 764 if (ret < 0) 765 return ret; 766 } 767 768 /* Verify the trigger exists */ 769 trig_num = find_type_by_name(trigger_name, "trigger"); 770 if (trig_num < 0) { 771 printf("Failed to find the trigger %s\n", trigger_name); 772 ret = -ENODEV; 773 goto error_free_triggername; 774 } 775 printf("INFO: iio trigger number=%d\n", trig_num); 776 777 if (!nodmp) 778 setup_dmp(dev_dir_name, p_event); 779 780 /* 781 * Construct the directory name for the associated buffer. 782 * As we know that the lis3l02dq has only one buffer this may 783 * be built rather than found. 784 */ 785 ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num); 786 if (ret < 0) { 787 ret = -ENOMEM; 788 goto error_free_triggername; 789 } 790 791 /* Set the device trigger to be the data rdy trigger found above */ 792 ret = write_sysfs_string_and_verify("trigger/current_trigger", 793 dev_dir_name, 794 trigger_name); 795 if (ret < 0) { 796 printf("Failed to write current_trigger file\n"); 797 goto error_free_buf_dir_name; 798 } 799 800 /* Setup ring buffer parameters 801 length must be even number because iio_store_to_sw_ring is expecting 802 half pointer to be equal to the read pointer, which is impossible 803 when buflen is odd number. This is actually a bug in the code */ 804 ret = write_sysfs_int("length", buf_dir_name, buf_len * 2); 805 if (ret < 0) 806 goto exit_here; 807 808 // gyro 809 if (accel_only) { 810 ret = enable_anglvel_se(dev_dir_name, &infoarray, &num_channels, 0); 811 if (ret < 0) 812 return ret; 813 ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 0); 814 if (ret < 0) 815 return ret; 816 } else { 817 ret = enable_anglvel_se(dev_dir_name, &infoarray, &num_channels, 1); 818 if (ret < 0) 819 return ret; 820 ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1); 821 if (ret < 0) 822 return ret; 823 } 824 825 // accel 826 ret = enable_accel_se(dev_dir_name, &infoarray, &num_channels, 1); 827 if (ret < 0) 828 return ret; 829 ret = write_sysfs_int_and_verify("accel_enable", dev_dir_name, 1); 830 if (ret < 0) 831 return ret; 832 833 // quaternion 834 if (!nodmp) { 835 ret = enable_quaternion_se(dev_dir_name, &infoarray, &num_channels, 1); 836 if (ret < 0) 837 return ret; 838 ret = write_sysfs_int_and_verify("three_axes_q_on", dev_dir_name, 1); 839 if (ret < 0) 840 return ret; 841 } else { 842 ret = enable_quaternion_se(dev_dir_name, &infoarray, &num_channels, 0); 843 if (ret < 0) 844 return ret; 845 ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0); 846 if (ret < 0) 847 return ret; 848 } 849 850 //sprintf(dmp_path, "%s/dmp_firmware", dev_dir_name); 851 //verify_img(dmp_path); 852 853 ret = build_channel_array(dev_dir_name, &infoarray, &num_channels); 854 if (ret) { 855 printf("Problem reading scan element information\n"); 856 goto exit_here; 857 } 858 859 /* enable the buffer */ 860 ret = write_sysfs_int_and_verify("enable", buf_dir_name, 1); 861 if (ret < 0) 862 goto exit_here; 863 scan_size = size_from_channelarray(infoarray, num_channels); 864 data = malloc(scan_size * buf_len); 865 if (!data) { 866 ret = -ENOMEM; 867 goto exit_here; 868 } 869 /*ADDED*/ 870 ret = write_sysfs_int_and_verify("master_enable", dev_dir_name, 1); 871 if (ret < 0) 872 return ret; 873 if (p_event) { 874 875 /* polling events from the DMP */ 876 init_dmp_event_fds(); 877 while(!_kbhit()) 878 poll_dmp_event_fds(); 879 close_dmp_event_fds(); 880 881 } else { 882 883 /* attempt to open non blocking the access dev */ 884 ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num); 885 if (ret < 0) { 886 ret = -ENOMEM; 887 goto error_free_data; 888 } 889 fp = open(buffer_access, O_RDONLY | O_NONBLOCK); 890 if (fp == -1) { /*If it isn't there make the node */ 891 printf("Failed to open %s\n", buffer_access); 892 ret = -errno; 893 goto error_free_buffer_access; 894 } 895 /* wait for events num_loops times */ 896 for (j = 0; j < num_loops; j++) { 897 if (!noevents) { 898 struct pollfd pfd = { 899 .fd = fp, 900 .events = POLLIN, 901 }; 902 poll(&pfd, 1, -1); 903 toread = 1; 904 if (j % 128 == 0) 905 usleep(timedelay); 906 907 } else { 908 usleep(timedelay); 909 toread = 1; 910 } 911 read_size = read(fp, data, toread * scan_size); 912 if (read_size == -EAGAIN) { 913 printf("nothing available\n"); 914 continue; 915 } 916 if (!p_event) { 917 for (i = 0; i < read_size / scan_size; i++) 918 process_scan(data + scan_size * i, infoarray, num_channels); 919 } 920 } 921 close(fp); 922 } 923 924 error_free_buffer_access: 925 free(buffer_access); 926 error_free_data: 927 free(data); 928 exit_here: 929 /* stop the ring buffer */ 930 ret = write_sysfs_int_and_verify("enable", buf_dir_name, 0); 931 /* disable the dmp */ 932 if (p_event) 933 ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0); 934 935 error_free_buf_dir_name: 936 free(buf_dir_name); 937 error_free_triggername: 938 if (datardytrigger) 939 free(trigger_name); 940 error_ret: 941 return ret; 942 } 943
