1 /* 2 * Copyright (C) 2012 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/sensors.h> 20 #include <fcntl.h> 21 #include <errno.h> 22 #include <dirent.h> 23 #include <math.h> 24 #include <poll.h> 25 #include <pthread.h> 26 #include <stdlib.h> 27 28 #include <linux/input.h> 29 30 #include <cutils/atomic.h> 31 #include <utils/Log.h> 32 33 #include "sensors.h" 34 #include "MPLSensor.h" 35 36 /* 37 * Vendor-defined Accel Load Calibration File Method 38 * @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame 39 * @return '0' for a successful load, '1' otherwise 40 * example: int AccelLoadConfig(long* offset); 41 * End of Vendor-defined Accel Load Cal Method 42 */ 43 44 /*****************************************************************************/ 45 /* The SENSORS Module */ 46 47 #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION 48 #define LOCAL_SENSORS (MPLSensor::NumSensors + 1) 49 #else 50 #define LOCAL_SENSORS MPLSensor::NumSensors 51 #endif 52 53 static struct sensor_t sSensorList[LOCAL_SENSORS]; 54 static int sensors = (sizeof(sSensorList) / sizeof(sensor_t)); 55 56 static int open_sensors(const struct hw_module_t* module, const char* id, 57 struct hw_device_t** device); 58 59 static int sensors__get_sensors_list(struct sensors_module_t* module, 60 struct sensor_t const** list) 61 { 62 *list = sSensorList; 63 return sensors; 64 } 65 66 static struct hw_module_methods_t sensors_module_methods = { 67 open: open_sensors 68 }; 69 70 struct sensors_module_t HAL_MODULE_INFO_SYM = { 71 common: { 72 tag: HARDWARE_MODULE_TAG, 73 version_major: 1, 74 version_minor: 0, 75 id: SENSORS_HARDWARE_MODULE_ID, 76 name: "Invensense module", 77 author: "Invensense Inc.", 78 methods: &sensors_module_methods, 79 dso: NULL, 80 reserved: {0} 81 }, 82 get_sensors_list: sensors__get_sensors_list, 83 set_operation_mode: NULL 84 }; 85 86 struct sensors_poll_context_t { 87 sensors_poll_device_1_t device; // must be first 88 89 sensors_poll_context_t(); 90 ~sensors_poll_context_t(); 91 int activate(int handle, int enabled); 92 int setDelay(int handle, int64_t ns); 93 int pollEvents(sensors_event_t* data, int count); 94 int query(int what, int *value); 95 int batch(int handle, int flags, int64_t period_ns, int64_t timeout); 96 int flush(int handle); 97 98 private: 99 enum { 100 mpl = 0, 101 compass, 102 dmpOrient, 103 dmpSign, 104 dmpPed, 105 numSensorDrivers, // wake pipe goes here 106 numFds, 107 }; 108 109 struct pollfd mPollFds[numFds]; 110 SensorBase *mSensor; 111 CompassSensor *mCompassSensor; 112 113 static const size_t wake = numSensorDrivers; 114 static const char WAKE_MESSAGE = 'W'; 115 int mWritePipeFd; 116 }; 117 118 /******************************************************************************/ 119 120 sensors_poll_context_t::sensors_poll_context_t() { 121 VFUNC_LOG; 122 123 /* TODO: Handle external pressure sensor */ 124 mCompassSensor = new CompassSensor(); 125 MPLSensor *mplSensor = new MPLSensor(mCompassSensor); 126 127 /* For Vendor-defined Accel Calibration File Load 128 * Use the Following Constructor and Pass Your Load Cal File Function 129 * 130 * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig); 131 */ 132 133 // setup the callback object for handing mpl callbacks 134 setCallbackObject(mplSensor); 135 136 // populate the sensor list 137 sensors = 138 mplSensor->populateSensorList(sSensorList, sizeof(sSensorList)); 139 140 mSensor = mplSensor; 141 mPollFds[mpl].fd = mSensor->getFd(); 142 mPollFds[mpl].events = POLLIN; 143 mPollFds[mpl].revents = 0; 144 145 mPollFds[compass].fd = mCompassSensor->getFd(); 146 mPollFds[compass].events = POLLIN; 147 mPollFds[compass].revents = 0; 148 149 mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd(); 150 mPollFds[dmpOrient].events = POLLPRI; 151 mPollFds[dmpOrient].revents = 0; 152 153 mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd(); 154 mPollFds[dmpSign].events = POLLPRI; 155 mPollFds[dmpSign].revents = 0; 156 157 mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd(); 158 mPollFds[dmpPed].events = POLLPRI; 159 mPollFds[dmpPed].revents = 0; 160 161 /* Timer based sensor initialization */ 162 int wakeFds[2]; 163 int result = pipe(wakeFds); 164 LOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno)); 165 fcntl(wakeFds[0], F_SETFL, O_NONBLOCK); 166 fcntl(wakeFds[1], F_SETFL, O_NONBLOCK); 167 mWritePipeFd = wakeFds[1]; 168 169 mPollFds[numSensorDrivers].fd = wakeFds[0]; 170 mPollFds[numSensorDrivers].events = POLLIN; 171 mPollFds[numSensorDrivers].revents = 0; 172 } 173 174 sensors_poll_context_t::~sensors_poll_context_t() { 175 FUNC_LOG; 176 delete mSensor; 177 delete mCompassSensor; 178 for (int i = 0; i < numSensorDrivers; i++) { 179 close(mPollFds[i].fd); 180 } 181 close(mWritePipeFd); 182 } 183 184 int sensors_poll_context_t::activate(int handle, int enabled) { 185 FUNC_LOG; 186 187 int err; 188 err = mSensor->enable(handle, enabled); 189 if (!err) { 190 const char wakeMessage(WAKE_MESSAGE); 191 int result = write(mWritePipeFd, &wakeMessage, 1); 192 LOGE_IF(result < 0, 193 "error sending wake message (%s)", strerror(errno)); 194 } 195 return err; 196 } 197 198 int sensors_poll_context_t::setDelay(int handle, int64_t ns) 199 { 200 FUNC_LOG; 201 return mSensor->setDelay(handle, ns); 202 } 203 204 int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count) 205 { 206 VHANDLER_LOG; 207 208 int nbEvents = 0; 209 int nb, polltime = -1; 210 211 polltime = ((MPLSensor*) mSensor)->getStepCountPollTime(); 212 213 // look for new events 214 nb = poll(mPollFds, numSensorDrivers, polltime); 215 LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime); 216 if (nb > 0) { 217 for (int i = 0; count && i < numSensorDrivers; i++) { 218 if (mPollFds[i].revents & (POLLIN | POLLPRI)) { 219 nb = 0; 220 if (i == mpl) { 221 ((MPLSensor*) mSensor)->buildMpuEvent(); 222 mPollFds[i].revents = 0; 223 } else if (i == compass) { 224 ((MPLSensor*) mSensor)->buildCompassEvent(); 225 mPollFds[i].revents = 0; 226 } else if (i == dmpOrient) { 227 nb = ((MPLSensor*)mSensor)-> 228 readDmpOrientEvents(data, count); 229 mPollFds[dmpOrient].revents= 0; 230 if (isDmpScreenAutoRotationEnabled() && nb > 0) { 231 count -= nb; 232 nbEvents += nb; 233 data += nb; 234 } 235 } else if (i == dmpSign) { 236 LOGI("HAL: dmpSign interrupt"); 237 nb = ((MPLSensor*) mSensor)-> 238 readDmpSignificantMotionEvents(data, count); 239 mPollFds[i].revents = 0; 240 count -= nb; 241 nbEvents += nb; 242 data += nb; 243 } else if (i == dmpPed) { 244 LOGI("HAL: dmpPed interrupt"); 245 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 246 data, count, ID_P, SENSOR_TYPE_STEP_DETECTOR, 0); 247 mPollFds[i].revents = 0; 248 count -= nb; 249 nbEvents += nb; 250 data += nb; 251 } 252 #if 1 253 if(nb == 0) { 254 nb = ((MPLSensor*) mSensor)->readEvents(data, count); 255 LOGI_IF(0, "sensors_mpl:readEvents() - " 256 "i=%d, nb=%d, count=%d, nbEvents=%d, " 257 "data->timestamp=%lld, data->data[0]=%f,", 258 i, nb, count, nbEvents, data->timestamp, 259 data->data[0]); 260 if (nb > 0) { 261 count -= nb; 262 nbEvents += nb; 263 data += nb; 264 } 265 } 266 #endif 267 } 268 } 269 270 /* to see if any step counter events */ 271 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { 272 nb = 0; 273 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 274 data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0); 275 LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " 276 "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, " 277 "data->data[0]=%f,", 278 nb, count, nbEvents, data->timestamp, data->data[0]); 279 if (nb > 0) { 280 count -= nb; 281 nbEvents += nb; 282 data += nb; 283 } 284 } 285 } else if(nb == 0) { 286 /* to see if any step counter events */ 287 if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { 288 nb = 0; 289 nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( 290 data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0); 291 LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " 292 "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, " 293 "data->data[0]=%f,", 294 nb, count, nbEvents, data->timestamp, data->data[0]); 295 if (nb > 0) { 296 count -= nb; 297 nbEvents += nb; 298 data += nb; 299 } 300 } 301 302 if (mPollFds[numSensorDrivers].revents & POLLIN) { 303 char msg; 304 int result = read(mPollFds[numSensorDrivers].fd, &msg, 1); 305 LOGE_IF(result < 0, 306 "error reading from wake pipe (%s)", strerror(errno)); 307 mPollFds[numSensorDrivers].revents = 0; 308 } 309 } 310 return nbEvents; 311 } 312 313 int sensors_poll_context_t::query(int what, int* value) 314 { 315 FUNC_LOG; 316 return mSensor->query(what, value); 317 } 318 319 int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, 320 int64_t timeout) 321 { 322 FUNC_LOG; 323 return mSensor->batch(handle, flags, period_ns, timeout); 324 } 325 326 int sensors_poll_context_t::flush(int handle) 327 { 328 FUNC_LOG; 329 return mSensor->flush(handle); 330 } 331 332 /******************************************************************************/ 333 334 static int poll__close(struct hw_device_t *dev) 335 { 336 FUNC_LOG; 337 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 338 if (ctx) { 339 delete ctx; 340 } 341 return 0; 342 } 343 344 static int poll__activate(struct sensors_poll_device_t *dev, 345 int handle, int enabled) 346 { 347 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 348 return ctx->activate(handle, enabled); 349 } 350 351 static int poll__setDelay(struct sensors_poll_device_t *dev, 352 int handle, int64_t ns) 353 { 354 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 355 int s= ctx->setDelay(handle, ns); 356 return s; 357 } 358 359 static int poll__poll(struct sensors_poll_device_t *dev, 360 sensors_event_t* data, int count) 361 { 362 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 363 return ctx->pollEvents(data, count); 364 } 365 366 static int poll__batch(struct sensors_poll_device_1 *dev, 367 int handle, int flags, int64_t period_ns, int64_t timeout) 368 { 369 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 370 return ctx->batch(handle, flags, period_ns, timeout); 371 } 372 373 static int poll__flush(struct sensors_poll_device_1 *dev, 374 int handle) 375 { 376 sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; 377 return ctx->flush(handle); 378 } 379 /******************************************************************************/ 380 381 /** Open a new instance of a sensor device using name */ 382 static int open_sensors(const struct hw_module_t* module, const char* id, 383 struct hw_device_t** device) 384 { 385 FUNC_LOG; 386 int status = -EINVAL; 387 sensors_poll_context_t *dev = new sensors_poll_context_t(); 388 389 memset(&dev->device, 0, sizeof(sensors_poll_device_1)); 390 391 dev->device.common.tag = HARDWARE_DEVICE_TAG; 392 dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_0; 393 dev->device.common.module = const_cast<hw_module_t*>(module); 394 dev->device.common.close = poll__close; 395 dev->device.activate = poll__activate; 396 dev->device.setDelay = poll__setDelay; 397 dev->device.poll = poll__poll; 398 399 /* Batch processing */ 400 dev->device.batch = poll__batch; 401 dev->device.flush = poll__flush; 402 403 *device = &dev->device.common; 404 status = 0; 405 406 return status; 407 } 408
