diff options
Diffstat (limited to '60xx/libsensors_iio/MPLSupport.cpp')
| -rw-r--r-- | 60xx/libsensors_iio/MPLSupport.cpp | 190 |
1 files changed, 190 insertions, 0 deletions
diff --git a/60xx/libsensors_iio/MPLSupport.cpp b/60xx/libsensors_iio/MPLSupport.cpp new file mode 100644 index 0000000..9773562 --- /dev/null +++ b/60xx/libsensors_iio/MPLSupport.cpp @@ -0,0 +1,190 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#define LOG_NDEBUG 0 + +#include <MPLSupport.h> +#include <string.h> +#include <stdio.h> +#include <fcntl.h> + +#include "log.h" +#include "SensorBase.h" + +#include "ml_sysfs_helper.h" +#include "local_log_def.h" + +int64_t getTimestamp() +{ + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec; +} + +int64_t timevalToNano(timeval const& t) { + return t.tv_sec * 1000000000LL + t.tv_usec * 1000; +} + +int inv_read_data(char *fname, long *data) +{ + VFUNC_LOG; + + char buf[sizeof(long) * 4]; + int count, fd; + + fd = open(fname, O_RDONLY); + if(fd < 0) { + LOGE("HAL:Error opening %s", fname); + return -1; + } + memset(buf, 0, sizeof(buf)); + count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 1) { + close(fd); + return -1; + } else { + count = sscanf(buf, "%ld", data); + if(count) + LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data); + } + close(fd); + + return 0; +} + +/* This one DOES NOT close FDs for you */ +int read_attribute_sensor(int fd, char* data, unsigned int size) +{ + VFUNC_LOG; + + int count = 0; + if (fd > 0) { + count = pread(fd, data, size, 0); + if(count < 1) { + LOGE("HAL:read fails with error code=%d", count); + } + } + return count; +} + +/** + * @brief Enable a sensor through the sysfs file descriptor + * provided. + * @note this function one closes FD after the write + * @param fd + * the file descriptor to write into + * @param en + * the value to write, typically 1 or 0 + * @return the errno whenever applicable. + */ +int enable_sysfs_sensor(int fd, int en) +{ + VFUNC_LOG; + + int nb; + int err = 0; + + char c = en ? '1' : '0'; + nb = write(fd, &c, 1); + + if (nb <= 0) { + err = errno; + LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)", + c, nb, strerror(err), err); + } + close(fd); + + + return err; +} + +/* This one closes FDs for you */ +int write_attribute_sensor(int fd, long data) +{ + VFUNC_LOG; + + int num_b = 0; + + if (fd >= 0) { + char buf[80]; + sprintf(buf, "%ld", data); + num_b = write(fd, buf, strlen(buf) + 1); + if (num_b <= 0) { + int err = errno; + LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err); + } else { + LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data); + } + close(fd); + } + + return num_b; +} + +int read_sysfs_int(char *filename, int *var) +{ + int res=0; + FILE *sysfsfp; + + sysfsfp = fopen(filename, "r"); + if (sysfsfp != NULL) { + if (fscanf(sysfsfp, "%d\n", var) < 1) { + LOGE("HAL:ERR failed to read an int from %s.", filename); + res = -EINVAL; + } + fclose(sysfsfp); + } else { + res = -errno; + LOGE("HAL:ERR open file %s to read with error %d", filename, res); + } + return res; +} + +int write_sysfs_int(char *filename, int var) +{ + int res = 0; + FILE *sysfsfp; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + var, filename, getTimestamp()); + sysfsfp = fopen(filename, "w"); + if (sysfsfp == NULL) { + res = -errno; + LOGE("HAL:ERR open file %s to write with error %d", filename, res); + return res; + } + int fpres, fcres = 0; + fpres = fprintf(sysfsfp, "%d\n", var); + /* fprintf() can succeed because it never actually writes to the + * underlying sysfs node. + */ + if (fpres < 0) { + res = -errno; + fclose(sysfsfp); + } else { + fcres = fclose(sysfsfp); + /* Check for errors from: fflush(), write(), and close() */ + if (fcres < 0) { + res = -errno; + } + } + if (fpres < 0 || fcres < 0) { + LOGE("HAL:ERR failed to write %d to %s (err=%d) print/close=%d/%d", + var, filename, res, fpres, fcres); + } + return res; +} |