Automatically reconnect sensors (156da080) · Commits · e / os / platform_frameworks_native

services/sensorservice/SensorDevice.cpp

+156 −20

Original line number	Diff line number	Diff line
		@@ -59,11 +59,22 @@ static status_t StatusFromResult(Result result) {
		}
		}

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

		// Used internally by the framework to wake the Event FMQ. These values must start after
		// the last value of EventQueueFlagBits
		enum EventQueueFlagBitsInternal : uint32_t {
		INTERNAL_WAKE = 1 << 16,
		};

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

		struct SensorsCallback : public ISensorsCallback {
		@@ -81,11 +92,20 @@ struct SensorsCallback : public ISensorsCallback {
		};

		SensorDevice::SensorDevice()
		: mHidlTransportErrors(20), mRestartWaiter(new HidlServiceRegistrationWaiter()) {
		: mHidlTransportErrors(20),
		mRestartWaiter(new HidlServiceRegistrationWaiter()),
		mReconnecting(false) {
		if (!connectHidlService()) {
		return;
		}

		initializeSensorList();

		mIsDirectReportSupported =
		(checkReturn(mSensors->unregisterDirectChannel(-1)) != Result::INVALID_OPERATION);
		}

		void SensorDevice::initializeSensorList() {
		float minPowerMa = 0.001; // 1 microAmp

		checkReturn(mSensors->getSensorsList(
		@@ -110,9 +130,6 @@ SensorDevice::SensorDevice()
		checkReturn(mSensors->activate(list[i].sensorHandle, 0 /* enabled */));
		}
		}));

		mIsDirectReportSupported =
		(checkReturn(mSensors->unregisterDirectChannel(-1)) != Result::INVALID_OPERATION);
		}

		SensorDevice::~SensorDevice() {
		@@ -204,6 +221,108 @@ SensorDevice::HalConnectionStatus SensorDevice::connectHidlServiceV2_0() {
		return connectionStatus;
		}

		void SensorDevice::prepareForReconnect() {
		mReconnecting = true;

		// Wake up the polling thread so it returns and allows the SensorService to initiate
		// a reconnect.
		mEventQueueFlag->wake(asBaseType(INTERNAL_WAKE));
		}

		void SensorDevice::reconnect() {
		Mutex::Autolock _l(mLock);
		mSensors = nullptr;

		auto previousActivations = mActivationCount;
		auto previousSensorList = mSensorList;

		mActivationCount.clear();
		mSensorList.clear();

		if (connectHidlServiceV2_0() == HalConnectionStatus::CONNECTED) {
		initializeSensorList();

		if (sensorHandlesChanged(previousSensorList, mSensorList)) {
		LOG_ALWAYS_FATAL("Sensor handles changed, cannot re-enable sensors.");
		} else {
		reactivateSensors(previousActivations);
		}
		}
		mReconnecting = false;
		}

		bool SensorDevice::sensorHandlesChanged(const Vector<sensor_t>& oldSensorList,
		const Vector<sensor_t>& newSensorList) {
		bool didChange = false;

		if (oldSensorList.size() != newSensorList.size()) {
		didChange = true;
		}

		for (size_t i = 0; i < newSensorList.size() && !didChange; i++) {
		bool found = false;
		const sensor_t& newSensor = newSensorList[i];
		for (size_t j = 0; j < oldSensorList.size() && !found; j++) {
		const sensor_t& prevSensor = oldSensorList[j];
		if (prevSensor.handle == newSensor.handle) {
		found = true;
		if (!sensorIsEquivalent(prevSensor, newSensor)) {
		didChange = true;
		}
		}
		}

		if (!found) {
		// Could not find the new sensor in the old list of sensors, the lists must
		// have changed.
		didChange = true;
		}
		}
		return didChange;
		}

		bool SensorDevice::sensorIsEquivalent(const sensor_t& prevSensor, const sensor_t& newSensor) {
		bool equivalent = true;
		if (prevSensor.handle != newSensor.handle \|\|
		(strcmp(prevSensor.vendor, newSensor.vendor) != 0) \|\|
		(strcmp(prevSensor.stringType, newSensor.stringType) != 0) \|\|
		(strcmp(prevSensor.requiredPermission, newSensor.requiredPermission) != 0) \|\|
		(prevSensor.version != newSensor.version) \|\|
		(prevSensor.type != newSensor.type) \|\|
		(std::abs(prevSensor.maxRange - newSensor.maxRange) > 0.001f) \|\|
		(std::abs(prevSensor.resolution - newSensor.resolution) > 0.001f) \|\|
		(std::abs(prevSensor.power - newSensor.power) > 0.001f) \|\|
		(prevSensor.minDelay != newSensor.minDelay) \|\|
		(prevSensor.fifoReservedEventCount != newSensor.fifoReservedEventCount) \|\|
		(prevSensor.fifoMaxEventCount != newSensor.fifoMaxEventCount) \|\|
		(prevSensor.maxDelay != newSensor.maxDelay) \|\|
		(prevSensor.flags != newSensor.flags)) {
		equivalent = false;
		}
		return equivalent;
		}

		void SensorDevice::reactivateSensors(const DefaultKeyedVector<int, Info>& previousActivations) {
		for (size_t i = 0; i < mSensorList.size(); i++) {
		int handle = mSensorList[i].handle;
		ssize_t activationIndex = previousActivations.indexOfKey(handle);
		if (activationIndex < 0 \|\| previousActivations[activationIndex].numActiveClients() <= 0) {
		continue;
		}

		const Info& info = previousActivations[activationIndex];
		for (size_t j = 0; j < info.batchParams.size(); j++) {
		const BatchParams& batchParams = info.batchParams[j];
		status_t res = batchLocked(info.batchParams.keyAt(j), handle, 0 /* flags */,
		batchParams.mTSample, batchParams.mTBatch);

		if (res == NO_ERROR) {
		activateLocked(info.batchParams.keyAt(j), handle, true /* enabled */);
		}
		}
		}
		}

		void SensorDevice::handleDynamicSensorConnection(int handle, bool connected) {
		// not need to check mSensors because this is is only called after successful poll()
		if (connected) {
		@@ -261,6 +380,8 @@ status_t SensorDevice::initCheck() const {
		}

		ssize_t SensorDevice::poll(sensors_event_t* buffer, size_t count) {
		if (mSensors == nullptr) return NO_INIT;

		ssize_t eventsRead = 0;
		if (mSensors->supportsMessageQueues()) {
		eventsRead = pollFmq(buffer, count);
		@@ -274,8 +395,6 @@ ssize_t SensorDevice::poll(sensors_event_t* buffer, size_t count) {
		}

		ssize_t SensorDevice::pollHal(sensors_event_t* buffer, size_t count) {
		if (mSensors == nullptr) return NO_INIT;

		ssize_t err;
		int numHidlTransportErrors = 0;
		bool hidlTransportError = false;
		@@ -320,10 +439,6 @@ ssize_t SensorDevice::pollHal(sensors_event_t* buffer, size_t count) {
		}

		ssize_t SensorDevice::pollFmq(sensors_event_t* buffer, size_t maxNumEventsToRead) {
		if (mSensors == nullptr) {
		return NO_INIT;
		}

		ssize_t eventsRead = 0;
		size_t availableEvents = mEventQueue->availableToRead();

		@@ -334,13 +449,19 @@ ssize_t SensorDevice::pollFmq(sensors_event_t* buffer, size_t maxNumEventsToRead
		// 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.
		mEventQueueFlag->wait(static_cast<uint32_t>(EventQueueFlagBits::READ_AND_PROCESS),
		&eventFlagState);
		mEventQueueFlag->wait(asBaseType(EventQueueFlagBits::READ_AND_PROCESS) \|
		asBaseType(INTERNAL_WAKE), &eventFlagState);
		availableEvents = mEventQueue->availableToRead();

		if (availableEvents == 0) {
		if ((eventFlagState & asBaseType(EventQueueFlagBits::READ_AND_PROCESS)) &&
		availableEvents == 0) {
		ALOGW("Event FMQ wake without any events");
		}

		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()});
		@@ -387,7 +508,8 @@ Return<void> SensorDevice::onDynamicSensorsDisconnected(
		}

		void SensorDevice::writeWakeLockHandled(uint32_t count) {
		if (mSensors->supportsMessageQueues() && !mWakeLockQueue->write(&count)) {
		if (mSensors != nullptr && mSensors->supportsMessageQueues() &&
		!mWakeLockQueue->write(&count)) {
		ALOGW("Failed to write wake lock handled");
		}
		}
		@@ -406,10 +528,15 @@ void SensorDevice::autoDisable(void *ident, int handle) {
		status_t SensorDevice::activate(void* ident, int handle, int enabled) {
		if (mSensors == nullptr) return NO_INIT;

		status_t err(NO_ERROR);
		Mutex::Autolock _l(mLock);
		return activateLocked(ident, handle, enabled);
		}

		status_t SensorDevice::activateLocked(void* ident, int handle, int enabled) {
		bool actuateHardware = false;

		Mutex::Autolock _l(mLock);
		status_t err(NO_ERROR);

		ssize_t activationIndex = mActivationCount.indexOfKey(handle);
		if (activationIndex < 0) {
		ALOGW("Handle %d cannot be found in activation record", handle);
		@@ -509,6 +636,11 @@ status_t SensorDevice::batch(
		ident, handle, flags, samplingPeriodNs, maxBatchReportLatencyNs);

		Mutex::Autolock _l(mLock);
		return batchLocked(ident, handle, flags, samplingPeriodNs, maxBatchReportLatencyNs);
		}

		status_t SensorDevice::batchLocked(void* ident, int handle, int flags, int64_t samplingPeriodNs,
		int64_t maxBatchReportLatencyNs) {
		ssize_t activationIndex = mActivationCount.indexOfKey(handle);
		if (activationIndex < 0) {
		ALOGW("Handle %d cannot be found in activation record", handle);
		@@ -739,7 +871,7 @@ int32_t SensorDevice::configureDirectChannel(int32_t sensorHandle,

		// ---------------------------------------------------------------------------

		int SensorDevice::Info::numActiveClients() {
		int SensorDevice::Info::numActiveClients() const {
		SensorDevice& device(SensorDevice::getInstance());
		int num = 0;
		for (size_t i = 0; i < batchParams.size(); ++i) {
		@@ -838,8 +970,12 @@ void SensorDevice::convertToSensorEvents(
		}

		void SensorDevice::handleHidlDeath(const std::string & detail) {
		if (!SensorDevice::getInstance().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");
		}
		}

		// ---------------------------------------------------------------------------

services/sensorservice/SensorDevice.h

+16 −1

Original line number	Diff line number	Diff line
		@@ -77,6 +77,8 @@ public:
		};

		~SensorDevice();
		void prepareForReconnect();
		void reconnect();

		ssize_t getSensorList(sensor_t const** list);

		@@ -114,6 +116,10 @@ public:
		hardware::Return<void> onDynamicSensorsDisconnected(
		const hardware::hidl_vec<int32_t> &dynamicSensorHandlesRemoved);

		bool isReconnecting() const {
		return mReconnecting;
		}

		// Dumpable
		virtual std::string dump() const;
		private:
		@@ -167,7 +173,7 @@ private:
		// the removed ident. If index >=0, ident is present and successfully removed.
		ssize_t removeBatchParamsForIdent(void* ident);

		int numActiveClients();
		int numActiveClients() const;
		};
		DefaultKeyedVector<int, Info> mActivationCount;

		@@ -179,6 +185,11 @@ private:
		SortedVector<void *> mDisabledClients;
		SensorDevice();
		bool connectHidlService();
		void initializeSensorList();
		void reactivateSensors(const DefaultKeyedVector<int, Info>& previousActivations);
		static bool sensorHandlesChanged(const Vector<sensor_t>& oldSensorList,
		const Vector<sensor_t>& newSensorList);
		static bool sensorIsEquivalent(const sensor_t& prevSensor, const sensor_t& newSensor);

		enum HalConnectionStatus {
		CONNECTED, // Successfully connected to the HAL
		@@ -191,6 +202,9 @@ private:

		ssize_t pollHal(sensors_event_t* buffer, size_t count);
		ssize_t pollFmq(sensors_event_t* buffer, size_t count);
		status_t activateLocked(void* ident, int handle, int enabled);
		status_t batchLocked(void* ident, int handle, int flags, int64_t samplingPeriodNs,
		int64_t maxBatchReportLatencyNs);

		static void handleHidlDeath(const std::string &detail);
		template<typename T>
		@@ -228,6 +242,7 @@ private:
		std::array<Event, SensorEventQueue::MAX_RECEIVE_BUFFER_EVENT_COUNT> mEventBuffer;

		sp<SensorsHalDeathReceivier> mSensorsHalDeathReceiver;
		std::atomic_bool mReconnecting;
		};

		// ---------------------------------------------------------------------------

services/sensorservice/SensorService.cpp

+7 −2

Original line number	Diff line number	Diff line
		@@ -633,9 +633,14 @@ bool SensorService::threadLoop() {
		do {
		ssize_t count = device.poll(mSensorEventBuffer, numEventMax);
		if (count < 0) {
		if(count == DEAD_OBJECT && device.isReconnecting()) {
		device.reconnect();
		continue;
		} else {
		ALOGE("sensor poll failed (%s)", strerror(-count));
		break;
		}
		}

		// Reset sensors_event_t.flags to zero for all events in the buffer.
		for (int i = 0; i < count; i++) {