LineageOS/android_device_google_contexthub
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implement sensor direct report mode

Browse Source 
This change implements direct report for accel, gyro and mag at normal
and fast rate level. It supports ashmem shared memory.

The direct report and traditional sensor subscription are still
tangled up in this implementation.

Bug: 30985702
Bug: 33588372
Test: tested via demo app

Change-Id: Idc4f626dfe624558718c1b04f16898a0ea247e62
This commit is contained in:
Peng Xu
2016-11-03 11:56:49 -07:00
parent 0e3c473520
commit cf5df9cf40
9 changed files with 512 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
sensorhal/Android.mk
View File

@@ -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

73
sensorhal/directchannel.cpp Normal file
View File

@@ -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 <cutils/ashmem.h>
#include <sys/mman.h>
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<LockfreeBuffer>(new LockfreeBuffer(mBase, mSize));
if (!mBuffer) {
mError = NO_MEMORY;
}
}
AshmemDirectChannel::~AshmemDirectChannel() {
if (mBase) {
mBuffer = nullptr;
::munmap(mBase, mSize);
}
::close(mAshmemFd);
}
} // namespace android

53
sensorhal/directchannel.h Normal file
View File

@@ -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 <hardware/sensors.h>
#include <memory>
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<LockfreeBuffer> 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_

280
sensorhal/hubconnection.cpp
View File

@@ -35,10 +35,15 @@
#include <linux/input.h>
#include <linux/uinput.h>
#include <cutils/ashmem.h>
#include <cutils/properties.h>
#include <hardware_legacy/power.h>
#include <media/stagefright/foundation/ADebug.h>
#include <algorithm>
#include <sstream>
#include <vector>
#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<int32_t, int32_t>());
mSensorToChannel.emplace(COMMS_SENSOR_GYRO, std::unordered_map<int32_t, int32_t>());
mSensorToChannel.emplace(COMMS_SENSOR_MAG, std::unordered_map<int32_t, int32_t>());
#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<DirectChannelBase> ch;
int ret = NO_MEMORY;
switch(mem->type) {
case SENSOR_DIRECT_MEM_TYPE_ASHMEM:
ch = std::unique_ptr<DirectChannelBase>(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<int32_t> 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<int32_t>(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<int32_t> *activeSensorList) {
Mutex::Autolock autoLock(mDirectChannelLock);
if (mDirectChannel.find(channel_handle) == mDirectChannel.end()) {
return BAD_VALUE;
}
std::vector<int32_t> 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

27
sensorhal/hubconnection.h
View File

@@ -28,10 +28,13 @@
#include <utils/Thread.h>
#include "activityeventhandler.h"
#include "directchannel.h"
#include "eventnums.h"
#include "hubdefs.h"
#include "ring.h"
#include <unordered_map>
#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<nsecs_t>(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<int32_t> *unstoppedSensors);
Mutex mDirectChannelLock;
//sensor_handle=>(channel_handle, rate_level)
std::unordered_map<int32_t, std::unordered_map<int32_t, int32_t> > mSensorToChannel;
//channel_handle=>ptr of Channel obj
std::unordered_map<int32_t, std::unique_ptr<DirectChannelBase>> mDirectChannel;
int32_t mDirectChannelHandle;
#endif
DISALLOW_EVIL_CONSTRUCTORS(HubConnection);
};

39
sensorhal/sensors.cpp
View File

@@ -22,10 +22,12 @@
#include "sensorlist.h"
#include "sensors.h"
#include <cutils/ashmem.h>
#include <errno.h>
#include <math.h>
#include <media/stagefright/foundation/ADebug.h>
#include <string.h>
#include <sys/mman.h>
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<SensorContext *>(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<SensorContext *>(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<SensorContext *>(dev)->config_direct_report(
sensor_handle, channel_handle, config);
}
bool SensorContext::getHubAlive() {
return mHubAlive;
}

12
sensorhal/sensors.h
View File

@@ -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);
};

25
util/common/ring.cpp
View File

@@ -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

15
util/common/ring.h
View File

@@ -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_