piranha_sensors.c

   1 /*
   2  * Copyright (C) 2013 Paul Kocialkowski
   3  *
   4  * This program is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdlib.h>
  19 #include <unistd.h>
  20 #include <stdint.h>
  21 #include <fcntl.h>
  22 #include <errno.h>
  23 #include <poll.h>
  24
  25 #include <hardware/sensors.h>
  26 #include <hardware/hardware.h>
  27
  28 #define LOG_TAG "piranha_sensors"
  29 #include <utils/Log.h>
  30
  31 #include "piranha_sensors.h"
  32
  33 /*
  34  * Sensors list
  35  */
  36
  37 struct sensor_t piranha_sensors[] = {
  38         { "BMA254 Acceleration Sensor", "Bosch", 1, SENSOR_TYPE_ACCELEROMETER,
  39                 SENSOR_TYPE_ACCELEROMETER, 19.6f, 0.0383f, 0.13f, 10000, {}, },
  40         { "YAS530 Magnetic Sensor", "Yamaha", 1, SENSOR_TYPE_MAGNETIC_FIELD,
  41                 SENSOR_TYPE_MAGNETIC_FIELD, 800.0f, 0.3f, 4.0f, 10000, {}, },
  42         { "YAS Orientation Sensor", "Yamaha", 1, SENSOR_TYPE_ORIENTATION,
  43                 SENSOR_TYPE_ORIENTATION, 360.0f, 0.1f, 0.0f, 10000, {}, },
  44         { "BH1721 Light Sensor", "ROHM", 1, SENSOR_TYPE_LIGHT,
  45                 SENSOR_TYPE_LIGHT, 0.0f, 0.0f, 0.0f, 0, {}, },
  46 };
  47
  48 int piranha_sensors_count = sizeof(piranha_sensors) / sizeof(struct sensor_t);
  49
  50 struct piranha_sensors_handlers *piranha_sensors_handlers[] = {
  51         &bma250,
  52         &yas530c,
  53         &yas_orientation,
  54         &bh1721,
  55 };
  56
  57 int piranha_sensors_handlers_count = sizeof(piranha_sensors_handlers) /
  58         sizeof(struct piranha_sensors_handlers *);
  59
  60 /*
  61  * Piranha Sensors
  62  */
  63
  64 int piranha_sensors_activate(struct sensors_poll_device_t *dev, int handle, int enabled)
  65 {
  66         struct piranha_sensors_device *device;
  67         int i;
  68
  69         LOGD("%s(%p, %d, %d)", __func__, dev, handle, enabled);
  70
  71         if (dev == NULL)
  72                 return -EINVAL;
  73
  74         device = (struct piranha_sensors_device *) dev;
  75
  76         if (device->handlers == NULL || device->handlers_count <= 0)
  77                 return -EINVAL;
  78
  79         for (i=0 ; i < device->handlers_count ; i++) {
  80                 if (device->handlers[i] == NULL)
  81                         continue;
  82
  83                 if (device->handlers[i]->handle == handle) {
  84                         if (enabled && device->handlers[i]->activate != NULL)
  85                                 return device->handlers[i]->activate(device->handlers[i]);
  86                         else if (!enabled && device->handlers[i]->deactivate != NULL)
  87                                 return device->handlers[i]->deactivate(device->handlers[i]);
  88                 }
  89         }
  90
  91         return -1;
  92 }
  93
  94 int piranha_sensors_set_delay(struct sensors_poll_device_t *dev, int handle, int64_t ns)
  95 {
  96         struct piranha_sensors_device *device;
  97         int i;
  98
  99         LOGD("%s(%p, %d, %ld)", __func__, dev, handle, (long int) ns);
 100
 101         if (dev == NULL)
 102                 return -EINVAL;
 103
 104         device = (struct piranha_sensors_device *) dev;
 105
 106         if (device->handlers == NULL || device->handlers_count <= 0)
 107                 return -EINVAL;
 108
 109         for (i=0 ; i < device->handlers_count ; i++) {
 110                 if (device->handlers[i] == NULL)
 111                         continue;
 112
 113                 if (device->handlers[i]->handle == handle && device->handlers[i]->set_delay != NULL)
 114                         return device->handlers[i]->set_delay(device->handlers[i], ns);
 115         }
 116
 117         return 0;
 118 }
 119
 120 int piranha_sensors_poll(struct sensors_poll_device_t *dev,
 121         struct sensors_event_t* data, int count)
 122 {
 123         struct piranha_sensors_device *device;
 124         int index, i, j;
 125         int c, n;
 126         int rc;
 127
 128 //      LOGD("%s(%p, %p, %d)", __func__, dev, data, count);
 129
 130         if (dev == NULL)
 131                 return -EINVAL;
 132
 133         device = (struct piranha_sensors_device *) dev;
 134
 135         if (device->handlers == NULL || device->handlers_count <= 0 || device->poll_fds == NULL)
 136                 return -EINVAL;
 137
 138         memset(device->poll_fds, 0, device->handlers_count * sizeof(struct pollfd));
 139
 140         index = 0;
 141         for (i=0 ; i < device->handlers_count ; i++) {
 142                 if (device->handlers[i] == NULL)
 143                         continue;
 144
 145                 if (device->handlers[i]->activated && device->handlers[i]->poll_fd >= 0) {
 146                         device->poll_fds[index].fd = device->handlers[i]->poll_fd;
 147                         device->poll_fds[index].events = POLLIN;
 148                         index++;
 149                 }
 150         }
 151
 152         // No sensor is enabled
 153         if (index == 0) {
 154                 usleep(3000);
 155                 return 0;
 156         }
 157
 158         n = 0;
 159         for (c=0 ; c < count ; c++) {
 160                 rc = poll(device->poll_fds, index, 10);
 161                 if (rc <= 0)
 162                         continue;
 163
 164                 for (i=0 ; i < index ; i++) {
 165                         for (j=0 ; j < device->handlers_count ; j++) {
 166                                 if (device->handlers[j] == NULL)
 167                                         continue;
 168
 169                                 if (device->poll_fds[i].revents & POLLIN && device->poll_fds[i].fd == device->handlers[j]->poll_fd && device->handlers[j]->get_data != NULL) {
 170                                         rc = device->handlers[j]->get_data(device->handlers[j], &data[n]);
 171                                         if (rc < 0)
 172                                                 continue;
 173
 174                                         n++;
 175                                 }
 176                         }
 177
 178                         device->poll_fds[i].revents = 0;
 179                 }
 180         }
 181
 182         return n;
 183
 184 error:
 185         return 0;
 186 }
 187
 188 /*
 189  * Interface
 190  */
 191
 192 int piranha_sensors_close(hw_device_t *device)
 193 {
 194         struct piranha_sensors_device *piranha_sensors_device;
 195         int i;
 196
 197         LOGD("%s(%p)", __func__, device);
 198
 199         if (device == NULL)
 200                 return -EINVAL;
 201
 202         piranha_sensors_device = (struct piranha_sensors_device *) device;
 203
 204         if (piranha_sensors_device->poll_fds != NULL)
 205                 free(piranha_sensors_device->poll_fds);
 206
 207         for (i=0 ; i < piranha_sensors_device->handlers_count ; i++) {
 208                 if (piranha_sensors_device->handlers[i] == NULL || piranha_sensors_device->handlers[i]->deinit == NULL)
 209                         continue;
 210
 211                 piranha_sensors_device->handlers[i]->deinit(piranha_sensors_device->handlers[i]);
 212         }
 213
 214         free(device);
 215
 216         return 0;
 217 }
 218
 219 int piranha_sensors_open(const struct hw_module_t* module, const char *id,
 220         struct hw_device_t** device)
 221 {
 222         struct piranha_sensors_device *piranha_sensors_device;
 223         int i;
 224
 225         LOGD("%s(%p, %s, %p)", __func__, module, id, device);
 226
 227         if (module == NULL || device == NULL)
 228                 return -EINVAL;
 229
 230         piranha_sensors_device = (struct piranha_sensors_device *)
 231                 calloc(1, sizeof(struct piranha_sensors_device));
 232         piranha_sensors_device->device.common.tag = HARDWARE_DEVICE_TAG;
 233         piranha_sensors_device->device.common.version = 0;
 234         piranha_sensors_device->device.common.module = (struct hw_module_t *) module;
 235         piranha_sensors_device->device.common.close = piranha_sensors_close;
 236         piranha_sensors_device->device.activate = piranha_sensors_activate;
 237         piranha_sensors_device->device.setDelay = piranha_sensors_set_delay;
 238         piranha_sensors_device->device.poll = piranha_sensors_poll;
 239
 240         piranha_sensors_device->handlers = piranha_sensors_handlers;
 241         piranha_sensors_device->handlers_count = piranha_sensors_handlers_count;
 242         piranha_sensors_device->poll_fds = (struct pollfd *)
 243                 malloc(piranha_sensors_handlers_count * sizeof(struct pollfd));
 244
 245         for (i=0 ; i < piranha_sensors_handlers_count ; i++) {
 246                 if (piranha_sensors_handlers[i] == NULL || piranha_sensors_handlers[i]->init == NULL)
 247                         continue;
 248
 249                 piranha_sensors_handlers[i]->init(piranha_sensors_handlers[i], piranha_sensors_device);
 250         }
 251
 252         *device = &(piranha_sensors_device->device.common);
 253
 254         return 0;
 255 }
 256
 257 int piranha_sensors_get_sensors_list(struct sensors_module_t* module,
 258         const struct sensor_t **sensors_p)
 259 {
 260         LOGD("%s(%p, %p)", __func__, module, sensors_p);
 261
 262         if (sensors_p == NULL)
 263                 return -EINVAL;
 264
 265         *sensors_p = piranha_sensors;
 266         return piranha_sensors_count;
 267 }
 268
 269 struct hw_module_methods_t piranha_sensors_module_methods = {
 270         .open = piranha_sensors_open,
 271 };
 272
 273 struct sensors_module_t HAL_MODULE_INFO_SYM = {
 274         .common = {
 275                 .tag = HARDWARE_MODULE_TAG,
 276                 .version_major = 1,
 277                 .version_minor = 0,
 278                 .id = SENSORS_HARDWARE_MODULE_ID,
 279                 .name = "Piranha Sensors",
 280                 .author = "Paul Kocialkowski",
 281                 .methods = &piranha_sensors_module_methods,
 282         },
 283         .get_sensors_list = piranha_sensors_get_sensors_list,
 284 };