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