From 42669afe03aabd45a9a3b5e68015dbbe37e6d5b9 Mon Sep 17 00:00:00 2001 From: Chih-Wei Huang Date: Mon, 25 May 2015 00:36:41 +0800 Subject: [PATCH] iio-sensors: initial porting for iio based sensors --- Android.mk | 9 + iio-sensors.cpp | 504 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sensors.mk | 1 + 3 files changed, 514 insertions(+) create mode 100644 iio-sensors.cpp diff --git a/Android.mk b/Android.mk index 4c5d0b6..a8c1ac2 100644 --- a/Android.mk +++ b/Android.mk @@ -14,6 +14,15 @@ LOCAL_SRC_FILES := hdaps.c include $(BUILD_SHARED_LIBRARY) include $(CLEAR_VARS) +LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw +LOCAL_SHARED_LIBRARIES := liblog libcutils +LOCAL_MODULE := sensors.iio +LOCAL_MODULE_TAGS := optional +LOCAL_SRC_FILES := iio-sensors.cpp +include external/stlport/libstlport.mk +include $(BUILD_SHARED_LIBRARY) + +include $(CLEAR_VARS) LOCAL_PRELINK_MODULE := false LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw LOCAL_SHARED_LIBRARIES := liblog libcutils diff --git a/iio-sensors.cpp b/iio-sensors.cpp new file mode 100644 index 0000000..d62db90 --- /dev/null +++ b/iio-sensors.cpp @@ -0,0 +1,504 @@ +/** + * + * IIO style sensor + * + * Copyright (C) 2015 The Android-x86 Open Source Project + * + * by Chih-Wei Huang + * + * Licensed under GPLv2 or later + * + **/ + +#define LOG_TAG "iio-sensors" + +//#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static const char *IIO_DIR = "/sys/bus/iio/devices/"; + +enum { + ID_ACCELERATION = SENSORS_HANDLE_BASE, + ID_MAGNETIC_FIELD, + ID_ORIENTATION, + ID_LIGHT, + ID_PROXIMITY, + ID_GYROSCOPE, + ID_PRESSURE, + ID_TEMPERATURE, + ID_ROT_VECTOR, + ID_SYNCOMPASS, + MAX_SENSORS +}; + + +// 720 LSG = 1G +#define LSG (1024.0f) +#define NUMOFACCDATA (8.0f) + +// conversion of acceleration data to SI units (m/s^2) +#define RANGE_A (2*GRAVITY_EARTH) +#define RESOLUTION_A (RANGE_A/(256*NUMOFACCDATA)) + +// conversion of magnetic data to uT units +#define RANGE_M (2048.0f) +#define RESOLUTION_M (0.01) + +/* conversion of orientation data to degree units */ +#define CONVERT_O (1.0f/64.0f) +#define CONVERT_O_A (CONVERT_O) +#define CONVERT_O_P (CONVERT_O) +#define CONVERT_O_R (-CONVERT_O) + +// conversion of gyro data to SI units (radian/sec) +#define RANGE_G (2000.0f*(float)M_PI/180.0f) +#define RESOLUTION_G (RANGE_G/(2000*NUMOFACCDATA)) + +// conversion of pressure and temperature data +#define CONVERT_PRESSURE (1.0f/100.0f) +#define CONVERT_TEMPERATURE (1.0f/100.0f) + +#define SENSOR_STATE_MASK (0x7FFF) + +// proximity threshold +#define PROXIMITY_THRESHOLD_GP2A (5.0f) + +// used in timespec_to_ns calculations +const long NSEC_PER_SEC = 1000000000L; + +#define BIT(x) (1 << (x)) + +struct SensorEvent : public sensors_event_t { + SensorEvent(int32_t id, int32_t t); +}; + +SensorEvent::SensorEvent(int32_t id, int32_t t) +{ + version = sizeof(sensors_event_t); + sensor = id; + type = t; + + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + timestamp = int64_t(ts.tv_sec) * NSEC_PER_SEC + ts.tv_nsec; +} + +class SensorBase : public sensor_t { + public: + SensorBase(); + virtual ~SensorBase(); + + operator bool() const { return enabled; } + int setDelay(int64_t ns); + bool scan(const char *d); + virtual int activate(bool); + virtual int readEvents(sensors_event_t *data, int); + + protected: + int read_sysfs_str(const char *file, char *buf); + int read_sysfs_int(const char *file); + float read_sysfs_float(const char *file); + + bool enabled; + char *path; + const char ***nodes; + struct timespec delay; +}; + +SensorBase::SensorBase() +{ + memset(this, 0, sizeof(SensorBase)); + + vendor = "Android-x86 Open Source Project"; + version = 1; + + delay.tv_nsec = 200000000L; +} + +SensorBase::~SensorBase() +{ + free(path); +} + +int SensorBase::setDelay(int64_t ns) +{ + delay.tv_sec = ns / NSEC_PER_SEC; + delay.tv_nsec = ns % NSEC_PER_SEC; + return 0; +} + +bool SensorBase::scan(const char *p) +{ + int i; + char node[PATH_MAX]; + while (const char **ns = *nodes) { + for (i = 0; ns[i]; ++i) { + snprintf(node, PATH_MAX, "%s/%s", p, ns[i]); + if (access(node, F_OK)) + break; + } + if (!ns[i]) + break; + nodes++; + } + if (*nodes) { + path = strdup(p); + node[0] = '\0'; + for (i = 0; (*nodes)[i]; ++i) + strncat(strncat(node, (*nodes)[i], 1024), " ", 1024); + ALOGD("found node %s: %s", path, node); + } + return (path != 0); +} + +int SensorBase::activate(bool e) +{ + enabled = e; + return 0; +} + +int SensorBase::readEvents(sensors_event_t *data, int) +{ + nanosleep(&delay, 0); + SensorEvent *e = new (data) SensorEvent(handle, type); + return 1; +} + +int SensorBase::read_sysfs_str(const char *file, char *buf) +{ + int res = 0; + char filename[PATH_MAX]; + snprintf(filename, PATH_MAX, "%s/%s", path, file); + int fd = open(filename, O_RDONLY); + if (fd >= 0) { + ssize_t sz = read(fd, buf, 4096); + if (sz < 0) { + ALOGE("failed to read from %s: %s", filename, strerror(errno)); + res = -errno; + } + close(fd); + } + return res; +} + +int SensorBase::read_sysfs_int(const char *file) +{ + char buf[4096]; + return read_sysfs_str(file, buf) ? 0 : atoi(buf); +} + +float SensorBase::read_sysfs_float(const char *file) +{ + char buf[4096]; + return read_sysfs_str(file, buf) ? 0 : atof(buf); +} + +template class Sensor : SensorBase { + public: + Sensor(); + virtual int readEvents(sensors_event_t *data, int); + + static SensorBase *probe(const char *d); +}; + +template +SensorBase *Sensor::probe(const char *p) +{ + Sensor *s = new Sensor; + s->handle = H; + if (!s->scan(p)) { + delete s; + s = 0; + } + return s; +} + +template<> Sensor::Sensor() +{ + static const char *ns0[] = { "in_accel_scale", 0 }; + static const char **ns[] = { ns0, 0 }; + nodes = ns; + + name = "IIO Accelerometer Sensor"; + type = SENSOR_TYPE_ACCELEROMETER; + maxRange = RANGE_A; + resolution = RESOLUTION_A; + power = 0.23f; + minDelay = 10000; +} + +template<> int Sensor::readEvents(sensors_event_t *data, int cnt) +{ + static float scale = read_sysfs_float((*nodes)[0]); + int ret = SensorBase::readEvents(data, cnt); + // TODO: read orientation from the properties + for (int i = 0; i < ret; ++i) { + data[i].acceleration.x = -scale * read_sysfs_int("in_accel_x_raw"); + data[i].acceleration.y = scale * read_sysfs_int("in_accel_y_raw"); + data[i].acceleration.z = -scale * read_sysfs_int("in_accel_z_raw"); + data[i].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH; + } + return ret; +} + +template<> Sensor::Sensor() +{ + static const char *ns0[] = { "in_magn_scale", 0, 0 }; + static const char *ns1[] = { "in_magn_x_scale", "in_magn_y_scale", "in_magn_z_scale", 0 }; + static const char **ns[] = { ns0, ns1, 0 }; + nodes = ns; + + name = "IIO Magnetic Sensor"; + type = SENSOR_TYPE_MAGNETIC_FIELD; + maxRange = RANGE_M; + resolution = RESOLUTION_M; + power = 0.1f; + minDelay = 0; +} + +template<> int Sensor::readEvents(sensors_event_t *data, int cnt) +{ + static float scale_x = read_sysfs_float((*nodes)[0]); + static float scale_y = (*nodes)[1] ? read_sysfs_float((*nodes)[1]) : scale_x; + static float scale_z = (*nodes)[2] ? read_sysfs_float((*nodes)[2]) : scale_x; + int ret = SensorBase::readEvents(data, cnt); + for (int i = 0; i < ret; ++i) { + data[i].magnetic.x = scale_x * read_sysfs_int("in_magn_x_raw"); + data[i].magnetic.y = scale_y * read_sysfs_int("in_magn_y_raw"); + data[i].magnetic.z = scale_z * read_sysfs_int("in_magn_z_raw"); + data[i].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH; + } + return ret; +} + +template<> Sensor::Sensor() +{ + static const char *ns0[] = { "in_illuminance_scale", 0 }; + static const char *ns1[] = { "in_illuminance_calibscale", 0 }; + static const char **ns[] = { ns0, ns1, 0 }; + nodes = ns; + + name = "IIO Ambient Light Sensor"; + type = SENSOR_TYPE_LIGHT; + maxRange = 50000.0f; + resolution = 1.0f; + power = 0.75f; + minDelay = 0; +} + +template<> int Sensor::readEvents(sensors_event_t *data, int cnt) +{ + static float scale = read_sysfs_float((*nodes)[0]); + int ret = SensorBase::readEvents(data, cnt); + for (int i = 0; i < ret; ++i) { + data[i].light = scale * read_sysfs_int("in_illuminance_input"); + } + return ret; +} + +template<> Sensor::Sensor() +{ + static const char *ns0[] = { "in_anglvel_scale", 0 }; + static const char **ns[] = { ns0, 0 }; + nodes = ns; + + name = "IIO Gyro 3D Sensor"; + type = SENSOR_TYPE_GYROSCOPE; + maxRange = RANGE_G; + resolution = RESOLUTION_G; + power = 6.10f; + minDelay = 0; +} + +template<> int Sensor::readEvents(sensors_event_t *data, int cnt) +{ + static float scale = read_sysfs_float((*nodes)[0]); + int ret = SensorBase::readEvents(data, cnt); + // TODO: read orientation from the properties + for (int i = 0; i < ret; ++i) { + data[i].gyro.x = scale * read_sysfs_int("in_anglvel_x_raw"); + data[i].gyro.y = scale * read_sysfs_int("in_anglvel_y_raw"); + data[i].gyro.z = scale * read_sysfs_int("in_anglvel_z_raw"); + data[i].gyro.status = SENSOR_STATUS_ACCURACY_HIGH; + } + return ret; +} + +static SensorBase *(*probeSensors[])(const char *) = { + Sensor::probe, + Sensor::probe, + Sensor::probe, + Sensor::probe, +}; + +class SensorPollContext : sensors_poll_device_t { + public: + SensorPollContext(const struct hw_module_t *module, struct hw_device_t **device); + ~SensorPollContext(); + + bool isValid() const; + int getSensor(struct sensor_t const **list) const; + + private: + static int poll_close(struct hw_device_t *dev); + static int poll_activate(struct sensors_poll_device_t *dev, int handle, int enabled); + static int poll_setDelay(struct sensors_poll_device_t *dev, int handle, int64_t ns); + static int poll_poll(struct sensors_poll_device_t *dev, sensors_event_t *data, int count); + + int doPoll(sensors_event_t *data, int count); + + sensor_t sensors_list[MAX_SENSORS]; + SensorBase *sensors[MAX_SENSORS]; + int count; +}; + +static SensorPollContext *sctx = 0; + +SensorPollContext::SensorPollContext(const struct hw_module_t *module, struct hw_device_t **device) +{ + memset(this, 0, sizeof(*this)); + + common.tag = HARDWARE_DEVICE_TAG; + common.module = const_cast(module); + common.close = poll_close; + activate = poll_activate; + setDelay = poll_setDelay; + poll = poll_poll; + *device = &common; + + char path[PATH_MAX]; + strcpy(path, IIO_DIR); + int len = strlen(path); + if (DIR *dir = opendir(path)) { + while (struct dirent *de = readdir(dir)) { + if (!strncmp(de->d_name, "iio:device", 10)) { + strcpy(path + len, de->d_name); + for (int i = 0; probeSensors[i] && i < MAX_SENSORS; ++i) { + if (SensorBase *s = probeSensors[i](path)) { + sensors[i] = s; + sensors_list[count++] =*s; + } + } + } + } + } + ALOGD("%s: module=%p sensors: %d", __FUNCTION__, module, count); +} + +SensorPollContext::~SensorPollContext() +{ + for (int i = 0; i < MAX_SENSORS; ++i) { + delete sensors[i]; + } +} + +bool SensorPollContext::isValid() const +{ + return count > 0; +} + +int SensorPollContext::getSensor(struct sensor_t const **list) const +{ + *list = sensors_list; + return count; +} + +int SensorPollContext::poll_close(struct hw_device_t *dev) +{ + ALOGD("%s: dev=%p", __FUNCTION__, dev); + SensorPollContext *ctx = reinterpret_cast(dev); + delete ctx; + if (sctx == ctx) { + sctx = 0; + } else { + ALOGW("close a ctx(%p) rather than sctx(%p)", ctx, sctx); + } + return 0; +} + +int SensorPollContext::poll_activate(struct sensors_poll_device_t *dev, int handle, int enabled) +{ + ALOGD("%s: dev=%p handle=%d enabled=%d", __FUNCTION__, dev, handle, enabled); + SensorPollContext *ctx = reinterpret_cast(dev); + if (handle >= 0 && handle < MAX_SENSORS && ctx->sensors[handle]) + return ctx->sensors[handle]->activate(enabled); + else + return -EINVAL; +} + +int SensorPollContext::poll_setDelay(struct sensors_poll_device_t *dev, int handle, int64_t ns) +{ + ALOGD("%s: handle=%d delay-ns=%lld", __FUNCTION__, handle, ns); + SensorPollContext *ctx = reinterpret_cast(dev); + if (handle >= 0 && handle < MAX_SENSORS && ctx->sensors[handle]) + return ctx->sensors[handle]->setDelay(ns); + else + return -EINVAL; +} + +int SensorPollContext::poll_poll(struct sensors_poll_device_t *dev, sensors_event_t *data, int count) +{ + ALOGV("%s: dev=%p data=%p count=%d", __FUNCTION__, dev, data, count); + SensorPollContext *ctx = reinterpret_cast(dev); + return ctx->doPoll(data, count); +} + +int SensorPollContext::doPoll(sensors_event_t *data, int cnt) +{ + if (!isValid()) + return 0; + + int events = 0; + for (int i = 0; cnt > 0 && i < MAX_SENSORS; ++i) { + if (sensors[i] && *sensors[i]) { + int nb = sensors[i]->readEvents(data, cnt); + cnt -= nb; + data += nb; + events += nb; + } + } + + return events; +} + +static int open_iio_sensors(const struct hw_module_t *module, const char *id, struct hw_device_t **device) +{ + ALOGD("%s: id=%s", __FUNCTION__, id); + if (!sctx) { + sctx = new SensorPollContext(module, device); + } + return (sctx && sctx->isValid()) ? 0 : -EINVAL; +} + +static int sensors_get_sensors_list(struct sensors_module_t *, struct sensor_t const **list) +{ + ALOGD("enter %s", __FUNCTION__); + return sctx ? sctx->getSensor(list) : 0; +} + +static struct hw_module_methods_t sensors_methods = { + open: open_iio_sensors +}; + +struct sensors_module_t HAL_MODULE_INFO_SYM = { + common: { + tag: HARDWARE_MODULE_TAG, + version_major: 1, + version_minor: 0, + id: SENSORS_HARDWARE_MODULE_ID, + name: "IIO Sensors", + author: "Chih-Wei Huang", + methods: &sensors_methods, + dso: 0, + reserved: { } + }, + get_sensors_list: sensors_get_sensors_list +}; diff --git a/sensors.mk b/sensors.mk index 3c440ce..c3ae4bf 100644 --- a/sensors.mk +++ b/sensors.mk @@ -2,6 +2,7 @@ PRODUCT_PACKAGES := \ sensors.hdaps \ + sensors.iio \ sensors.kbd \ sensors.s103t \ sensors.w500 \ -- 2.11.0