1 /*
2  * Copyright (C) 2010 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include "SensorDevice.h"
18 
19 #include "android/hardware/sensors/2.0/types.h"
20 #include "android/hardware/sensors/2.1/ISensorsCallback.h"
21 #include "android/hardware/sensors/2.1/types.h"
22 #include "convertV2_1.h"
23 
24 #include <android-base/logging.h>
25 #include <android/util/ProtoOutputStream.h>
26 #include <frameworks/base/core/proto/android/service/sensor_service.proto.h>
27 #include <sensors/convert.h>
28 #include <cutils/atomic.h>
29 #include <utils/Errors.h>
30 #include <utils/Singleton.h>
31 
32 #include <cstddef>
33 #include <chrono>
34 #include <cinttypes>
35 #include <thread>
36 
37 using namespace android::hardware::sensors;
38 using namespace android::hardware::sensors::V1_0;
39 using namespace android::hardware::sensors::V1_0::implementation;
40 using android::hardware::sensors::V2_0::EventQueueFlagBits;
41 using android::hardware::sensors::V2_0::WakeLockQueueFlagBits;
42 using android::hardware::sensors::V2_1::ISensorsCallback;
43 using android::hardware::sensors::V2_1::implementation::convertToOldSensorInfo;
44 using android::hardware::sensors::V2_1::implementation::convertToNewSensorInfos;
45 using android::hardware::sensors::V2_1::implementation::convertToNewEvents;
46 using android::hardware::sensors::V2_1::implementation::ISensorsWrapperV1_0;
47 using android::hardware::sensors::V2_1::implementation::ISensorsWrapperV2_0;
48 using android::hardware::sensors::V2_1::implementation::ISensorsWrapperV2_1;
49 using android::hardware::hidl_vec;
50 using android::hardware::Return;
51 using android::SensorDeviceUtils::HidlServiceRegistrationWaiter;
52 using android::util::ProtoOutputStream;
53 
54 namespace android {
55 // ---------------------------------------------------------------------------
56 
57 ANDROID_SINGLETON_STATIC_INSTANCE(SensorDevice)
58 
59 namespace {
60 
statusFromResult(Result result)61 status_t statusFromResult(Result result) {
62     switch (result) {
63         case Result::OK:
64             return OK;
65         case Result::BAD_VALUE:
66             return BAD_VALUE;
67         case Result::PERMISSION_DENIED:
68             return PERMISSION_DENIED;
69         case Result::INVALID_OPERATION:
70             return INVALID_OPERATION;
71         case Result::NO_MEMORY:
72             return NO_MEMORY;
73     }
74 }
75 
76 template<typename EnumType>
asBaseType(EnumType value)77 constexpr typename std::underlying_type<EnumType>::type asBaseType(EnumType value) {
78     return static_cast<typename std::underlying_type<EnumType>::type>(value);
79 }
80 
81 // Used internally by the framework to wake the Event FMQ. These values must start after
82 // the last value of EventQueueFlagBits
83 enum EventQueueFlagBitsInternal : uint32_t {
84     INTERNAL_WAKE =  1 << 16,
85 };
86 
87 }  // anonymous namespace
88 
serviceDied(uint64_t,const wp<::android::hidl::base::V1_0::IBase> &)89 void SensorsHalDeathReceivier::serviceDied(
90         uint64_t /* cookie */,
91         const wp<::android::hidl::base::V1_0::IBase>& /* service */) {
92     ALOGW("Sensors HAL died, attempting to reconnect.");
93     SensorDevice::getInstance().prepareForReconnect();
94 }
95 
96 struct SensorsCallback : public ISensorsCallback {
97     using Result = ::android::hardware::sensors::V1_0::Result;
98     using SensorInfo = ::android::hardware::sensors::V2_1::SensorInfo;
99 
onDynamicSensorsConnected_2_1android::SensorsCallback100     Return<void> onDynamicSensorsConnected_2_1(
101             const hidl_vec<SensorInfo> &dynamicSensorsAdded) override {
102         return SensorDevice::getInstance().onDynamicSensorsConnected(dynamicSensorsAdded);
103     }
104 
onDynamicSensorsConnectedandroid::SensorsCallback105     Return<void> onDynamicSensorsConnected(
106             const hidl_vec<V1_0::SensorInfo> &dynamicSensorsAdded) override {
107         return SensorDevice::getInstance().onDynamicSensorsConnected(
108                 convertToNewSensorInfos(dynamicSensorsAdded));
109     }
110 
onDynamicSensorsDisconnectedandroid::SensorsCallback111     Return<void> onDynamicSensorsDisconnected(
112             const hidl_vec<int32_t> &dynamicSensorHandlesRemoved) override {
113         return SensorDevice::getInstance().onDynamicSensorsDisconnected(
114                 dynamicSensorHandlesRemoved);
115     }
116 };
117 
SensorDevice()118 SensorDevice::SensorDevice()
119         : mHidlTransportErrors(20),
120           mRestartWaiter(new HidlServiceRegistrationWaiter()),
121           mEventQueueFlag(nullptr),
122           mWakeLockQueueFlag(nullptr),
123           mReconnecting(false) {
124     if (!connectHidlService()) {
125         return;
126     }
127 
128     initializeSensorList();
129 
130     mIsDirectReportSupported =
131             (checkReturnAndGetStatus(mSensors->unregisterDirectChannel(-1)) != INVALID_OPERATION);
132 }
133 
initializeSensorList()134 void SensorDevice::initializeSensorList() {
135     checkReturn(mSensors->getSensorsList(
136             [&](const auto &list) {
137                 const size_t count = list.size();
138 
139                 mActivationCount.setCapacity(count);
140                 Info model;
141                 for (size_t i=0 ; i < count; i++) {
142                     sensor_t sensor;
143                     convertToSensor(convertToOldSensorInfo(list[i]), &sensor);
144 
145                     if (sensor.type < static_cast<int>(SensorType::DEVICE_PRIVATE_BASE)) {
146                         sensor.resolution = SensorDeviceUtils::resolutionForSensor(sensor);
147 
148                         // Some sensors don't have a default resolution and will be left at 0.
149                         // Don't crash in this case since CTS will verify that devices don't go to
150                         // production with a resolution of 0.
151                         if (sensor.resolution != 0) {
152                             float quantizedRange = sensor.maxRange;
153                             SensorDeviceUtils::quantizeValue(
154                                     &quantizedRange, sensor.resolution, /*factor=*/ 1);
155                             // Only rewrite maxRange if the requantization produced a "significant"
156                             // change, which is fairly arbitrarily defined as resolution / 8.
157                             // Smaller deltas are permitted, as they may simply be due to floating
158                             // point representation error, etc.
159                             if (fabsf(sensor.maxRange - quantizedRange) > sensor.resolution / 8) {
160                                 ALOGW("%s's max range %.12f is not a multiple of the resolution "
161                                       "%.12f - updated to %.12f", sensor.name, sensor.maxRange,
162                                       sensor.resolution, quantizedRange);
163                                 sensor.maxRange = quantizedRange;
164                             }
165                         } else {
166                             // Don't crash here or the device will go into a crashloop.
167                             ALOGW("%s should have a non-zero resolution", sensor.name);
168                         }
169                     }
170 
171                     // Sanity check and clamp power if it is 0 (or close)
172                     constexpr float MIN_POWER_MA = 0.001; // 1 microAmp
173                     if (sensor.power < MIN_POWER_MA) {
174                         ALOGI("%s's reported power %f invalid, clamped to %f",
175                               sensor.name, sensor.power, MIN_POWER_MA);
176                         sensor.power = MIN_POWER_MA;
177                     }
178                     mSensorList.push_back(sensor);
179 
180                     mActivationCount.add(list[i].sensorHandle, model);
181 
182                     // Only disable all sensors on HAL 1.0 since HAL 2.0
183                     // handles this in its initialize method
184                     if (!mSensors->supportsMessageQueues()) {
185                         checkReturn(mSensors->activate(list[i].sensorHandle,
186                                     0 /* enabled */));
187                     }
188                 }
189             }));
190 }
191 
~SensorDevice()192 SensorDevice::~SensorDevice() {
193     if (mEventQueueFlag != nullptr) {
194         hardware::EventFlag::deleteEventFlag(&mEventQueueFlag);
195         mEventQueueFlag = nullptr;
196     }
197 
198     if (mWakeLockQueueFlag != nullptr) {
199         hardware::EventFlag::deleteEventFlag(&mWakeLockQueueFlag);
200         mWakeLockQueueFlag = nullptr;
201     }
202 }
203 
connectHidlService()204 bool SensorDevice::connectHidlService() {
205     HalConnectionStatus status = connectHidlServiceV2_1();
206     if (status == HalConnectionStatus::DOES_NOT_EXIST) {
207         status = connectHidlServiceV2_0();
208     }
209 
210     if (status == HalConnectionStatus::DOES_NOT_EXIST) {
211         status = connectHidlServiceV1_0();
212     }
213     return (status == HalConnectionStatus::CONNECTED);
214 }
215 
connectHidlServiceV1_0()216 SensorDevice::HalConnectionStatus SensorDevice::connectHidlServiceV1_0() {
217     // SensorDevice will wait for HAL service to start if HAL is declared in device manifest.
218     size_t retry = 10;
219     HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;
220 
221     while (retry-- > 0) {
222         sp<V1_0::ISensors> sensors = V1_0::ISensors::getService();
223         if (sensors == nullptr) {
224             // no sensor hidl service found
225             connectionStatus = HalConnectionStatus::DOES_NOT_EXIST;
226             break;
227         }
228 
229         mSensors = new ISensorsWrapperV1_0(sensors);
230         mRestartWaiter->reset();
231         // Poke ISensor service. If it has lingering connection from previous generation of
232         // system server, it will kill itself. There is no intention to handle the poll result,
233         // which will be done since the size is 0.
234         if(mSensors->poll(0, [](auto, const auto &, const auto &) {}).isOk()) {
235             // ok to continue
236             connectionStatus = HalConnectionStatus::CONNECTED;
237             break;
238         }
239 
240         // hidl service is restarting, pointer is invalid.
241         mSensors = nullptr;
242         connectionStatus = HalConnectionStatus::FAILED_TO_CONNECT;
243         ALOGI("%s unsuccessful, remaining retry %zu.", __FUNCTION__, retry);
244         mRestartWaiter->wait();
245     }
246 
247     return connectionStatus;
248 }
249 
connectHidlServiceV2_0()250 SensorDevice::HalConnectionStatus SensorDevice::connectHidlServiceV2_0() {
251     HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;
252     sp<V2_0::ISensors> sensors = V2_0::ISensors::getService();
253 
254     if (sensors == nullptr) {
255         connectionStatus = HalConnectionStatus::DOES_NOT_EXIST;
256     } else {
257         mSensors = new ISensorsWrapperV2_0(sensors);
258         connectionStatus = initializeHidlServiceV2_X();
259     }
260 
261     return connectionStatus;
262 }
263 
connectHidlServiceV2_1()264 SensorDevice::HalConnectionStatus SensorDevice::connectHidlServiceV2_1() {
265     HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;
266     sp<V2_1::ISensors> sensors = V2_1::ISensors::getService();
267 
268     if (sensors == nullptr) {
269         connectionStatus = HalConnectionStatus::DOES_NOT_EXIST;
270     } else {
271         mSensors = new ISensorsWrapperV2_1(sensors);
272         connectionStatus = initializeHidlServiceV2_X();
273     }
274 
275     return connectionStatus;
276 }
277 
initializeHidlServiceV2_X()278 SensorDevice::HalConnectionStatus SensorDevice::initializeHidlServiceV2_X() {
279     HalConnectionStatus connectionStatus = HalConnectionStatus::UNKNOWN;
280 
281     mWakeLockQueue = std::make_unique<WakeLockQueue>(
282             SensorEventQueue::MAX_RECEIVE_BUFFER_EVENT_COUNT,
283             true /* configureEventFlagWord */);
284 
285     hardware::EventFlag::deleteEventFlag(&mEventQueueFlag);
286     hardware::EventFlag::createEventFlag(mSensors->getEventQueue()->getEventFlagWord(), &mEventQueueFlag);
287 
288     hardware::EventFlag::deleteEventFlag(&mWakeLockQueueFlag);
289     hardware::EventFlag::createEventFlag(mWakeLockQueue->getEventFlagWord(),
290                                             &mWakeLockQueueFlag);
291 
292     CHECK(mSensors != nullptr && mWakeLockQueue != nullptr &&
293             mEventQueueFlag != nullptr && mWakeLockQueueFlag != nullptr);
294 
295     status_t status = checkReturnAndGetStatus(mSensors->initialize(
296             *mWakeLockQueue->getDesc(),
297             new SensorsCallback()));
298 
299     if (status != NO_ERROR) {
300         connectionStatus = HalConnectionStatus::FAILED_TO_CONNECT;
301         ALOGE("Failed to initialize Sensors HAL (%s)", strerror(-status));
302     } else {
303         connectionStatus = HalConnectionStatus::CONNECTED;
304         mSensorsHalDeathReceiver = new SensorsHalDeathReceivier();
305         mSensors->linkToDeath(mSensorsHalDeathReceiver, 0 /* cookie */);
306     }
307 
308     return connectionStatus;
309 }
310 
prepareForReconnect()311 void SensorDevice::prepareForReconnect() {
312     mReconnecting = true;
313 
314     // Wake up the polling thread so it returns and allows the SensorService to initiate
315     // a reconnect.
316     mEventQueueFlag->wake(asBaseType(INTERNAL_WAKE));
317 }
318 
reconnect()319 void SensorDevice::reconnect() {
320     Mutex::Autolock _l(mLock);
321     mSensors = nullptr;
322 
323     auto previousActivations = mActivationCount;
324     auto previousSensorList = mSensorList;
325 
326     mActivationCount.clear();
327     mSensorList.clear();
328 
329     if (connectHidlService()) {
330         initializeSensorList();
331 
332         if (sensorHandlesChanged(previousSensorList, mSensorList)) {
333             LOG_ALWAYS_FATAL("Sensor handles changed, cannot re-enable sensors.");
334         } else {
335             reactivateSensors(previousActivations);
336         }
337     }
338     mReconnecting = false;
339 }
340 
sensorHandlesChanged(const Vector<sensor_t> & oldSensorList,const Vector<sensor_t> & newSensorList)341 bool SensorDevice::sensorHandlesChanged(const Vector<sensor_t>& oldSensorList,
342                                         const Vector<sensor_t>& newSensorList) {
343     bool didChange = false;
344 
345     if (oldSensorList.size() != newSensorList.size()) {
346         ALOGI("Sensor list size changed from %zu to %zu", oldSensorList.size(),
347               newSensorList.size());
348         didChange = true;
349     }
350 
351     for (size_t i = 0; i < newSensorList.size() && !didChange; i++) {
352         bool found = false;
353         const sensor_t& newSensor = newSensorList[i];
354         for (size_t j = 0; j < oldSensorList.size() && !found; j++) {
355             const sensor_t& prevSensor = oldSensorList[j];
356             if (prevSensor.handle == newSensor.handle) {
357                 found = true;
358                 if (!sensorIsEquivalent(prevSensor, newSensor)) {
359                     ALOGI("Sensor %s not equivalent to previous version", newSensor.name);
360                     didChange = true;
361                 }
362             }
363         }
364 
365         if (!found) {
366             // Could not find the new sensor in the old list of sensors, the lists must
367             // have changed.
368             ALOGI("Sensor %s (handle %d) did not exist before", newSensor.name, newSensor.handle);
369             didChange = true;
370         }
371     }
372     return didChange;
373 }
374 
sensorIsEquivalent(const sensor_t & prevSensor,const sensor_t & newSensor)375 bool SensorDevice::sensorIsEquivalent(const sensor_t& prevSensor, const sensor_t& newSensor) {
376     bool equivalent = true;
377     if (prevSensor.handle != newSensor.handle ||
378             (strcmp(prevSensor.vendor, newSensor.vendor) != 0) ||
379             (strcmp(prevSensor.stringType, newSensor.stringType) != 0) ||
380             (strcmp(prevSensor.requiredPermission, newSensor.requiredPermission) != 0) ||
381             (prevSensor.version != newSensor.version) ||
382             (prevSensor.type != newSensor.type) ||
383             (std::abs(prevSensor.maxRange - newSensor.maxRange) > 0.001f) ||
384             (std::abs(prevSensor.resolution - newSensor.resolution) > 0.001f) ||
385             (std::abs(prevSensor.power - newSensor.power) > 0.001f) ||
386             (prevSensor.minDelay != newSensor.minDelay) ||
387             (prevSensor.fifoReservedEventCount != newSensor.fifoReservedEventCount) ||
388             (prevSensor.fifoMaxEventCount != newSensor.fifoMaxEventCount) ||
389             (prevSensor.maxDelay != newSensor.maxDelay) ||
390             (prevSensor.flags != newSensor.flags)) {
391         equivalent = false;
392     }
393     return equivalent;
394 }
395 
reactivateSensors(const DefaultKeyedVector<int,Info> & previousActivations)396 void SensorDevice::reactivateSensors(const DefaultKeyedVector<int, Info>& previousActivations) {
397     for (size_t i = 0; i < mSensorList.size(); i++) {
398         int handle = mSensorList[i].handle;
399         ssize_t activationIndex = previousActivations.indexOfKey(handle);
400         if (activationIndex < 0 || previousActivations[activationIndex].numActiveClients() <= 0) {
401             continue;
402         }
403 
404         const Info& info = previousActivations[activationIndex];
405         for (size_t j = 0; j < info.batchParams.size(); j++) {
406             const BatchParams& batchParams = info.batchParams[j];
407             status_t res = batchLocked(info.batchParams.keyAt(j), handle, 0 /* flags */,
408                     batchParams.mTSample, batchParams.mTBatch);
409 
410             if (res == NO_ERROR) {
411                 activateLocked(info.batchParams.keyAt(j), handle, true /* enabled */);
412             }
413         }
414     }
415 }
416 
handleDynamicSensorConnection(int handle,bool connected)417 void SensorDevice::handleDynamicSensorConnection(int handle, bool connected) {
418     // not need to check mSensors because this is is only called after successful poll()
419     if (connected) {
420         Info model;
421         mActivationCount.add(handle, model);
422         checkReturn(mSensors->activate(handle, 0 /* enabled */));
423     } else {
424         mActivationCount.removeItem(handle);
425     }
426 }
427 
dump() const428 std::string SensorDevice::dump() const {
429     if (mSensors == nullptr) return "HAL not initialized\n";
430 
431     String8 result;
432     result.appendFormat("Total %zu h/w sensors, %zu running %zu disabled clients:\n",
433                         mSensorList.size(), mActivationCount.size(), mDisabledClients.size());
434 
435     Mutex::Autolock _l(mLock);
436     for (const auto & s : mSensorList) {
437         int32_t handle = s.handle;
438         const Info& info = mActivationCount.valueFor(handle);
439         if (info.numActiveClients() == 0) continue;
440 
441         result.appendFormat("0x%08x) active-count = %zu; ", handle, info.batchParams.size());
442 
443         result.append("sampling_period(ms) = {");
444         for (size_t j = 0; j < info.batchParams.size(); j++) {
445             const BatchParams& params = info.batchParams[j];
446             result.appendFormat("%.1f%s%s", params.mTSample / 1e6f,
447                 isClientDisabledLocked(info.batchParams.keyAt(j)) ? "(disabled)" : "",
448                 (j < info.batchParams.size() - 1) ? ", " : "");
449         }
450         result.appendFormat("}, selected = %.2f ms; ", info.bestBatchParams.mTSample / 1e6f);
451 
452         result.append("batching_period(ms) = {");
453         for (size_t j = 0; j < info.batchParams.size(); j++) {
454             const BatchParams& params = info.batchParams[j];
455             result.appendFormat("%.1f%s%s", params.mTBatch / 1e6f,
456                     isClientDisabledLocked(info.batchParams.keyAt(j)) ? "(disabled)" : "",
457                     (j < info.batchParams.size() - 1) ? ", " : "");
458         }
459         result.appendFormat("}, selected = %.2f ms\n", info.bestBatchParams.mTBatch / 1e6f);
460     }
461 
462     return result.string();
463 }
464 
465 /**
466  * Dump debugging information as android.service.SensorDeviceProto protobuf message using
467  * ProtoOutputStream.
468  *
469  * See proto definition and some notes about ProtoOutputStream in
470  * frameworks/base/core/proto/android/service/sensor_service.proto
471  */
dump(ProtoOutputStream * proto) const472 void SensorDevice::dump(ProtoOutputStream* proto) const {
473     using namespace service::SensorDeviceProto;
474     if (mSensors == nullptr) {
475         proto->write(INITIALIZED , false);
476         return;
477     }
478     proto->write(INITIALIZED , true);
479     proto->write(TOTAL_SENSORS , int(mSensorList.size()));
480     proto->write(ACTIVE_SENSORS , int(mActivationCount.size()));
481 
482     Mutex::Autolock _l(mLock);
483     for (const auto & s : mSensorList) {
484         int32_t handle = s.handle;
485         const Info& info = mActivationCount.valueFor(handle);
486         if (info.numActiveClients() == 0) continue;
487 
488         uint64_t token = proto->start(SENSORS);
489         proto->write(SensorProto::HANDLE , handle);
490         proto->write(SensorProto::ACTIVE_COUNT , int(info.batchParams.size()));
491         for (size_t j = 0; j < info.batchParams.size(); j++) {
492             const BatchParams& params = info.batchParams[j];
493             proto->write(SensorProto::SAMPLING_PERIOD_MS , params.mTSample / 1e6f);
494             proto->write(SensorProto::BATCHING_PERIOD_MS , params.mTBatch / 1e6f);
495         }
496         proto->write(SensorProto::SAMPLING_PERIOD_SELECTED , info.bestBatchParams.mTSample / 1e6f);
497         proto->write(SensorProto::BATCHING_PERIOD_SELECTED , info.bestBatchParams.mTBatch / 1e6f);
498         proto->end(token);
499     }
500 }
501 
getSensorList(sensor_t const ** list)502 ssize_t SensorDevice::getSensorList(sensor_t const** list) {
503     *list = &mSensorList[0];
504 
505     return mSensorList.size();
506 }
507 
initCheck() const508 status_t SensorDevice::initCheck() const {
509     return mSensors != nullptr ? NO_ERROR : NO_INIT;
510 }
511 
poll(sensors_event_t * buffer,size_t count)512 ssize_t SensorDevice::poll(sensors_event_t* buffer, size_t count) {
513     if (mSensors == nullptr) return NO_INIT;
514 
515     ssize_t eventsRead = 0;
516     if (mSensors->supportsMessageQueues()) {
517         eventsRead = pollFmq(buffer, count);
518     } else if (mSensors->supportsPolling()) {
519         eventsRead = pollHal(buffer, count);
520     } else {
521         ALOGE("Must support polling or FMQ");
522         eventsRead = -1;
523     }
524     return eventsRead;
525 }
526 
pollHal(sensors_event_t * buffer,size_t count)527 ssize_t SensorDevice::pollHal(sensors_event_t* buffer, size_t count) {
528     ssize_t err;
529     int numHidlTransportErrors = 0;
530     bool hidlTransportError = false;
531 
532     do {
533         auto ret = mSensors->poll(
534                 count,
535                 [&](auto result,
536                     const auto &events,
537                     const auto &dynamicSensorsAdded) {
538                     if (result == Result::OK) {
539                         convertToSensorEventsAndQuantize(convertToNewEvents(events),
540                                 convertToNewSensorInfos(dynamicSensorsAdded), buffer);
541                         err = (ssize_t)events.size();
542                     } else {
543                         err = statusFromResult(result);
544                     }
545                 });
546 
547         if (ret.isOk())  {
548             hidlTransportError = false;
549         } else {
550             hidlTransportError = true;
551             numHidlTransportErrors++;
552             if (numHidlTransportErrors > 50) {
553                 // Log error and bail
554                 ALOGE("Max Hidl transport errors this cycle : %d", numHidlTransportErrors);
555                 handleHidlDeath(ret.description());
556             } else {
557                 std::this_thread::sleep_for(std::chrono::milliseconds(10));
558             }
559         }
560     } while (hidlTransportError);
561 
562     if(numHidlTransportErrors > 0) {
563         ALOGE("Saw %d Hidl transport failures", numHidlTransportErrors);
564         HidlTransportErrorLog errLog(time(nullptr), numHidlTransportErrors);
565         mHidlTransportErrors.add(errLog);
566         mTotalHidlTransportErrors++;
567     }
568 
569     return err;
570 }
571 
pollFmq(sensors_event_t * buffer,size_t maxNumEventsToRead)572 ssize_t SensorDevice::pollFmq(sensors_event_t* buffer, size_t maxNumEventsToRead) {
573     ssize_t eventsRead = 0;
574     size_t availableEvents = mSensors->getEventQueue()->availableToRead();
575 
576     if (availableEvents == 0) {
577         uint32_t eventFlagState = 0;
578 
579         // Wait for events to become available. This is necessary so that the Event FMQ's read() is
580         // able to be called with the correct number of events to read. If the specified number of
581         // events is not available, then read() would return no events, possibly introducing
582         // additional latency in delivering events to applications.
583         mEventQueueFlag->wait(asBaseType(EventQueueFlagBits::READ_AND_PROCESS) |
584                               asBaseType(INTERNAL_WAKE), &eventFlagState);
585         availableEvents = mSensors->getEventQueue()->availableToRead();
586 
587         if ((eventFlagState & asBaseType(INTERNAL_WAKE)) && mReconnecting) {
588             ALOGD("Event FMQ internal wake, returning from poll with no events");
589             return DEAD_OBJECT;
590         }
591     }
592 
593     size_t eventsToRead = std::min({availableEvents, maxNumEventsToRead, mEventBuffer.size()});
594     if (eventsToRead > 0) {
595         if (mSensors->getEventQueue()->read(mEventBuffer.data(), eventsToRead)) {
596             // Notify the Sensors HAL that sensor events have been read. This is required to support
597             // the use of writeBlocking by the Sensors HAL.
598             mEventQueueFlag->wake(asBaseType(EventQueueFlagBits::EVENTS_READ));
599 
600             for (size_t i = 0; i < eventsToRead; i++) {
601                 convertToSensorEvent(mEventBuffer[i], &buffer[i]);
602                 android::SensorDeviceUtils::quantizeSensorEventValues(&buffer[i],
603                         getResolutionForSensor(buffer[i].sensor));
604             }
605             eventsRead = eventsToRead;
606         } else {
607             ALOGW("Failed to read %zu events, currently %zu events available",
608                     eventsToRead, availableEvents);
609         }
610     }
611 
612     return eventsRead;
613 }
614 
onDynamicSensorsConnected(const hidl_vec<SensorInfo> & dynamicSensorsAdded)615 Return<void> SensorDevice::onDynamicSensorsConnected(
616         const hidl_vec<SensorInfo> &dynamicSensorsAdded) {
617     std::unique_lock<std::mutex> lock(mDynamicSensorsMutex);
618 
619     // Allocate a sensor_t structure for each dynamic sensor added and insert
620     // it into the dictionary of connected dynamic sensors keyed by handle.
621     for (size_t i = 0; i < dynamicSensorsAdded.size(); ++i) {
622         const SensorInfo &info = dynamicSensorsAdded[i];
623 
624         auto it = mConnectedDynamicSensors.find(info.sensorHandle);
625         CHECK(it == mConnectedDynamicSensors.end());
626 
627         sensor_t *sensor = new sensor_t();
628         convertToSensor(convertToOldSensorInfo(info), sensor);
629 
630         mConnectedDynamicSensors.insert(
631                 std::make_pair(sensor->handle, sensor));
632     }
633 
634     mDynamicSensorsCv.notify_all();
635 
636     return Return<void>();
637 }
638 
onDynamicSensorsDisconnected(const hidl_vec<int32_t> & dynamicSensorHandlesRemoved)639 Return<void> SensorDevice::onDynamicSensorsDisconnected(
640         const hidl_vec<int32_t> &dynamicSensorHandlesRemoved) {
641     (void) dynamicSensorHandlesRemoved;
642     // TODO: Currently dynamic sensors do not seem to be removed
643     return Return<void>();
644 }
645 
writeWakeLockHandled(uint32_t count)646 void SensorDevice::writeWakeLockHandled(uint32_t count) {
647     if (mSensors != nullptr && mSensors->supportsMessageQueues()) {
648         if (mWakeLockQueue->write(&count)) {
649             mWakeLockQueueFlag->wake(asBaseType(WakeLockQueueFlagBits::DATA_WRITTEN));
650         } else {
651             ALOGW("Failed to write wake lock handled");
652         }
653     }
654 }
655 
autoDisable(void * ident,int handle)656 void SensorDevice::autoDisable(void *ident, int handle) {
657     Mutex::Autolock _l(mLock);
658     ssize_t activationIndex = mActivationCount.indexOfKey(handle);
659     if (activationIndex < 0) {
660         ALOGW("Handle %d cannot be found in activation record", handle);
661         return;
662     }
663     Info& info(mActivationCount.editValueAt(activationIndex));
664     info.removeBatchParamsForIdent(ident);
665     if (info.numActiveClients() == 0) {
666         info.isActive = false;
667     }
668 }
669 
activate(void * ident,int handle,int enabled)670 status_t SensorDevice::activate(void* ident, int handle, int enabled) {
671     if (mSensors == nullptr) return NO_INIT;
672 
673     Mutex::Autolock _l(mLock);
674     return activateLocked(ident, handle, enabled);
675 }
676 
activateLocked(void * ident,int handle,int enabled)677 status_t SensorDevice::activateLocked(void* ident, int handle, int enabled) {
678     bool activateHardware = false;
679 
680     status_t err(NO_ERROR);
681 
682     ssize_t activationIndex = mActivationCount.indexOfKey(handle);
683     if (activationIndex < 0) {
684         ALOGW("Handle %d cannot be found in activation record", handle);
685         return BAD_VALUE;
686     }
687     Info& info(mActivationCount.editValueAt(activationIndex));
688 
689     ALOGD_IF(DEBUG_CONNECTIONS,
690              "SensorDevice::activate: ident=%p, handle=0x%08x, enabled=%d, count=%zu",
691              ident, handle, enabled, info.batchParams.size());
692 
693     if (enabled) {
694         ALOGD_IF(DEBUG_CONNECTIONS, "enable index=%zd", info.batchParams.indexOfKey(ident));
695 
696         if (isClientDisabledLocked(ident)) {
697             ALOGW("SensorDevice::activate, isClientDisabledLocked(%p):true, handle:%d",
698                     ident, handle);
699             return NO_ERROR;
700         }
701 
702         if (info.batchParams.indexOfKey(ident) >= 0) {
703             if (info.numActiveClients() > 0 && !info.isActive) {
704                 activateHardware = true;
705             }
706         } else {
707             // Log error. Every activate call should be preceded by a batch() call.
708             ALOGE("\t >>>ERROR: activate called without batch");
709         }
710     } else {
711         ALOGD_IF(DEBUG_CONNECTIONS, "disable index=%zd", info.batchParams.indexOfKey(ident));
712 
713         // If a connected dynamic sensor is deactivated, remove it from the
714         // dictionary.
715         auto it = mConnectedDynamicSensors.find(handle);
716         if (it != mConnectedDynamicSensors.end()) {
717             delete it->second;
718             mConnectedDynamicSensors.erase(it);
719         }
720 
721         if (info.removeBatchParamsForIdent(ident) >= 0) {
722             if (info.numActiveClients() == 0) {
723                 // This is the last connection, we need to de-activate the underlying h/w sensor.
724                 activateHardware = true;
725             } else {
726                 // Call batch for this sensor with the previously calculated best effort
727                 // batch_rate and timeout. One of the apps has unregistered for sensor
728                 // events, and the best effort batch parameters might have changed.
729                 ALOGD_IF(DEBUG_CONNECTIONS,
730                          "\t>>> actuating h/w batch 0x%08x %" PRId64 " %" PRId64, handle,
731                          info.bestBatchParams.mTSample, info.bestBatchParams.mTBatch);
732                 checkReturn(mSensors->batch(
733                         handle, info.bestBatchParams.mTSample, info.bestBatchParams.mTBatch));
734             }
735         } else {
736             // sensor wasn't enabled for this ident
737         }
738 
739         if (isClientDisabledLocked(ident)) {
740             return NO_ERROR;
741         }
742     }
743 
744     if (activateHardware) {
745         err = doActivateHardwareLocked(handle, enabled);
746 
747         if (err != NO_ERROR && enabled) {
748             // Failure when enabling the sensor. Clean up on failure.
749             info.removeBatchParamsForIdent(ident);
750         } else {
751             // Update the isActive flag if there is no error. If there is an error when disabling a
752             // sensor, still set the flag to false since the batch parameters have already been
753             // removed. This ensures that everything remains in-sync.
754             info.isActive = enabled;
755         }
756     }
757 
758     return err;
759 }
760 
doActivateHardwareLocked(int handle,bool enabled)761 status_t SensorDevice::doActivateHardwareLocked(int handle, bool enabled) {
762     ALOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w activate handle=%d enabled=%d", handle,
763              enabled);
764     status_t err = checkReturnAndGetStatus(mSensors->activate(handle, enabled));
765     ALOGE_IF(err, "Error %s sensor %d (%s)", enabled ? "activating" : "disabling", handle,
766              strerror(-err));
767     return err;
768 }
769 
batch(void * ident,int handle,int flags,int64_t samplingPeriodNs,int64_t maxBatchReportLatencyNs)770 status_t SensorDevice::batch(
771         void* ident,
772         int handle,
773         int flags,
774         int64_t samplingPeriodNs,
775         int64_t maxBatchReportLatencyNs) {
776     if (mSensors == nullptr) return NO_INIT;
777 
778     if (samplingPeriodNs < MINIMUM_EVENTS_PERIOD) {
779         samplingPeriodNs = MINIMUM_EVENTS_PERIOD;
780     }
781     if (maxBatchReportLatencyNs < 0) {
782         maxBatchReportLatencyNs = 0;
783     }
784 
785     ALOGD_IF(DEBUG_CONNECTIONS,
786              "SensorDevice::batch: ident=%p, handle=0x%08x, flags=%d, period_ns=%" PRId64 " timeout=%" PRId64,
787              ident, handle, flags, samplingPeriodNs, maxBatchReportLatencyNs);
788 
789     Mutex::Autolock _l(mLock);
790     return batchLocked(ident, handle, flags, samplingPeriodNs, maxBatchReportLatencyNs);
791 }
792 
batchLocked(void * ident,int handle,int flags,int64_t samplingPeriodNs,int64_t maxBatchReportLatencyNs)793 status_t SensorDevice::batchLocked(void* ident, int handle, int flags, int64_t samplingPeriodNs,
794                                    int64_t maxBatchReportLatencyNs) {
795     ssize_t activationIndex = mActivationCount.indexOfKey(handle);
796     if (activationIndex < 0) {
797         ALOGW("Handle %d cannot be found in activation record", handle);
798         return BAD_VALUE;
799     }
800     Info& info(mActivationCount.editValueAt(activationIndex));
801 
802     if (info.batchParams.indexOfKey(ident) < 0) {
803         BatchParams params(samplingPeriodNs, maxBatchReportLatencyNs);
804         info.batchParams.add(ident, params);
805     } else {
806         // A batch has already been called with this ident. Update the batch parameters.
807         info.setBatchParamsForIdent(ident, flags, samplingPeriodNs, maxBatchReportLatencyNs);
808     }
809 
810     status_t err =  updateBatchParamsLocked(handle, info);
811     if (err != NO_ERROR) {
812         ALOGE("sensor batch failed %p 0x%08x %" PRId64 " %" PRId64 " err=%s",
813               mSensors.get(), handle, info.bestBatchParams.mTSample,
814               info.bestBatchParams.mTBatch, strerror(-err));
815         info.removeBatchParamsForIdent(ident);
816     }
817 
818     return err;
819 }
820 
updateBatchParamsLocked(int handle,Info & info)821 status_t SensorDevice::updateBatchParamsLocked(int handle, Info &info) {
822     BatchParams prevBestBatchParams = info.bestBatchParams;
823     // Find the minimum of all timeouts and batch_rates for this sensor.
824     info.selectBatchParams();
825 
826     ALOGD_IF(DEBUG_CONNECTIONS,
827              "\t>>> curr_period=%" PRId64 " min_period=%" PRId64
828              " curr_timeout=%" PRId64 " min_timeout=%" PRId64,
829              prevBestBatchParams.mTSample, info.bestBatchParams.mTSample,
830              prevBestBatchParams.mTBatch, info.bestBatchParams.mTBatch);
831 
832     status_t err(NO_ERROR);
833     // If the min period or min timeout has changed since the last batch call, call batch.
834     if (prevBestBatchParams != info.bestBatchParams && info.numActiveClients() > 0) {
835         ALOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w BATCH 0x%08x %" PRId64 " %" PRId64, handle,
836                  info.bestBatchParams.mTSample, info.bestBatchParams.mTBatch);
837         err = checkReturnAndGetStatus(mSensors->batch(
838                 handle, info.bestBatchParams.mTSample, info.bestBatchParams.mTBatch));
839     }
840 
841     return err;
842 }
843 
setDelay(void * ident,int handle,int64_t samplingPeriodNs)844 status_t SensorDevice::setDelay(void* ident, int handle, int64_t samplingPeriodNs) {
845     return batch(ident, handle, 0, samplingPeriodNs, 0);
846 }
847 
getHalDeviceVersion() const848 int SensorDevice::getHalDeviceVersion() const {
849     if (mSensors == nullptr) return -1;
850     return SENSORS_DEVICE_API_VERSION_1_4;
851 }
852 
flush(void * ident,int handle)853 status_t SensorDevice::flush(void* ident, int handle) {
854     if (mSensors == nullptr) return NO_INIT;
855     if (isClientDisabled(ident)) return INVALID_OPERATION;
856     ALOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w flush %d", handle);
857     return checkReturnAndGetStatus(mSensors->flush(handle));
858 }
859 
isClientDisabled(void * ident) const860 bool SensorDevice::isClientDisabled(void* ident) const {
861     Mutex::Autolock _l(mLock);
862     return isClientDisabledLocked(ident);
863 }
864 
isClientDisabledLocked(void * ident) const865 bool SensorDevice::isClientDisabledLocked(void* ident) const {
866     return mDisabledClients.count(ident) > 0;
867 }
868 
getDisabledClientsLocked() const869 std::vector<void *> SensorDevice::getDisabledClientsLocked() const {
870     std::vector<void *> vec;
871     for (const auto& it : mDisabledClients) {
872         vec.push_back(it.first);
873     }
874 
875     return vec;
876 }
877 
addDisabledReasonForIdentLocked(void * ident,DisabledReason reason)878 void SensorDevice::addDisabledReasonForIdentLocked(void* ident, DisabledReason reason) {
879     mDisabledClients[ident] |= 1 << reason;
880 }
881 
removeDisabledReasonForIdentLocked(void * ident,DisabledReason reason)882 void SensorDevice::removeDisabledReasonForIdentLocked(void* ident, DisabledReason reason) {
883     if (isClientDisabledLocked(ident)) {
884         mDisabledClients[ident] &= ~(1 << reason);
885         if (mDisabledClients[ident] == 0) {
886             mDisabledClients.erase(ident);
887         }
888     }
889 }
890 
setUidStateForConnection(void * ident,SensorService::UidState state)891 void SensorDevice::setUidStateForConnection(void* ident, SensorService::UidState state) {
892     Mutex::Autolock _l(mLock);
893     if (state == SensorService::UID_STATE_ACTIVE) {
894         removeDisabledReasonForIdentLocked(ident, DisabledReason::DISABLED_REASON_UID_IDLE);
895     } else {
896         addDisabledReasonForIdentLocked(ident, DisabledReason::DISABLED_REASON_UID_IDLE);
897     }
898 
899     for (size_t i = 0; i< mActivationCount.size(); ++i) {
900         int handle = mActivationCount.keyAt(i);
901         Info& info = mActivationCount.editValueAt(i);
902 
903         if (info.hasBatchParamsForIdent(ident)) {
904             updateBatchParamsLocked(handle, info);
905             bool disable = info.numActiveClients() == 0 && info.isActive;
906             bool enable = info.numActiveClients() > 0 && !info.isActive;
907 
908             if ((enable || disable) &&
909                 doActivateHardwareLocked(handle, enable) == NO_ERROR) {
910                 info.isActive = enable;
911             }
912         }
913     }
914 }
915 
isSensorActive(int handle) const916 bool SensorDevice::isSensorActive(int handle) const {
917     Mutex::Autolock _l(mLock);
918     ssize_t activationIndex = mActivationCount.indexOfKey(handle);
919     if (activationIndex < 0) {
920         return false;
921     }
922     return mActivationCount.valueAt(activationIndex).isActive;
923 }
924 
onMicSensorAccessChanged(void * ident,int handle,nsecs_t samplingPeriodNs)925 void SensorDevice::onMicSensorAccessChanged(void* ident, int handle, nsecs_t samplingPeriodNs) {
926     Mutex::Autolock _l(mLock);
927     ssize_t activationIndex = mActivationCount.indexOfKey(handle);
928     if (activationIndex < 0) {
929         ALOGW("Handle %d cannot be found in activation record", handle);
930         return;
931     }
932     Info& info(mActivationCount.editValueAt(activationIndex));
933     if (info.hasBatchParamsForIdent(ident)) {
934         ssize_t index = info.batchParams.indexOfKey(ident);
935         BatchParams& params = info.batchParams.editValueAt(index);
936         params.mTSample = samplingPeriodNs;
937     }
938 }
939 
enableAllSensors()940 void SensorDevice::enableAllSensors() {
941     if (mSensors == nullptr) return;
942     Mutex::Autolock _l(mLock);
943 
944     for (void *client : getDisabledClientsLocked()) {
945         removeDisabledReasonForIdentLocked(
946             client, DisabledReason::DISABLED_REASON_SERVICE_RESTRICTED);
947     }
948 
949     for (size_t i = 0; i< mActivationCount.size(); ++i) {
950         Info& info = mActivationCount.editValueAt(i);
951         if (info.batchParams.isEmpty()) continue;
952         info.selectBatchParams();
953         const int sensor_handle = mActivationCount.keyAt(i);
954         ALOGD_IF(DEBUG_CONNECTIONS, "\t>> reenable actuating h/w sensor enable handle=%d ",
955                    sensor_handle);
956         status_t err = checkReturnAndGetStatus(mSensors->batch(
957                 sensor_handle,
958                 info.bestBatchParams.mTSample,
959                 info.bestBatchParams.mTBatch));
960         ALOGE_IF(err, "Error calling batch on sensor %d (%s)", sensor_handle, strerror(-err));
961 
962         if (err == NO_ERROR) {
963             err = checkReturnAndGetStatus(mSensors->activate(sensor_handle, 1 /* enabled */));
964             ALOGE_IF(err, "Error activating sensor %d (%s)", sensor_handle, strerror(-err));
965         }
966 
967         if (err == NO_ERROR) {
968             info.isActive = true;
969         }
970     }
971 }
972 
disableAllSensors()973 void SensorDevice::disableAllSensors() {
974     if (mSensors == nullptr) return;
975     Mutex::Autolock _l(mLock);
976     for (size_t i = 0; i< mActivationCount.size(); ++i) {
977         Info& info = mActivationCount.editValueAt(i);
978         // Check if this sensor has been activated previously and disable it.
979         if (info.batchParams.size() > 0) {
980            const int sensor_handle = mActivationCount.keyAt(i);
981            ALOGD_IF(DEBUG_CONNECTIONS, "\t>> actuating h/w sensor disable handle=%d ",
982                    sensor_handle);
983            checkReturn(mSensors->activate(sensor_handle, 0 /* enabled */));
984 
985            // Add all the connections that were registered for this sensor to the disabled
986            // clients list.
987            for (size_t j = 0; j < info.batchParams.size(); ++j) {
988                addDisabledReasonForIdentLocked(
989                    info.batchParams.keyAt(j), DisabledReason::DISABLED_REASON_SERVICE_RESTRICTED);
990                ALOGI("added %p to mDisabledClients", info.batchParams.keyAt(j));
991            }
992 
993            info.isActive = false;
994         }
995     }
996 }
997 
injectSensorData(const sensors_event_t * injected_sensor_event)998 status_t SensorDevice::injectSensorData(
999         const sensors_event_t *injected_sensor_event) {
1000     if (mSensors == nullptr) return NO_INIT;
1001     ALOGD_IF(DEBUG_CONNECTIONS,
1002             "sensor_event handle=%d ts=%" PRId64 " data=%.2f, %.2f, %.2f %.2f %.2f %.2f",
1003             injected_sensor_event->sensor,
1004             injected_sensor_event->timestamp, injected_sensor_event->data[0],
1005             injected_sensor_event->data[1], injected_sensor_event->data[2],
1006             injected_sensor_event->data[3], injected_sensor_event->data[4],
1007             injected_sensor_event->data[5]);
1008 
1009     Event ev;
1010     V2_1::implementation::convertFromSensorEvent(*injected_sensor_event, &ev);
1011 
1012     return checkReturnAndGetStatus(mSensors->injectSensorData(ev));
1013 }
1014 
setMode(uint32_t mode)1015 status_t SensorDevice::setMode(uint32_t mode) {
1016     if (mSensors == nullptr) return NO_INIT;
1017     return checkReturnAndGetStatus(mSensors->setOperationMode(
1018             static_cast<hardware::sensors::V1_0::OperationMode>(mode)));
1019 }
1020 
registerDirectChannel(const sensors_direct_mem_t * memory)1021 int32_t SensorDevice::registerDirectChannel(const sensors_direct_mem_t* memory) {
1022     if (mSensors == nullptr) return NO_INIT;
1023     Mutex::Autolock _l(mLock);
1024 
1025     SharedMemType type;
1026     switch (memory->type) {
1027         case SENSOR_DIRECT_MEM_TYPE_ASHMEM:
1028             type = SharedMemType::ASHMEM;
1029             break;
1030         case SENSOR_DIRECT_MEM_TYPE_GRALLOC:
1031             type = SharedMemType::GRALLOC;
1032             break;
1033         default:
1034             return BAD_VALUE;
1035     }
1036 
1037     SharedMemFormat format;
1038     if (memory->format != SENSOR_DIRECT_FMT_SENSORS_EVENT) {
1039         return BAD_VALUE;
1040     }
1041     format = SharedMemFormat::SENSORS_EVENT;
1042 
1043     SharedMemInfo mem = {
1044         .type = type,
1045         .format = format,
1046         .size = static_cast<uint32_t>(memory->size),
1047         .memoryHandle = memory->handle,
1048     };
1049 
1050     int32_t ret;
1051     checkReturn(mSensors->registerDirectChannel(mem,
1052             [&ret](auto result, auto channelHandle) {
1053                 if (result == Result::OK) {
1054                     ret = channelHandle;
1055                 } else {
1056                     ret = statusFromResult(result);
1057                 }
1058             }));
1059     return ret;
1060 }
1061 
unregisterDirectChannel(int32_t channelHandle)1062 void SensorDevice::unregisterDirectChannel(int32_t channelHandle) {
1063     if (mSensors == nullptr) return;
1064     Mutex::Autolock _l(mLock);
1065     checkReturn(mSensors->unregisterDirectChannel(channelHandle));
1066 }
1067 
configureDirectChannel(int32_t sensorHandle,int32_t channelHandle,const struct sensors_direct_cfg_t * config)1068 int32_t SensorDevice::configureDirectChannel(int32_t sensorHandle,
1069         int32_t channelHandle, const struct sensors_direct_cfg_t *config) {
1070     if (mSensors == nullptr) return NO_INIT;
1071     Mutex::Autolock _l(mLock);
1072 
1073     RateLevel rate;
1074     switch(config->rate_level) {
1075         case SENSOR_DIRECT_RATE_STOP:
1076             rate = RateLevel::STOP;
1077             break;
1078         case SENSOR_DIRECT_RATE_NORMAL:
1079             rate = RateLevel::NORMAL;
1080             break;
1081         case SENSOR_DIRECT_RATE_FAST:
1082             rate = RateLevel::FAST;
1083             break;
1084         case SENSOR_DIRECT_RATE_VERY_FAST:
1085             rate = RateLevel::VERY_FAST;
1086             break;
1087         default:
1088             return BAD_VALUE;
1089     }
1090 
1091     int32_t ret;
1092     checkReturn(mSensors->configDirectReport(sensorHandle, channelHandle, rate,
1093             [&ret, rate] (auto result, auto token) {
1094                 if (rate == RateLevel::STOP) {
1095                     ret = statusFromResult(result);
1096                 } else {
1097                     if (result == Result::OK) {
1098                         ret = token;
1099                     } else {
1100                         ret = statusFromResult(result);
1101                     }
1102                 }
1103             }));
1104 
1105     return ret;
1106 }
1107 
1108 // ---------------------------------------------------------------------------
1109 
numActiveClients() const1110 int SensorDevice::Info::numActiveClients() const {
1111     SensorDevice& device(SensorDevice::getInstance());
1112     int num = 0;
1113     for (size_t i = 0; i < batchParams.size(); ++i) {
1114         if (!device.isClientDisabledLocked(batchParams.keyAt(i))) {
1115             ++num;
1116         }
1117     }
1118     return num;
1119 }
1120 
setBatchParamsForIdent(void * ident,int,int64_t samplingPeriodNs,int64_t maxBatchReportLatencyNs)1121 status_t SensorDevice::Info::setBatchParamsForIdent(void* ident, int,
1122                                                     int64_t samplingPeriodNs,
1123                                                     int64_t maxBatchReportLatencyNs) {
1124     ssize_t index = batchParams.indexOfKey(ident);
1125     if (index < 0) {
1126         ALOGE("Info::setBatchParamsForIdent(ident=%p, period_ns=%" PRId64
1127               " timeout=%" PRId64 ") failed (%s)",
1128               ident, samplingPeriodNs, maxBatchReportLatencyNs, strerror(-index));
1129         return BAD_INDEX;
1130     }
1131     BatchParams& params = batchParams.editValueAt(index);
1132     params.mTSample = samplingPeriodNs;
1133     params.mTBatch = maxBatchReportLatencyNs;
1134     return NO_ERROR;
1135 }
1136 
selectBatchParams()1137 void SensorDevice::Info::selectBatchParams() {
1138     BatchParams bestParams; // default to max Tsample and max Tbatch
1139     SensorDevice& device(SensorDevice::getInstance());
1140 
1141     for (size_t i = 0; i < batchParams.size(); ++i) {
1142         if (device.isClientDisabledLocked(batchParams.keyAt(i))) {
1143             continue;
1144         }
1145         bestParams.merge(batchParams[i]);
1146     }
1147     // if mTBatch <= mTSample, it is in streaming mode. set mTbatch to 0 to demand this explicitly.
1148     if (bestParams.mTBatch <= bestParams.mTSample) {
1149         bestParams.mTBatch = 0;
1150     }
1151     bestBatchParams = bestParams;
1152 }
1153 
removeBatchParamsForIdent(void * ident)1154 ssize_t SensorDevice::Info::removeBatchParamsForIdent(void* ident) {
1155     ssize_t idx = batchParams.removeItem(ident);
1156     if (idx >= 0) {
1157         selectBatchParams();
1158     }
1159     return idx;
1160 }
1161 
notifyConnectionDestroyed(void * ident)1162 void SensorDevice::notifyConnectionDestroyed(void* ident) {
1163     Mutex::Autolock _l(mLock);
1164     mDisabledClients.erase(ident);
1165 }
1166 
isDirectReportSupported() const1167 bool SensorDevice::isDirectReportSupported() const {
1168     return mIsDirectReportSupported;
1169 }
1170 
convertToSensorEvent(const Event & src,sensors_event_t * dst)1171 void SensorDevice::convertToSensorEvent(
1172         const Event &src, sensors_event_t *dst) {
1173     V2_1::implementation::convertToSensorEvent(src, dst);
1174 
1175     if (src.sensorType == V2_1::SensorType::DYNAMIC_SENSOR_META) {
1176         const DynamicSensorInfo &dyn = src.u.dynamic;
1177 
1178         dst->dynamic_sensor_meta.connected = dyn.connected;
1179         dst->dynamic_sensor_meta.handle = dyn.sensorHandle;
1180         if (dyn.connected) {
1181             std::unique_lock<std::mutex> lock(mDynamicSensorsMutex);
1182             // Give MAX_DYN_SENSOR_WAIT_SEC for onDynamicSensorsConnected to be invoked since it
1183             // can be received out of order from this event due to a bug in the HIDL spec that
1184             // marks it as oneway.
1185             auto it = mConnectedDynamicSensors.find(dyn.sensorHandle);
1186             if (it == mConnectedDynamicSensors.end()) {
1187                 mDynamicSensorsCv.wait_for(lock, MAX_DYN_SENSOR_WAIT,
1188                         [&, dyn]{
1189                             return mConnectedDynamicSensors.find(dyn.sensorHandle)
1190                                     != mConnectedDynamicSensors.end();
1191                 });
1192                 it = mConnectedDynamicSensors.find(dyn.sensorHandle);
1193                 CHECK(it != mConnectedDynamicSensors.end());
1194             }
1195 
1196             dst->dynamic_sensor_meta.sensor = it->second;
1197 
1198             memcpy(dst->dynamic_sensor_meta.uuid,
1199                    dyn.uuid.data(),
1200                    sizeof(dst->dynamic_sensor_meta.uuid));
1201         }
1202     }
1203 }
1204 
convertToSensorEventsAndQuantize(const hidl_vec<Event> & src,const hidl_vec<SensorInfo> & dynamicSensorsAdded,sensors_event_t * dst)1205 void SensorDevice::convertToSensorEventsAndQuantize(
1206         const hidl_vec<Event> &src,
1207         const hidl_vec<SensorInfo> &dynamicSensorsAdded,
1208         sensors_event_t *dst) {
1209 
1210     if (dynamicSensorsAdded.size() > 0) {
1211         onDynamicSensorsConnected(dynamicSensorsAdded);
1212     }
1213 
1214     for (size_t i = 0; i < src.size(); ++i) {
1215         V2_1::implementation::convertToSensorEvent(src[i], &dst[i]);
1216         android::SensorDeviceUtils::quantizeSensorEventValues(&dst[i],
1217                 getResolutionForSensor(dst[i].sensor));
1218     }
1219 }
1220 
getResolutionForSensor(int sensorHandle)1221 float SensorDevice::getResolutionForSensor(int sensorHandle) {
1222     for (size_t i = 0; i < mSensorList.size(); i++) {
1223       if (sensorHandle == mSensorList[i].handle) {
1224         return mSensorList[i].resolution;
1225       }
1226     }
1227 
1228     auto it = mConnectedDynamicSensors.find(sensorHandle);
1229     if (it != mConnectedDynamicSensors.end()) {
1230       return it->second->resolution;
1231     }
1232 
1233     return 0;
1234 }
1235 
handleHidlDeath(const std::string & detail)1236 void SensorDevice::handleHidlDeath(const std::string & detail) {
1237     if (!mSensors->supportsMessageQueues()) {
1238         // restart is the only option at present.
1239         LOG_ALWAYS_FATAL("Abort due to ISensors hidl service failure, detail: %s.", detail.c_str());
1240     } else {
1241         ALOGD("ISensors HAL died, death recipient will attempt reconnect");
1242     }
1243 }
1244 
checkReturnAndGetStatus(const Return<Result> & ret)1245 status_t SensorDevice::checkReturnAndGetStatus(const Return<Result>& ret) {
1246     checkReturn(ret);
1247     return (!ret.isOk()) ? DEAD_OBJECT : statusFromResult(ret);
1248 }
1249 
1250 // ---------------------------------------------------------------------------
1251 }; // namespace android
1252