1 /*
2 * Copyright (C) 2014 Invensense, Inc.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <MPLSupport.h>
18 #include <dirent.h>
19 #include <string.h>
20 #include <stdio.h>
21 #include "log.h"
22 #include "SensorBase.h"
23 #include <fcntl.h>
24
25 #include "ml_sysfs_helper.h"
26 #include "ml_load_dmp.h"
27
inv_read_data(char * fname,long * data)28 int inv_read_data(char *fname, long *data)
29 {
30 VFUNC_LOG;
31
32 char buf[sizeof(long) * 4];
33 int count, fd;
34
35 fd = open(fname, O_RDONLY);
36 if(fd < 0) {
37 LOGE("HAL:Error opening %s", fname);
38 return -1;
39 }
40 memset(buf, 0, sizeof(buf));
41 count = read_attribute_sensor(fd, buf, sizeof(buf));
42 if(count < 1) {
43 close(fd);
44 return -1;
45 } else {
46 count = sscanf(buf, "%ld", data);
47 if(count)
48 LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data);
49 }
50 close(fd);
51
52 return 0;
53 }
54
55 /* This one DOES NOT close FDs for you */
read_attribute_sensor(int fd,char * data,unsigned int size)56 int read_attribute_sensor(int fd, char* data, unsigned int size)
57 {
58 VFUNC_LOG;
59
60 int count = 0;
61 if (fd > 0) {
62 count = pread(fd, data, size, 0);
63 if(count < 1) {
64 LOGE("HAL:read fails with error code=%d", count);
65 }
66 }
67 return count;
68 }
69
70 /**
71 * @brief Enable a sensor through the sysfs file descriptor
72 * provided.
73 * @note this function one closes FD after the write
74 * @param fd
75 * the file descriptor to write into
76 * @param en
77 * the value to write, typically 1 or 0
78 * @return the errno whenever applicable.
79 */
enable_sysfs_sensor(int fd,int en)80 int enable_sysfs_sensor(int fd, int en)
81 {
82 VFUNC_LOG;
83
84 int nb;
85 int err = 0;
86
87 char c = en ? '1' : '0';
88 nb = write(fd, &c, 1);
89
90 if (nb <= 0) {
91 err = errno;
92 LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)",
93 c, nb, strerror(err), err);
94 }
95 close(fd);
96
97
98 return -err;
99 }
100
101 /* This one closes FDs for you */
write_attribute_sensor(int fd,long data)102 int write_attribute_sensor(int fd, long data)
103 {
104 VFUNC_LOG;
105
106 int num_b = 0;
107
108 if (fd >= 0) {
109 char buf[80];
110 sprintf(buf, "%ld", data);
111 num_b = write(fd, buf, strlen(buf) + 1);
112 if (num_b <= 0) {
113 int err = errno;
114 LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
115 } else {
116 LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
117 }
118 close(fd);
119 }
120
121 return num_b;
122 }
123
124 /* This one DOES NOT close FDs for you */
write_attribute_sensor_continuous(int fd,long data)125 int write_attribute_sensor_continuous(int fd, long data)
126 {
127 VFUNC_LOG;
128
129 int num_b = 0;
130
131 if (fd >= 0) {
132 char buf[80];
133 sprintf(buf, "%ld", data);
134 num_b = write(fd, buf, strlen(buf) + 1);
135 if (num_b <= 0) {
136 int err = errno;
137 LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err);
138 } else {
139 LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data);
140 }
141 }
142
143 return num_b;
144 }
145
read_sysfs_int(char * filename,int * var)146 int read_sysfs_int(char *filename, int *var)
147 {
148 int res=0;
149 FILE *sysfsfp;
150
151 sysfsfp = fopen(filename, "r");
152 if (sysfsfp != NULL) {
153 if (fscanf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) {
154 res = errno;
155 LOGE("HAL:ERR open file %s to read with error %d", filename, res);
156 }
157 }
158 return -res;
159 }
160
read_sysfs_int64(char * filename,int64_t * var)161 int read_sysfs_int64(char *filename, int64_t *var)
162 {
163 int res=0;
164 FILE *sysfsfp;
165
166 sysfsfp = fopen(filename, "r");
167 if (sysfsfp != NULL) {
168 if (fscanf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {
169 res = errno;
170 LOGE("HAL:ERR open file %s to read with error %d", filename, res);
171 }
172 }
173 return -res;
174 }
175
convert_long_to_hex_char(long * quat,unsigned char * hex,int numElement)176 void convert_long_to_hex_char(long* quat, unsigned char* hex, int numElement)
177 {
178 int bytePosition = 0;
179 for (int index = 0; index < numElement; index++) {
180 for (int i = 0; i < 4; i++) {
181 hex[bytePosition] = (int) ((quat[index] >> (4-1-i) * 8) & 0xFF);
182 //LOGI("e%d quat[%d]: %x", index, bytePosition, hex[bytePosition]);
183 bytePosition++;
184 }
185 }
186 return;
187 }
188
write_sysfs_int(char * filename,int var)189 int write_sysfs_int(char *filename, int var)
190 {
191 int res=0;
192 FILE *sysfsfp;
193
194 sysfsfp = fopen(filename, "w");
195 if (sysfsfp != NULL) {
196 if (fprintf(sysfsfp, "%d\n", var) < 0 || fclose(sysfsfp) < 0) {
197 res = errno;
198 LOGE("HAL:ERR open file %s to write with error %d", filename, res);
199 }
200 }
201 return -res;
202 }
203
write_sysfs_longlong(char * filename,int64_t var)204 int write_sysfs_longlong(char *filename, int64_t var)
205 {
206 int res=0;
207 FILE *sysfsfp;
208
209 sysfsfp = fopen(filename, "w");
210 if (sysfsfp != NULL) {
211 if (fprintf(sysfsfp, "%lld\n", var) < 0 || fclose(sysfsfp) < 0) {
212 res = errno;
213 LOGE("HAL:ERR open file %s to write with error %d", filename, res);
214 }
215 }
216 return -res;
217 }
218
fill_dev_full_name_by_prefix(const char * dev_prefix,char * dev_full_name,int len)219 int fill_dev_full_name_by_prefix(const char* dev_prefix,
220 char *dev_full_name, int len)
221 {
222 char cand_name[20];
223 int prefix_len = strlen(dev_prefix);
224 strncpy(cand_name, dev_prefix, sizeof(cand_name) / sizeof(cand_name[0]));
225
226 // try adding a number, 0-9
227 for(int cand_postfix = 0; cand_postfix < 10; cand_postfix++) {
228 snprintf(&cand_name[prefix_len],
229 sizeof(cand_name) / sizeof(cand_name[0]),
230 "%d", cand_postfix);
231 int dev_num = find_type_by_name(cand_name, "iio:device");
232 if (dev_num != -ENODEV) {
233 strncpy(dev_full_name, cand_name, len);
234 return 0;
235 }
236 }
237 // try adding a small letter, a-z
238 for(char cand_postfix = 'a'; cand_postfix <= 'z'; cand_postfix++) {
239 snprintf(&cand_name[prefix_len],
240 sizeof(cand_name) / sizeof(cand_name[0]),
241 "%c", cand_postfix);
242 int dev_num = find_type_by_name(cand_name, "iio:device");
243 if (dev_num != -ENODEV) {
244 strncpy(dev_full_name, cand_name, len);
245 return 0;
246 }
247 }
248 // try adding a capital letter, A-Z
249 for(char cand_postfix = 'A'; cand_postfix <= 'Z'; cand_postfix++) {
250 snprintf(&cand_name[prefix_len],
251 sizeof(cand_name) / sizeof(cand_name[0]),
252 "%c", cand_postfix);
253 int dev_num = find_type_by_name(cand_name, "iio:device");
254 if (dev_num != -ENODEV) {
255 strncpy(dev_full_name, cand_name, len);
256 return 0;
257 }
258 }
259 return 1;
260 }
261
dump_dmp_img(const char * outFile)262 void dump_dmp_img(const char *outFile)
263 {
264 FILE *fp;
265 int i;
266
267 char sysfs_path[MAX_SYSFS_NAME_LEN];
268 char dmp_path[MAX_SYSFS_NAME_LEN];
269
270 inv_get_sysfs_path(sysfs_path);
271 sprintf(dmp_path, "%s%s", sysfs_path, "/dmp_firmware");
272
273 LOGI("HAL DEBUG:dump DMP image");
274 LOGI("HAL DEBUG:open %s\n", dmp_path);
275 LOGI("HAL DEBUG:write to %s", outFile);
276
277 read_dmp_img(dmp_path, (char *)outFile);
278 }
279
read_sysfs_dir(bool fileMode,char * sysfs_path)280 int read_sysfs_dir(bool fileMode, char *sysfs_path)
281 {
282 VFUNC_LOG;
283
284 int res = 0;
285 char full_path[MAX_SYSFS_NAME_LEN];
286 int fd;
287 char buf[sizeof(long) *4];
288 long data;
289
290 DIR *dp;
291 struct dirent *ep;
292
293 dp = opendir (sysfs_path);
294
295 if (dp != NULL)
296 {
297 LOGI("******************** System Sysfs Dump ***************************");
298 LOGV_IF(0,"HAL DEBUG: opened directory %s", sysfs_path);
299 while ((ep = readdir (dp))) {
300 if(ep != NULL) {
301 LOGV_IF(0,"file name %s", ep->d_name);
302 if(!strcmp(ep->d_name, ".") || !strcmp(ep->d_name, "..") ||
303 !strcmp(ep->d_name, "uevent") || !strcmp(ep->d_name, "dev") ||
304 !strcmp(ep->d_name, "self_test"))
305 continue;
306 sprintf(full_path, "%s%s%s", sysfs_path, "/", ep->d_name);
307 LOGV_IF(0,"HAL DEBUG: reading %s", full_path);
308 fd = open(full_path, O_RDONLY);
309 if (fd > -1) {
310 memset(buf, 0, sizeof(buf));
311 res = read_attribute_sensor(fd, buf, sizeof(buf));
312 close(fd);
313 if (res > 0) {
314 res = sscanf(buf, "%ld", &data);
315 if (res)
316 LOGI("HAL DEBUG:sysfs:cat %s = %ld", full_path, data);
317 } else {
318 LOGV_IF(0,"HAL DEBUG: error reading %s", full_path);
319 }
320 } else {
321 LOGV_IF(0,"HAL DEBUG: error opening %s", full_path);
322 }
323 close(fd);
324 }
325 }
326 closedir(dp);
327 } else{
328 LOGI("HAL DEBUG: could not open directory %s", sysfs_path);
329 }
330
331 return res;
332 }
333
inv_float_to_q16(float * fdata,long * ldata)334 int inv_float_to_q16(float *fdata, long *ldata)
335 {
336
337 if (!fdata || !ldata)
338 return -1;
339 ldata[0] = (long)(fdata[0] * 65536.f);
340 ldata[1] = (long)(fdata[1] * 65536.f);
341 ldata[2] = (long)(fdata[2] * 65536.f);
342 return 0;
343 }
344
inv_long_to_q16(long * fdata,long * ldata)345 int inv_long_to_q16(long *fdata, long *ldata)
346 {
347
348 if (!fdata || !ldata)
349 return -1;
350 ldata[0] = (fdata[1] * 65536.f);
351 ldata[1] = (fdata[2] * 65536.f);
352 ldata[2] = (fdata[3] * 65536.f);
353 return 0;
354 }
355
inv_float_to_round(float * fdata,long * ldata)356 int inv_float_to_round(float *fdata, long *ldata)
357 {
358
359 if (!fdata || !ldata)
360 return -1;
361 ldata[0] = (long)fdata[0];
362 ldata[1] = (long)fdata[1];
363 ldata[2] = (long)fdata[2];
364 return 0;
365 }
366
inv_float_to_round2(float * fdata,short * ldata)367 int inv_float_to_round2(float *fdata, short *ldata)
368 {
369
370 if (!fdata || !ldata)
371 return -1;
372 ldata[0] = (short)fdata[0];
373 ldata[1] = (short)fdata[1];
374 ldata[2] = (short)fdata[2];
375 return 0;
376 }
377
inv_long_to_float(long * ldata,float * fdata)378 int inv_long_to_float(long *ldata, float *fdata)
379 {
380
381 if (!ldata || !fdata)
382 return -1;
383 fdata[0] = (float)ldata[0];
384 fdata[1] = (float)ldata[1];
385 fdata[2] = (float)ldata[2];
386 return 0;
387 }
388