| /* |
| * Copyright (C) 2012 Invensense, Inc. |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| |
| #define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__) |
| |
| #include <hardware/sensors.h> |
| #include <fcntl.h> |
| #include <errno.h> |
| #include <dirent.h> |
| #include <math.h> |
| #include <poll.h> |
| #include <pthread.h> |
| #include <stdlib.h> |
| |
| #include <linux/input.h> |
| |
| #include <cutils/atomic.h> |
| #include <utils/Log.h> |
| |
| #include "sensors.h" |
| #include "MPLSensor.h" |
| |
| /* |
| * Vendor-defined Accel Load Calibration File Method |
| * @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame |
| * @return '0' for a successful load, '1' otherwise |
| * example: int AccelLoadConfig(long* offset); |
| * End of Vendor-defined Accel Load Cal Method |
| */ |
| |
| /*****************************************************************************/ |
| /* The SENSORS Module */ |
| |
| #ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION |
| #define LOCAL_SENSORS (MPLSensor::NumSensors + 1) |
| #else |
| #define LOCAL_SENSORS MPLSensor::NumSensors |
| #endif |
| |
| static struct sensor_t sSensorList[LOCAL_SENSORS]; |
| static int sensors = (sizeof(sSensorList) / sizeof(sensor_t)); |
| |
| static int open_sensors(const struct hw_module_t* module, const char* id, |
| struct hw_device_t** device); |
| |
| static int sensors__get_sensors_list(struct sensors_module_t* module, |
| struct sensor_t const** list) |
| { |
| *list = sSensorList; |
| return sensors; |
| } |
| |
| static struct hw_module_methods_t sensors_module_methods = { |
| open: open_sensors |
| }; |
| |
| struct sensors_module_t HAL_MODULE_INFO_SYM = { |
| common: { |
| tag: HARDWARE_MODULE_TAG, |
| version_major: 1, |
| version_minor: 0, |
| id: SENSORS_HARDWARE_MODULE_ID, |
| name: "Invensense module", |
| author: "Invensense Inc.", |
| methods: &sensors_module_methods, |
| dso: NULL, |
| reserved: {0} |
| }, |
| get_sensors_list: sensors__get_sensors_list, |
| set_operation_mode: NULL |
| }; |
| |
| struct sensors_poll_context_t { |
| sensors_poll_device_1_t device; // must be first |
| |
| sensors_poll_context_t(); |
| ~sensors_poll_context_t(); |
| int activate(int handle, int enabled); |
| int setDelay(int handle, int64_t ns); |
| int pollEvents(sensors_event_t* data, int count); |
| int query(int what, int *value); |
| int batch(int handle, int flags, int64_t period_ns, int64_t timeout); |
| int flush(int handle); |
| |
| private: |
| enum { |
| mpl = 0, |
| compass, |
| dmpOrient, |
| dmpSign, |
| dmpPed, |
| numSensorDrivers, // wake pipe goes here |
| numFds, |
| }; |
| |
| struct pollfd mPollFds[numFds]; |
| SensorBase *mSensor; |
| CompassSensor *mCompassSensor; |
| |
| static const size_t wake = numSensorDrivers; |
| static const char WAKE_MESSAGE = 'W'; |
| int mWritePipeFd; |
| }; |
| |
| /******************************************************************************/ |
| |
| sensors_poll_context_t::sensors_poll_context_t() { |
| VFUNC_LOG; |
| |
| /* TODO: Handle external pressure sensor */ |
| mCompassSensor = new CompassSensor(); |
| MPLSensor *mplSensor = new MPLSensor(mCompassSensor); |
| |
| /* For Vendor-defined Accel Calibration File Load |
| * Use the Following Constructor and Pass Your Load Cal File Function |
| * |
| * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig); |
| */ |
| |
| // setup the callback object for handing mpl callbacks |
| setCallbackObject(mplSensor); |
| |
| // populate the sensor list |
| sensors = |
| mplSensor->populateSensorList(sSensorList, sizeof(sSensorList)); |
| |
| mSensor = mplSensor; |
| mPollFds[mpl].fd = mSensor->getFd(); |
| mPollFds[mpl].events = POLLIN; |
| mPollFds[mpl].revents = 0; |
| |
| mPollFds[compass].fd = mCompassSensor->getFd(); |
| mPollFds[compass].events = POLLIN; |
| mPollFds[compass].revents = 0; |
| |
| mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd(); |
| mPollFds[dmpOrient].events = POLLPRI; |
| mPollFds[dmpOrient].revents = 0; |
| |
| mPollFds[dmpSign].fd = ((MPLSensor*) mSensor)->getDmpSignificantMotionFd(); |
| mPollFds[dmpSign].events = POLLPRI; |
| mPollFds[dmpSign].revents = 0; |
| |
| mPollFds[dmpPed].fd = ((MPLSensor*) mSensor)->getDmpPedometerFd(); |
| mPollFds[dmpPed].events = POLLPRI; |
| mPollFds[dmpPed].revents = 0; |
| |
| /* Timer based sensor initialization */ |
| int wakeFds[2]; |
| int result = pipe(wakeFds); |
| LOGE_IF(result<0, "error creating wake pipe (%s)", strerror(errno)); |
| fcntl(wakeFds[0], F_SETFL, O_NONBLOCK); |
| fcntl(wakeFds[1], F_SETFL, O_NONBLOCK); |
| mWritePipeFd = wakeFds[1]; |
| |
| mPollFds[numSensorDrivers].fd = wakeFds[0]; |
| mPollFds[numSensorDrivers].events = POLLIN; |
| mPollFds[numSensorDrivers].revents = 0; |
| } |
| |
| sensors_poll_context_t::~sensors_poll_context_t() { |
| FUNC_LOG; |
| delete mSensor; |
| delete mCompassSensor; |
| for (int i = 0; i < numSensorDrivers; i++) { |
| close(mPollFds[i].fd); |
| } |
| close(mWritePipeFd); |
| } |
| |
| int sensors_poll_context_t::activate(int handle, int enabled) { |
| FUNC_LOG; |
| |
| int err; |
| err = mSensor->enable(handle, enabled); |
| if (!err) { |
| const char wakeMessage(WAKE_MESSAGE); |
| int result = write(mWritePipeFd, &wakeMessage, 1); |
| LOGE_IF(result < 0, |
| "error sending wake message (%s)", strerror(errno)); |
| } |
| return err; |
| } |
| |
| int sensors_poll_context_t::setDelay(int handle, int64_t ns) |
| { |
| FUNC_LOG; |
| return mSensor->setDelay(handle, ns); |
| } |
| |
| int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count) |
| { |
| VHANDLER_LOG; |
| |
| int nbEvents = 0; |
| int nb, polltime = -1; |
| |
| polltime = ((MPLSensor*) mSensor)->getStepCountPollTime(); |
| |
| // look for new events |
| nb = poll(mPollFds, numSensorDrivers, polltime); |
| LOGI_IF(0, "poll nb=%d, count=%d, pt=%d", nb, count, polltime); |
| if (nb > 0) { |
| for (int i = 0; count && i < numSensorDrivers; i++) { |
| if (mPollFds[i].revents & (POLLIN | POLLPRI)) { |
| nb = 0; |
| if (i == mpl) { |
| ((MPLSensor*) mSensor)->buildMpuEvent(); |
| mPollFds[i].revents = 0; |
| } else if (i == compass) { |
| ((MPLSensor*) mSensor)->buildCompassEvent(); |
| mPollFds[i].revents = 0; |
| } else if (i == dmpOrient) { |
| nb = ((MPLSensor*)mSensor)-> |
| readDmpOrientEvents(data, count); |
| mPollFds[dmpOrient].revents= 0; |
| if (isDmpScreenAutoRotationEnabled() && nb > 0) { |
| count -= nb; |
| nbEvents += nb; |
| data += nb; |
| } |
| } else if (i == dmpSign) { |
| LOGI("HAL: dmpSign interrupt"); |
| nb = ((MPLSensor*) mSensor)-> |
| readDmpSignificantMotionEvents(data, count); |
| mPollFds[i].revents = 0; |
| count -= nb; |
| nbEvents += nb; |
| data += nb; |
| } else if (i == dmpPed) { |
| LOGI("HAL: dmpPed interrupt"); |
| nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( |
| data, count, ID_P, SENSOR_TYPE_STEP_DETECTOR, 0); |
| mPollFds[i].revents = 0; |
| count -= nb; |
| nbEvents += nb; |
| data += nb; |
| } |
| #if 1 |
| if(nb == 0) { |
| nb = ((MPLSensor*) mSensor)->readEvents(data, count); |
| LOGI_IF(0, "sensors_mpl:readEvents() - " |
| "i=%d, nb=%d, count=%d, nbEvents=%d, " |
| "data->timestamp=%lld, data->data[0]=%f,", |
| i, nb, count, nbEvents, data->timestamp, |
| data->data[0]); |
| if (nb > 0) { |
| count -= nb; |
| nbEvents += nb; |
| data += nb; |
| } |
| } |
| #endif |
| } |
| } |
| |
| /* to see if any step counter events */ |
| if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { |
| nb = 0; |
| nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( |
| data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0); |
| LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " |
| "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, " |
| "data->data[0]=%f,", |
| nb, count, nbEvents, data->timestamp, data->data[0]); |
| if (nb > 0) { |
| count -= nb; |
| nbEvents += nb; |
| data += nb; |
| } |
| } |
| } else if(nb == 0) { |
| /* to see if any step counter events */ |
| if(((MPLSensor*) mSensor)->hasStepCountPendingEvents() == true) { |
| nb = 0; |
| nb = ((MPLSensor*) mSensor)->readDmpPedometerEvents( |
| data, count, ID_SC, SENSOR_TYPE_STEP_COUNTER, 0); |
| LOGI_IF(SensorBase::HANDLER_DATA, "sensors_mpl:readStepCount() - " |
| "nb=%d, count=%d, nbEvents=%d, data->timestamp=%lld, " |
| "data->data[0]=%f,", |
| nb, count, nbEvents, data->timestamp, data->data[0]); |
| if (nb > 0) { |
| count -= nb; |
| nbEvents += nb; |
| data += nb; |
| } |
| } |
| |
| if (mPollFds[numSensorDrivers].revents & POLLIN) { |
| char msg; |
| int result = read(mPollFds[numSensorDrivers].fd, &msg, 1); |
| LOGE_IF(result < 0, |
| "error reading from wake pipe (%s)", strerror(errno)); |
| mPollFds[numSensorDrivers].revents = 0; |
| } |
| } |
| return nbEvents; |
| } |
| |
| int sensors_poll_context_t::query(int what, int* value) |
| { |
| FUNC_LOG; |
| return mSensor->query(what, value); |
| } |
| |
| int sensors_poll_context_t::batch(int handle, int flags, int64_t period_ns, |
| int64_t timeout) |
| { |
| FUNC_LOG; |
| return mSensor->batch(handle, flags, period_ns, timeout); |
| } |
| |
| int sensors_poll_context_t::flush(int handle) |
| { |
| FUNC_LOG; |
| return mSensor->flush(handle); |
| } |
| |
| /******************************************************************************/ |
| |
| static int poll__close(struct hw_device_t *dev) |
| { |
| FUNC_LOG; |
| sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; |
| if (ctx) { |
| delete ctx; |
| } |
| return 0; |
| } |
| |
| static int poll__activate(struct sensors_poll_device_t *dev, |
| int handle, int enabled) |
| { |
| sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; |
| return ctx->activate(handle, enabled); |
| } |
| |
| static int poll__setDelay(struct sensors_poll_device_t *dev, |
| int handle, int64_t ns) |
| { |
| sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; |
| int s= ctx->setDelay(handle, ns); |
| return s; |
| } |
| |
| static int poll__poll(struct sensors_poll_device_t *dev, |
| sensors_event_t* data, int count) |
| { |
| sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; |
| return ctx->pollEvents(data, count); |
| } |
| |
| static int poll__batch(struct sensors_poll_device_1 *dev, |
| int handle, int flags, int64_t period_ns, int64_t timeout) |
| { |
| sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; |
| return ctx->batch(handle, flags, period_ns, timeout); |
| } |
| |
| static int poll__flush(struct sensors_poll_device_1 *dev, |
| int handle) |
| { |
| sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; |
| return ctx->flush(handle); |
| } |
| /******************************************************************************/ |
| |
| /** Open a new instance of a sensor device using name */ |
| static int open_sensors(const struct hw_module_t* module, const char* id, |
| struct hw_device_t** device) |
| { |
| FUNC_LOG; |
| int status = -EINVAL; |
| sensors_poll_context_t *dev = new sensors_poll_context_t(); |
| |
| memset(&dev->device, 0, sizeof(sensors_poll_device_1)); |
| |
| dev->device.common.tag = HARDWARE_DEVICE_TAG; |
| dev->device.common.version = SENSORS_DEVICE_API_VERSION_1_0; |
| dev->device.common.module = const_cast<hw_module_t*>(module); |
| dev->device.common.close = poll__close; |
| dev->device.activate = poll__activate; |
| dev->device.setDelay = poll__setDelay; |
| dev->device.poll = poll__poll; |
| |
| /* Batch processing */ |
| dev->device.batch = poll__batch; |
| dev->device.flush = poll__flush; |
| |
| *device = &dev->device.common; |
| status = 0; |
| |
| return status; |
| } |