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
28int verbose = false;
29
30// paths
31char *dev_dir_name;
32
33/**************************************************
34   This _kbhit() function is courtesy of the web
35***************************************************/
36int _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
56void 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
63static 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
207error_read_data:
208    return ret;
209}
210
211/*
212    Main
213*/
214
215int 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
282error_ret:
283    return ret;
284}
285