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 <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         char cm[PROPERTY_VALUE_MAX];
 243         float m[9];
 244         int v[3];
 245
 246         property_get("hal.sensors.iio.accel.matrix", cm, "-1,0,0,0,1,0,0,0,-1" );
 247         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]);
 248
 249         for (int i = 0; i < ret; ++i) {
 250                 v[0] = read_sysfs_int("in_accel_x_raw");
 251                 v[1] = read_sysfs_int("in_accel_y_raw");
 252                 v[2] = read_sysfs_int("in_accel_z_raw");
 253                 // create matrix * vector product
 254                 data[i].acceleration.x = scale * (m[0] * v[0] + m[1] * v[1] + m[2] * v[2]);
 255                 data[i].acceleration.y = scale * (m[3] * v[0] + m[4] * v[1] + m[5] * v[2]);
 256                 data[i].acceleration.z = scale * (m[6] * v[0] + m[7] * v[1] + m[8] * v[2]);
 257                 data[i].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
 258         }
 259         return ret;
 260 }
 261
 262 template<> Sensor<ID_MAGNETIC_FIELD>::Sensor()
 263 {
 264         static const char *ns0[] = { "in_magn_scale", 0, 0 };
 265         static const char *ns1[] = { "in_magn_x_scale", "in_magn_y_scale", "in_magn_z_scale", 0 };
 266         static const char **ns[] = { ns0, ns1, 0 };
 267         nodes = ns;
 268
 269         name = "IIO Magnetic Sensor";
 270         type = SENSOR_TYPE_MAGNETIC_FIELD;
 271         maxRange = RANGE_M;
 272         resolution = RESOLUTION_M;
 273         power = 0.1f;
 274         minDelay = 0;
 275 }
 276
 277 template<> int Sensor<ID_MAGNETIC_FIELD>::readEvents(sensors_event_t *data, int cnt)
 278 {
 279         static float scale_x = read_sysfs_float((*nodes)[0]);
 280         static float scale_y = (*nodes)[1] ? read_sysfs_float((*nodes)[1]) : scale_x;
 281         static float scale_z = (*nodes)[2] ? read_sysfs_float((*nodes)[2]) : scale_x;
 282         int ret = SensorBase::readEvents(data, cnt);
 283         char cm[PROPERTY_VALUE_MAX];
 284         float m[9];
 285         int v[3];
 286
 287         property_get("hal.sensors.iio.magn.matrix", cm, "1,0,0,0,1,0,0,0,1" );
 288         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]);
 289
 290         for (int i = 0; i < ret; ++i) {
 291                 v[0] = read_sysfs_int("in_magn_x_raw");
 292                 v[1] = read_sysfs_int("in_magn_y_raw");
 293                 v[2] = read_sysfs_int("in_magn_z_raw");
 294                 // create matrix * vector product
 295                 data[i].magnetic.x = scale_x * (m[0] * v[0] + m[1] * v[1] + m[2] * v[2]);
 296                 data[i].magnetic.y = scale_y * (m[3] * v[0] + m[4] * v[1] + m[5] * v[2]);
 297                 data[i].magnetic.z = scale_z * (m[6] * v[0] + m[7] * v[1] + m[8] * v[2]);
 298                 data[i].magnetic.status = SENSOR_STATUS_ACCURACY_HIGH;
 299         }
 300         return ret;
 301 }
 302
 303 template<> Sensor<ID_LIGHT>::Sensor()
 304 {
 305         static const char *ns0[] = { "in_illuminance_scale", 0 };
 306         static const char *ns1[] = { "in_illuminance_calibscale", 0 };
 307         static const char **ns[] = { ns0, ns1, 0 };
 308         nodes = ns;
 309
 310         name = "IIO Ambient Light Sensor";
 311         type = SENSOR_TYPE_LIGHT;
 312         maxRange = 50000.0f;
 313         resolution = 1.0f;
 314         power = 0.75f;
 315         minDelay = 0;
 316 }
 317
 318 template<> int Sensor<ID_LIGHT>::readEvents(sensors_event_t *data, int cnt)
 319 {
 320         static float scale = read_sysfs_float((*nodes)[0]);
 321         int ret = SensorBase::readEvents(data, cnt);
 322         for (int i = 0; i < ret; ++i) {
 323                 data[i].light = scale * read_sysfs_int("in_illuminance_input");
 324         }
 325         return ret;
 326 }
 327
 328 template<> Sensor<ID_GYROSCOPE>::Sensor()
 329 {
 330         static const char *ns0[] = { "in_anglvel_scale", 0 };
 331         static const char **ns[] = { ns0, 0 };
 332         nodes = ns;
 333
 334         name = "IIO Gyro 3D Sensor";
 335         type = SENSOR_TYPE_GYROSCOPE;
 336         maxRange = RANGE_G;
 337         resolution = RESOLUTION_G;
 338         power = 6.10f;
 339         minDelay = 0;
 340 }
 341
 342 template<> int Sensor<ID_GYROSCOPE>::readEvents(sensors_event_t *data, int cnt)
 343 {
 344         static float scale = read_sysfs_float((*nodes)[0]);
 345         int ret = SensorBase::readEvents(data, cnt);
 346         char cm[PROPERTY_VALUE_MAX];
 347         float m[9];
 348         int v[3];
 349
 350         property_get("hal.sensors.iio.anglvel.matrix", cm, "1,0,0,0,1,0,0,0,1" );
 351         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]);
 352
 353         for (int i = 0; i < ret; ++i) {
 354                 v[0] = read_sysfs_int("in_anglvel_x_raw");
 355                 v[1] = read_sysfs_int("in_anglvel_y_raw");
 356                 v[2] = read_sysfs_int("in_anglvel_z_raw");
 357                 // create matrix * vector product
 358                 data[i].gyro.x = scale * (m[0] * v[0] + m[1] * v[1] + m[2] * v[2]);
 359                 data[i].gyro.y = scale * (m[3] * v[0] + m[4] * v[1] + m[5] * v[2]);
 360                 data[i].gyro.z = scale * (m[6] * v[0] + m[7] * v[1] + m[8] * v[2]);
 361                 data[i].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
 362         }
 363         return ret;
 364 }
 365
 366 static SensorBase *(*probeSensors[])(const char *) = {
 367         Sensor<ID_ACCELERATION>::probe,
 368         Sensor<ID_MAGNETIC_FIELD>::probe,
 369         Sensor<ID_LIGHT>::probe,
 370         Sensor<ID_GYROSCOPE>::probe,
 371 };
 372
 373 class SensorPollContext : sensors_poll_device_t {
 374   public:
 375         SensorPollContext(const struct hw_module_t *module, struct hw_device_t **device);
 376         ~SensorPollContext();
 377
 378         bool isValid() const;
 379         int getSensor(struct sensor_t const **list) const;
 380
 381   private:
 382         static int poll_close(struct hw_device_t *dev);
 383         static int poll_activate(struct sensors_poll_device_t *dev, int handle, int enabled);
 384         static int poll_setDelay(struct sensors_poll_device_t *dev, int handle, int64_t ns);
 385         static int poll_poll(struct sensors_poll_device_t *dev, sensors_event_t *data, int count);
 386
 387         int doPoll(sensors_event_t *data, int count);
 388
 389         sensor_t sensors_list[MAX_SENSORS];
 390         SensorBase *sensors[MAX_SENSORS];
 391         int count;
 392 };
 393
 394 static SensorPollContext *sctx = 0;
 395
 396 SensorPollContext::SensorPollContext(const struct hw_module_t *module, struct hw_device_t **device)
 397 {
 398         memset(this, 0, sizeof(*this));
 399
 400         common.tag     = HARDWARE_DEVICE_TAG;
 401         common.module  = const_cast<struct hw_module_t *>(module);
 402         common.close   = poll_close;
 403         activate       = poll_activate;
 404         setDelay       = poll_setDelay;
 405         poll           = poll_poll;
 406         *device        = &common;
 407
 408         char path[PATH_MAX];
 409         strcpy(path, IIO_DIR);
 410         int len = strlen(path);
 411         if (DIR *dir = opendir(path)) {
 412                 while (struct dirent *de = readdir(dir)) {
 413                         if (!strncmp(de->d_name, "iio:device", 10)) {
 414                                 strcpy(path + len, de->d_name);
 415                                 for (size_t i = 0; i < (sizeof(probeSensors) / sizeof(*probeSensors)); ++i) {
 416                                         if (SensorBase *s = probeSensors[i](path)) {
 417                                                 sensors[i] = s;
 418                                                 sensors_list[count++] = *s;
 419                                                 ALOGD("found %s", __FUNCTION__, s->name);
 420                                         }
 421                                 }
 422                         }
 423                 }
 424         }
 425         ALOGD("%s: module=%p sensors: %d", __FUNCTION__, module, count);
 426 }
 427
 428 SensorPollContext::~SensorPollContext()
 429 {
 430         for (int i = 0; i < MAX_SENSORS; ++i) {
 431                 delete sensors[i];
 432         }
 433 }
 434
 435 bool SensorPollContext::isValid() const
 436 {
 437         return count > 0;
 438 }
 439
 440 int SensorPollContext::getSensor(struct sensor_t const **list) const
 441 {
 442         *list = sensors_list;
 443         return count;
 444 }
 445
 446 int SensorPollContext::poll_close(struct hw_device_t *dev)
 447 {
 448         ALOGD("%s: dev=%p", __FUNCTION__, dev);
 449         SensorPollContext *ctx = reinterpret_cast<SensorPollContext *>(dev);
 450         delete ctx;
 451         if (sctx == ctx) {
 452                 sctx = 0;
 453         } else {
 454                 ALOGW("close a ctx(%p) rather than sctx(%p)", ctx, sctx);
 455         }
 456         return 0;
 457 }
 458
 459 int SensorPollContext::poll_activate(struct sensors_poll_device_t *dev, int handle, int enabled)
 460 {
 461         ALOGD("%s: dev=%p handle=%d enabled=%d", __FUNCTION__, dev, handle, enabled);
 462         SensorPollContext *ctx = reinterpret_cast<SensorPollContext *>(dev);
 463         if (handle >= 0 && handle < MAX_SENSORS && ctx->sensors[handle])
 464                 return ctx->sensors[handle]->activate(enabled);
 465         else
 466                 return -EINVAL;
 467 }
 468
 469 int SensorPollContext::poll_setDelay(struct sensors_poll_device_t *dev, int handle, int64_t ns)
 470 {
 471         ALOGD("%s: handle=%d delay-ns=%lld", __FUNCTION__, handle, ns);
 472         SensorPollContext *ctx = reinterpret_cast<SensorPollContext *>(dev);
 473         if (handle >= 0 && handle < MAX_SENSORS && ctx->sensors[handle])
 474                 return ctx->sensors[handle]->setDelay(ns);
 475         else
 476                 return -EINVAL;
 477 }
 478
 479 int SensorPollContext::poll_poll(struct sensors_poll_device_t *dev, sensors_event_t *data, int count)
 480 {
 481         ALOGV("%s: dev=%p data=%p count=%d", __FUNCTION__, dev, data, count);
 482         SensorPollContext *ctx = reinterpret_cast<SensorPollContext *>(dev);
 483         return ctx->doPoll(data, count);
 484 }
 485
 486 int SensorPollContext::doPoll(sensors_event_t *data, int cnt)
 487 {
 488         if (!isValid())
 489                 return 0;
 490
 491         int events = 0;
 492         for (int i = 0; cnt > 0 && i < MAX_SENSORS; ++i) {
 493                 if (sensors[i] && *sensors[i]) {
 494                         int nb = sensors[i]->readEvents(data, cnt);
 495                         cnt -= nb;
 496                         data += nb;
 497                         events += nb;
 498                 }
 499         }
 500
 501         return events;
 502 }
 503
 504 static int open_iio_sensors(const struct hw_module_t *module, const char *id, struct hw_device_t **device)
 505 {
 506         ALOGD("%s: id=%s", __FUNCTION__, id);
 507         if (!sctx) {
 508                 sctx = new SensorPollContext(module, device);
 509         }
 510         return (sctx && sctx->isValid()) ? 0 : -EINVAL;
 511 }
 512
 513 static int sensors_get_sensors_list(struct sensors_module_t *, struct sensor_t const **list)
 514 {
 515         ALOGD("enter %s", __FUNCTION__);
 516         return sctx ? sctx->getSensor(list) : 0;
 517 }
 518
 519 static struct hw_module_methods_t sensors_methods = {
 520         open: open_iio_sensors
 521 };
 522
 523 struct sensors_module_t HAL_MODULE_INFO_SYM = {
 524         common: {
 525                 tag: HARDWARE_MODULE_TAG,
 526                 version_major: 1,
 527                 version_minor: 0,
 528                 id: SENSORS_HARDWARE_MODULE_ID,
 529                 name: "IIO Sensors",
 530                 author: "Chih-Wei Huang",
 531                 methods: &sensors_methods,
 532                 dso: 0,
 533                 reserved: { }
 534         },
 535         get_sensors_list: sensors_get_sensors_list,
 536         set_operation_mode: 0
 537 };