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
28int 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 */
56int 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 */
80int 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 */
102int 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 */
125int 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
146int 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
161int 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
176void 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
189int 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
204int 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
219int 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
262void 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
280int 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
334int 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
345int 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
356int 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
367int 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
378int 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