diff --git a/sensorhal/Android.mk b/sensorhal/Android.mk index 9c004521..4f39a462 100644 --- a/sensorhal/Android.mk +++ b/sensorhal/Android.mk @@ -63,6 +63,10 @@ LOCAL_MODULE_OWNER := google LOCAL_CFLAGS += $(COMMON_CFLAGS) +ifeq ($(NANOHUB_SENSORHAL_DIRECT_REPORT_ENABLED), true) +LOCAL_CFLAGS += -DDIRECT_REPORT_ENABLED +endif + LOCAL_C_INCLUDES += \ device/google/contexthub/firmware/os/inc \ device/google/contexthub/util/common @@ -129,11 +133,16 @@ ifeq ($(NANOHUB_SENSORHAL_DOUBLE_TOUCH_ENABLED), true) LOCAL_CFLAGS += -DDOUBLE_TOUCH_ENABLED endif +ifeq ($(NANOHUB_SENSORHAL_DIRECT_REPORT_ENABLED), true) +LOCAL_CFLAGS += -DDIRECT_REPORT_ENABLED +endif + LOCAL_C_INCLUDES += \ device/google/contexthub/firmware/os/inc LOCAL_SRC_FILES := \ - hubconnection.cpp + hubconnection.cpp \ + directchannel.cpp LOCAL_STATIC_LIBRARIES := \ libhubutilcommon diff --git a/sensorhal/directchannel.cpp b/sensorhal/directchannel.cpp new file mode 100644 index 00000000..40364e61 --- /dev/null +++ b/sensorhal/directchannel.cpp @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2017 The Android Open Source Project + * + * 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. + */ + +#include "directchannel.h" + +#include +#include + +namespace android { + +bool DirectChannelBase::isValid() { + return mBuffer != nullptr; +} + +int DirectChannelBase::getError() { + return mError; +} + +void DirectChannelBase::write(const sensors_event_t * ev) { + if (isValid()) { + mBuffer->write(ev, 1); + } +} + +AshmemDirectChannel::AshmemDirectChannel(const struct sensors_direct_mem_t *mem) : mAshmemFd(0) { + mAshmemFd = mem->handle->data[0]; + + if (!::ashmem_valid(mAshmemFd)) { + mError = BAD_VALUE; + return; + } + + if ((size_t)::ashmem_get_size_region(mAshmemFd) != mem->size) { + mError = BAD_VALUE; + return; + } + + mSize = mem->size; + + mBase = ::mmap(NULL, mem->size, PROT_WRITE, MAP_SHARED, mAshmemFd, 0); + if (mBase == nullptr) { + mError = NO_MEMORY; + return; + } + + mBuffer = std::unique_ptr(new LockfreeBuffer(mBase, mSize)); + if (!mBuffer) { + mError = NO_MEMORY; + } +} + +AshmemDirectChannel::~AshmemDirectChannel() { + if (mBase) { + mBuffer = nullptr; + ::munmap(mBase, mSize); + } + ::close(mAshmemFd); +} + +} // namespace android diff --git a/sensorhal/directchannel.h b/sensorhal/directchannel.h new file mode 100644 index 00000000..ea0c1fe2 --- /dev/null +++ b/sensorhal/directchannel.h @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2016 The Android Open Source Project + * + * 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. + */ + +#ifndef DIRECTCHANNEL_H_ +#define DIRECTCHANNEL_H_ + +#include "ring.h" +#include +#include + +namespace android { + +class DirectChannelBase { +public: + DirectChannelBase() : mError(NO_ERROR) { } + virtual ~DirectChannelBase() {} + + bool isValid(); + int getError(); + void write(const sensors_event_t * ev); + +protected: + int mError; + std::unique_ptr mBuffer; + + size_t mSize; + void* mBase; +}; + +class AshmemDirectChannel : public DirectChannelBase { +public: + AshmemDirectChannel(const struct sensors_direct_mem_t *mem); + virtual ~AshmemDirectChannel(); +private: + int mAshmemFd; +}; + +} // namespace android + +#endif // DIRECTCHANNEL_H_ diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp index 8b8f4893..82f0a92d 100644 --- a/sensorhal/hubconnection.cpp +++ b/sensorhal/hubconnection.cpp @@ -35,10 +35,15 @@ #include #include +#include #include #include #include +#include +#include +#include + #define APP_ID_GET_VENDOR(appid) ((appid) >> 24) #define APP_ID_MAKE(vendor, app) ((((uint64_t)(vendor)) << 24) | ((app) & 0x00FFFFFF)) #define APP_ID_VENDOR_GOOGLE 0x476f6f676cULL // "Googl" @@ -237,6 +242,13 @@ HubConnection::HubConnection() queueActivate(COMMS_SENSOR_HALL, true /* enable */); } #endif // LID_STATE_REPORTING_ENABLED + +#ifdef DIRECT_REPORT_ENABLED + mDirectChannelHandle = 1; + mSensorToChannel.emplace(COMMS_SENSOR_ACCEL, std::unordered_map()); + mSensorToChannel.emplace(COMMS_SENSOR_GYRO, std::unordered_map()); + mSensorToChannel.emplace(COMMS_SENSOR_MAG, std::unordered_map()); +#endif // DIRECT_REPORT_ENABLED } HubConnection::~HubConnection() @@ -557,12 +569,15 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se switch (sensor) { case COMMS_SENSOR_ACCEL: - if (mSensorState[COMMS_SENSOR_ACCEL].enable) { - sv = &initEv(&nev[cnt++], timestamp, type, sensor)->acceleration; - sv->x = sample->ix * ACCEL_RAW_KSCALE; - sv->y = sample->iy * ACCEL_RAW_KSCALE; - sv->z = sample->iz * ACCEL_RAW_KSCALE; - sv->status = SENSOR_STATUS_ACCURACY_HIGH; + sv = &initEv(&nev[cnt], timestamp, type, sensor)->acceleration; + sv->x = sample->ix * ACCEL_RAW_KSCALE; + sv->y = sample->iy * ACCEL_RAW_KSCALE; + sv->z = sample->iz * ACCEL_RAW_KSCALE; + sv->status = SENSOR_STATUS_ACCURACY_HIGH; + sendDirectReportEvent(&nev[cnt], 1); + + if (mSensorState[sensor].enable) { + ++cnt; } if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable) { @@ -600,12 +615,15 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se switch (sensor) { case COMMS_SENSOR_ACCEL: + sv = &initEv(&nev[cnt], timestamp, type, sensor)->acceleration; + sv->x = sample->x; + sv->y = sample->y; + sv->z = sample->z; + sv->status = SENSOR_STATUS_ACCURACY_HIGH; + sendDirectReportEvent(&nev[cnt], 1); + if (mSensorState[sensor].enable) { - sv = &initEv(&nev[cnt++], timestamp, type, sensor)->acceleration; - sv->x = sample->x; - sv->y = sample->y; - sv->z = sample->z; - sv->status = SENSOR_STATUS_ACCURACY_HIGH; + ++cnt; } if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable) { @@ -621,12 +639,15 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se } break; case COMMS_SENSOR_GYRO: + sv = &initEv(&nev[cnt], timestamp, type, sensor)->gyro; + sv->x = sample->x; + sv->y = sample->y; + sv->z = sample->z; + sv->status = SENSOR_STATUS_ACCURACY_HIGH; + sendDirectReportEvent(&nev[cnt], 1); + if (mSensorState[sensor].enable) { - sv = &initEv(&nev[cnt++], timestamp, type, sensor)->gyro; - sv->x = sample->x; - sv->y = sample->y; - sv->z = sample->z; - sv->status = SENSOR_STATUS_ACCURACY_HIGH; + ++cnt; } if (mSensorState[COMMS_SENSOR_GYRO_UNCALIBRATED].enable) { @@ -656,12 +677,15 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se case COMMS_SENSOR_MAG: magAccuracyUpdate(sample->x, sample->y, sample->z); + sv = &initEv(&nev[cnt], timestamp, type, sensor)->magnetic; + sv->x = sample->x; + sv->y = sample->y; + sv->z = sample->z; + sv->status = mMagAccuracy; + sendDirectReportEvent(&nev[cnt], 1); + if (mSensorState[sensor].enable) { - sv = &initEv(&nev[cnt++], timestamp, type, sensor)->magnetic; - sv->x = sample->x; - sv->y = sample->y; - sv->z = sample->z; - sv->status = mMagAccuracy; + ++cnt; } if (mSensorState[COMMS_SENSOR_MAG_UNCALIBRATED].enable) { @@ -1301,6 +1325,9 @@ void HubConnection::initConfigCmd(struct ConfigCmd *cmd, int handle) cmd->rate = mSensorState[handle].rate; cmd->latency = mSensorState[handle].latency; } + + // will be a nop if direct report mode is not enabled + mergeDirectReportRequest(cmd, handle); } void HubConnection::queueActivate(int handle, bool enable) @@ -1566,4 +1593,215 @@ void HubConnection::sendFolioEvent(int32_t data) { } #endif // LID_STATE_REPORTING_ENABLED +#ifdef DIRECT_REPORT_ENABLED +void HubConnection::sendDirectReportEvent(const sensors_event_t *nev, size_t n) { + // short circuit to avoid lock operation + if (n == 0) { + return; + } + + // no intention to block sensor delivery thread. when lock is needed ignore + // the event (this only happens when the channel is reconfiured, so it's ok + if (mDirectChannelLock.tryLock() == NO_ERROR) { + while (n--) { + auto i = mSensorToChannel.find(nev->sensor); + if (i != mSensorToChannel.end()) { + for (auto &j : i->second) { + mDirectChannel[j.first]->write(nev); + } + } + ++nev; + } + mDirectChannelLock.unlock(); + } +} + +void HubConnection::mergeDirectReportRequest(struct ConfigCmd *cmd, int handle) { + auto j = mSensorToChannel.find(handle); + if (j != mSensorToChannel.end()) { + bool enable = false; + rate_q10_t rate; + + if (!j->second.empty()) { + int maxRateLevel = SENSOR_DIRECT_RATE_STOP; + for (auto &i : j->second) { + maxRateLevel = (i.second) > maxRateLevel ? i.second : maxRateLevel; + } + switch(maxRateLevel) { + case SENSOR_DIRECT_RATE_NORMAL: + enable = true; + rate = period_ns_to_frequency_q10(20000000ull); // NORMAL = 50Hz + break; + case SENSOR_DIRECT_RATE_FAST: + enable = true; + rate = period_ns_to_frequency_q10(5000000ull); // FAST = 200Hz + break; + default: + break; + } + } + + if (enable) { + cmd->rate = (rate > cmd->rate || cmd->cmd == CONFIG_CMD_DISABLE) ? rate : cmd->rate; + cmd->latency = 0; + cmd->cmd = CONFIG_CMD_ENABLE; + } + } +} + +int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *mem) { + std::unique_ptr ch; + int ret = NO_MEMORY; + + switch(mem->type) { + case SENSOR_DIRECT_MEM_TYPE_ASHMEM: + ch = std::unique_ptr(new AshmemDirectChannel(mem)); + if (ch) { + if (ch->isValid()) { + Mutex::Autolock autoLock(mDirectChannelLock); + ret = mDirectChannelHandle++; + mDirectChannel.insert(std::make_pair(ret, std::move(ch))); + } else { + ret = ch->getError(); + ALOGE("AshmemDirectChannel %p has error %d upon init", ch.get(), ret); + } + } + break; + case SENSOR_DIRECT_MEM_TYPE_GRALLOC: + default: + ret = INVALID_OPERATION; + } + + return ret; +} + +int HubConnection::removeDirectChannel(int channel_handle) { + // make sure no active sensor in this channel + std::vector activeSensorList; + stopAllDirectReportOnChannel(channel_handle, &activeSensorList); + + // sensor service is responsible for stop all sensors before remove direct + // channel. Thus, this is an error. + if (!activeSensorList.empty()) { + std::stringstream ss; + std::copy(activeSensorList.begin(), activeSensorList.end(), + std::ostream_iterator(ss, ",")); + ALOGE("Removing channel %d when sensors (%s) are not stopped.", + channel_handle, ss.str().c_str()); + } + + // remove the channel record + Mutex::Autolock autoLock(mDirectChannelLock); + mDirectChannel.erase(channel_handle); + return NO_ERROR; +} + +int HubConnection::stopAllDirectReportOnChannel( + int channel_handle, std::vector *activeSensorList) { + Mutex::Autolock autoLock(mDirectChannelLock); + if (mDirectChannel.find(channel_handle) == mDirectChannel.end()) { + return BAD_VALUE; + } + + std::vector sensorToStop; + for (auto &it : mSensorToChannel) { + auto j = it.second.find(channel_handle); + if (j != it.second.end()) { + it.second.erase(j); + if (it.second.empty()) { + sensorToStop.push_back(it.first); + } + } + } + + if (activeSensorList != nullptr) { + *activeSensorList = sensorToStop; + } + + // re-evaluate and send config for all sensor that need to be stopped + bool ret = true; + for (auto sensor_handle : sensorToStop) { + Mutex::Autolock autoLock2(mLock); + struct ConfigCmd cmd; + initConfigCmd(&cmd, sensor_handle); + + int result = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd))); + ret = ret && (result == sizeof(cmd)); + } + return ret ? NO_ERROR : BAD_VALUE; +} + +int HubConnection::configDirectReport(int sensor_handle, int channel_handle, int rate_level) { + if (sensor_handle == -1 && rate_level == SENSOR_DIRECT_RATE_STOP) { + return stopAllDirectReportOnChannel(channel_handle, nullptr); + } + + if (!isValidHandle(sensor_handle)) { + return BAD_VALUE; + } + + // clamp to fast + if (rate_level > SENSOR_DIRECT_RATE_FAST) { + rate_level = SENSOR_DIRECT_RATE_FAST; + } + + // manage direct channel data structure + Mutex::Autolock autoLock(mDirectChannelLock); + auto i = mDirectChannel.find(channel_handle); + if (i == mDirectChannel.end()) { + return BAD_VALUE; + } + + auto j = mSensorToChannel.find(sensor_handle); + if (j == mSensorToChannel.end()) { + return BAD_VALUE; + } + + j->second.erase(channel_handle); + if (rate_level != SENSOR_DIRECT_RATE_STOP) { + j->second.insert(std::make_pair(channel_handle, rate_level)); + } + + Mutex::Autolock autoLock2(mLock); + struct ConfigCmd cmd; + initConfigCmd(&cmd, sensor_handle); + + int ret = TEMP_FAILURE_RETRY(write(mFd, &cmd, sizeof(cmd))); + + if (rate_level == SENSOR_DIRECT_RATE_STOP) { + ret = NO_ERROR; + } else { + ret = (ret == sizeof(cmd)) ? sensor_handle : BAD_VALUE; + } + return ret; +} + +bool HubConnection::isDirectReportSupported() const { + return true; +} +#else // DIRECT_REPORT_ENABLED +// nop functions if feature is turned off +int HubConnection::addDirectChannel(const struct sensors_direct_mem_t *) { + return INVALID_OPERATION; +} + +int HubConnection::removeDirectChannel(int) { + return INVALID_OPERATION; +} + +int HubConnection::configDirectReport(int, int, int) { + return INVALID_OPERATION; +} + +void HubConnection::sendDirectReportEvent(const sensors_event_t *, size_t) { +} + +void HubConnection::mergeDirectReportRequest(struct ConfigCmd *, int) { +} + +bool HubConnection::isDirectReportSupported() const { + return false; +} +#endif // DIRECT_REPORT_ENABLED + } // namespace android diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h index a8916122..2432002f 100644 --- a/sensorhal/hubconnection.h +++ b/sensorhal/hubconnection.h @@ -28,10 +28,13 @@ #include #include "activityeventhandler.h" +#include "directchannel.h" #include "eventnums.h" #include "hubdefs.h" #include "ring.h" +#include + #define WAKELOCK_NAME "sensorHal" namespace android { @@ -95,6 +98,10 @@ private: return (nsecs_t)0; } + static inline uint64_t frequency_to_frequency_q10(float frequency) { + return period_ns_to_frequency_q10(static_cast(1e9f/frequency)); + } + enum { CONFIG_CMD_DISABLE = 0, @@ -258,6 +265,26 @@ private: int mDoubleTouchPollIndex; #endif // DOUBLE_TOUCH_ENABLED + // Direct report functions +public: + int addDirectChannel(const struct sensors_direct_mem_t *mem); + int removeDirectChannel(int channel_handle); + int configDirectReport(int sensor_handle, int channel_handle, int rate_level); + bool isDirectReportSupported() const; +private: + void sendDirectReportEvent(const sensors_event_t *nev, size_t n); + void mergeDirectReportRequest(struct ConfigCmd *cmd, int handle); +#ifdef DIRECT_REPORT_ENABLED + int stopAllDirectReportOnChannel( + int channel_handle, std::vector *unstoppedSensors); + Mutex mDirectChannelLock; + //sensor_handle=>(channel_handle, rate_level) + std::unordered_map > mSensorToChannel; + //channel_handle=>ptr of Channel obj + std::unordered_map> mDirectChannel; + int32_t mDirectChannelHandle; +#endif + DISALLOW_EVIL_CONSTRUCTORS(HubConnection); }; diff --git a/sensorhal/sensors.cpp b/sensorhal/sensors.cpp index 77ab3ba8..ce1b2734 100644 --- a/sensorhal/sensors.cpp +++ b/sensorhal/sensors.cpp @@ -22,10 +22,12 @@ #include "sensorlist.h" #include "sensors.h" +#include #include #include #include #include +#include using namespace android; @@ -44,7 +46,10 @@ SensorContext::SensorContext(const struct hw_module_t *module) device.poll = PollWrapper; device.batch = BatchWrapper; device.flush = FlushWrapper; - + if (mHubConnection->isDirectReportSupported()) { + device.register_direct_channel = RegisterDirectChannelWrapper; + device.config_direct_report = ConfigDirectReportWrapper; + } mHubAlive = (mHubConnection->initCheck() == OK && mHubConnection->getAliveCheck() == OK); } @@ -199,6 +204,38 @@ int SensorContext::FlushWrapper(struct sensors_poll_device_1 *dev, int handle) { return reinterpret_cast(dev)->flush(handle); } +int SensorContext::register_direct_channel( + const struct sensors_direct_mem_t *mem, int32_t channel_handle) { + if (mem) { + //add + return mHubConnection->addDirectChannel(mem); + } else { + //remove + mHubConnection->removeDirectChannel(channel_handle); + return NO_ERROR; + } +} + +int SensorContext::config_direct_report( + int32_t sensor_handle, int32_t channel_handle, const struct sensors_direct_cfg_t * config) { + int rate_level = config->rate_level; + return mHubConnection->configDirectReport(sensor_handle, channel_handle, rate_level); +} + +// static +int SensorContext::RegisterDirectChannelWrapper(struct sensors_poll_device_1 *dev, + const struct sensors_direct_mem_t* mem, int channel_handle) { + return reinterpret_cast(dev)->register_direct_channel( + mem, channel_handle); +} + +// static +int SensorContext::ConfigDirectReportWrapper(struct sensors_poll_device_1 *dev, + int sensor_handle, int channel_handle, const struct sensors_direct_cfg_t * config) { + return reinterpret_cast(dev)->config_direct_report( + sensor_handle, channel_handle, config); +} + bool SensorContext::getHubAlive() { return mHubAlive; } diff --git a/sensorhal/sensors.h b/sensorhal/sensors.h index 69ba2d6f..9802c7b9 100644 --- a/sensorhal/sensors.h +++ b/sensorhal/sensors.h @@ -45,6 +45,13 @@ private: int flush(int handle); + int register_direct_channel( + const struct sensors_direct_mem_t* mem, int channel_handle); + + int config_direct_report( + int sensor_handle, int channel_handle, const struct sensors_direct_cfg_t * config); + + // static wrappers static int CloseWrapper(struct hw_device_t *dev); static int ActivateWrapper( @@ -65,6 +72,11 @@ private: static int FlushWrapper(struct sensors_poll_device_1 *dev, int handle); + static int RegisterDirectChannelWrapper(struct sensors_poll_device_1 *dev, + const struct sensors_direct_mem_t* mem, int channel_handle); + static int ConfigDirectReportWrapper(struct sensors_poll_device_1 *dev, + int sensor_handle, int channel_handle, const struct sensors_direct_cfg_t * config); + DISALLOW_EVIL_CONSTRUCTORS(SensorContext); }; diff --git a/util/common/ring.cpp b/util/common/ring.cpp index 1d23b82c..26d44d62 100644 --- a/util/common/ring.cpp +++ b/util/common/ring.cpp @@ -104,5 +104,30 @@ ssize_t RingBuffer::read(sensors_event_t *ev, size_t size) { return size; } +LockfreeBuffer::LockfreeBuffer(void* buf, size_t size) + : mData((sensors_event_t *)buf), mSize(size/sizeof(sensors_event_t)), + mWritePos(0), mCounter(1) { + memset(mData, 0, size); +} + +LockfreeBuffer::~LockfreeBuffer() { + memset(mData, 0, mSize*sizeof(sensors_event_t)); +} + +void LockfreeBuffer::write(const sensors_event_t *ev, size_t size) { + if (!mSize) { + return; + } + + while(size--) { + mData[mWritePos] = *(ev++); + mData[mWritePos].reserved0 = mCounter++; + + if (++mWritePos >= mSize) { + mWritePos = 0; + } + } +} + } // namespace android diff --git a/util/common/ring.h b/util/common/ring.h index c77c3de8..31bf8482 100644 --- a/util/common/ring.h +++ b/util/common/ring.h @@ -43,6 +43,21 @@ private: DISALLOW_EVIL_CONSTRUCTORS(RingBuffer); }; +struct LockfreeBuffer { + LockfreeBuffer(void* buf, size_t size); + ~LockfreeBuffer(); + + // support single writer + void write(const sensors_event_t *ev, size_t size); +private: + sensors_event_t *mData; + size_t mSize; + size_t mWritePos; + int32_t mCounter; + + DISALLOW_EVIL_CONSTRUCTORS(LockfreeBuffer); +}; + } // namespace android #endif // RING_BUFFER_H_