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_sysfs_helper.h"
23 #include "mlos.h"
24 
25 #define POLL_TIME (2000) // 2sec
26 
27 // settings
28 int verbose = false;
29 
30 // paths
31 char *dev_dir_name;
32 
33 /**************************************************
34    This _kbhit() function is courtesy of the web
35 ***************************************************/
_kbhit(void)36 int _kbhit(void)
37 {
38     static const int STDIN = 0;
39     static bool initialized = false;
40 
41     if (!initialized) {
42         // Use termios to turn off line buffering
43         struct termios term;
44         tcgetattr(STDIN, &term);
45         term.c_lflag &= ~ICANON;
46         tcsetattr(STDIN, TCSANOW, &term);
47         setbuf(stdin, NULL);
48         initialized = true;
49     }
50 
51     int bytesWaiting;
52     ioctl(STDIN, FIONREAD, &bytesWaiting);
53     return bytesWaiting;
54 }
55 
get_sensor_data(char * d,short * sensor)56 void get_sensor_data(char *d, short *sensor)
57 {
58     int i;
59     for (i = 0; i < 3; i++)
60         sensor[i] = *(short *)(d + 2 + i * 2);
61 }
62 
read_data(char * buffer_access)63 static int read_data(char *buffer_access)
64 {
65 #define PRESSURE_HDR             0x8000
66 #define ACCEL_HDR                0x4000
67 #define GYRO_HDR                 0x2000
68 #define COMPASS_HDR              0x1000
69 #define LPQUAT_HDR               0x0800
70 #define SIXQUAT_HDR              0x0400
71 #define PEDQUAT_HDR              0x0200
72 #define STEP_DETECTOR_HDR        0x0100
73 
74     static int left_over_size = 0;
75     char data[1048], *dptr, tmp[24];
76     short sensor[3];
77     int q[3];
78     int ret, i, ind, fp;
79     int buf_size, read_size;
80     unsigned short hdr;
81     bool done_flag;
82 
83     fp = open(buffer_access, O_RDONLY | O_NONBLOCK);
84     if (fp == -1) { /* if it isn't there make the node */
85         printf("Failed to open %s\n", buffer_access);
86         ret = -errno;
87         goto error_read_data;
88     }
89     ind = 0;
90 
91     {
92         struct pollfd pfd = {
93             .fd = fp,
94             .events = POLLIN,
95         };
96         poll(&pfd, 1, -1);
97 
98         if (left_over_size > 0)
99             memcpy(data, tmp, left_over_size);
100         dptr = data + left_over_size;
101 
102         read_size = read(fp,  dptr, 1024);
103         if (read_size <= 0) {
104             printf("Wrong size=%d\n", read_size);
105             ret = -EINVAL;
106             goto error_read_data;
107         }
108 
109         ind = read_size + left_over_size;
110         dptr = data;
111         buf_size = ind - (dptr - data);
112         done_flag = false;
113         while ((buf_size > 0) && (!done_flag)) {
114             hdr = *((short *)(dptr));
115             if (hdr & 1)
116                 printf("STEP\n");
117 
118             switch (hdr & (~1)) {
119             case PRESSURE_HDR:
120                 if (buf_size >= 16) {
121                     get_sensor_data(dptr, sensor);
122                     dptr += 8;
123                     printf("PRESS, %d, %lld\n", (sensor[1] << 16) + (unsigned short)sensor[2], *(long long *)dptr);
124                 } else
125                     done_flag = true;
126                 break;
127             case ACCEL_HDR:
128                 if (buf_size >= 16) {
129                     get_sensor_data(dptr, sensor);
130                     dptr += 8;
131                     printf("ACCEL, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
132                 } else
133                     done_flag = true;
134                 break;
135             case GYRO_HDR:
136                 if (buf_size >= 16) {
137                     get_sensor_data(dptr, sensor);
138                     dptr += 8;
139                     printf("GYRO, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
140                 } else
141                     done_flag = true;
142                 break;
143             case COMPASS_HDR:
144                 if (buf_size >= 16) {
145                     get_sensor_data(dptr, sensor);
146                     dptr += 8;
147                     printf("COMPASS, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
148                 } else
149                     done_flag = true;
150                 break;
151             case PEDQUAT_HDR:
152                 if (buf_size >= 16) {
153                     get_sensor_data(dptr, sensor);
154                     dptr += 8;
155                     printf("LOW_RES_QUAT, %d, %d, %d, %lld\n", sensor[0], sensor[1], sensor[2], *(long long *)dptr);
156                 }  else
157                     done_flag = true;
158                 break;
159             case LPQUAT_HDR:
160                 if (buf_size >= 24) {
161                     q[0] = *(int *)(dptr + 4);
162                     dptr += 8;
163                     q[1] = *(int *)(dptr);
164                     q[2] = *(int *)(dptr + 4);
165                     dptr += 8;
166                     printf("LPQ_3AXES, %d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr);
167                 }  else
168                     done_flag = true;
169                 break;
170             case SIXQUAT_HDR:
171                 if (buf_size >= 24) {
172                     q[0] = *(int *)(dptr + 4);
173                     dptr += 8;
174                     q[1] = *(int *)(dptr);
175                     q[2] = *(int *)(dptr + 4);
176                     dptr += 8;
177                     printf("LPQ_6AXES, %d, %d, %d, %lld\n", q[0], q[1], q[2], *(long long *)dptr);
178                 }  else
179                     done_flag = true;
180                 break;
181             case STEP_DETECTOR_HDR:
182                 if (buf_size >= 16) {
183                     printf("STEP_DETECTOR, ");
184                     dptr += 8;
185                     printf("%lld\n", *(long long *)dptr);
186                 }  else
187                     done_flag = true;
188 
189                 break;
190             default:
191                 printf("unknown, \n");
192                 for (i = 0; i < 8; i++)
193                     printf("%02x, ", dptr[i]);
194                 printf("\n");
195                 break;
196             }
197             if (!done_flag)
198                 dptr += 8;
199             buf_size = ind - (dptr - data);
200         }
201         if (ind - (dptr - data) > 0)
202             memcpy(tmp, dptr, ind - (dptr - data));
203         left_over_size = ind - (dptr - data);
204     }
205     close(fp);
206 
207 error_read_data:
208     return ret;
209 }
210 
211 /*
212     Main
213 */
214 
main(int argc,char ** argv)215 int main(int argc, char **argv)
216 {
217     unsigned long num_loops = -1;
218     int ret, c, i;
219 
220     int dev_num;
221     char *buffer_access;
222     char chip_name[10];
223     char *dummy;
224     char device_name[10];
225     char sysfs[100];
226 
227     // all output to stdout must be delivered immediately, no buffering
228     setvbuf(stdout, NULL, _IONBF, 0);
229 
230     /* parse the command line parameters
231        TODO description
232     */
233     while ((c = getopt(argc, argv, "c:vh")) != -1) {
234         switch (c) {
235         case 'c':
236             num_loops = strtoul(optarg, &dummy, 10);
237             break;
238         case 'v':
239             verbose = true;
240             break;
241         case 'h':
242             //print_help();
243             // TODO write print_help helper function
244             break;
245         case '?':
246             return -1;
247         }
248     }
249 
250     // get info about the device and driver
251     inv_get_sysfs_path(sysfs);
252     if (inv_get_chip_name(chip_name) != INV_SUCCESS) {
253         printf("get chip name fail\n");
254         exit(0);
255     }
256     printf("INFO: chip_name=%s\n", chip_name);
257 
258     for (i = 0; i < strlen(chip_name); i++)
259         device_name[i] = tolower(chip_name[i]);
260     device_name[strlen(chip_name)] = '\0';
261     printf("INFO: device name=%s\n", device_name);
262 
263     /* Find the device requested */
264     dev_num = find_type_by_name(device_name, "iio:device");
265     if (dev_num < 0) {
266         printf("Failed to find the %s\n", device_name);
267         ret = -ENODEV;
268         goto error_ret;
269     }
270     printf("INFO: iio device number=%d\n", dev_num);
271 
272     /* attempt to open non blocking the access dev */
273     ret = asprintf(&buffer_access, "/dev/iio:device%d", dev_num);
274     if (ret < 0) {
275         ret = -ENOMEM;
276         goto error_ret;
277     }
278     while (num_loops == -1 || num_loops--)
279         read_data(buffer_access);
280     free(buffer_access);
281 
282 error_ret:
283     return ret;
284 }
285