1/* 2* Copyright (C) 2014 Invensense, Inc. 3* 4* Licensed under the Apache License, Version 2.0 (the "License"); 5* you may not use this file except in compliance with the License. 6* You may obtain a copy of the License at 7* 8* http://www.apache.org/licenses/LICENSE-2.0 9* 10* Unless required by applicable law or agreed to in writing, software 11* distributed under the License is distributed on an "AS IS" BASIS, 12* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13* See the License for the specific language governing permissions and 14* limitations under the License. 15*/ 16 17#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__) 18 19#include <hardware_legacy/power.h> 20#include <hardware/sensors.h> 21#include <fcntl.h> 22#include <errno.h> 23#include <dirent.h> 24#include <math.h> 25#include <poll.h> 26#include <pthread.h> 27#include <stdlib.h> 28 29#include <sys/queue.h> 30 31#include <linux/input.h> 32 33#include <utils/Atomic.h> 34#include <utils/Log.h> 35 36#include "sensors.h" 37#include "MPLSensor.h" 38 39/* 40 * Vendor-defined Accel Load Calibration File Method 41 * @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame 42 * @return '0' for a successful load, '1' otherwise 43 * example: int AccelLoadConfig(long* offset); 44 * End of Vendor-defined Accel Load Cal Method 45 */ 46 47/*****************************************************************************/ 48/* The SENSORS Module */ 49 50#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 51#define LOCAL_SENSORS (NumSensors + 1) 52#else 53#define LOCAL_SENSORS (NumSensors) 54#endif 55 56struct handle_entry { 57 SIMPLEQ_ENTRY(handle_entry) entries; 58 int handle; 59}; 60 61static SIMPLEQ_HEAD(simplehead, handle_entry) pending_flush_items_head; 62struct simplehead *headp; 63static pthread_mutex_t flush_handles_mutex = PTHREAD_MUTEX_INITIALIZER; 64 65static const char *smdWakelockStr = "significant motion"; 66 67static struct sensor_t sSensorList[LOCAL_SENSORS]; 68static int sensors = (sizeof(sSensorList) / sizeof(sensor_t)); 69 70static int open_sensors(const struct hw_module_t* module, const char* id, 71 struct hw_device_t** device); 72 73static int sensors__get_sensors_list(struct sensors_module_t* module, 74 struct sensor_t const** list) 75{ 76 *list = sSensorList; 77 return sensors; 78} 79 80static struct hw_module_methods_t sensors_module_methods = { 81 open: open_sensors 82}; 83 84struct sensors_module_t HAL_MODULE_INFO_SYM = { 85 common: { 86 tag: HARDWARE_MODULE_TAG, 87 version_major: 1, 88 version_minor: 0, 89 id: SENSORS_HARDWARE_MODULE_ID, 90 name: "Invensense module", 91 author: "Invensense Inc.", 92 methods: &sensors_module_methods, 93 dso: NULL, 94 reserved: {0} 95 }, 96 get_sensors_list: sensors__get_sensors_list, 97}; 98 99struct sensors_poll_context_t { 100 sensors_poll_device_1_t device; // must be first 101 102 sensors_poll_context_t(); 103 ~sensors_poll_context_t(); 104 int activate(int handle, int enabled); 105 int setDelay(int handle, int64_t ns); 106 int pollEvents(sensors_event_t* data, int count); 107 int query(int what, int *value); 108 int batch(int handle, int flags, int64_t period_ns, int64_t timeout); 109#if defined ANDROID_KITKAT 110 int flush(int handle); 111#endif 112 113private: 114 enum { 115 mpl = 0, 116 compass, 117 dmpOrient, 118 dmpSign, 119 dmpPed, 120 numSensorDrivers, 121 numFds, 122 }; 123 124 struct pollfd mPollFds[numFds]; 125 SensorBase *mSensor; 126 CompassSensor *mCompassSensor; 127 128 /* Significant Motion wakelock support */ 129 bool mSMDWakelockHeld; 130 131}; 132 133/******************************************************************************/ 134 135sensors_poll_context_t::sensors_poll_context_t() { 136 VFUNC_LOG; 137 138 /* TODO: Handle external pressure sensor */ 139 mCompassSensor = new CompassSensor(); 140 MPLSensor *mplSensor = new MPLSensor(mCompassSensor); 141 142 /* No significant motion events pending yet */ 143 mSMDWakelockHeld = false; 144 145 /* For Vendor-defined Accel Calibration File Load 146 * Use the Following Constructor and Pass Your Load Cal File Function 147 * 148 * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig); 149 */ 150 151 // Initialize pending flush queue 152 SIMPLEQ_INIT(&pending_flush_items_head); 153 154 // populate the sensor list 155 sensors = 156 mplSensor->populateSensorList(sSensorList, sizeof(sSensorList)); 157 158 mSensor = mplSensor; 159 mPollFds[mpl].fd = mSensor->getFd(); 160 mPollFds[mpl].events = POLLIN; 161 mPollFds[mpl].revents = 0; 162 163 mPollFds[compass].fd = mCompassSensor->getFd(); 164 mPollFds[compass].events = POLLIN; 165 mPollFds[compass].revents = 0; 166 167 mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd(); 168 mPollFds[dmpOrient].events = POLLPRI; 169 mPollFds[dmpOrient].revents = 0; 170 171 mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd(); 172 mPollFds[dmpSign].events = POLLPRI; 173 mPollFds[dmpSign].revents = 0; 174 175 mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd(); 176 mPollFds[dmpPed].events = POLLPRI; 177 mPollFds[dmpPed].revents = 0; 178} 179 180sensors_poll_context_t::~sensors_poll_context_t() { 181 FUNC_LOG; 182 delete mSensor; 183 delete mCompassSensor; 184 for (int i = 0; i < numSensorDrivers; i++) { 185 close(mPollFds[i].fd); 186 } 187} 188 189int sensors_poll_context_t::activate(int handle, int enabled) { 190 FUNC_LOG; 191 192 int err; 193 err = mSensor->enable(handle, enabled); 194 return err; 195} 196 197int sensors_poll_context_t::setDelay(int handle, int64_t ns) 198{ 199 FUNC_LOG; 200 return mSensor->setDelay(handle, ns); 201} 202 203int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count) 204{ 205 VHANDLER_LOG; 206 207 int nbEvents = 0; 208 int nb, polltime = -1; 209 210 if (mSMDWakelockHeld) { 211 mSMDWakelockHeld = false; 212 release_wake_lock(smdWakelockStr); 213 } 214 215 struct handle_entry *handle_element; 216 pthread_mutex_lock(&flush_handles_mutex); 217 if (!SIMPLEQ_EMPTY(&pending_flush_items_head)) { 218 sensors_event_t flushCompleteEvent; 219 flushCompleteEvent.type = SENSOR_TYPE_META_DATA; 220 flushCompleteEvent.sensor = 0; 221 handle_element = SIMPLEQ_FIRST(&pending_flush_items_head); 222 flushCompleteEvent.meta_data.sensor = handle_element->handle; 223 SIMPLEQ_REMOVE_HEAD(&pending_flush_items_head, entries); 224 free(handle_element); 225 memcpy(data, (void *) &flushCompleteEvent, sizeof(flushCompleteEvent)); 226 LOGI_IF(1, "pollEvents() Returning fake flush event completion for handle %d", 227 flushCompleteEvent.meta_data.sensor); 228 pthread_mutex_unlock(&flush_handles_mutex); 229 return 1; 230 } 231 pthread_mutex_unlock(&flush_handles_mutex); 232 233 polltime = ((MPLSensor*) mSensor)->getStepCountPollTime(); 234 235 // look for new events 236 nb = poll(mPollFds, numSensorDrivers, polltime); 237 LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime); 238 if (nb > 0) { 239 for (int i = 0; count && i < numSensorDrivers; i++) { 240 if (mPollFds[i].revents & (POLLIN | POLLPRI)) { 241 nb = 0; 242 if (i == mpl) { 243 ((MPLSensor*) mSensor)->buildMpuEvent(); 244 mPollFds[i].revents = 0; 245 } else if (i == compass) { 246 ((MPLSensor*) mSensor)->buildCompassEvent(); 247 mPollFds[i].revents = 0; 248 } else if (i == dmpOrient) { 249 nb = ((MPLSensor*)mSensor)-> 250 readDmpOrientEvents(data, count); 251 mPollFds[dmpOrient].revents= 0; 252 if (isDmpScreenAutoRotationEnabled() && nb > 0) { 253 count -= nb; 254 nbEvents += nb; 255 data += nb; 256 } 257 } else if (i == dmpSign) { 258 nb = ((MPLSensor*) mSensor)-> 259 readDmpSignificantMotionEvents(data, count); 260 mPollFds[i].revents = 0; 261 if (nb) { 262 if (!mSMDWakelockHeld) { 263 /* Hold wakelock until Sensor Services reads event */ 264 acquire_wake_lock(PARTIAL_WAKE_LOCK, smdWakelockStr); 265 LOGI_IF(1, "HAL: grabbed %s wakelock", smdWakelockStr); 266 mSMDWakelockHeld = true; 267 } 268 269 count -= nb; 270 nbEvents += nb; 271 data += nb; 272 } 273 } else if (i == dmpPed) { 274 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 275 data, count, ID_P, 0); 276 mPollFds[i].revents = 0; 277 count -= nb; 278 nbEvents += nb; 279 data += nb; 280 } 281 if(nb == 0) { 282 nb = ((MPLSensor*) mSensor)->readEvents(data, count); 283 LOGI_IF(0, "sensors_mpl:readEvents() - " 284 "i=%d, nb=%d, count=%d, nbEvents=%d, " 285 "data->timestamp=%lld, data->data[0]=%f,", 286 i, nb, count, nbEvents, data->timestamp, 287 data->data[0]); 288 if (nb > 0) { 289 count -= nb; 290 nbEvents += nb; 291 data += nb; 292 } 293 } 294 } 295 } 296 297 /* to see if any step counter events */ 298 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { 299 nb = 0; 300 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 301 data, count, ID_SC, 0); 302 LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " 303 "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ", 304 nb, count, nbEvents, data->timestamp); 305 if (nb > 0) { 306 count -= nb; 307 nbEvents += nb; 308 data += nb; 309 } 310 } 311 } else if(nb == 0) { 312 /* to see if any step counter events */ 313 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { 314 nb = 0; 315 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 316 data, count, ID_SC, 0); 317 LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " 318 "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, ", 319 nb, count, nbEvents, data->timestamp); 320 if (nb > 0) { 321 count -= nb; 322 nbEvents += nb; 323 data += nb; 324 } 325 } 326 } 327 return nbEvents; 328} 329 330int sensors_poll_context_t::query(int what, int* value) 331{ 332 FUNC_LOG; 333 return mSensor->query(what, value); 334} 335 336int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, 337 int64_t timeout) 338{ 339 FUNC_LOG; 340 return mSensor->batch(handle, flags, period_ns, timeout); 341} 342 343#if defined ANDROID_KITKAT 344void inv_pending_flush(int handle) { 345 struct handle_entry *the_entry; 346 pthread_mutex_lock(&flush_handles_mutex); 347 the_entry = (struct handle_entry*) malloc(sizeof(struct handle_entry)); 348 if (the_entry != NULL) { 349 LOGI_IF(0, "Inserting %d into pending list", handle); 350 the_entry->handle = handle; 351 SIMPLEQ_INSERT_TAIL(&pending_flush_items_head, the_entry, entries); 352 } else { 353 LOGE("ERROR malloc'ing space for pending handler flush entry"); 354 } 355 pthread_mutex_unlock(&flush_handles_mutex); 356} 357 358int sensors_poll_context_t::flush(int handle) 359{ 360 FUNC_LOG; 361 return mSensor->flush(handle); 362} 363#endif 364 365/******************************************************************************/ 366 367static int poll__close(struct hw_device_t *dev) 368{ 369 FUNC_LOG; 370 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 371 if (ctx) { 372 delete ctx; 373 } 374 return 0; 375} 376 377static int poll__activate(struct sensors_poll_device_t *dev, 378 int handle, int enabled) 379{ 380 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 381 return ctx->activate(handle, enabled); 382} 383 384static int poll__setDelay(struct sensors_poll_device_t *dev, 385 int handle, int64_t ns) 386{ 387 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 388 int s= ctx->setDelay(handle, ns); 389 return s; 390} 391 392static int poll__poll(struct sensors_poll_device_t *dev, 393 sensors_event_t* data, int count) 394{ 395 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 396 return ctx->pollEvents(data, count); 397} 398 399static int poll__query(struct sensors_poll_device_1 *dev, 400 int what, int *value) 401{ 402 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 403 return ctx->query(what, value); 404} 405 406static int poll__batch(struct sensors_poll_device_1 *dev, 407 int handle, int flags, int64_t period_ns, int64_t timeout) 408{ 409 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 410 return ctx->batch(handle, flags, period_ns, timeout); 411} 412 413#if defined ANDROID_KITKAT 414static int poll__flush(struct sensors_poll_device_1 *dev, 415 int handle) 416{ 417 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 418 int status = ctx->flush(handle); 419 if (handle == SENSORS_STEP_COUNTER_HANDLE) { 420 LOGI_IF(0, "creating flush completion event for handle %d", handle); 421 inv_pending_flush(handle); 422 return 0; 423 } 424 return status; 425} 426#endif 427/******************************************************************************/ 428 429/** Open a new instance of a sensor device using name */ 430static int open_sensors(const struct hw_module_t* module, const char* id, 431 struct hw_device_t** device) 432{ 433 FUNC_LOG; 434 int status = -EINVAL; 435 sensors_poll_context_t *dev = new sensors_poll_context_t(); 436 437 memset(&dev->device, 0, sizeof(sensors_poll_device_1)); 438 439 dev->device.common.tag = HARDWARE_DEVICE_TAG; 440#if defined ANDROID_KITKAT 441 dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_3; 442 dev->device.flush = poll__flush; 443#else 444 dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_0; 445#endif 446 dev->device.common.module = const_cast<hw_module_t*>(module); 447 dev->device.common.close = poll__close; 448 dev->device.activate = poll__activate; 449 dev->device.setDelay = poll__setDelay; 450 dev->device.poll = poll__poll; 451 dev->device.batch = poll__batch; 452 453 *device = &dev->device.common; 454 status = 0; 455 456 return status; 457} 458