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);
sipmle_print(struct dmp_struct * dmp,int d)54 static void sipmle_print(struct dmp_struct *dmp, int d){
55 printf("%s:%d\n", dmp->fname, d);
56 }
57
handle_smd()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
pedo_print()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
HandleTap(struct dmp_struct * dmp,int tap)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];
verify_img()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
inv_set_rate()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
setup_offset_and_bias()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
setup_dmp(char * dev_path)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
get_dmp_event(void * param)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
enable_gyro(int on)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
enable_gyro_output(int on)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
enable_compass(int on)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
enable_pressure(int on)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
enable_quaternion(int on)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 }
enable_step_detector(int on)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 }
enable_step_indicator(int on)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
enable_accel(int on)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 }
enable_accel_output(int on)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
enable_enable(int on)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 }
write_dmp_event(int on)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
random_delay()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 }
dmp_event_control(on)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 }
enable_motion(int on)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};
run_enable_sequence()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
run_disable_sequence()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 }
run_dmp_off()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 }
control_switch(void * param)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
get_sensor_data(char * d,short * sensor)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
read_data(void * param)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
inv_create_thread()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
enable_enable_main(int on)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
main(int argc,char ** argv)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