SensorDevice.cpp revision d333511e94afbcc6462dd9c81405f4a3e30ecac9 
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 <inttypes.h>
18#include <math.h>
19#include <stdint.h>
20#include <sys/types.h>
21
22#include <android-base/logging.h>
23#include <utils/Atomic.h>
24#include <utils/Errors.h>
25#include <utils/Singleton.h>
26
27#include "SensorDevice.h"
28#include "SensorService.h"
29
30#include <sensors/convert.h>
31
32using android::hardware::hidl_vec;
33
34using namespace android::hardware::sensors::V1_0;
35using namespace android::hardware::sensors::V1_0::implementation;
36
37namespace android {
38// ---------------------------------------------------------------------------
39
40ANDROID_SINGLETON_STATIC_INSTANCE(SensorDevice)
41
42static status_t StatusFromResult(Result result) {
43    switch (result) {
44        case Result::OK:
45            return OK;
46        case Result::BAD_VALUE:
47            return BAD_VALUE;
48        case Result::PERMISSION_DENIED:
49            return PERMISSION_DENIED;
50        case Result::INVALID_OPERATION:
51            return INVALID_OPERATION;
52        case Result::NO_MEMORY:
53            return NO_MEMORY;
54    }
55}
56
57SensorDevice::SensorDevice() {
58    mSensors = ISensors::getService();
59
60    if (mSensors == NULL) {
61        return;
62    }
63
64    mSensors->getSensorsList(
65            [&](const auto &list) {
66                const size_t count = list.size();
67
68                mActivationCount.setCapacity(count);
69                Info model;
70                for (size_t i=0 ; i < count; i++) {
71                    sensor_t sensor;
72                    convertToSensor(list[i], &sensor);
73                    mSensorList.push_back(sensor);
74
75                    mActivationCount.add(list[i].sensorHandle, model);
76
77                    mSensors->activate(list[i].sensorHandle, 0 /* enabled */);
78                }
79            });
80
81    mIsDirectReportSupported =
82           (mSensors->unregisterDirectChannel(-1) != Result::INVALID_OPERATION);
83}
84
85void SensorDevice::handleDynamicSensorConnection(int handle, bool connected) {
86    if (connected) {
87        Info model;
88        mActivationCount.add(handle, model);
89        mSensors->activate(handle, 0 /* enabled */);
90    } else {
91        mActivationCount.removeItem(handle);
92    }
93}
94
95std::string SensorDevice::dump() const {
96    if (mSensors == NULL) return "HAL not initialized\n";
97
98    String8 result;
99    mSensors->getSensorsList([&](const auto &list) {
100            const size_t count = list.size();
101
102            result.appendFormat(
103                "Total %zu h/w sensors, %zu running:\n",
104                count,
105                mActivationCount.size());
106
107            Mutex::Autolock _l(mLock);
108            for (size_t i = 0 ; i < count ; i++) {
109                const Info& info = mActivationCount.valueFor(
110                    list[i].sensorHandle);
111
112                if (info.batchParams.isEmpty()) continue;
113                result.appendFormat(
114                    "0x%08x) active-count = %zu; ",
115                    list[i].sensorHandle,
116                    info.batchParams.size());
117
118                result.append("sampling_period(ms) = {");
119                for (size_t j = 0; j < info.batchParams.size(); j++) {
120                    const BatchParams& params = info.batchParams.valueAt(j);
121                    result.appendFormat(
122                        "%.1f%s",
123                        params.batchDelay / 1e6f,
124                        j < info.batchParams.size() - 1 ? ", " : "");
125                }
126                result.appendFormat(
127                        "}, selected = %.1f ms; ",
128                        info.bestBatchParams.batchDelay / 1e6f);
129
130                result.append("batching_period(ms) = {");
131                for (size_t j = 0; j < info.batchParams.size(); j++) {
132                    BatchParams params = info.batchParams.valueAt(j);
133
134                    result.appendFormat(
135                            "%.1f%s",
136                            params.batchTimeout / 1e6f,
137                            j < info.batchParams.size() - 1 ? ", " : "");
138                }
139
140                result.appendFormat(
141                        "}, selected = %.1f ms\n",
142                        info.bestBatchParams.batchTimeout / 1e6f);
143            }
144        });
145
146    return result.string();
147}
148
149ssize_t SensorDevice::getSensorList(sensor_t const** list) {
150    *list = &mSensorList[0];
151
152    return mSensorList.size();
153}
154
155status_t SensorDevice::initCheck() const {
156    return mSensors != NULL ? NO_ERROR : NO_INIT;
157}
158
159ssize_t SensorDevice::poll(sensors_event_t* buffer, size_t count) {
160    if (mSensors == NULL) return NO_INIT;
161
162    ssize_t err;
163
164    mSensors->poll(
165            count,
166            [&](auto result,
167                const auto &events,
168                const auto &dynamicSensorsAdded) {
169                if (result == Result::OK) {
170                    convertToSensorEvents(events, dynamicSensorsAdded, buffer);
171                    err = (ssize_t)events.size();
172                } else {
173                    err = StatusFromResult(result);
174                }
175            });
176
177    return err;
178}
179
180void SensorDevice::autoDisable(void *ident, int handle) {
181    Info& info( mActivationCount.editValueFor(handle) );
182    Mutex::Autolock _l(mLock);
183    info.removeBatchParamsForIdent(ident);
184}
185
186status_t SensorDevice::activate(void* ident, int handle, int enabled) {
187    if (mSensors == NULL) return NO_INIT;
188
189    status_t err(NO_ERROR);
190    bool actuateHardware = false;
191
192    Mutex::Autolock _l(mLock);
193    Info& info( mActivationCount.editValueFor(handle) );
194
195    ALOGD_IF(DEBUG_CONNECTIONS,
196             "SensorDevice::activate: ident=%p, handle=0x%08x, enabled=%d, count=%zu",
197             ident, handle, enabled, info.batchParams.size());
198
199    if (enabled) {
200        ALOGD_IF(DEBUG_CONNECTIONS, "enable index=%zd", info.batchParams.indexOfKey(ident));
201
202        if (isClientDisabledLocked(ident)) {
203            ALOGE("SensorDevice::activate, isClientDisabledLocked(%p):true, handle:%d",
204                    ident, handle);
205            return INVALID_OPERATION;
206        }
207
208        if (info.batchParams.indexOfKey(ident) >= 0) {
209          if (info.numActiveClients() == 1) {
210              // This is the first connection, we need to activate the underlying h/w sensor.
211              actuateHardware = true;
212          }
213        } else {
214            // Log error. Every activate call should be preceded by a batch() call.
215            ALOGE("\t >>>ERROR: activate called without batch");
216        }
217    } else {
218        ALOGD_IF(DEBUG_CONNECTIONS, "disable index=%zd", info.batchParams.indexOfKey(ident));
219
220        // If a connected dynamic sensor is deactivated, remove it from the
221        // dictionary.
222        auto it = mConnectedDynamicSensors.find(handle);
223        if (it != mConnectedDynamicSensors.end()) {
224            delete it->second;
225            mConnectedDynamicSensors.erase(it);
226        }
227
228        if (info.removeBatchParamsForIdent(ident) >= 0) {
229            if (info.numActiveClients() == 0) {
230                // This is the last connection, we need to de-activate the underlying h/w sensor.
231                actuateHardware = true;
232            } else {
233                // Call batch for this sensor with the previously calculated best effort
234                // batch_rate and timeout. One of the apps has unregistered for sensor
235                // events, and the best effort batch parameters might have changed.
236                ALOGD_IF(DEBUG_CONNECTIONS,
237                         "\t>>> actuating h/w batch %d %d %" PRId64 " %" PRId64, handle,
238                         info.bestBatchParams.flags, info.bestBatchParams.batchDelay,
239                         info.bestBatchParams.batchTimeout);
240                mSensors->batch(
241                        handle,
242                        info.bestBatchParams.batchDelay,
243                        info.bestBatchParams.batchTimeout);
244            }
245        } else {
246            // sensor wasn't enabled for this ident
247        }
248
249        if (isClientDisabledLocked(ident)) {
250            return NO_ERROR;
251        }
252    }
253
254    if (actuateHardware) {
255        ALOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w activate handle=%d enabled=%d", handle,
256                 enabled);
257        err = StatusFromResult(mSensors->activate(handle, enabled));
258        ALOGE_IF(err, "Error %s sensor %d (%s)", enabled ? "activating" : "disabling", handle,
259                 strerror(-err));
260
261        if (err != NO_ERROR && enabled) {
262            // Failure when enabling the sensor. Clean up on failure.
263            info.removeBatchParamsForIdent(ident);
264        }
265    }
266
267    return err;
268}
269
270status_t SensorDevice::batch(
271        void* ident,
272        int handle,
273        int flags,
274        int64_t samplingPeriodNs,
275        int64_t maxBatchReportLatencyNs) {
276    if (mSensors == NULL) return NO_INIT;
277
278    if (samplingPeriodNs < MINIMUM_EVENTS_PERIOD) {
279        samplingPeriodNs = MINIMUM_EVENTS_PERIOD;
280    }
281
282    ALOGD_IF(DEBUG_CONNECTIONS,
283             "SensorDevice::batch: ident=%p, handle=0x%08x, flags=%d, period_ns=%" PRId64 " timeout=%" PRId64,
284             ident, handle, flags, samplingPeriodNs, maxBatchReportLatencyNs);
285
286    Mutex::Autolock _l(mLock);
287    Info& info(mActivationCount.editValueFor(handle));
288
289    if (info.batchParams.indexOfKey(ident) < 0) {
290        BatchParams params(flags, samplingPeriodNs, maxBatchReportLatencyNs);
291        info.batchParams.add(ident, params);
292    } else {
293        // A batch has already been called with this ident. Update the batch parameters.
294        info.setBatchParamsForIdent(ident, flags, samplingPeriodNs, maxBatchReportLatencyNs);
295    }
296
297    BatchParams prevBestBatchParams = info.bestBatchParams;
298    // Find the minimum of all timeouts and batch_rates for this sensor.
299    info.selectBatchParams();
300
301    ALOGD_IF(DEBUG_CONNECTIONS,
302             "\t>>> curr_period=%" PRId64 " min_period=%" PRId64
303             " curr_timeout=%" PRId64 " min_timeout=%" PRId64,
304             prevBestBatchParams.batchDelay, info.bestBatchParams.batchDelay,
305             prevBestBatchParams.batchTimeout, info.bestBatchParams.batchTimeout);
306
307    status_t err(NO_ERROR);
308    // If the min period or min timeout has changed since the last batch call, call batch.
309    if (prevBestBatchParams != info.bestBatchParams) {
310        ALOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w BATCH %d %d %" PRId64 " %" PRId64, handle,
311                 info.bestBatchParams.flags, info.bestBatchParams.batchDelay,
312                 info.bestBatchParams.batchTimeout);
313        err = StatusFromResult(
314                mSensors->batch(
315                    handle,
316                    info.bestBatchParams.batchDelay,
317                    info.bestBatchParams.batchTimeout));
318        if (err != NO_ERROR) {
319            ALOGE("sensor batch failed %p %d %d %" PRId64 " %" PRId64 " err=%s",
320                  mSensors.get(), handle,
321                  info.bestBatchParams.flags, info.bestBatchParams.batchDelay,
322                  info.bestBatchParams.batchTimeout, strerror(-err));
323            info.removeBatchParamsForIdent(ident);
324        }
325    }
326    return err;
327}
328
329status_t SensorDevice::setDelay(void* ident, int handle, int64_t samplingPeriodNs) {
330    if (mSensors == NULL) return NO_INIT;
331    if (samplingPeriodNs < MINIMUM_EVENTS_PERIOD) {
332        samplingPeriodNs = MINIMUM_EVENTS_PERIOD;
333    }
334    Mutex::Autolock _l(mLock);
335    if (isClientDisabledLocked(ident)) return INVALID_OPERATION;
336    Info& info( mActivationCount.editValueFor(handle) );
337    // If the underlying sensor is NOT in continuous mode, setDelay() should return an error.
338    // Calling setDelay() in batch mode is an invalid operation.
339    if (info.bestBatchParams.batchTimeout != 0) {
340      return INVALID_OPERATION;
341    }
342    ssize_t index = info.batchParams.indexOfKey(ident);
343    if (index < 0) {
344        return BAD_INDEX;
345    }
346    BatchParams& params = info.batchParams.editValueAt(index);
347    params.batchDelay = samplingPeriodNs;
348    info.selectBatchParams();
349
350    return StatusFromResult(
351            mSensors->batch(handle, info.bestBatchParams.batchDelay, 0));
352}
353
354int SensorDevice::getHalDeviceVersion() const {
355    if (mSensors == NULL) return -1;
356    return SENSORS_DEVICE_API_VERSION_1_4;
357}
358
359status_t SensorDevice::flush(void* ident, int handle) {
360    if (isClientDisabled(ident)) return INVALID_OPERATION;
361    ALOGD_IF(DEBUG_CONNECTIONS, "\t>>> actuating h/w flush %d", handle);
362    return StatusFromResult(mSensors->flush(handle));
363}
364
365bool SensorDevice::isClientDisabled(void* ident) {
366    Mutex::Autolock _l(mLock);
367    return isClientDisabledLocked(ident);
368}
369
370bool SensorDevice::isClientDisabledLocked(void* ident) {
371    return mDisabledClients.indexOf(ident) >= 0;
372}
373
374void SensorDevice::enableAllSensors() {
375    Mutex::Autolock _l(mLock);
376    mDisabledClients.clear();
377    ALOGI("cleared mDisabledClients");
378    for (size_t i = 0; i< mActivationCount.size(); ++i) {
379        Info& info = mActivationCount.editValueAt(i);
380        if (info.batchParams.isEmpty()) continue;
381        info.selectBatchParams();
382        const int sensor_handle = mActivationCount.keyAt(i);
383        ALOGD_IF(DEBUG_CONNECTIONS, "\t>> reenable actuating h/w sensor enable handle=%d ",
384                   sensor_handle);
385        status_t err = StatusFromResult(
386                mSensors->batch(
387                    sensor_handle,
388                    info.bestBatchParams.batchDelay,
389                    info.bestBatchParams.batchTimeout));
390        ALOGE_IF(err, "Error calling batch on sensor %d (%s)", sensor_handle, strerror(-err));
391
392        if (err == NO_ERROR) {
393            err = StatusFromResult(
394                    mSensors->activate(sensor_handle, 1 /* enabled */));
395            ALOGE_IF(err, "Error activating sensor %d (%s)", sensor_handle, strerror(-err));
396        }
397    }
398}
399
400void SensorDevice::disableAllSensors() {
401    Mutex::Autolock _l(mLock);
402   for (size_t i = 0; i< mActivationCount.size(); ++i) {
403        const Info& info = mActivationCount.valueAt(i);
404        // Check if this sensor has been activated previously and disable it.
405        if (info.batchParams.size() > 0) {
406           const int sensor_handle = mActivationCount.keyAt(i);
407           ALOGD_IF(DEBUG_CONNECTIONS, "\t>> actuating h/w sensor disable handle=%d ",
408                   sensor_handle);
409           mSensors->activate(sensor_handle, 0 /* enabled */);
410
411           // Add all the connections that were registered for this sensor to the disabled
412           // clients list.
413           for (size_t j = 0; j < info.batchParams.size(); ++j) {
414               mDisabledClients.add(info.batchParams.keyAt(j));
415               ALOGI("added %p to mDisabledClients", info.batchParams.keyAt(j));
416           }
417        }
418    }
419}
420
421status_t SensorDevice::injectSensorData(
422        const sensors_event_t *injected_sensor_event) {
423    ALOGD_IF(DEBUG_CONNECTIONS,
424            "sensor_event handle=%d ts=%" PRId64 " data=%.2f, %.2f, %.2f %.2f %.2f %.2f",
425            injected_sensor_event->sensor,
426            injected_sensor_event->timestamp, injected_sensor_event->data[0],
427            injected_sensor_event->data[1], injected_sensor_event->data[2],
428            injected_sensor_event->data[3], injected_sensor_event->data[4],
429            injected_sensor_event->data[5]);
430
431    Event ev;
432    convertFromSensorEvent(*injected_sensor_event, &ev);
433
434    return StatusFromResult(mSensors->injectSensorData(ev));
435}
436
437status_t SensorDevice::setMode(uint32_t mode) {
438
439     return StatusFromResult(
440             mSensors->setOperationMode(
441                 static_cast<hardware::sensors::V1_0::OperationMode>(mode)));
442}
443
444// ---------------------------------------------------------------------------
445
446int SensorDevice::Info::numActiveClients() {
447    SensorDevice& device(SensorDevice::getInstance());
448    int num = 0;
449    for (size_t i = 0; i < batchParams.size(); ++i) {
450        if (!device.isClientDisabledLocked(batchParams.keyAt(i))) {
451            ++num;
452        }
453    }
454    return num;
455}
456
457status_t SensorDevice::Info::setBatchParamsForIdent(void* ident, int flags,
458                                                    int64_t samplingPeriodNs,
459                                                    int64_t maxBatchReportLatencyNs) {
460    ssize_t index = batchParams.indexOfKey(ident);
461    if (index < 0) {
462        ALOGE("Info::setBatchParamsForIdent(ident=%p, period_ns=%" PRId64 " timeout=%" PRId64 ") failed (%s)",
463              ident, samplingPeriodNs, maxBatchReportLatencyNs, strerror(-index));
464        return BAD_INDEX;
465    }
466    BatchParams& params = batchParams.editValueAt(index);
467    params.flags = flags;
468    params.batchDelay = samplingPeriodNs;
469    params.batchTimeout = maxBatchReportLatencyNs;
470    return NO_ERROR;
471}
472
473void SensorDevice::Info::selectBatchParams() {
474    BatchParams bestParams(0, -1, -1);
475    SensorDevice& device(SensorDevice::getInstance());
476
477    for (size_t i = 0; i < batchParams.size(); ++i) {
478        if (device.isClientDisabledLocked(batchParams.keyAt(i))) continue;
479        BatchParams params = batchParams.valueAt(i);
480        if (bestParams.batchDelay == -1 || params.batchDelay < bestParams.batchDelay) {
481            bestParams.batchDelay = params.batchDelay;
482        }
483        if (bestParams.batchTimeout == -1 || params.batchTimeout < bestParams.batchTimeout) {
484            bestParams.batchTimeout = params.batchTimeout;
485        }
486    }
487    bestBatchParams = bestParams;
488}
489
490ssize_t SensorDevice::Info::removeBatchParamsForIdent(void* ident) {
491    ssize_t idx = batchParams.removeItem(ident);
492    if (idx >= 0) {
493        selectBatchParams();
494    }
495    return idx;
496}
497
498void SensorDevice::notifyConnectionDestroyed(void* ident) {
499    Mutex::Autolock _l(mLock);
500    mDisabledClients.remove(ident);
501}
502
503int32_t SensorDevice::registerDirectChannel(const sensors_direct_mem_t* memory) {
504    Mutex::Autolock _l(mLock);
505
506    SharedMemType type;
507    switch (memory->type) {
508        case SENSOR_DIRECT_MEM_TYPE_ASHMEM:
509            type = SharedMemType::ASHMEM;
510            break;
511        case SENSOR_DIRECT_MEM_TYPE_GRALLOC:
512            type = SharedMemType::GRALLOC;
513            break;
514        default:
515            return BAD_VALUE;
516    }
517
518    SharedMemFormat format;
519    if (memory->format != SENSOR_DIRECT_FMT_SENSORS_EVENT) {
520        return BAD_VALUE;
521    }
522    format = SharedMemFormat::SENSORS_EVENT;
523
524    SharedMemInfo mem = {
525        .type = type,
526        .format = format,
527        .size = static_cast<uint32_t>(memory->size),
528        .memoryHandle = memory->handle,
529    };
530
531    int32_t ret;
532    mSensors->registerDirectChannel(mem,
533            [&ret](auto result, auto channelHandle) {
534                if (result == Result::OK) {
535                    ret = channelHandle;
536                } else {
537                    ret = StatusFromResult(result);
538                }
539            });
540    return ret;
541}
542
543void SensorDevice::unregisterDirectChannel(int32_t channelHandle) {
544    Mutex::Autolock _l(mLock);
545    mSensors->unregisterDirectChannel(channelHandle);
546}
547
548int32_t SensorDevice::configureDirectChannel(int32_t sensorHandle,
549        int32_t channelHandle, const struct sensors_direct_cfg_t *config) {
550    Mutex::Autolock _l(mLock);
551
552    RateLevel rate;
553    switch(config->rate_level) {
554        case SENSOR_DIRECT_RATE_STOP:
555            rate = RateLevel::STOP;
556            break;
557        case SENSOR_DIRECT_RATE_NORMAL:
558            rate = RateLevel::NORMAL;
559            break;
560        case SENSOR_DIRECT_RATE_FAST:
561            rate = RateLevel::FAST;
562            break;
563        case SENSOR_DIRECT_RATE_VERY_FAST:
564            rate = RateLevel::VERY_FAST;
565            break;
566        default:
567            return BAD_VALUE;
568    }
569
570    int32_t ret;
571    mSensors->configDirectReport(sensorHandle, channelHandle, rate,
572            [&ret, rate] (auto result, auto token) {
573                if (rate == RateLevel::STOP) {
574                    ret = StatusFromResult(result);
575                } else {
576                    if (result == Result::OK) {
577                        ret = token;
578                    } else {
579                        ret = StatusFromResult(result);
580                    }
581                }
582            });
583
584    return ret;
585}
586
587bool SensorDevice::isDirectReportSupported() const {
588    return mIsDirectReportSupported;
589}
590
591void SensorDevice::convertToSensorEvent(
592        const Event &src, sensors_event_t *dst) {
593    ::android::hardware::sensors::V1_0::implementation::convertToSensorEvent(
594            src, dst);
595
596    if (src.sensorType == SensorType::DYNAMIC_SENSOR_META) {
597        const DynamicSensorInfo &dyn = src.u.dynamic;
598
599        dst->dynamic_sensor_meta.connected = dyn.connected;
600        dst->dynamic_sensor_meta.handle = dyn.sensorHandle;
601        if (dyn.connected) {
602            auto it = mConnectedDynamicSensors.find(dyn.sensorHandle);
603            CHECK(it != mConnectedDynamicSensors.end());
604
605            dst->dynamic_sensor_meta.sensor = it->second;
606
607            memcpy(dst->dynamic_sensor_meta.uuid,
608                   dyn.uuid.data(),
609                   sizeof(dst->dynamic_sensor_meta.uuid));
610        }
611    }
612}
613
614void SensorDevice::convertToSensorEvents(
615        const hidl_vec<Event> &src,
616        const hidl_vec<SensorInfo> &dynamicSensorsAdded,
617        sensors_event_t *dst) {
618    // Allocate a sensor_t structure for each dynamic sensor added and insert
619    // it into the dictionary of connected dynamic sensors keyed by handle.
620    for (size_t i = 0; i < dynamicSensorsAdded.size(); ++i) {
621        const SensorInfo &info = dynamicSensorsAdded[i];
622
623        auto it = mConnectedDynamicSensors.find(info.sensorHandle);
624        CHECK(it == mConnectedDynamicSensors.end());
625
626        sensor_t *sensor = new sensor_t;
627        convertToSensor(info, sensor);
628
629        mConnectedDynamicSensors.insert(
630                std::make_pair(sensor->handle, sensor));
631    }
632
633    for (size_t i = 0; i < src.size(); ++i) {
634        convertToSensorEvent(src[i], &dst[i]);
635    }
636}
637
638// ---------------------------------------------------------------------------
639}; // namespace android
640