iio-sensors.cpp

   1 /**
   2  *
   3  * IIO style sensor
   4  *
   5  * Copyright (C) 2015 The Android-x86 Open Source Project
   6  *
   7  * by Chih-Wei Huang <cwhuang@linux.org.tw>
   8  *
   9  * Licensed under GPLv2 or later
  10  *
  11  **/
  12
  13 #define LOG_TAG "iio-sensors"
  14
  15 //#include <cmath>
  16 #include <new>
  17 #include <cerrno>
  18 #include <cstdlib>
  19 #include <cstring>
  20 #include <fcntl.h>
  21 #include <dirent.h>
  22 #include <cutils/log.h>
  23 #include <hardware/sensors.h>
  24 #include <cutils/properties.h>
  25
  26 static const char *IIO_DIR        = "/sys/bus/iio/devices/";
  27
  28 enum {
  29         ID_ACCELERATION           = SENSORS_HANDLE_BASE,
  30         ID_MAGNETIC_FIELD,
  31         ID_ORIENTATION,
  32         ID_LIGHT,
  33         ID_PROXIMITY,
  34         ID_GYROSCOPE,
  35         ID_PRESSURE,
  36         ID_TEMPERATURE,
  37         ID_ROT_VECTOR,
  38         ID_SYNCOMPASS,
  39         MAX_SENSORS
  40 };
  41
  42
  43 // 720 LSG = 1G
  44 #define LSG                         (1024.0f)
  45 #define NUMOFACCDATA                (8.0f)
  46
  47 // conversion of acceleration data to SI units (m/s^2)
  48 #define RANGE_A                     (2*GRAVITY_EARTH)
  49 #define RESOLUTION_A                (RANGE_A/(256*NUMOFACCDATA))
  50
  51 // conversion of magnetic data to uT units
  52 #define RANGE_M                     (2048.0f)
  53 #define RESOLUTION_M                (0.01)
  54
  55 /* conversion of orientation data to degree units */
  56 #define CONVERT_O                   (1.0f/64.0f)
  57 #define CONVERT_O_A                 (CONVERT_O)
  58 #define CONVERT_O_P                 (CONVERT_O)
  59 #define CONVERT_O_R                 (-CONVERT_O)
  60
  61 // conversion of gyro data to SI units (radian/sec)
  62 #define RANGE_G                     (2000.0f*(float)M_PI/180.0f)
  63 #define RESOLUTION_G                (RANGE_G/(2000*NUMOFACCDATA))
  64
  65 // conversion of pressure and temperature data
  66 #define CONVERT_PRESSURE            (1.0f/100.0f)
  67 #define CONVERT_TEMPERATURE         (1.0f/100.0f)
  68
  69 #define SENSOR_STATE_MASK           (0x7FFF)
  70
  71 // proximity threshold
  72 #define PROXIMITY_THRESHOLD_GP2A    (5.0f)
  73
  74 // used in timespec_to_ns calculations
  75 const long NSEC_PER_SEC           = 1000000000L;
  76
  77 #define BIT(x) (1 << (x))
  78
  79 struct SensorEvent : public sensors_event_t {
  80         SensorEvent(int32_t id, int32_t t);
  81 };
  82
  83 SensorEvent::SensorEvent(int32_t id, int32_t t)
  84 {
  85         version = sizeof(sensors_event_t);
  86         sensor = id;
  87         type = t;
  88
  89         struct timespec ts;
  90         clock_gettime(CLOCK_MONOTONIC, &ts);
  91         timestamp = int64_t(ts.tv_sec) * NSEC_PER_SEC + ts.tv_nsec;
  92 }
  93
  94 class SensorBase : public sensor_t {
  95   public:
  96         SensorBase();
  97         virtual ~SensorBase();
  98
  99         operator bool() const { return enabled; }
 100         int setDelay(int64_t ns);
 101         bool scan(const char *d);
 102         virtual int activate(bool);
 103         virtual int readEvents(sensors_event_t *data, int);
 104
 105   protected:
 106         int read_sysfs_str(const char *file, char *buf);
 107         int read_sysfs_int(const char *file);
 108         float read_sysfs_float(const char *file);
 109
 110         bool enabled;
 111         char *path;
 112         const char ***nodes;
 113         struct timespec delay;
 114 };
 115
 116 SensorBase::SensorBase()
 117 {
 118         memset(this, 0, sizeof(SensorBase));
 119
 120         vendor = "Android-x86 Open Source Project";
 121         version = 1;
 122
 123         delay.tv_nsec = 200000000L;
 124 }
 125
 126 SensorBase::~SensorBase()
 127 {
 128         free(path);
 129 }
 130
 131 int SensorBase::setDelay(int64_t ns)
 132 {
 133         delay.tv_sec = ns / NSEC_PER_SEC;
 134         delay.tv_nsec = ns % NSEC_PER_SEC;
 135         return 0;
 136 }
 137
 138 bool SensorBase::scan(const char *p)
 139 {
 140         int i;
 141         char node[PATH_MAX];
 142         while (const char **ns = *nodes) {
 143                 for (i = 0; ns[i]; ++i) {
 144                         snprintf(node, PATH_MAX, "%s/%s", p, ns[i]);
 145                         if (access(node, F_OK))
 146                                 break;
 147                 }
 148                 if (!ns[i])
 149                         break;
 150                 nodes++;
 151         }
 152         if (*nodes) {
 153                 path = strdup(p);
 154                 node[0] = '\0';
 155                 for (i = 0; (*nodes)[i]; ++i)
 156                         strncat(strncat(node, (*nodes)[i], 1024), " ", 1024);
 157                 ALOGD("found node %s: %s", path, node);
 158         }
 159         return (path != 0);
 160 }
 161
 162 int SensorBase::activate(bool e)
 163 {
 164         enabled = e;
 165         return 0;
 166 }
 167
 168 int SensorBase::readEvents(sensors_event_t *data, int)
 169 {
 170         nanosleep(&delay, 0);
 171         SensorEvent *e = new (data) SensorEvent(handle, type);
 172         return 1;
 173 }
 174
 175 int SensorBase::read_sysfs_str(const char *file, char *buf)
 176 {
 177         int res = 0;
 178         char filename[PATH_MAX];
 179         snprintf(filename, PATH_MAX, "%s/%s", path, file);
 180         int fd = open(filename, O_RDONLY);
 181         if (fd >= 0) {
 182                 ssize_t sz = read(fd, buf, 4096);
 183                 if (sz < 0) {
 184                         ALOGE("failed to read from %s: %s", filename, strerror(errno));
 185                         res = -errno;
 186                 }
 187                 close(fd);
 188         }
 189         return res;
 190 }
 191
 192 int SensorBase::read_sysfs_int(const char *file)
 193 {
 194         char buf[4096];
 195         return read_sysfs_str(file, buf) ? 0 : atoi(buf);
 196 }
 197
 198 float SensorBase::read_sysfs_float(const char *file)
 199 {
 200         char buf[4096];
 201         return read_sysfs_str(file, buf) ? 0 : atof(buf);
 202 }
 203
 204 template <int H> class Sensor : SensorBase {
 205   public:
 206         Sensor();
 207         virtual int readEvents(sensors_event_t *data, int);
 208
 209         static SensorBase *probe(const char *d);
 210 };
 211
 212 template<int H>
 213 SensorBase *Sensor<H>::probe(const char *p)
 214 {
 215         Sensor<H> *s = new Sensor<H>;
 216         s->handle = H;
 217         if (!s->scan(p)) {
 218                 delete s;
 219                 s = 0;
 220         }
 221         return s;
 222 }
 223
 224 template<> Sensor<ID_ACCELERATION>::Sensor()
 225 {
 226         static const char *ns0[] = { "in_accel_scale", 0 };
 227         static const char **ns[] = { ns0, 0 };
 228         nodes = ns;
 229
 230         name = "IIO Accelerometer Sensor";
 231         type = SENSOR_TYPE_ACCELEROMETER;
 232         maxRange = RANGE_A;
 233         resolution = RESOLUTION_A;
 234         power = 0.23f;
 235         minDelay = 10000;
 236 }
 237
 238 template<> int Sensor<ID_ACCELERATION>::readEvents(sensors_event_t *data, int cnt)
 239 {
 240         static float scale = read_sysfs_float((*nodes)[0]);
 241         int ret = SensorBase::readEvents(data, cnt);
 242         // TODO: read orientation from the properties
 243         for (int i = 0; i < ret; ++i) {
 244                 data[i].acceleration.x = -scale * read_sysfs_int("in_accel_x_raw");
 245                 data[i].acceleration.y =  scale * read_sysfs_int("in_accel_y_raw");
 246                 data[i].acceleration.z = -scale * read_sysfs_int("in_accel_z_raw");
 247                 data[i].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
 248         }
 249         return ret;
 250 }
 251
 252 template<> Sensor<ID_MAGNETIC_FIELD>::Sensor()
 253 {
 254         static const char *ns0[] = { "in_magn_scale", 0, 0 };
 255         static const char *ns1[] = { "in_magn_x_scale", "in_magn_y_scale", "in_magn_z_scale", 0 };
 256         static const char **ns[] = { ns0, ns1, 0 };
 257         nodes = ns;
 258
 259         name = "IIO Magnetic Sensor";
 260         type = SENSOR_TYPE_MAGNETIC_FIELD;
 261         maxRange = RANGE_M;
 262         resolution = RESOLUTION_M;
 263         power = 0.1f;
 264         minDelay = 0;
 265 }
 266
 267 template<> int Sensor<ID_MAGNETIC_FIELD>::readEvents(sensors_event_t *data, int cnt)
 268 {
 269         static float scale_x = read_sysfs_float((*nodes)[0]);
 270         static float scale_y = (*nodes)[1] ? read_sysfs_float((*nodes)[1]) : scale_x;
 271         static float scale_z = (*nodes)[2] ? read_sysfs_float((*nodes)[2]) : scale_x;
 272         int ret = SensorBase::readEvents(data, cnt);
 273         for (int i = 0; i < ret; ++i) {
 274                 data[i].magnetic.x = scale_x * read_sysfs_int("in_magn_x_raw");
 275                 data[i].magnetic.y = scale_y * read_sysfs_int("in_magn_y_raw");
 276                 data[i].magnetic.z = scale_z * read_sysfs_int("in_magn_z_raw");
 277                 data[i].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
 278         }
 279         return ret;
 280 }
 281
 282 template<> Sensor<ID_LIGHT>::Sensor()
 283 {
 284         static const char *ns0[] = { "in_illuminance_scale", 0 };
 285         static const char *ns1[] = { "in_illuminance_calibscale", 0 };
 286         static const char **ns[] = { ns0, ns1, 0 };
 287         nodes = ns;
 288
 289         name = "IIO Ambient Light Sensor";
 290         type = SENSOR_TYPE_LIGHT;
 291         maxRange = 50000.0f;
 292         resolution = 1.0f;
 293         power = 0.75f;
 294         minDelay = 0;
 295 }
 296
 297 template<> int Sensor<ID_LIGHT>::readEvents(sensors_event_t *data, int cnt)
 298 {
 299         static float scale = read_sysfs_float((*nodes)[0]);
 300         int ret = SensorBase::readEvents(data, cnt);
 301         for (int i = 0; i < ret; ++i) {
 302                 data[i].light = scale * read_sysfs_int("in_illuminance_input");
 303         }
 304         return ret;
 305 }
 306
 307 template<> Sensor<ID_GYROSCOPE>::Sensor()
 308 {
 309         static const char *ns0[] = { "in_anglvel_scale", 0 };
 310         static const char **ns[] = { ns0, 0 };
 311         nodes = ns;
 312
 313         name = "IIO Gyro 3D Sensor";
 314         type = SENSOR_TYPE_GYROSCOPE;
 315         maxRange = RANGE_G;
 316         resolution = RESOLUTION_G;
 317         power = 6.10f;
 318         minDelay = 0;
 319 }
 320
 321 template<> int Sensor<ID_GYROSCOPE>::readEvents(sensors_event_t *data, int cnt)
 322 {
 323         static float scale = read_sysfs_float((*nodes)[0]);
 324         int ret = SensorBase::readEvents(data, cnt);
 325         // TODO: read orientation from the properties
 326         for (int i = 0; i < ret; ++i) {
 327                 data[i].gyro.x = scale * read_sysfs_int("in_anglvel_x_raw");
 328                 data[i].gyro.y = scale * read_sysfs_int("in_anglvel_y_raw");
 329                 data[i].gyro.z = scale * read_sysfs_int("in_anglvel_z_raw");
 330                 data[i].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 331         }
 332         return ret;
 333 }
 334
 335 static SensorBase *(*probeSensors[])(const char *) = {
 336         Sensor<ID_ACCELERATION>::probe,
 337         Sensor<ID_MAGNETIC_FIELD>::probe,
 338         Sensor<ID_LIGHT>::probe,
 339         Sensor<ID_GYROSCOPE>::probe,
 340 };
 341
 342 class SensorPollContext : sensors_poll_device_t {
 343   public:
 344         SensorPollContext(const struct hw_module_t *module, struct hw_device_t **device);
 345         ~SensorPollContext();
 346
 347         bool isValid() const;
 348         int getSensor(struct sensor_t const **list) const;
 349
 350   private:
 351         static int poll_close(struct hw_device_t *dev);
 352         static int poll_activate(struct sensors_poll_device_t *dev, int handle, int enabled);
 353         static int poll_setDelay(struct sensors_poll_device_t *dev, int handle, int64_t ns);
 354         static int poll_poll(struct sensors_poll_device_t *dev, sensors_event_t *data, int count);
 355
 356         int doPoll(sensors_event_t *data, int count);
 357
 358         sensor_t sensors_list[MAX_SENSORS];
 359         SensorBase *sensors[MAX_SENSORS];
 360         int count;
 361 };
 362
 363 static SensorPollContext *sctx = 0;
 364
 365 SensorPollContext::SensorPollContext(const struct hw_module_t *module, struct hw_device_t **device)
 366 {
 367         memset(this, 0, sizeof(*this));
 368
 369         common.tag     = HARDWARE_DEVICE_TAG;
 370         common.module  = const_cast<struct hw_module_t *>(module);
 371         common.close   = poll_close;
 372         activate       = poll_activate;
 373         setDelay       = poll_setDelay;
 374         poll           = poll_poll;
 375         *device        = &common;
 376
 377         char path[PATH_MAX];
 378         strcpy(path, IIO_DIR);
 379         int len = strlen(path);
 380         if (DIR *dir = opendir(path)) {
 381                 while (struct dirent *de = readdir(dir)) {
 382                         if (!strncmp(de->d_name, "iio:device", 10)) {
 383                                 strcpy(path + len, de->d_name);
 384                                 for (int i = 0; probeSensors[i] && i < MAX_SENSORS; ++i) {
 385                                         if (SensorBase *s = probeSensors[i](path)) {
 386                                                 sensors[i] = s;
 387                                                 sensors_list[count++] =*s;
 388                                         }
 389                                 }
 390                         }
 391                 }
 392         }
 393         ALOGD("%s: module=%p sensors: %d", __FUNCTION__, module, count);
 394 }
 395
 396 SensorPollContext::~SensorPollContext()
 397 {
 398         for (int i = 0; i < MAX_SENSORS; ++i) {
 399                 delete sensors[i];
 400         }
 401 }
 402
 403 bool SensorPollContext::isValid() const
 404 {
 405         return count > 0;
 406 }
 407
 408 int SensorPollContext::getSensor(struct sensor_t const **list) const
 409 {
 410         *list = sensors_list;
 411         return count;
 412 }
 413
 414 int SensorPollContext::poll_close(struct hw_device_t *dev)
 415 {
 416         ALOGD("%s: dev=%p", __FUNCTION__, dev);
 417         SensorPollContext *ctx = reinterpret_cast<SensorPollContext *>(dev);
 418         delete ctx;
 419         if (sctx == ctx) {
 420                 sctx = 0;
 421         } else {
 422                 ALOGW("close a ctx(%p) rather than sctx(%p)", ctx, sctx);
 423         }
 424         return 0;
 425 }
 426
 427 int SensorPollContext::poll_activate(struct sensors_poll_device_t *dev, int handle, int enabled)
 428 {
 429         ALOGD("%s: dev=%p handle=%d enabled=%d", __FUNCTION__, dev, handle, enabled);
 430         SensorPollContext *ctx = reinterpret_cast<SensorPollContext *>(dev);
 431         if (handle >= 0 && handle < MAX_SENSORS && ctx->sensors[handle])
 432                 return ctx->sensors[handle]->activate(enabled);
 433         else
 434                 return -EINVAL;
 435 }
 436
 437 int SensorPollContext::poll_setDelay(struct sensors_poll_device_t *dev, int handle, int64_t ns)
 438 {
 439         ALOGD("%s: handle=%d delay-ns=%lld", __FUNCTION__, handle, ns);
 440         SensorPollContext *ctx = reinterpret_cast<SensorPollContext *>(dev);
 441         if (handle >= 0 && handle < MAX_SENSORS && ctx->sensors[handle])
 442                 return ctx->sensors[handle]->setDelay(ns);
 443         else
 444                 return -EINVAL;
 445 }
 446
 447 int SensorPollContext::poll_poll(struct sensors_poll_device_t *dev, sensors_event_t *data, int count)
 448 {
 449         ALOGV("%s: dev=%p data=%p count=%d", __FUNCTION__, dev, data, count);
 450         SensorPollContext *ctx = reinterpret_cast<SensorPollContext *>(dev);
 451         return ctx->doPoll(data, count);
 452 }
 453
 454 int SensorPollContext::doPoll(sensors_event_t *data, int cnt)
 455 {
 456         if (!isValid())
 457                 return 0;
 458
 459         int events = 0;
 460         for (int i = 0; cnt > 0 && i < MAX_SENSORS; ++i) {
 461                 if (sensors[i] && *sensors[i]) {
 462                         int nb = sensors[i]->readEvents(data, cnt);
 463                         cnt -= nb;
 464                         data += nb;
 465                         events += nb;
 466                 }
 467         }
 468
 469         return events;
 470 }
 471
 472 static int open_iio_sensors(const struct hw_module_t *module, const char *id, struct hw_device_t **device)
 473 {
 474         ALOGD("%s: id=%s", __FUNCTION__, id);
 475         if (!sctx) {
 476                 sctx = new SensorPollContext(module, device);
 477         }
 478         return (sctx && sctx->isValid()) ? 0 : -EINVAL;
 479 }
 480
 481 static int sensors_get_sensors_list(struct sensors_module_t *, struct sensor_t const **list)
 482 {
 483         ALOGD("enter %s", __FUNCTION__);
 484         return sctx ? sctx->getSensor(list) : 0;
 485 }
 486
 487 static struct hw_module_methods_t sensors_methods = {
 488         open: open_iio_sensors
 489 };
 490
 491 struct sensors_module_t HAL_MODULE_INFO_SYM = {
 492         common: {
 493                 tag: HARDWARE_MODULE_TAG,
 494                 version_major: 1,
 495                 version_minor: 0,
 496                 id: SENSORS_HARDWARE_MODULE_ID,
 497                 name: "IIO Sensors",
 498                 author: "Chih-Wei Huang",
 499                 methods: &sensors_methods,
 500                 dso: 0,
 501                 reserved: { }
 502         },
 503         get_sensors_list: sensors_get_sensors_list
 504 };