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