SensorService.cpp revision a5b8e8bfa9f3416ce61ee08162fb139afde60488 
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 <stdint.h>
18#include <math.h>
19#include <sys/types.h>
20
21#include <cutils/properties.h>
22
23#include <utils/SortedVector.h>
24#include <utils/KeyedVector.h>
25#include <utils/threads.h>
26#include <utils/Atomic.h>
27#include <utils/Errors.h>
28#include <utils/RefBase.h>
29#include <utils/Singleton.h>
30#include <utils/String16.h>
31
32#include <binder/BinderService.h>
33#include <binder/IServiceManager.h>
34#include <binder/PermissionCache.h>
35
36#include <gui/ISensorServer.h>
37#include <gui/ISensorEventConnection.h>
38#include <gui/SensorEventQueue.h>
39
40#include <hardware/sensors.h>
41
42#include "CorrectedGyroSensor.h"
43#include "GravitySensor.h"
44#include "LinearAccelerationSensor.h"
45#include "OrientationSensor.h"
46#include "RotationVectorSensor.h"
47#include "SensorFusion.h"
48#include "SensorService.h"
49
50namespace android {
51// ---------------------------------------------------------------------------
52
53/*
54 * Notes:
55 *
56 * - what about a gyro-corrected magnetic-field sensor?
57 * - run mag sensor from time to time to force calibration
58 * - gravity sensor length is wrong (=> drift in linear-acc sensor)
59 *
60 */
61
62SensorService::SensorService()
63    : mInitCheck(NO_INIT)
64{
65}
66
67void SensorService::onFirstRef()
68{
69    ALOGD("nuSensorService starting...");
70
71    SensorDevice& dev(SensorDevice::getInstance());
72
73    if (dev.initCheck() == NO_ERROR) {
74        sensor_t const* list;
75        ssize_t count = dev.getSensorList(&list);
76        if (count > 0) {
77            ssize_t orientationIndex = -1;
78            bool hasGyro = false;
79            uint32_t virtualSensorsNeeds =
80                    (1<<SENSOR_TYPE_GRAVITY) |
81                    (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
82                    (1<<SENSOR_TYPE_ROTATION_VECTOR);
83
84            mLastEventSeen.setCapacity(count);
85            for (ssize_t i=0 ; i<count ; i++) {
86                registerSensor( new HardwareSensor(list[i]) );
87                switch (list[i].type) {
88                    case SENSOR_TYPE_ORIENTATION:
89                        orientationIndex = i;
90                        break;
91                    case SENSOR_TYPE_GYROSCOPE:
92                        hasGyro = true;
93                        break;
94                    case SENSOR_TYPE_GRAVITY:
95                    case SENSOR_TYPE_LINEAR_ACCELERATION:
96                    case SENSOR_TYPE_ROTATION_VECTOR:
97                        virtualSensorsNeeds &= ~(1<<list[i].type);
98                        break;
99                }
100            }
101
102            // it's safe to instantiate the SensorFusion object here
103            // (it wants to be instantiated after h/w sensors have been
104            // registered)
105            const SensorFusion& fusion(SensorFusion::getInstance());
106
107            if (hasGyro) {
108                // Always instantiate Android's virtual sensors. Since they are
109                // instantiated behind sensors from the HAL, they won't
110                // interfere with applications, unless they looks specifically
111                // for them (by name).
112
113                registerVirtualSensor( new RotationVectorSensor() );
114                registerVirtualSensor( new GravitySensor(list, count) );
115                registerVirtualSensor( new LinearAccelerationSensor(list, count) );
116
117                // these are optional
118                registerVirtualSensor( new OrientationSensor() );
119                registerVirtualSensor( new CorrectedGyroSensor(list, count) );
120            }
121
122            // build the sensor list returned to users
123            mUserSensorList = mSensorList;
124
125            if (hasGyro) {
126                // virtual debugging sensors are not added to mUserSensorList
127                registerVirtualSensor( new GyroDriftSensor() );
128            }
129
130            if (hasGyro &&
131                    (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR))) {
132                // if we have the fancy sensor fusion, and it's not provided by the
133                // HAL, use our own (fused) orientation sensor by removing the
134                // HAL supplied one form the user list.
135                if (orientationIndex >= 0) {
136                    mUserSensorList.removeItemsAt(orientationIndex);
137                }
138            }
139
140            // debugging sensor list
141            for (size_t i=0 ; i<mSensorList.size() ; i++) {
142                switch (mSensorList[i].getType()) {
143                    case SENSOR_TYPE_GRAVITY:
144                    case SENSOR_TYPE_LINEAR_ACCELERATION:
145                    case SENSOR_TYPE_ROTATION_VECTOR:
146                        if (strstr(mSensorList[i].getVendor().string(), "Google")) {
147                            mUserSensorListDebug.add(mSensorList[i]);
148                        }
149                        break;
150                    default:
151                        mUserSensorListDebug.add(mSensorList[i]);
152                        break;
153                }
154            }
155
156            run("SensorService", PRIORITY_URGENT_DISPLAY);
157            mInitCheck = NO_ERROR;
158        }
159    }
160}
161
162void SensorService::registerSensor(SensorInterface* s)
163{
164    sensors_event_t event;
165    memset(&event, 0, sizeof(event));
166
167    const Sensor sensor(s->getSensor());
168    // add to the sensor list (returned to clients)
169    mSensorList.add(sensor);
170    // add to our handle->SensorInterface mapping
171    mSensorMap.add(sensor.getHandle(), s);
172    // create an entry in the mLastEventSeen array
173    mLastEventSeen.add(sensor.getHandle(), event);
174}
175
176void SensorService::registerVirtualSensor(SensorInterface* s)
177{
178    registerSensor(s);
179    mVirtualSensorList.add( s );
180}
181
182SensorService::~SensorService()
183{
184    for (size_t i=0 ; i<mSensorMap.size() ; i++)
185        delete mSensorMap.valueAt(i);
186}
187
188static const String16 sDump("android.permission.DUMP");
189
190status_t SensorService::dump(int fd, const Vector<String16>& args)
191{
192    const size_t SIZE = 1024;
193    char buffer[SIZE];
194    String8 result;
195    if (!PermissionCache::checkCallingPermission(sDump)) {
196        snprintf(buffer, SIZE, "Permission Denial: "
197                "can't dump SurfaceFlinger from pid=%d, uid=%d\n",
198                IPCThreadState::self()->getCallingPid(),
199                IPCThreadState::self()->getCallingUid());
200        result.append(buffer);
201    } else {
202        Mutex::Autolock _l(mLock);
203        snprintf(buffer, SIZE, "Sensor List:\n");
204        result.append(buffer);
205        for (size_t i=0 ; i<mSensorList.size() ; i++) {
206            const Sensor& s(mSensorList[i]);
207            const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle()));
208            snprintf(buffer, SIZE,
209                    "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | "
210                    "last=<%5.1f,%5.1f,%5.1f>\n",
211                    s.getName().string(),
212                    s.getVendor().string(),
213                    s.getHandle(),
214                    s.getMinDelay() ? (1000000.0f / s.getMinDelay()) : 0.0f,
215                    e.data[0], e.data[1], e.data[2]);
216            result.append(buffer);
217        }
218        SensorFusion::getInstance().dump(result, buffer, SIZE);
219        SensorDevice::getInstance().dump(result, buffer, SIZE);
220
221        snprintf(buffer, SIZE, "%d active connections\n",
222                mActiveConnections.size());
223        result.append(buffer);
224        snprintf(buffer, SIZE, "Active sensors:\n");
225        result.append(buffer);
226        for (size_t i=0 ; i<mActiveSensors.size() ; i++) {
227            int handle = mActiveSensors.keyAt(i);
228            snprintf(buffer, SIZE, "%s (handle=0x%08x, connections=%d)\n",
229                    getSensorName(handle).string(),
230                    handle,
231                    mActiveSensors.valueAt(i)->getNumConnections());
232            result.append(buffer);
233        }
234    }
235    write(fd, result.string(), result.size());
236    return NO_ERROR;
237}
238
239bool SensorService::threadLoop()
240{
241    ALOGD("nuSensorService thread starting...");
242
243    const size_t numEventMax = 16;
244    const size_t minBufferSize = numEventMax + numEventMax * mVirtualSensorList.size();
245    sensors_event_t buffer[minBufferSize];
246    sensors_event_t scratch[minBufferSize];
247    SensorDevice& device(SensorDevice::getInstance());
248    const size_t vcount = mVirtualSensorList.size();
249
250    ssize_t count;
251    do {
252        count = device.poll(buffer, numEventMax);
253        if (count<0) {
254            ALOGE("sensor poll failed (%s)", strerror(-count));
255            break;
256        }
257
258        recordLastValue(buffer, count);
259
260        // handle virtual sensors
261        if (count && vcount) {
262            sensors_event_t const * const event = buffer;
263            const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
264                    getActiveVirtualSensors());
265            const size_t activeVirtualSensorCount = virtualSensors.size();
266            if (activeVirtualSensorCount) {
267                size_t k = 0;
268                SensorFusion& fusion(SensorFusion::getInstance());
269                if (fusion.isEnabled()) {
270                    for (size_t i=0 ; i<size_t(count) ; i++) {
271                        fusion.process(event[i]);
272                    }
273                }
274                for (size_t i=0 ; i<size_t(count) && k<minBufferSize ; i++) {
275                    for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
276                        if (count + k >= minBufferSize) {
277                            ALOGE("buffer too small to hold all events: "
278                                    "count=%u, k=%u, size=%u",
279                                    count, k, minBufferSize);
280                            break;
281                        }
282                        sensors_event_t out;
283                        SensorInterface* si = virtualSensors.valueAt(j);
284                        if (si->process(&out, event[i])) {
285                            buffer[count + k] = out;
286                            k++;
287                        }
288                    }
289                }
290                if (k) {
291                    // record the last synthesized values
292                    recordLastValue(&buffer[count], k);
293                    count += k;
294                    // sort the buffer by time-stamps
295                    sortEventBuffer(buffer, count);
296                }
297            }
298        }
299
300        // send our events to clients...
301        const SortedVector< wp<SensorEventConnection> > activeConnections(
302                getActiveConnections());
303        size_t numConnections = activeConnections.size();
304        for (size_t i=0 ; i<numConnections ; i++) {
305            sp<SensorEventConnection> connection(
306                    activeConnections[i].promote());
307            if (connection != 0) {
308                connection->sendEvents(buffer, count, scratch);
309            }
310        }
311    } while (count >= 0 || Thread::exitPending());
312
313    ALOGW("Exiting SensorService::threadLoop => aborting...");
314    abort();
315    return false;
316}
317
318void SensorService::recordLastValue(
319        sensors_event_t const * buffer, size_t count)
320{
321    Mutex::Autolock _l(mLock);
322
323    // record the last event for each sensor
324    int32_t prev = buffer[0].sensor;
325    for (size_t i=1 ; i<count ; i++) {
326        // record the last event of each sensor type in this buffer
327        int32_t curr = buffer[i].sensor;
328        if (curr != prev) {
329            mLastEventSeen.editValueFor(prev) = buffer[i-1];
330            prev = curr;
331        }
332    }
333    mLastEventSeen.editValueFor(prev) = buffer[count-1];
334}
335
336void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count)
337{
338    struct compar {
339        static int cmp(void const* lhs, void const* rhs) {
340            sensors_event_t const* l = static_cast<sensors_event_t const*>(lhs);
341            sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs);
342            return l->timestamp - r->timestamp;
343        }
344    };
345    qsort(buffer, count, sizeof(sensors_event_t), compar::cmp);
346}
347
348SortedVector< wp<SensorService::SensorEventConnection> >
349SensorService::getActiveConnections() const
350{
351    Mutex::Autolock _l(mLock);
352    return mActiveConnections;
353}
354
355DefaultKeyedVector<int, SensorInterface*>
356SensorService::getActiveVirtualSensors() const
357{
358    Mutex::Autolock _l(mLock);
359    return mActiveVirtualSensors;
360}
361
362String8 SensorService::getSensorName(int handle) const {
363    size_t count = mUserSensorList.size();
364    for (size_t i=0 ; i<count ; i++) {
365        const Sensor& sensor(mUserSensorList[i]);
366        if (sensor.getHandle() == handle) {
367            return sensor.getName();
368        }
369    }
370    String8 result("unknown");
371    return result;
372}
373
374Vector<Sensor> SensorService::getSensorList()
375{
376    char value[PROPERTY_VALUE_MAX];
377    property_get("debug.sensors", value, "0");
378    if (atoi(value)) {
379        return mUserSensorListDebug;
380    }
381    return mUserSensorList;
382}
383
384sp<ISensorEventConnection> SensorService::createSensorEventConnection()
385{
386    sp<SensorEventConnection> result(new SensorEventConnection(this));
387    return result;
388}
389
390void SensorService::cleanupConnection(SensorEventConnection* c)
391{
392    Mutex::Autolock _l(mLock);
393    const wp<SensorEventConnection> connection(c);
394    size_t size = mActiveSensors.size();
395    ALOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size);
396    for (size_t i=0 ; i<size ; ) {
397        int handle = mActiveSensors.keyAt(i);
398        if (c->hasSensor(handle)) {
399            ALOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle);
400            SensorInterface* sensor = mSensorMap.valueFor( handle );
401            ALOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle);
402            if (sensor) {
403                sensor->activate(c, false);
404            }
405        }
406        SensorRecord* rec = mActiveSensors.valueAt(i);
407        ALOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle);
408        ALOGD_IF(DEBUG_CONNECTIONS,
409                "removing connection %p for sensor[%d].handle=0x%08x",
410                c, i, handle);
411
412        if (rec && rec->removeConnection(connection)) {
413            ALOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection");
414            mActiveSensors.removeItemsAt(i, 1);
415            mActiveVirtualSensors.removeItem(handle);
416            delete rec;
417            size--;
418        } else {
419            i++;
420        }
421    }
422    mActiveConnections.remove(connection);
423}
424
425status_t SensorService::enable(const sp<SensorEventConnection>& connection,
426        int handle)
427{
428    if (mInitCheck != NO_ERROR)
429        return mInitCheck;
430
431    Mutex::Autolock _l(mLock);
432    SensorInterface* sensor = mSensorMap.valueFor(handle);
433    status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE);
434    if (err == NO_ERROR) {
435        SensorRecord* rec = mActiveSensors.valueFor(handle);
436        if (rec == 0) {
437            rec = new SensorRecord(connection);
438            mActiveSensors.add(handle, rec);
439            if (sensor->isVirtual()) {
440                mActiveVirtualSensors.add(handle, sensor);
441            }
442        } else {
443            if (rec->addConnection(connection)) {
444                // this sensor is already activated, but we are adding a
445                // connection that uses it. Immediately send down the last
446                // known value of the requested sensor if it's not a
447                // "continuous" sensor.
448                if (sensor->getSensor().getMinDelay() == 0) {
449                    sensors_event_t scratch;
450                    sensors_event_t& event(mLastEventSeen.editValueFor(handle));
451                    if (event.version == sizeof(sensors_event_t)) {
452                        connection->sendEvents(&event, 1);
453                    }
454                }
455            }
456        }
457        if (err == NO_ERROR) {
458            // connection now active
459            if (connection->addSensor(handle)) {
460                // the sensor was added (which means it wasn't already there)
461                // so, see if this connection becomes active
462                if (mActiveConnections.indexOf(connection) < 0) {
463                    mActiveConnections.add(connection);
464                }
465            } else {
466                ALOGW("sensor %08x already enabled in connection %p (ignoring)",
467                        handle, connection.get());
468            }
469        }
470    }
471    return err;
472}
473
474status_t SensorService::disable(const sp<SensorEventConnection>& connection,
475        int handle)
476{
477    if (mInitCheck != NO_ERROR)
478        return mInitCheck;
479
480    status_t err = NO_ERROR;
481    Mutex::Autolock _l(mLock);
482    SensorRecord* rec = mActiveSensors.valueFor(handle);
483    if (rec) {
484        // see if this connection becomes inactive
485        connection->removeSensor(handle);
486        if (connection->hasAnySensor() == false) {
487            mActiveConnections.remove(connection);
488        }
489        // see if this sensor becomes inactive
490        if (rec->removeConnection(connection)) {
491            mActiveSensors.removeItem(handle);
492            mActiveVirtualSensors.removeItem(handle);
493            delete rec;
494        }
495        SensorInterface* sensor = mSensorMap.valueFor(handle);
496        err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
497    }
498    return err;
499}
500
501status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection,
502        int handle, nsecs_t ns)
503{
504    if (mInitCheck != NO_ERROR)
505        return mInitCheck;
506
507    SensorInterface* sensor = mSensorMap.valueFor(handle);
508    if (!sensor)
509        return BAD_VALUE;
510
511    if (ns < 0)
512        return BAD_VALUE;
513
514    nsecs_t minDelayNs = sensor->getSensor().getMinDelayNs();
515    if (ns < minDelayNs) {
516        ns = minDelayNs;
517    }
518
519    if (ns < MINIMUM_EVENTS_PERIOD)
520        ns = MINIMUM_EVENTS_PERIOD;
521
522    return sensor->setDelay(connection.get(), handle, ns);
523}
524
525// ---------------------------------------------------------------------------
526
527SensorService::SensorRecord::SensorRecord(
528        const sp<SensorEventConnection>& connection)
529{
530    mConnections.add(connection);
531}
532
533bool SensorService::SensorRecord::addConnection(
534        const sp<SensorEventConnection>& connection)
535{
536    if (mConnections.indexOf(connection) < 0) {
537        mConnections.add(connection);
538        return true;
539    }
540    return false;
541}
542
543bool SensorService::SensorRecord::removeConnection(
544        const wp<SensorEventConnection>& connection)
545{
546    ssize_t index = mConnections.indexOf(connection);
547    if (index >= 0) {
548        mConnections.removeItemsAt(index, 1);
549    }
550    return mConnections.size() ? false : true;
551}
552
553// ---------------------------------------------------------------------------
554
555SensorService::SensorEventConnection::SensorEventConnection(
556        const sp<SensorService>& service)
557    : mService(service), mChannel(new BitTube())
558{
559}
560
561SensorService::SensorEventConnection::~SensorEventConnection()
562{
563    ALOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this);
564    mService->cleanupConnection(this);
565}
566
567void SensorService::SensorEventConnection::onFirstRef()
568{
569}
570
571bool SensorService::SensorEventConnection::addSensor(int32_t handle) {
572    Mutex::Autolock _l(mConnectionLock);
573    if (mSensorInfo.indexOf(handle) < 0) {
574        mSensorInfo.add(handle);
575        return true;
576    }
577    return false;
578}
579
580bool SensorService::SensorEventConnection::removeSensor(int32_t handle) {
581    Mutex::Autolock _l(mConnectionLock);
582    if (mSensorInfo.remove(handle) >= 0) {
583        return true;
584    }
585    return false;
586}
587
588bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const {
589    Mutex::Autolock _l(mConnectionLock);
590    return mSensorInfo.indexOf(handle) >= 0;
591}
592
593bool SensorService::SensorEventConnection::hasAnySensor() const {
594    Mutex::Autolock _l(mConnectionLock);
595    return mSensorInfo.size() ? true : false;
596}
597
598status_t SensorService::SensorEventConnection::sendEvents(
599        sensors_event_t const* buffer, size_t numEvents,
600        sensors_event_t* scratch)
601{
602    // filter out events not for this connection
603    size_t count = 0;
604    if (scratch) {
605        Mutex::Autolock _l(mConnectionLock);
606        size_t i=0;
607        while (i<numEvents) {
608            const int32_t curr = buffer[i].sensor;
609            if (mSensorInfo.indexOf(curr) >= 0) {
610                do {
611                    scratch[count++] = buffer[i++];
612                } while ((i<numEvents) && (buffer[i].sensor == curr));
613            } else {
614                i++;
615            }
616        }
617    } else {
618        scratch = const_cast<sensors_event_t *>(buffer);
619        count = numEvents;
620    }
621
622    // NOTE: ASensorEvent and sensors_event_t are the same type
623    ssize_t size = SensorEventQueue::write(mChannel,
624            reinterpret_cast<ASensorEvent const*>(scratch), count);
625    if (size == -EAGAIN) {
626        // the destination doesn't accept events anymore, it's probably
627        // full. For now, we just drop the events on the floor.
628        //ALOGW("dropping %d events on the floor", count);
629        return size;
630    }
631
632    return size < 0 ? status_t(size) : status_t(NO_ERROR);
633}
634
635sp<BitTube> SensorService::SensorEventConnection::getSensorChannel() const
636{
637    return mChannel;
638}
639
640status_t SensorService::SensorEventConnection::enableDisable(
641        int handle, bool enabled)
642{
643    status_t err;
644    if (enabled) {
645        err = mService->enable(this, handle);
646    } else {
647        err = mService->disable(this, handle);
648    }
649    return err;
650}
651
652status_t SensorService::SensorEventConnection::setEventRate(
653        int handle, nsecs_t ns)
654{
655    return mService->setEventRate(this, handle, ns);
656}
657
658// ---------------------------------------------------------------------------
659}; // namespace android
660
661