Implements HidlSensorHalWrapper (24804dcd) · Commits · e / os / platform_frameworks_native

services/sensorservice/Android.bp

+1 −0

Original line number	Diff line number	Diff line
		@@ -15,6 +15,7 @@ cc_library_shared {
		"CorrectedGyroSensor.cpp",
		"Fusion.cpp",
		"GravitySensor.cpp",
		"HidlSensorHalWrapper.cpp",
		"LinearAccelerationSensor.cpp",
		"OrientationSensor.cpp",
		"RecentEventLogger.cpp",

services/sensorservice/HidlSensorHalWrapper.cpp

0 → 100644

+585 −0

Original line number	Diff line number	Diff line
		/*
		* Copyright (C) 2021 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 "HidlSensorHalWrapper.h"
		#include "android/hardware/sensors/2.0/types.h"
		#include "android/hardware/sensors/2.1/ISensorsCallback.h"
		#include "android/hardware/sensors/2.1/types.h"
		#include "convertV2_1.h"

		#include <android-base/logging.h>

		using android::hardware::hidl_vec;
		using android::hardware::sensors::V1_0::RateLevel;
		using android::hardware::sensors::V1_0::Result;
		using android::hardware::sensors::V1_0::SharedMemFormat;
		using android::hardware::sensors::V1_0::SharedMemInfo;
		using android::hardware::sensors::V1_0::SharedMemType;
		using android::hardware::sensors::V2_0::EventQueueFlagBits;
		using android::hardware::sensors::V2_0::WakeLockQueueFlagBits;
		using android::hardware::sensors::V2_1::Event;
		using android::hardware::sensors::V2_1::ISensorsCallback;
		using android::hardware::sensors::V2_1::implementation::convertFromSensorEvent;
		using android::hardware::sensors::V2_1::implementation::convertToNewEvents;
		using android::hardware::sensors::V2_1::implementation::convertToNewSensorInfos;
		using android::hardware::sensors::V2_1::implementation::convertToSensor;
		using android::hardware::sensors::V2_1::implementation::ISensorsWrapperV1_0;
		using android::hardware::sensors::V2_1::implementation::ISensorsWrapperV2_0;
		using android::hardware::sensors::V2_1::implementation::ISensorsWrapperV2_1;

		namespace android {

		namespace {

		status_t statusFromResult(Result result) {
		switch (result) {
		case Result::OK:
		return OK;
		case Result::BAD_VALUE:
		return BAD_VALUE;
		case Result::PERMISSION_DENIED:
		return PERMISSION_DENIED;
		case Result::INVALID_OPERATION:
		return INVALID_OPERATION;
		case Result::NO_MEMORY:
		return NO_MEMORY;
		}
		}

		template <typename EnumType>
		constexpr typename std::underlying_type<EnumType>::type asBaseType(EnumType value) {
		return static_cast<typename std::underlying_type<EnumType>::type>(value);
		}

		enum EventQueueFlagBitsInternal : uint32_t {
		INTERNAL_WAKE = 1 << 16,
		};

		} // anonymous namespace

		void SensorsHalDeathReceiver::serviceDied(
		uint64_t /* cookie /, const wp<::android::hidl::base::V1_0::IBase>& / service */) {
		ALOGW("Sensors HAL died, attempting to reconnect.");
		mHidlSensorHalWrapper->prepareForReconnect();
		}

		struct SensorsCallback : public ISensorsCallback {
		using Result = ::android::hardware::sensors::V1_0::Result;
		using SensorInfo = ::android::hardware::sensors::V2_1::SensorInfo;

		SensorsCallback(ISensorHalWrapper::SensorDeviceCallback* sensorDeviceCallback) {
		mSensorDeviceCallback = sensorDeviceCallback;
		}

		Return<void> onDynamicSensorsConnected_2_1(
		const hidl_vec<SensorInfo>& dynamicSensorsAdded) override {
		std::vector<sensor_t> sensors;
		for (const android::hardware::sensors::V2_1::SensorInfo& info : dynamicSensorsAdded) {
		sensor_t sensor;
		convertToSensor(info, &sensor);
		sensors.push_back(sensor);
		}

		mSensorDeviceCallback->onDynamicSensorsConnected(sensors);
		return Return<void>();
		}

		Return<void> onDynamicSensorsConnected(
		const hidl_vec<android::hardware::sensors::V1_0::SensorInfo>& dynamicSensorsAdded)
		override {
		return onDynamicSensorsConnected_2_1(convertToNewSensorInfos(dynamicSensorsAdded));
		}

		Return<void> onDynamicSensorsDisconnected(
		const hidl_vec<int32_t>& dynamicSensorHandlesRemoved) override {
		mSensorDeviceCallback->onDynamicSensorsDisconnected(dynamicSensorHandlesRemoved);
		return Return<void>();
		}

		private:
		ISensorHalWrapper::SensorDeviceCallback* mSensorDeviceCallback;
		};

		bool HidlSensorHalWrapper::supportsPolling() {
		return mSensors->supportsPolling();
		}

		bool HidlSensorHalWrapper::supportsMessageQueues() {
		return mSensors->supportsMessageQueues();
		}

		bool HidlSensorHalWrapper::connect(SensorDeviceCallback* callback) {
		mSensorDeviceCallback = callback;
		bool ret = connectHidlService();
		if (mEventQueueFlag != nullptr) {
		mEventQueueFlag->wake(asBaseType(INTERNAL_WAKE));
		}
		return ret;
		}

		void HidlSensorHalWrapper::prepareForReconnect() {
		mReconnecting = true;
		if (mEventQueueFlag != nullptr) {
		mEventQueueFlag->wake(asBaseType(INTERNAL_WAKE));
		}
		}

		ssize_t HidlSensorHalWrapper::poll(sensors_event_t* buffer, size_t count) {
		ssize_t err;
		int numHidlTransportErrors = 0;
		bool hidlTransportError = false;

		do {
		auto ret = mSensors->poll(
		count, [&](auto result, const auto& events, const auto& dynamicSensorsAdded) {
		if (result == Result::OK) {
		convertToSensorEventsAndQuantize(convertToNewEvents(events),
		convertToNewSensorInfos(
		dynamicSensorsAdded),
		buffer);
		err = (ssize_t)events.size();
		} else {
		err = statusFromResult(result);
		}
		});

		if (ret.isOk()) {
		hidlTransportError = false;
		} else {
		hidlTransportError = true;
		numHidlTransportErrors++;
		if (numHidlTransportErrors > 50) {
		// Log error and bail
		ALOGE("Max Hidl transport errors this cycle : %d", numHidlTransportErrors);
		handleHidlDeath(ret.description());
		} else {
		std::this_thread::sleep_for(std::chrono::milliseconds(10));
		}
		}
		} while (hidlTransportError);

		if (numHidlTransportErrors > 0) {
		ALOGE("Saw %d Hidl transport failures", numHidlTransportErrors);
		HidlTransportErrorLog errLog(time(nullptr), numHidlTransportErrors);
		mHidlTransportErrors.add(errLog);
		mTotalHidlTransportErrors++;
		}

		return err;
		}

		ssize_t HidlSensorHalWrapper::pollFmq(sensors_event_t* buffer, size_t maxNumEventsToRead) {
		ssize_t eventsRead = 0;
		size_t availableEvents = mSensors->getEventQueue()->availableToRead();

		if (availableEvents == 0) {
		uint32_t eventFlagState = 0;

		// Wait for events to become available. This is necessary so that the Event FMQ's read() is
		// able to be called with the correct number of events to read. If the specified number of
		// events is not available, then read() would return no events, possibly introducing
		// additional latency in delivering events to applications.
		if (mEventQueueFlag != nullptr) {
		mEventQueueFlag->wait(asBaseType(EventQueueFlagBits::READ_AND_PROCESS) \|
		asBaseType(INTERNAL_WAKE),
		&eventFlagState);
		}
		availableEvents = mSensors->getEventQueue()->availableToRead();

		if ((eventFlagState & asBaseType(INTERNAL_WAKE)) && mReconnecting) {
		ALOGD("Event FMQ internal wake, returning from poll with no events");
		return DEAD_OBJECT;
		}
		}

		size_t eventsToRead = std::min({availableEvents, maxNumEventsToRead, mEventBuffer.size()});
		if (eventsToRead > 0) {
		if (mSensors->getEventQueue()->read(mEventBuffer.data(), eventsToRead)) {
		// Notify the Sensors HAL that sensor events have been read. This is required to support
		// the use of writeBlocking by the Sensors HAL.
		if (mEventQueueFlag != nullptr) {
		mEventQueueFlag->wake(asBaseType(EventQueueFlagBits::EVENTS_READ));
		}

		for (size_t i = 0; i < eventsToRead; i++) {
		convertToSensorEvent(mEventBuffer[i], &buffer[i]);
		android::SensorDeviceUtils::quantizeSensorEventValues(&buffer[i],
		getResolutionForSensor(
		buffer[i].sensor));
		}
		eventsRead = eventsToRead;
		} else {
		ALOGW("Failed to read %zu events, currently %zu events available", eventsToRead,
		availableEvents);
		}
		}

		return eventsRead;
		}

		std::vector<sensor_t> HidlSensorHalWrapper::getSensorsList() {
		std::vector<sensor_t> sensorsFound;
		if (mSensors != nullptr) {
		checkReturn(mSensors->getSensorsList([&](const auto& list) {
		for (size_t i = 0; i < list.size(); i++) {
		sensor_t sensor;
		convertToSensor(list[i], &sensor);
		sensorsFound.push_back(sensor);

		// Only disable all sensors on HAL 1.0 since HAL 2.0
		// handles this in its initialize method
		if (!mSensors->supportsMessageQueues()) {
		checkReturn(mSensors->activate(list[i].sensorHandle, 0 /* enabled */));
		}
		}
		}));
		}

		return sensorsFound;
		}

		status_t HidlSensorHalWrapper::setOperationMode(SensorService::Mode mode) {
		if (mSensors == nullptr) return NO_INIT;
		return checkReturnAndGetStatus(
		mSensors->setOperationMode(static_cast<hardware::sensors::V1_0::OperationMode>(mode)));
		}

		status_t HidlSensorHalWrapper::activate(int32_t sensorHandle, bool enabled) {
		if (mSensors == nullptr) return NO_INIT;
		return checkReturnAndGetStatus(mSensors->activate(sensorHandle, enabled));
		}

		status_t HidlSensorHalWrapper::batch(int32_t sensorHandle, int64_t samplingPeriodNs,
		int64_t maxReportLatencyNs) {
		if (mSensors == nullptr) return NO_INIT;
		return checkReturnAndGetStatus(
		mSensors->batch(sensorHandle, samplingPeriodNs, maxReportLatencyNs));
		}

		status_t HidlSensorHalWrapper::flush(int32_t sensorHandle) {
		if (mSensors == nullptr) return NO_INIT;
		return checkReturnAndGetStatus(mSensors->flush(sensorHandle));
		}

		status_t HidlSensorHalWrapper::injectSensorData(const sensors_event_t* event) {
		if (mSensors == nullptr) return NO_INIT;

		Event ev;
		convertFromSensorEvent(*event, &ev);
		return checkReturnAndGetStatus(mSensors->injectSensorData(ev));
		}

		status_t HidlSensorHalWrapper::registerDirectChannel(const sensors_direct_mem_t* memory,
		int32_t* /channelHandle/) {
		if (mSensors == nullptr) return NO_INIT;

		SharedMemType type;
		switch (memory->type) {
		case SENSOR_DIRECT_MEM_TYPE_ASHMEM:
		type = SharedMemType::ASHMEM;
		break;
		case SENSOR_DIRECT_MEM_TYPE_GRALLOC:
		type = SharedMemType::GRALLOC;
		break;
		default:
		return BAD_VALUE;
		}

		SharedMemFormat format;
		if (memory->format != SENSOR_DIRECT_FMT_SENSORS_EVENT) {
		return BAD_VALUE;
		}
		format = SharedMemFormat::SENSORS_EVENT;

		SharedMemInfo mem = {
		.type = type,
		.format = format,
		.size = static_cast<uint32_t>(memory->size),
		.memoryHandle = memory->handle,
		};

		status_t ret;
		checkReturn(mSensors->registerDirectChannel(mem, [&ret](auto result, auto channelHandle) {
		if (result == Result::OK) {
		ret = channelHandle;
		} else {
		ret = statusFromResult(result);
		}
		}));
		return ret;
		}

		status_t HidlSensorHalWrapper::unregisterDirectChannel(int32_t channelHandle) {
		if (mSensors == nullptr) return NO_INIT;
		return checkReturnAndGetStatus(mSensors->unregisterDirectChannel(channelHandle));
		}

		status_t HidlSensorHalWrapper::configureDirectChannel(int32_t sensorHandle, int32_t channelHandle,
		const struct sensors_direct_cfg_t* config) {
		if (mSensors == nullptr) return NO_INIT;

		RateLevel rate;
		switch (config->rate_level) {
		case SENSOR_DIRECT_RATE_STOP:
		rate = RateLevel::STOP;
		break;
		case SENSOR_DIRECT_RATE_NORMAL:
		rate = RateLevel::NORMAL;
		break;
		case SENSOR_DIRECT_RATE_FAST:
		rate = RateLevel::FAST;
		break;
		case SENSOR_DIRECT_RATE_VERY_FAST:
		rate = RateLevel::VERY_FAST;
		break;
		default:
		return BAD_VALUE;
		}

		status_t ret;
		checkReturn(mSensors->configDirectReport(sensorHandle, channelHandle, rate,
		[&ret, rate](auto result, auto token) {
		if (rate == RateLevel::STOP) {
		ret = statusFromResult(result);
		} else {
		if (result == Result::OK) {
		ret = token;
		} else {
		ret = statusFromResult(result);
		}
		}
		}));

		return ret;
		}

		void HidlSensorHalWrapper::writeWakeLockHandled(uint32_t count) {
		if (mWakeLockQueue->write(&count)) {
		mWakeLockQueueFlag->wake(asBaseType(WakeLockQueueFlagBits::DATA_WRITTEN));
		} else {
		ALOGW("Failed to write wake lock handled");
		}
		}

		void HidlSensorHalWrapper::onDynamicSensorsConnected(
		const std::vector<sensor_t>& dynamicSensorsAdded) {
		std::unique_lock<std::mutex> lock(mDynamicSensorsMutex);

		// Allocate a sensor_t structure for each dynamic sensor added and insert
		// it into the dictionary of connected dynamic sensors keyed by handle.
		for (size_t i = 0; i < dynamicSensorsAdded.size(); ++i) {
		const sensor_t& sensor = dynamicSensorsAdded[i];

		auto it = mConnectedDynamicSensors.find(sensor.handle);
		CHECK(it == mConnectedDynamicSensors.end());

		mConnectedDynamicSensors.insert(std::make_pair(sensor.handle, sensor));
		}

		mDynamicSensorsCv.notify_all();
		}

		void HidlSensorHalWrapper::onDynamicSensorsDisconnected(
		const std::vector<int32_t>& /dynamicSensorHandlesRemoved/) {
		// TODO: Currently dynamic sensors do not seem to be removed
		}

		status_t HidlSensorHalWrapper::checkReturnAndGetStatus(const hardware::Return<Result>& ret) {
		checkReturn(ret);
		return (!ret.isOk()) ? DEAD_OBJECT : statusFromResult(ret);
		}

		void HidlSensorHalWrapper::handleHidlDeath(const std::string& detail) {
		if (!mSensors->supportsMessageQueues()) {
		// restart is the only option at present.
		LOG_ALWAYS_FATAL("Abort due to ISensors hidl service failure, detail: %s.", detail.c_str());
		} else {
		ALOGD("ISensors HAL died, death recipient will attempt reconnect");
		}
		}

		bool HidlSensorHalWrapper::connectHidlService() {
		HalConnectionStatus status = connectHidlServiceV2_1();
		if (status == HalConnectionStatus::DOES_NOT_EXIST) {
		status = connectHidlServiceV2_0();
		}

		if (status == HalConnectionStatus::DOES_NOT_EXIST) {
		status = connectHidlServiceV1_0();
		}
		return (status == HalConnectionStatus::CONNECTED);
		}

		ISensorHalWrapper::HalConnectionStatus HidlSensorHalWrapper::connectHidlServiceV1_0() {
		// SensorDevice will wait for HAL service to start if HAL is declared in device manifest.
		size_t retry = 10;
		HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;

		while (retry-- > 0) {
		sp<android::hardware::sensors::V1_0::ISensors> sensors =
		android::hardware::sensors::V1_0::ISensors::getService();
		if (sensors == nullptr) {
		// no sensor hidl service found
		connectionStatus = HalConnectionStatus::DOES_NOT_EXIST;
		break;
		}

		mSensors = new ISensorsWrapperV1_0(sensors);
		mRestartWaiter->reset();
		// Poke ISensor service. If it has lingering connection from previous generation of
		// system server, it will kill itself. There is no intention to handle the poll result,
		// which will be done since the size is 0.
		if (mSensors->poll(0, [](auto, const auto&, const auto&) {}).isOk()) {
		// ok to continue
		connectionStatus = HalConnectionStatus::CONNECTED;
		break;
		}

		// hidl service is restarting, pointer is invalid.
		mSensors = nullptr;
		connectionStatus = HalConnectionStatus::FAILED_TO_CONNECT;
		ALOGI("%s unsuccessful, remaining retry %zu.", __FUNCTION__, retry);
		mRestartWaiter->wait();
		}

		return connectionStatus;
		}

		ISensorHalWrapper::HalConnectionStatus HidlSensorHalWrapper::connectHidlServiceV2_0() {
		HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;
		sp<android::hardware::sensors::V2_0::ISensors> sensors =
		android::hardware::sensors::V2_0::ISensors::getService();

		if (sensors == nullptr) {
		connectionStatus = HalConnectionStatus::DOES_NOT_EXIST;
		} else {
		mSensors = new ISensorsWrapperV2_0(sensors);
		connectionStatus = initializeHidlServiceV2_X();
		}

		return connectionStatus;
		}

		ISensorHalWrapper::HalConnectionStatus HidlSensorHalWrapper::connectHidlServiceV2_1() {
		HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;
		sp<android::hardware::sensors::V2_1::ISensors> sensors =
		android::hardware::sensors::V2_1::ISensors::getService();

		if (sensors == nullptr) {
		connectionStatus = HalConnectionStatus::DOES_NOT_EXIST;
		} else {
		mSensors = new ISensorsWrapperV2_1(sensors);
		connectionStatus = initializeHidlServiceV2_X();
		}

		return connectionStatus;
		}

		ISensorHalWrapper::HalConnectionStatus HidlSensorHalWrapper::initializeHidlServiceV2_X() {
		HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;

		mWakeLockQueue =
		std::make_unique<WakeLockQueue>(SensorEventQueue::MAX_RECEIVE_BUFFER_EVENT_COUNT,
		true /* configureEventFlagWord */);

		hardware::EventFlag::deleteEventFlag(&mEventQueueFlag);
		hardware::EventFlag::createEventFlag(mSensors->getEventQueue()->getEventFlagWord(),
		&mEventQueueFlag);

		hardware::EventFlag::deleteEventFlag(&mWakeLockQueueFlag);
		hardware::EventFlag::createEventFlag(mWakeLockQueue->getEventFlagWord(), &mWakeLockQueueFlag);

		CHECK(mSensors != nullptr && mWakeLockQueue != nullptr && mEventQueueFlag != nullptr &&
		mWakeLockQueueFlag != nullptr);

		mCallback = new SensorsCallback(mSensorDeviceCallback);
		status_t status =
		checkReturnAndGetStatus(mSensors->initialize(*mWakeLockQueue->getDesc(), mCallback));

		if (status != NO_ERROR) {
		connectionStatus = HalConnectionStatus::FAILED_TO_CONNECT;
		ALOGE("Failed to initialize Sensors HAL (%s)", strerror(-status));
		} else {
		connectionStatus = HalConnectionStatus::CONNECTED;
		mSensorsHalDeathReceiver = new SensorsHalDeathReceiver(this);
		mSensors->linkToDeath(mSensorsHalDeathReceiver, 0 /* cookie */);
		}

		return connectionStatus;
		}

		void HidlSensorHalWrapper::convertToSensorEvent(const Event& src, sensors_event_t* dst) {
		android::hardware::sensors::V2_1::implementation::convertToSensorEvent(src, dst);

		if (src.sensorType == android::hardware::sensors::V2_1::SensorType::DYNAMIC_SENSOR_META) {
		const hardware::sensors::V1_0::DynamicSensorInfo& dyn = src.u.dynamic;

		dst->dynamic_sensor_meta.connected = dyn.connected;
		dst->dynamic_sensor_meta.handle = dyn.sensorHandle;
		if (dyn.connected) {
		std::unique_lock<std::mutex> lock(mDynamicSensorsMutex);
		// Give MAX_DYN_SENSOR_WAIT_SEC for onDynamicSensorsConnected to be invoked since it
		// can be received out of order from this event due to a bug in the HIDL spec that
		// marks it as oneway.
		auto it = mConnectedDynamicSensors.find(dyn.sensorHandle);
		if (it == mConnectedDynamicSensors.end()) {
		mDynamicSensorsCv.wait_for(lock, MAX_DYN_SENSOR_WAIT, [&, dyn] {
		return mConnectedDynamicSensors.find(dyn.sensorHandle) !=
		mConnectedDynamicSensors.end();
		});
		it = mConnectedDynamicSensors.find(dyn.sensorHandle);
		CHECK(it != mConnectedDynamicSensors.end());
		}

		dst->dynamic_sensor_meta.sensor = &it->second;

		memcpy(dst->dynamic_sensor_meta.uuid, dyn.uuid.data(),
		sizeof(dst->dynamic_sensor_meta.uuid));
		}
		}
		}

		void HidlSensorHalWrapper::convertToSensorEventsAndQuantize(
		const hidl_vec<Event>& src, const hidl_vec<SensorInfo>& dynamicSensorsAdded,
		sensors_event_t* dst) {
		if (dynamicSensorsAdded.size() > 0 && mCallback != nullptr) {
		mCallback->onDynamicSensorsConnected_2_1(dynamicSensorsAdded);
		}

		for (size_t i = 0; i < src.size(); ++i) {
		android::hardware::sensors::V2_1::implementation::convertToSensorEvent(src[i], &dst[i]);
		android::SensorDeviceUtils::quantizeSensorEventValues(&dst[i],
		getResolutionForSensor(
		dst[i].sensor));
		}
		}

		float HidlSensorHalWrapper::getResolutionForSensor(int sensorHandle) {
		for (size_t i = 0; i < mSensorList.size(); i++) {
		if (sensorHandle == mSensorList[i].handle) {
		return mSensorList[i].resolution;
		}
		}

		auto it = mConnectedDynamicSensors.find(sensorHandle);
		if (it != mConnectedDynamicSensors.end()) {
		return it->second.resolution;
		}

		return 0;
		}

		} // namespace android