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 ***************************************************/ 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 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 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 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
