diff options
-rw-r--r-- | sensors/multihal_sensors.cpp | 187 | ||||
-rw-r--r-- | sensors/multihal_sensors.h | 42 | ||||
-rw-r--r-- | sensors/multihal_sensors_epoll.cpp | 4 | ||||
-rw-r--r-- | sensors/multihal_sensors_qemu.cpp | 15 | ||||
-rw-r--r-- | sensors/sensor_list.cpp | 28 |
5 files changed, 199 insertions, 77 deletions
diff --git a/sensors/multihal_sensors.cpp b/sensors/multihal_sensors.cpp index 3502add9..104ff7f6 100644 --- a/sensors/multihal_sensors.cpp +++ b/sensors/multihal_sensors.cpp @@ -26,7 +26,9 @@ using ahs21::SensorType; using ahs10::SensorFlagBits; using ahs10::MetaDataEventType; -MultihalSensors::MultihalSensors() : m_qemuSensorsFd(qemud_channel_open("sensors")) { +MultihalSensors::MultihalSensors() + : m_qemuSensorsFd(qemud_channel_open("sensors")) + , m_batchInfo(getSensorNumber()) { if (!m_qemuSensorsFd.ok()) { ALOGE("%s:%d: m_qemuSensorsFd is not opened", __func__, __LINE__); ::abort(); @@ -56,13 +58,16 @@ MultihalSensors::MultihalSensors() : m_qemuSensorsFd(qemud_channel_open("sensors ::abort(); } buffer[len] = 0; - uint32_t availableSensorsMask = 0; - if (sscanf(buffer, "%u", &availableSensorsMask) != 1) { + uint32_t hostSensorsMask = 0; + if (sscanf(buffer, "%u", &hostSensorsMask) != 1) { ALOGE("%s:%d: Can't parse qemud response", __func__, __LINE__); ::abort(); } m_availableSensorsMask = - availableSensorsMask & ((1u << getSensorNumber()) - 1); + hostSensorsMask & ((1u << getSensorNumber()) - 1); + + ALOGI("%s:%d: host sensors mask=%x, available sensors mask=%x", + __func__, __LINE__, hostSensorsMask, m_availableSensorsMask); if (!::android::base::Socketpair(AF_LOCAL, SOCK_STREAM, 0, &m_callersFd, &m_sensorThreadFd)) { @@ -70,11 +75,17 @@ MultihalSensors::MultihalSensors() : m_qemuSensorsFd(qemud_channel_open("sensors ::abort(); } - m_sensorThread = std::thread(qemuSensorListenerThreadStart, this); + m_sensorThread = std::thread(&MultihalSensors::qemuSensorListenerThread, this); + m_batchThread = std::thread(&MultihalSensors::batchThread, this); } MultihalSensors::~MultihalSensors() { - disableAllSensors(); + setAllQemuSensors(false); + + m_batchRunning = false; + m_batchUpdated.notify_one(); + m_batchThread.join(); + qemuSensorThreadSendCommand(kCMD_QUIT); m_sensorThread.join(); } @@ -104,7 +115,7 @@ Return<void> MultihalSensors::getSensorsList_2_1(getSensorsList_2_1_cb _hidl_cb) } Return<Result> MultihalSensors::setOperationMode(const OperationMode mode) { - std::unique_lock<std::mutex> lock(m_apiMtx); + std::unique_lock<std::mutex> lock(m_mtx); if (m_activeSensorsMask) { return Result::INVALID_OPERATION; @@ -120,25 +131,31 @@ Return<Result> MultihalSensors::activate(const int32_t sensorHandle, return Result::BAD_VALUE; } - std::unique_lock<std::mutex> lock(m_apiMtx); + std::unique_lock<std::mutex> lock(m_mtx); + BatchInfo& batchInfo = m_batchInfo[sensorHandle]; - uint32_t newActiveMask; if (enabled) { - newActiveMask = m_activeSensorsMask | (1u << sensorHandle); - } else { - newActiveMask = m_activeSensorsMask & ~(1u << sensorHandle); - } - if (m_activeSensorsMask == newActiveMask) { - return Result::OK; - } - - if (m_opMode == OperationMode::NORMAL) { - if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), sensorHandle, enabled)) { - return Result::INVALID_OPERATION; + const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle); + LOG_ALWAYS_FATAL_IF(!sensor); + if (!(sensor->flags & static_cast<uint32_t>(SensorFlagBits::ON_CHANGE_MODE))) { + if (batchInfo.samplingPeriodNs <= 0) { + return Result::BAD_VALUE; + } + + BatchEventRef batchEventRef; + batchEventRef.timestamp = + ::android::elapsedRealtimeNano() + batchInfo.samplingPeriodNs; + batchEventRef.sensorHandle = sensorHandle; + batchEventRef.generation = ++batchInfo.generation; + + m_batchQueue.push(batchEventRef); + m_batchUpdated.notify_one(); } - } - m_activeSensorsMask = newActiveMask; + m_activeSensorsMask = m_activeSensorsMask | (1u << sensorHandle); + } else { + m_activeSensorsMask = m_activeSensorsMask & ~(1u << sensorHandle); + } return Result::OK; } @@ -147,26 +164,35 @@ Return<Result> MultihalSensors::batch(const int32_t sensorHandle, const int64_t maxReportLatencyNs) { (void)maxReportLatencyNs; + if (!isSensorHandleValid(sensorHandle)) { + return Result::BAD_VALUE; + } + const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle); - if (sensor) { - if (samplingPeriodNs >= sensor->minDelay) { - return Result::OK; - } else { - return Result::BAD_VALUE; - } - } else { + LOG_ALWAYS_FATAL_IF(!sensor); + + if (samplingPeriodNs < sensor->minDelay) { return Result::BAD_VALUE; } + + std::unique_lock<std::mutex> lock(m_mtx); + if (m_opMode == OperationMode::NORMAL) { + m_batchInfo[sensorHandle].samplingPeriodNs = samplingPeriodNs; + } + + return Result::OK; } Return<Result> MultihalSensors::flush(const int32_t sensorHandle) { - const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle); - if (!sensor) { + if (!isSensorHandleValid(sensorHandle)) { return Result::BAD_VALUE; } - std::unique_lock<std::mutex> lock(m_apiMtx); - if (!(m_activeSensorsMask & (1u << sensorHandle))) { + const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle); + LOG_ALWAYS_FATAL_IF(!sensor); + + std::unique_lock<std::mutex> lock(m_mtx); + if (!isSensorActive(sensorHandle)) { return Result::BAD_VALUE; } @@ -175,48 +201,58 @@ Return<Result> MultihalSensors::flush(const int32_t sensorHandle) { event.sensorType = SensorType::META_DATA; event.u.meta.what = MetaDataEventType::META_DATA_FLUSH_COMPLETE; - postSensorEventLocked(event); + doPostSensorEventLocked(*sensor, event); return Result::OK; } Return<Result> MultihalSensors::injectSensorData_2_1(const Event& event) { + if (!isSensorHandleValid(event.sensorHandle)) { + return Result::BAD_VALUE; + } if (event.sensorType == SensorType::ADDITIONAL_INFO) { return Result::OK; } - std::unique_lock<std::mutex> lock(m_apiMtx); + std::unique_lock<std::mutex> lock(m_mtx); if (m_opMode != OperationMode::DATA_INJECTION) { return Result::INVALID_OPERATION; } const SensorInfo* sensor = getSensorInfoByHandle(event.sensorHandle); - if (!sensor) { - return Result::BAD_VALUE; - } + LOG_ALWAYS_FATAL_IF(!sensor); if (sensor->type != event.sensorType) { return Result::BAD_VALUE; } - postSensorEventLocked(event); + doPostSensorEventLocked(*sensor, event); return Result::OK; } Return<Result> MultihalSensors::initialize(const sp<IHalProxyCallback>& halProxyCallback) { - std::unique_lock<std::mutex> lock(m_apiMtx); - disableAllSensors(); + std::unique_lock<std::mutex> lock(m_mtx); + setAllQemuSensors(true); // we need to start sampling sensors for batching m_opMode = OperationMode::NORMAL; m_halProxyCallback = halProxyCallback; return Result::OK; } void MultihalSensors::postSensorEvent(const Event& event) { - std::unique_lock<std::mutex> lock(m_apiMtx); - postSensorEventLocked(event); + const SensorInfo* sensor = getSensorInfoByHandle(event.sensorHandle); + LOG_ALWAYS_FATAL_IF(!sensor); + + std::unique_lock<std::mutex> lock(m_mtx); + if (sensor->flags & static_cast<uint32_t>(SensorFlagBits::ON_CHANGE_MODE)) { + if (isSensorActive(event.sensorHandle)) { + doPostSensorEventLocked(*sensor, event); + } + } else { // CONTINUOUS_MODE + m_batchInfo[event.sensorHandle].event = event; + } } -void MultihalSensors::postSensorEventLocked(const Event& event) { +void MultihalSensors::doPostSensorEventLocked(const SensorInfo& sensor, + const Event& event) { const bool isWakeupEvent = - getSensorInfoByHandle(event.sensorHandle)->flags & - static_cast<uint32_t>(SensorFlagBits::WAKE_UP); + sensor.flags & static_cast<uint32_t>(SensorFlagBits::WAKE_UP); m_halProxyCallback->postEvents( {event}, @@ -227,6 +263,65 @@ bool MultihalSensors::qemuSensorThreadSendCommand(const char cmd) const { return TEMP_FAILURE_RETRY(write(m_callersFd.get(), &cmd, 1)) == 1; } +bool MultihalSensors::isSensorHandleValid(int sensorHandle) const { + if (!goldfish::isSensorHandleValid(sensorHandle)) { + return false; + } + + if (!(m_availableSensorsMask & (1u << sensorHandle))) { + return false; + } + + return true; +} + +void MultihalSensors::batchThread() { + using high_resolution_clock = std::chrono::high_resolution_clock; + + while (m_batchRunning) { + std::unique_lock<std::mutex> lock(m_mtx); + if (m_batchQueue.empty()) { + m_batchUpdated.wait(lock); + } else { + const int64_t t = m_batchQueue.top().timestamp; + const auto d = std::chrono::nanoseconds(t); + high_resolution_clock::time_point waitUntil(d); + m_batchUpdated.wait_until(lock, waitUntil); + } + + const int64_t nowNs = ::android::elapsedRealtimeNano(); + while (!m_batchQueue.empty() && (nowNs >= m_batchQueue.top().timestamp)) { + BatchEventRef evRef = m_batchQueue.top(); + m_batchQueue.pop(); + + const int sensorHandle = evRef.sensorHandle; + LOG_ALWAYS_FATAL_IF(!goldfish::isSensorHandleValid(sensorHandle)); + if (!isSensorActive(sensorHandle)) { + continue; + } + + BatchInfo &batchInfo = m_batchInfo[sensorHandle]; + if (batchInfo.event.sensorType == SensorType::META_DATA) { + ALOGW("%s:%d the host has not provided value yet for sensorHandle=%d", + __func__, __LINE__, sensorHandle); + } else { + batchInfo.event.timestamp = evRef.timestamp; + const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle); + LOG_ALWAYS_FATAL_IF(!sensor); + doPostSensorEventLocked(*sensor, batchInfo.event); + } + + if (evRef.generation == batchInfo.generation) { + const int64_t samplingPeriodNs = batchInfo.samplingPeriodNs; + LOG_ALWAYS_FATAL_IF(samplingPeriodNs <= 0); + + evRef.timestamp += samplingPeriodNs; + m_batchQueue.push(evRef); + } + } + } +} + /// not supported ////////////////////////////////////////////////////////////// Return<void> MultihalSensors::registerDirectChannel(const SharedMemInfo& mem, registerDirectChannel_cb _hidl_cb) { diff --git a/sensors/multihal_sensors.h b/sensors/multihal_sensors.h index 80ed6a3b..382af7f7 100644 --- a/sensors/multihal_sensors.h +++ b/sensors/multihal_sensors.h @@ -17,8 +17,12 @@ #pragma once #include <android-base/unique_fd.h> #include <V2_1/SubHal.h> +#include <atomic> +#include <condition_variable> #include <cstdint> +#include <queue> #include <thread> +#include <vector> namespace goldfish { namespace ahs = ::android::hardware::sensors; @@ -26,6 +30,7 @@ namespace ahs21 = ahs::V2_1; namespace ahs10 = ahs::V1_0; using ahs21::implementation::IHalProxyCallback; +using ahs21::SensorInfo; using ahs21::Event; using ahs10::OperationMode; using ahs10::RateLevel; @@ -81,14 +86,18 @@ private: float lastHingeAngle2Value = kSensorNoValue; }; + bool isSensorHandleValid(int sensorHandle) const; + bool isSensorActive(int sensorHandle) const { + return m_activeSensorsMask & (1u << sensorHandle); // m_mtx required + } static bool activateQemuSensorImpl(int pipe, int sensorHandle, bool enabled); - bool disableAllSensors(); + bool setAllQemuSensors(bool enabled); void parseQemuSensorEvent(const int pipe, QemuSensorsProtocolState* state); void postSensorEvent(const Event& event); - void postSensorEventLocked(const Event& event); + void doPostSensorEventLocked(const SensorInfo& sensor, const Event& event); void qemuSensorListenerThread(); - static void qemuSensorListenerThreadStart(MultihalSensors* that); + void batchThread(); static constexpr char kCMD_QUIT = 'q'; bool qemuSensorThreadSendCommand(char cmd) const; @@ -105,7 +114,32 @@ private: uint32_t m_activeSensorsMask = 0; OperationMode m_opMode = OperationMode::NORMAL; sp<IHalProxyCallback> m_halProxyCallback; - mutable std::mutex m_apiMtx; + + // batching + struct BatchEventRef { + int64_t timestamp = -1; + int sensorHandle = -1; + int generation = 0; + + bool operator<(const BatchEventRef &rhs) const { + // not a typo, we want m_batchQueue.top() to be the smallest timestamp + return timestamp > rhs.timestamp; + } + }; + + struct BatchInfo { + Event event; + int64_t samplingPeriodNs = 0; + int generation = 0; + }; + + std::priority_queue<BatchEventRef> m_batchQueue; + std::vector<BatchInfo> m_batchInfo; + std::condition_variable m_batchUpdated; + std::thread m_batchThread; + std::atomic<bool> m_batchRunning = true; + + mutable std::mutex m_mtx; }; } // namespace goldfish diff --git a/sensors/multihal_sensors_epoll.cpp b/sensors/multihal_sensors_epoll.cpp index 0be12245..cf2ae56b 100644 --- a/sensors/multihal_sensors_epoll.cpp +++ b/sensors/multihal_sensors_epoll.cpp @@ -38,10 +38,6 @@ int qemuSensortThreadRcvCommand(const int fd) { } } // namespace -void MultihalSensors::qemuSensorListenerThreadStart(MultihalSensors* that) { - that->qemuSensorListenerThread(); -} - void MultihalSensors::qemuSensorListenerThread() { const unique_fd epollFd(epoll_create1(0)); if (!epollFd.ok()) { diff --git a/sensors/multihal_sensors_qemu.cpp b/sensors/multihal_sensors_qemu.cpp index a07d569b..f5da3a15 100644 --- a/sensors/multihal_sensors_qemu.cpp +++ b/sensors/multihal_sensors_qemu.cpp @@ -69,19 +69,16 @@ bool MultihalSensors::activateQemuSensorImpl(const int pipe, } } -bool MultihalSensors::disableAllSensors() { - if (m_opMode == OperationMode::NORMAL) { - uint32_t mask = m_activeSensorsMask; - for (int i = 0; mask; ++i, mask >>= 1) { - if (mask & 1) { - if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), i, false)) { - return false; - } +bool MultihalSensors::setAllQemuSensors(const bool enabled) { + uint32_t mask = m_availableSensorsMask; + for (int i = 0; mask; ++i, mask >>= 1) { + if (mask & 1) { + if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), i, enabled)) { + return false; } } } - m_activeSensorsMask = 0; return true; } diff --git a/sensors/sensor_list.cpp b/sensors/sensor_list.cpp index 245379fa..d228ba62 100644 --- a/sensors/sensor_list.cpp +++ b/sensors/sensor_list.cpp @@ -122,11 +122,11 @@ const SensorInfo kAllSensors[] = { .maxRange = 80.0, .resolution = 1.0, .power = 0.0, - .minDelay = 10000, + .minDelay = 0, .fifoReservedEventCount = 0, .fifoMaxEventCount = 0, .requiredPermission = "", - .maxDelay = 500000, + .maxDelay = 0, .flags = SensorFlagBits::DATA_INJECTION | SensorFlagBits::ON_CHANGE_MODE }, @@ -140,11 +140,11 @@ const SensorInfo kAllSensors[] = { .maxRange = 1.0, .resolution = 1.0, .power = 20.0, - .minDelay = 10000, + .minDelay = 0, .fifoReservedEventCount = 0, .fifoMaxEventCount = 0, .requiredPermission = "", - .maxDelay = 500000, + .maxDelay = 0, .flags = SensorFlagBits::DATA_INJECTION | SensorFlagBits::ON_CHANGE_MODE | SensorFlagBits::WAKE_UP @@ -159,11 +159,11 @@ const SensorInfo kAllSensors[] = { .maxRange = 40000.0, .resolution = 1.0, .power = 20.0, - .minDelay = 10000, + .minDelay = 0, .fifoReservedEventCount = 0, .fifoMaxEventCount = 0, .requiredPermission = "", - .maxDelay = 500000, + .maxDelay = 0, .flags = SensorFlagBits::DATA_INJECTION | SensorFlagBits::ON_CHANGE_MODE }, @@ -195,11 +195,11 @@ const SensorInfo kAllSensors[] = { .maxRange = 100.0, .resolution = 1.0, .power = 20.0, - .minDelay = 10000, + .minDelay = 0, .fifoReservedEventCount = 0, .fifoMaxEventCount = 0, .requiredPermission = "", - .maxDelay = 500000, + .maxDelay = 0, .flags = SensorFlagBits::DATA_INJECTION | SensorFlagBits::ON_CHANGE_MODE }, @@ -248,11 +248,11 @@ const SensorInfo kAllSensors[] = { .maxRange = 360, .resolution = 1.0, .power = 3.0, - .minDelay = 10000, + .minDelay = 0, .fifoReservedEventCount = 0, .fifoMaxEventCount = 0, .requiredPermission = "", - .maxDelay = 500000, + .maxDelay = 0, .flags = SensorFlagBits::DATA_INJECTION | SensorFlagBits::ON_CHANGE_MODE | SensorFlagBits::WAKE_UP @@ -267,11 +267,11 @@ const SensorInfo kAllSensors[] = { .maxRange = 360, .resolution = 1.0, .power = 3.0, - .minDelay = 10000, + .minDelay = 0, .fifoReservedEventCount = 0, .fifoMaxEventCount = 0, .requiredPermission = "", - .maxDelay = 500000, + .maxDelay = 0, .flags = SensorFlagBits::DATA_INJECTION | SensorFlagBits::ON_CHANGE_MODE | SensorFlagBits::WAKE_UP @@ -286,11 +286,11 @@ const SensorInfo kAllSensors[] = { .maxRange = 360, .resolution = 1.0, .power = 3.0, - .minDelay = 10000, + .minDelay = 0, .fifoReservedEventCount = 0, .fifoMaxEventCount = 0, .requiredPermission = "", - .maxDelay = 500000, + .maxDelay = 0, .flags = SensorFlagBits::DATA_INJECTION | SensorFlagBits::ON_CHANGE_MODE | SensorFlagBits::WAKE_UP |