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 ***************************************************/
_kbhit(void)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 */
size_from_channelarray(struct iio_channel_info * channels,int num_channels)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
print2byte(int input,struct iio_channel_info * info)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 */
process_scan(char * data,struct iio_channel_info * infoarray,int num_channels)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
enable_flick(char * p,int on)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
verify_img(char * dmp_path)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
setup_dmp(char * dev_path,int p_event)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
handle_flick(int flick)331 void handle_flick(int flick)
332 {
333 printf("flick=%x\n", flick);
334 }
335
handle_display_orientation(int orient)336 void handle_display_orientation(int orient)
337 {
338 printf("display_orientation=%x\n", orient);
339 }
340
handle_motion(int motion)341 void handle_motion(int motion)
342 {
343 printf("motion=%x\n", motion);
344 }
345
handle_orientation(int orient)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
handle_tap(int tap)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
handle_pedometer(int * got_event)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
dump_dmp_event_struct(void)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
init_dmp_event_fds(void)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
close_dmp_event_fds(void)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
poll_dmp_event_fds(void)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
main(int argc,char ** argv)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