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