1 /* Industrialio buffer test code. 2 * 3 * Copyright (c) 2008 Jonathan Cameron 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 * This program is primarily intended as an example application. 10 * Reads the current buffer setup from sysfs and starts a short capture 11 * from the specified device, pretty printing the result after appropriate 12 * conversion. 13 * 14 * Command line parameters 15 * generic_buffer -n <device_name> -t <trigger_name> 16 * If trigger name is not specified the program assumes you want a dataready 17 * trigger associated with the device and goes looking for it. 18 * 19 */ 20 21 #include <unistd.h> 22 #include <dirent.h> 23 #include <fcntl.h> 24 #include <stdio.h> 25 #include <errno.h> 26 #include <sys/stat.h> 27 #include <dirent.h> 28 #include <linux/types.h> 29 #include <string.h> 30 #include <poll.h> 31 #include "iio_utils.h" 32 #include "ml_load_dmp.h" 33 #include "ml_sysfs_helper.h" 34 #include "authenticate.h" 35 36 #define FLICK_SUPPORTED (0) 37 38 /** 39 * size_from_channelarray() - calculate the storage size of a scan 40 * @channels: the channel info array 41 * @num_channels: size of the channel info array 42 * 43 * Has the side effect of filling the channels[i].location values used 44 * in processing the buffer output. 45 **/ 46 int size_from_channelarray(struct iio_channel_info *channels, int num_channels) 47 { 48 int bytes = 0; 49 int i = 0; 50 while (i < num_channels) { 51 if (bytes % channels[i].bytes == 0) 52 channels[i].location = bytes; 53 else 54 channels[i].location = bytes - bytes%channels[i].bytes 55 + channels[i].bytes; 56 bytes = channels[i].location + channels[i].bytes; 57 i++; 58 } 59 return bytes; 60 } 61 62 void print2byte(int input, struct iio_channel_info *info) 63 { 64 /* shift before conversion to avoid sign extension 65 of left aligned data */ 66 input = input >> info->shift; 67 if (info->is_signed) { 68 int16_t val = input; 69 val &= (1 << info->bits_used) - 1; 70 val = (int16_t)(val << (16 - info->bits_used)) >> 71 (16 - info->bits_used); 72 /*printf("%d, %05f, scale=%05f", val, 73 (float)(val + info->offset)*info->scale, info->scale);*/ 74 printf("%d, ", val); 75 76 } else { 77 uint16_t val = input; 78 val &= (1 << info->bits_used) - 1; 79 printf("%05f ", ((float)val + info->offset)*info->scale); 80 } 81 } 82 /** 83 * process_scan() - print out the values in SI units 84 * @data: pointer to the start of the scan 85 * @infoarray: information about the channels. Note 86 * size_from_channelarray must have been called first to fill the 87 * location offsets. 88 * @num_channels: the number of active channels 89 **/ 90 void process_scan(char *data, 91 struct iio_channel_info *infoarray, 92 int num_channels) 93 { 94 int k; 95 //char *tmp; 96 for (k = 0; k < num_channels; k++) { 97 switch (infoarray[k].bytes) { 98 /* only a few cases implemented so far */ 99 case 2: 100 print2byte(*(uint16_t *)(data + infoarray[k].location), 101 &infoarray[k]); 102 //tmp = data + infoarray[k].location; 103 break; 104 case 4: 105 if (infoarray[k].is_signed) { 106 int32_t val = *(int32_t *) 107 (data + 108 infoarray[k].location); 109 if ((val >> infoarray[k].bits_used) & 1) 110 val = (val & infoarray[k].mask) | 111 ~infoarray[k].mask; 112 /* special case for timestamp */ 113 printf(" %d ", val); 114 } 115 break; 116 case 8: 117 if (infoarray[k].is_signed) { 118 int64_t val = *(int64_t *) 119 (data + 120 infoarray[k].location); 121 if ((val >> infoarray[k].bits_used) & 1) 122 val = (val & infoarray[k].mask) | 123 ~infoarray[k].mask; 124 /* special case for timestamp */ 125 if (infoarray[k].scale == 1.0f && 126 infoarray[k].offset == 0.0f) 127 printf(" %lld", val); 128 else 129 printf("%05f ", ((float)val + 130 infoarray[k].offset)* 131 infoarray[k].scale); 132 } 133 break; 134 default: 135 break; 136 } 137 } 138 printf("\n"); 139 } 140 141 #if FLICK_SUPPORTED /* hide flick, not offially supported */ 142 void enable_flick(char *p, int on){ 143 int ret; 144 printf("flick:%s\n", p); 145 ret = write_sysfs_int_and_verify("flick_int_on", p, on); 146 if (ret < 0) 147 return; 148 ret = write_sysfs_int_and_verify("flick_upper", p, 3147790); 149 if (ret < 0) 150 return; 151 ret = write_sysfs_int_and_verify("flick_lower", p, -3147790); 152 if (ret < 0) 153 return; 154 155 ret = write_sysfs_int_and_verify("flick_counter", p, 50); 156 if (ret < 0) 157 return; 158 ret = write_sysfs_int_and_verify("flick_message_on", p, 0); 159 if (ret < 0) 160 return; 161 ret = write_sysfs_int_and_verify("flick_axis", p, 0); 162 } 163 #endif 164 165 void HandleOrient(int orient) 166 { 167 if (orient & 0x01) 168 printf("INV_X_UP\n"); 169 if (orient & 0x02) 170 printf("INV_X_DOWN\n"); 171 if (orient & 0x04) 172 printf("INV_Y_UP\n"); 173 if (orient & 0x08) 174 printf("INV_Y_DOWN\n"); 175 if (orient & 0x10) 176 printf("INV_Z_UP\n"); 177 if (orient & 0x20) 178 printf("INV_Z_DOWN\n"); 179 if (orient & 0x40) 180 printf("INV_ORIENTATION_FLIP\n"); 181 } 182 183 void HandleTap(int tap) 184 { 185 int tap_dir = tap/8; 186 int tap_num = tap%8 + 1; 187 188 switch (tap_dir) { 189 case 1: 190 printf("INV_TAP_AXIS_X_POS\n"); 191 break; 192 case 2: 193 printf("INV_TAP_AXIS_X_NEG\n"); 194 break; 195 case 3: 196 printf("INV_TAP_AXIS_Y_POS\n"); 197 break; 198 case 4: 199 printf("INV_TAP_AXIS_Y_NEG\n"); 200 break; 201 case 5: 202 printf("INV_TAP_AXIS_Z_POS\n"); 203 break; 204 case 6: 205 printf("INV_TAP_AXIS_Z_NEG\n"); 206 break; 207 default: 208 break; 209 } 210 printf("Tap number: %d\n", tap_num); 211 } 212 #define DMP_CODE_SIZE 3060 213 void verify_img(char *dmp_path){ 214 FILE *fp; 215 int i; 216 char dmp_img[DMP_CODE_SIZE]; 217 if ((fp = fopen(dmp_path, "rb")) < 0 ) { 218 perror("dmp fail"); 219 } 220 i = fread(dmp_img, 1, DMP_CODE_SIZE, fp); 221 printf("Result=%d\n", i); 222 fclose(fp); 223 fp = fopen("/dev/read_img.h", "wt"); 224 fprintf(fp, "char rec[]={\n"); 225 for(i=0; i<DMP_CODE_SIZE; i++) { 226 fprintf(fp, "0x%02x, ", dmp_img[i]); 227 if(((i+1)%16) == 0) { 228 fprintf(fp, "\n"); 229 } 230 } 231 fprintf(fp, "};\n "); 232 fclose(fp); 233 } 234 235 void setup_dmp(char *dev_path, int p_event){ 236 char sysfs_path[200]; 237 char dmp_path[200]; 238 int ret; 239 FILE *fd; 240 sprintf(sysfs_path, "%s", dev_path); 241 printf("sysfs: %s\n", sysfs_path); 242 ret = write_sysfs_int_and_verify("power_state", sysfs_path, 1); 243 if (ret < 0) 244 return; 245 246 ret = write_sysfs_int("in_accel_scale", dev_path, 0); 247 if (ret < 0) 248 return; 249 ret = write_sysfs_int("in_anglvel_scale", dev_path, 2); 250 if (ret < 0) 251 return; 252 ret = write_sysfs_int("sampling_frequency", sysfs_path, 200); 253 if (ret < 0) 254 return; 255 ret = write_sysfs_int_and_verify("firmware_loaded", sysfs_path, 0); 256 if (ret < 0) 257 return; 258 sprintf(dmp_path, "%s/dmp_firmware", dev_path); 259 if ((fd = fopen(dmp_path, "wb")) < 0 ) { 260 perror("dmp fail"); 261 } 262 inv_load_dmp(fd); 263 fclose(fd); 264 printf("firmware_loaded=%d\n", read_sysfs_posint("firmware_loaded", sysfs_path)); 265 ret = write_sysfs_int_and_verify("in_accel_x_offset", sysfs_path, 0xabcd0000); 266 ret = write_sysfs_int_and_verify("in_accel_y_offset", sysfs_path, 0xffff0000); 267 ret = write_sysfs_int_and_verify("in_accel_z_offset", sysfs_path, 0xcdef0000); 268 269 ret = write_sysfs_int_and_verify("dmp_on", sysfs_path, 1); 270 if (ret < 0) 271 return; 272 ret = write_sysfs_int_and_verify("dmp_int_on", sysfs_path, 1); 273 if (ret < 0) 274 return; 275 /* selelct which event to enable and interrupt on/off here */ 276 //enable_flick(sysfs_path, 1); 277 ret = write_sysfs_int_and_verify("tap_on", sysfs_path, 1); 278 if (ret < 0) 279 return; 280 ret = write_sysfs_int_and_verify("display_orientation_on", sysfs_path, 1); 281 if (ret < 0) 282 return; 283 ret = write_sysfs_int_and_verify("orientation_on", sysfs_path, 1); 284 if (ret < 0) 285 return; 286 printf("rate\n"); 287 ret = write_sysfs_int_and_verify("dmp_output_rate", sysfs_path, 25); 288 if (ret < 0) 289 return; 290 ret = write_sysfs_int_and_verify("dmp_event_int_on", sysfs_path, p_event); 291 if (ret < 0) 292 return; 293 //verify_img(dmp_path); 294 } 295 296 void get_dmp_event(char *dev_dir_name) 297 { 298 char file_name[100]; 299 int i; 300 #if FLICK_SUPPORTED /* hide flick, not offially supported */ 301 int fp_tap, fp_orient, fp_disp, fp_flick; 302 const int n_gest = 6; 303 #else 304 int fp_tap, fp_orient, fp_disp, fp_motion; 305 //int fp_no_motion; 306 const int n_gest = 4; 307 #endif 308 int data; 309 char d[6]; 310 FILE *fp; 311 struct pollfd pfd[4]; 312 printf("%s\n", dev_dir_name); 313 while(1) { 314 sprintf(file_name, "%s/event_tap", dev_dir_name); 315 fp_tap = open(file_name, O_RDONLY | O_NONBLOCK); 316 sprintf(file_name, "%s/event_orientation", dev_dir_name); 317 fp_orient = open(file_name, O_RDONLY | O_NONBLOCK); 318 sprintf(file_name, "%s/event_display_orientation", dev_dir_name); 319 fp_disp = open(file_name, O_RDONLY | O_NONBLOCK); 320 321 //sprintf(file_name, "%s/event_accel_motion", dev_dir_name); 322 sprintf(file_name, "%s/event_accel_wom", dev_dir_name); 323 fp_motion = open(file_name, O_RDONLY | O_NONBLOCK); 324 //sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name); 325 //fp_no_motion = open(file_name, O_RDONLY | O_NONBLOCK); 326 #if FLICK_SUPPORTED /* hide flick, not offially supported */ 327 sprintf(file_name, "%s/event_flick", dev_dir_name); 328 fp_flick = open(file_name, O_RDONLY | O_NONBLOCK); 329 #endif 330 331 pfd[0].fd = fp_tap; 332 pfd[0].events = POLLPRI|POLLERR, 333 pfd[0].revents = 0; 334 335 pfd[1].fd = fp_orient; 336 pfd[1].events = POLLPRI|POLLERR, 337 pfd[1].revents = 0; 338 339 pfd[2].fd = fp_disp; 340 pfd[2].events = POLLPRI|POLLERR, 341 pfd[2].revents = 0; 342 343 pfd[3].fd = fp_motion; 344 pfd[3].events = POLLPRI|POLLERR, 345 pfd[3].revents = 0; 346 347 //pfd[4].fd = fp_no_motion; 348 //pfd[4].events = POLLPRI|POLLERR, 349 //pfd[4].revents = 0; 350 351 #if FLICK_SUPPORTED /* hide flick, not offially supported */ 352 pfd[5].fd = fp_flick; 353 pfd[5].events = POLLPRI|POLLERR, 354 pfd[5].revents = 0; 355 #endif 356 357 read(fp_tap, d, 4); 358 read(fp_orient, d, 4); 359 read(fp_disp, d, 4); 360 read(fp_motion, d, 4); 361 //read(fp_no_motion, d, 4); 362 #if FLICK_SUPPORTED /* hide flick, not offially supported */ 363 read(fp_flick, d, 4); 364 #endif 365 366 poll(pfd, n_gest, -1); 367 close(fp_tap); 368 close(fp_orient); 369 close(fp_disp); 370 close(fp_motion); 371 //close(fp_no_motion); 372 #if FLICK_SUPPORTED /* hide flick, not offially supported */ 373 close(fp_flick); 374 #endif 375 for (i = 0; i < ARRAY_SIZE(pfd); i++) { 376 if(pfd[i].revents != 0) { 377 switch (i){ 378 case 0: 379 sprintf(file_name, "%s/event_tap", dev_dir_name); 380 fp = fopen(file_name, "rt"); 381 fscanf(fp, "%d\n", &data); 382 printf("tap=%x\n", data); 383 HandleTap(data); 384 fclose(fp); 385 break; 386 case 1: 387 sprintf(file_name, "%s/event_orientation", dev_dir_name); 388 fp = fopen(file_name, "rt"); 389 fscanf(fp, "%d\n", &data); 390 printf("orient=%x\n", data); 391 HandleOrient(data); 392 fclose(fp); 393 break; 394 case 2: 395 sprintf(file_name, "%s/event_display_orientation", dev_dir_name); 396 fp = fopen(file_name, "rt"); 397 fscanf(fp, "%d\n", &data); 398 printf("display_orient=%x\n", data); 399 fclose(fp); 400 break; 401 case 3: 402 sprintf(file_name, "%s/event_accel_wom", dev_dir_name); 403 fp = fopen(file_name, "rt"); 404 fscanf(fp, "%d\n", &data); 405 printf("motion=%x\n", data); 406 fclose(fp); 407 break; 408 case 4: 409 sprintf(file_name, "%s/event_accel_no_motion", dev_dir_name); 410 fp = fopen(file_name, "rt"); 411 fscanf(fp, "%d\n", &data); 412 printf("No motion=%x\n", data); 413 fclose(fp); 414 break; 415 416 #if FLICK_SUPPORTED /* hide flick, not offially supported */ 417 case 5: 418 sprintf(file_name, "%s/event_flick", dev_dir_name); 419 fp = fopen(file_name, "rt"); 420 fscanf(fp, "%d\n", &data); 421 printf("flick=%x\n", data); 422 fclose(fp); 423 break; 424 #endif 425 } 426 } 427 } 428 } 429 } 430 431 432 int main(int argc, char **argv) 433 { 434 unsigned long num_loops = 2; 435 unsigned long timedelay = 100000; 436 unsigned long buf_len = 128; 437 438 int ret, c, i, j, toread; 439 int fp; 440 441 int num_channels; 442 char *trigger_name = NULL; 443 char *dev_dir_name, *buf_dir_name; 444 445 int datardytrigger = 1; 446 char *data; 447 int read_size; 448 int dev_num, trig_num; 449 char *buffer_access; 450 int scan_size; 451 int noevents = 0; 452 int p_event = 0, nodmp = 0; 453 char *dummy; 454 char chip_name[10]; 455 char device_name[10]; 456 char sysfs[100]; 457 458 struct iio_channel_info *infoarray; 459 /* -r means no DMP is enabled (raw) -> should be used for mpu3050. 460 -p means no print of data */ 461 /* when using -p, 1 means orientation, 2 means tap, 3 means flick */ 462 while ((c = getopt(argc, argv, "l:w:c:pret:")) != -1) { 463 switch (c) { 464 case 't': 465 trigger_name = optarg; 466 datardytrigger = 0; 467 break; 468 case 'e': 469 noevents = 1; 470 break; 471 case 'p': 472 p_event = 1; 473 break; 474 case 'r': 475 nodmp = 1; 476 break; 477 case 'c': 478 num_loops = strtoul(optarg, &dummy, 10); 479 break; 480 case 'w': 481 timedelay = strtoul(optarg, &dummy, 10); 482 break; 483 case 'l': 484 buf_len = strtoul(optarg, &dummy, 10); 485 break; 486 case '?': 487 return -1; 488 } 489 } 490 inv_get_sysfs_path(sysfs); 491 printf("sss:::%s\n", sysfs); 492 if (inv_get_chip_name(chip_name) != INV_SUCCESS) { 493 printf("get chip name fail\n"); 494 exit(0); 495 } 496 printf("chip_name=%s\n", chip_name); 497 if (INV_SUCCESS != inv_check_key()) 498 printf("key check fail\n"); 499 else 500 printf("key authenticated\n"); 501 502 for (i=0; i<strlen(chip_name); i++) { 503 device_name[i] = tolower(chip_name[i]); 504 } 505 device_name[strlen(chip_name)] = '\0'; 506 printf("device name: %s\n", device_name); 507 508 /* Find the device requested */ 509 dev_num = find_type_by_name(device_name, "iio:device"); 510 if (dev_num < 0) { 511 printf("Failed to find the %s\n", device_name); 512 ret = -ENODEV; 513 goto error_ret; 514 } 515 printf("iio device number being used is %d\n", dev_num); 516 asprintf(&dev_dir_name, "%siio:device%d", iio_dir, dev_num); 517 if (trigger_name == NULL) { 518 /* 519 * Build the trigger name. If it is device associated it's 520 * name is <device_name>_dev[n] where n matches the device 521 * number found above 522 */ 523 ret = asprintf(&trigger_name, 524 "%s-dev%d", device_name, dev_num); 525 if (ret < 0) { 526 ret = -ENOMEM; 527 goto error_ret; 528 } 529 } 530 ret = write_sysfs_int("buffer/enable", dev_dir_name, 0); 531 532 ret = write_sysfs_int_and_verify("power_state", dev_dir_name, 1); 533 ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1); 534 ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1); 535 ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 1); 536 /* 537 ret = write_sysfs_int_and_verify("zero_motion_on", dev_dir_name, 1); 538 ret = write_sysfs_int_and_verify("zero_motion_dur", dev_dir_name, 12); 539 ret = write_sysfs_int_and_verify("zero_motion_threshold", dev_dir_name, 13); 540 541 ret = write_sysfs_int_and_verify("motion_on", dev_dir_name, 1); 542 ret = write_sysfs_int_and_verify("motion_dur", dev_dir_name, 1); 543 ret = write_sysfs_int_and_verify("motion_threshold", dev_dir_name, 1); 544 */ 545 ret = write_sysfs_int_and_verify("accel_wom_on", dev_dir_name, 1); 546 ret = write_sysfs_int_and_verify("accel_wom_threshold", dev_dir_name, 100); 547 /* Verify the trigger exists */ 548 trig_num = find_type_by_name(trigger_name, "trigger"); 549 if (trig_num < 0) { 550 printf("Failed to find the trigger %s\n", trigger_name); 551 ret = -ENODEV; 552 goto error_free_triggername; 553 } 554 printf("iio trigger number being used is %d\n", trig_num); 555 /* 556 * Parse the files in scan_elements to identify what channels are 557 * present 558 */ 559 ret = 0; 560 ret = enable(dev_dir_name, &infoarray, &num_channels); 561 if (ret) { 562 printf("error enable\n"); 563 goto error_free_triggername; 564 } 565 if (!nodmp) 566 setup_dmp(dev_dir_name, p_event); 567 568 /* 569 * Construct the directory name for the associated buffer. 570 * As we know that the lis3l02dq has only one buffer this may 571 * be built rather than found. 572 */ 573 ret = asprintf(&buf_dir_name, "%siio:device%d/buffer", iio_dir, dev_num); 574 if (ret < 0) { 575 ret = -ENOMEM; 576 goto error_free_triggername; 577 } 578 printf("%s %s\n", dev_dir_name, trigger_name); 579 580 /* Set the device trigger to be the data rdy trigger found above */ 581 ret = write_sysfs_string_and_verify("trigger/current_trigger", 582 dev_dir_name, 583 trigger_name); 584 if (ret < 0) { 585 printf("Failed to write current_trigger file\n"); 586 goto error_free_buf_dir_name; 587 } 588 /* Setup ring buffer parameters */ 589 /* length must be even number because iio_store_to_sw_ring is expecting 590 half pointer to be equal to the read pointer, which is impossible 591 when buflen is odd number. This is actually a bug in the code */ 592 ret = write_sysfs_int("length", buf_dir_name, buf_len*2); 593 if (ret < 0) 594 goto exit_here; 595 ret = write_sysfs_int_and_verify("gyro_enable", dev_dir_name, 1); 596 ret = write_sysfs_int_and_verify("accl_enable", dev_dir_name, 1); 597 //ret = write_sysfs_int_and_verify("compass_enable", dev_dir_name, 0); 598 if (nodmp == 0) { 599 ret = write_sysfs_int_and_verify("quaternion_on", dev_dir_name, 1); 600 } else { 601 ret = disable_q_out(dev_dir_name, &infoarray, &num_channels); 602 ret = write_sysfs_int_and_verify("dmp_on", dev_dir_name, 0); 603 } 604 ret = build_channel_array(dev_dir_name, &infoarray, &num_channels); 605 if (ret) { 606 printf("Problem reading scan element information\n"); 607 goto exit_here; 608 } 609 610 /* Enable the buffer */ 611 ret = write_sysfs_int("enable", buf_dir_name, 1); 612 if (ret < 0) 613 goto exit_here; 614 scan_size = size_from_channelarray(infoarray, num_channels); 615 data = malloc(scan_size*buf_len); 616 if (!data) { 617 ret = -ENOMEM; 618 goto exit_here; 619 } 620 621 ret = asprintf(&buffer_access, 622 "/dev/iio:device%d", 623 dev_num); 624 if (ret < 0) { 625 ret = -ENOMEM; 626 goto error_free_data; 627 } 628 if (p_event) { 629 get_dmp_event(dev_dir_name); 630 goto error_free_buffer_access; 631 } 632 /* Attempt to open non blocking the access dev */ 633 fp = open(buffer_access, O_RDONLY | O_NONBLOCK); 634 if (fp == -1) { /*If it isn't there make the node */ 635 printf("Failed to open %s\n", buffer_access); 636 ret = -errno; 637 goto error_free_buffer_access; 638 } 639 /* Wait for events 10 times */ 640 for (j = 0; j < num_loops; j++) { 641 if (!noevents) { 642 struct pollfd pfd = { 643 .fd = fp, 644 .events = POLLIN, 645 }; 646 poll(&pfd, 1, -1); 647 toread = 1; 648 if ((j%128)==0) 649 usleep(timedelay); 650 651 } else { 652 usleep(timedelay); 653 toread = 1; 654 } 655 read_size = read(fp, 656 data, 657 toread*scan_size); 658 if (read_size == -EAGAIN) { 659 printf("nothing available\n"); 660 continue; 661 } 662 if (0 == p_event) { 663 for (i = 0; i < read_size/scan_size; i++) 664 process_scan(data + scan_size*i, 665 infoarray, 666 num_channels); 667 } 668 } 669 close(fp); 670 error_free_buffer_access: 671 free(buffer_access); 672 error_free_data: 673 free(data); 674 exit_here: 675 /* Stop the ring buffer */ 676 ret = write_sysfs_int("enable", buf_dir_name, 0); 677 678 error_free_buf_dir_name: 679 free(buf_dir_name); 680 error_free_triggername: 681 if (datardytrigger) 682 free(trigger_name); 683 error_ret: 684 return ret; 685 } 686
