SensorService.cpp revision 907103bf186cfdd2ed9eb3b6c36de53ade7b16f6 
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                // virtual debugging sensors...
122                char value[PROPERTY_VALUE_MAX];
123                property_get("debug.sensors", value, "0");
124                if (atoi(value)) {
125                    registerVirtualSensor( new GyroDriftSensor() );
126                }
127            }
128
129            // build the sensor list returned to users
130            mUserSensorList = mSensorList;
131            if (hasGyro &&
132                    (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR))) {
133                // if we have the fancy sensor fusion, and it's not provided by the
134                // HAL, use our own (fused) orientation sensor by removing the
135                // HAL supplied one form the user list.
136                if (orientationIndex >= 0) {
137                    mUserSensorList.removeItemsAt(orientationIndex);
138                }
139            }
140
141            run("SensorService", PRIORITY_URGENT_DISPLAY);
142            mInitCheck = NO_ERROR;
143        }
144    }
145}
146
147void SensorService::registerSensor(SensorInterface* s)
148{
149    sensors_event_t event;
150    memset(&event, 0, sizeof(event));
151
152    const Sensor sensor(s->getSensor());
153    // add to the sensor list (returned to clients)
154    mSensorList.add(sensor);
155    // add to our handle->SensorInterface mapping
156    mSensorMap.add(sensor.getHandle(), s);
157    // create an entry in the mLastEventSeen array
158    mLastEventSeen.add(sensor.getHandle(), event);
159}
160
161void SensorService::registerVirtualSensor(SensorInterface* s)
162{
163    registerSensor(s);
164    mVirtualSensorList.add( s );
165}
166
167SensorService::~SensorService()
168{
169    for (size_t i=0 ; i<mSensorMap.size() ; i++)
170        delete mSensorMap.valueAt(i);
171}
172
173static const String16 sDump("android.permission.DUMP");
174
175status_t SensorService::dump(int fd, const Vector<String16>& args)
176{
177    const size_t SIZE = 1024;
178    char buffer[SIZE];
179    String8 result;
180    if (!PermissionCache::checkCallingPermission(sDump)) {
181        snprintf(buffer, SIZE, "Permission Denial: "
182                "can't dump SurfaceFlinger from pid=%d, uid=%d\n",
183                IPCThreadState::self()->getCallingPid(),
184                IPCThreadState::self()->getCallingUid());
185        result.append(buffer);
186    } else {
187        Mutex::Autolock _l(mLock);
188        snprintf(buffer, SIZE, "Sensor List:\n");
189        result.append(buffer);
190        for (size_t i=0 ; i<mSensorList.size() ; i++) {
191            const Sensor& s(mSensorList[i]);
192            const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle()));
193            snprintf(buffer, SIZE,
194                    "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | "
195                    "last=<%5.1f,%5.1f,%5.1f>\n",
196                    s.getName().string(),
197                    s.getVendor().string(),
198                    s.getHandle(),
199                    s.getMinDelay() ? (1000000.0f / s.getMinDelay()) : 0.0f,
200                    e.data[0], e.data[1], e.data[2]);
201            result.append(buffer);
202        }
203        SensorFusion::getInstance().dump(result, buffer, SIZE);
204        SensorDevice::getInstance().dump(result, buffer, SIZE);
205
206        snprintf(buffer, SIZE, "%d active connections\n",
207                mActiveConnections.size());
208        result.append(buffer);
209        snprintf(buffer, SIZE, "Active sensors:\n");
210        result.append(buffer);
211        for (size_t i=0 ; i<mActiveSensors.size() ; i++) {
212            int handle = mActiveSensors.keyAt(i);
213            snprintf(buffer, SIZE, "%s (handle=0x%08x, connections=%d)\n",
214                    getSensorName(handle).string(),
215                    handle,
216                    mActiveSensors.valueAt(i)->getNumConnections());
217            result.append(buffer);
218        }
219    }
220    write(fd, result.string(), result.size());
221    return NO_ERROR;
222}
223
224bool SensorService::threadLoop()
225{
226    ALOGD("nuSensorService thread starting...");
227
228    const size_t numEventMax = 16 * (1 + mVirtualSensorList.size());
229    sensors_event_t buffer[numEventMax];
230    sensors_event_t scratch[numEventMax];
231    SensorDevice& device(SensorDevice::getInstance());
232    const size_t vcount = mVirtualSensorList.size();
233
234    ssize_t count;
235    do {
236        count = device.poll(buffer, numEventMax);
237        if (count<0) {
238            ALOGE("sensor poll failed (%s)", strerror(-count));
239            break;
240        }
241
242        recordLastValue(buffer, count);
243
244        // handle virtual sensors
245        if (count && vcount) {
246            sensors_event_t const * const event = buffer;
247            const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
248                    getActiveVirtualSensors());
249            const size_t activeVirtualSensorCount = virtualSensors.size();
250            if (activeVirtualSensorCount) {
251                size_t k = 0;
252                SensorFusion& fusion(SensorFusion::getInstance());
253                if (fusion.isEnabled()) {
254                    for (size_t i=0 ; i<size_t(count) ; i++) {
255                        fusion.process(event[i]);
256                    }
257                }
258                for (size_t i=0 ; i<size_t(count) ; i++) {
259                    for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
260                        sensors_event_t out;
261                        if (virtualSensors.valueAt(j)->process(&out, event[i])) {
262                            buffer[count + k] = out;
263                            k++;
264                        }
265                    }
266                }
267                if (k) {
268                    // record the last synthesized values
269                    recordLastValue(&buffer[count], k);
270                    count += k;
271                    // sort the buffer by time-stamps
272                    sortEventBuffer(buffer, count);
273                }
274            }
275        }
276
277        // send our events to clients...
278        const SortedVector< wp<SensorEventConnection> > activeConnections(
279                getActiveConnections());
280        size_t numConnections = activeConnections.size();
281        for (size_t i=0 ; i<numConnections ; i++) {
282            sp<SensorEventConnection> connection(
283                    activeConnections[i].promote());
284            if (connection != 0) {
285                connection->sendEvents(buffer, count, scratch);
286            }
287        }
288    } while (count >= 0 || Thread::exitPending());
289
290    ALOGW("Exiting SensorService::threadLoop => aborting...");
291    abort();
292    return false;
293}
294
295void SensorService::recordLastValue(
296        sensors_event_t const * buffer, size_t count)
297{
298    Mutex::Autolock _l(mLock);
299
300    // record the last event for each sensor
301    int32_t prev = buffer[0].sensor;
302    for (size_t i=1 ; i<count ; i++) {
303        // record the last event of each sensor type in this buffer
304        int32_t curr = buffer[i].sensor;
305        if (curr != prev) {
306            mLastEventSeen.editValueFor(prev) = buffer[i-1];
307            prev = curr;
308        }
309    }
310    mLastEventSeen.editValueFor(prev) = buffer[count-1];
311}
312
313void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count)
314{
315    struct compar {
316        static int cmp(void const* lhs, void const* rhs) {
317            sensors_event_t const* l = static_cast<sensors_event_t const*>(lhs);
318            sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs);
319            return r->timestamp - l->timestamp;
320        }
321    };
322    qsort(buffer, count, sizeof(sensors_event_t), compar::cmp);
323}
324
325SortedVector< wp<SensorService::SensorEventConnection> >
326SensorService::getActiveConnections() const
327{
328    Mutex::Autolock _l(mLock);
329    return mActiveConnections;
330}
331
332DefaultKeyedVector<int, SensorInterface*>
333SensorService::getActiveVirtualSensors() const
334{
335    Mutex::Autolock _l(mLock);
336    return mActiveVirtualSensors;
337}
338
339String8 SensorService::getSensorName(int handle) const {
340    size_t count = mUserSensorList.size();
341    for (size_t i=0 ; i<count ; i++) {
342        const Sensor& sensor(mUserSensorList[i]);
343        if (sensor.getHandle() == handle) {
344            return sensor.getName();
345        }
346    }
347    String8 result("unknown");
348    return result;
349}
350
351Vector<Sensor> SensorService::getSensorList()
352{
353    return mUserSensorList;
354}
355
356sp<ISensorEventConnection> SensorService::createSensorEventConnection()
357{
358    sp<SensorEventConnection> result(new SensorEventConnection(this));
359    return result;
360}
361
362void SensorService::cleanupConnection(SensorEventConnection* c)
363{
364    Mutex::Autolock _l(mLock);
365    const wp<SensorEventConnection> connection(c);
366    size_t size = mActiveSensors.size();
367    ALOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size);
368    for (size_t i=0 ; i<size ; ) {
369        int handle = mActiveSensors.keyAt(i);
370        if (c->hasSensor(handle)) {
371            ALOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle);
372            SensorInterface* sensor = mSensorMap.valueFor( handle );
373            ALOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle);
374            if (sensor) {
375                sensor->activate(c, false);
376            }
377        }
378        SensorRecord* rec = mActiveSensors.valueAt(i);
379        ALOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle);
380        ALOGD_IF(DEBUG_CONNECTIONS,
381                "removing connection %p for sensor[%d].handle=0x%08x",
382                c, i, handle);
383
384        if (rec && rec->removeConnection(connection)) {
385            ALOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection");
386            mActiveSensors.removeItemsAt(i, 1);
387            mActiveVirtualSensors.removeItem(handle);
388            delete rec;
389            size--;
390        } else {
391            i++;
392        }
393    }
394    mActiveConnections.remove(connection);
395}
396
397status_t SensorService::enable(const sp<SensorEventConnection>& connection,
398        int handle)
399{
400    if (mInitCheck != NO_ERROR)
401        return mInitCheck;
402
403    Mutex::Autolock _l(mLock);
404    SensorInterface* sensor = mSensorMap.valueFor(handle);
405    status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE);
406    if (err == NO_ERROR) {
407        SensorRecord* rec = mActiveSensors.valueFor(handle);
408        if (rec == 0) {
409            rec = new SensorRecord(connection);
410            mActiveSensors.add(handle, rec);
411            if (sensor->isVirtual()) {
412                mActiveVirtualSensors.add(handle, sensor);
413            }
414        } else {
415            if (rec->addConnection(connection)) {
416                // this sensor is already activated, but we are adding a
417                // connection that uses it. Immediately send down the last
418                // known value of the requested sensor if it's not a
419                // "continuous" sensor.
420                if (sensor->getSensor().getMinDelay() == 0) {
421                    sensors_event_t scratch;
422                    sensors_event_t& event(mLastEventSeen.editValueFor(handle));
423                    if (event.version == sizeof(sensors_event_t)) {
424                        connection->sendEvents(&event, 1);
425                    }
426                }
427            }
428        }
429        if (err == NO_ERROR) {
430            // connection now active
431            if (connection->addSensor(handle)) {
432                // the sensor was added (which means it wasn't already there)
433                // so, see if this connection becomes active
434                if (mActiveConnections.indexOf(connection) < 0) {
435                    mActiveConnections.add(connection);
436                }
437            }
438        }
439    }
440    return err;
441}
442
443status_t SensorService::disable(const sp<SensorEventConnection>& connection,
444        int handle)
445{
446    if (mInitCheck != NO_ERROR)
447        return mInitCheck;
448
449    status_t err = NO_ERROR;
450    Mutex::Autolock _l(mLock);
451    SensorRecord* rec = mActiveSensors.valueFor(handle);
452    if (rec) {
453        // see if this connection becomes inactive
454        connection->removeSensor(handle);
455        if (connection->hasAnySensor() == false) {
456            mActiveConnections.remove(connection);
457        }
458        // see if this sensor becomes inactive
459        if (rec->removeConnection(connection)) {
460            mActiveSensors.removeItem(handle);
461            mActiveVirtualSensors.removeItem(handle);
462            delete rec;
463        }
464        SensorInterface* sensor = mSensorMap.valueFor(handle);
465        err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
466    }
467    return err;
468}
469
470status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection,
471        int handle, nsecs_t ns)
472{
473    if (mInitCheck != NO_ERROR)
474        return mInitCheck;
475
476    SensorInterface* sensor = mSensorMap.valueFor(handle);
477    if (!sensor)
478        return BAD_VALUE;
479
480    if (ns < 0)
481        return BAD_VALUE;
482
483    nsecs_t minDelayNs = sensor->getSensor().getMinDelayNs();
484    if (ns < minDelayNs) {
485        ns = minDelayNs;
486    }
487
488    if (ns < MINIMUM_EVENTS_PERIOD)
489        ns = MINIMUM_EVENTS_PERIOD;
490
491    return sensor->setDelay(connection.get(), handle, ns);
492}
493
494// ---------------------------------------------------------------------------
495
496SensorService::SensorRecord::SensorRecord(
497        const sp<SensorEventConnection>& connection)
498{
499    mConnections.add(connection);
500}
501
502bool SensorService::SensorRecord::addConnection(
503        const sp<SensorEventConnection>& connection)
504{
505    if (mConnections.indexOf(connection) < 0) {
506        mConnections.add(connection);
507        return true;
508    }
509    return false;
510}
511
512bool SensorService::SensorRecord::removeConnection(
513        const wp<SensorEventConnection>& connection)
514{
515    ssize_t index = mConnections.indexOf(connection);
516    if (index >= 0) {
517        mConnections.removeItemsAt(index, 1);
518    }
519    return mConnections.size() ? false : true;
520}
521
522// ---------------------------------------------------------------------------
523
524SensorService::SensorEventConnection::SensorEventConnection(
525        const sp<SensorService>& service)
526    : mService(service), mChannel(new BitTube())
527{
528}
529
530SensorService::SensorEventConnection::~SensorEventConnection()
531{
532    ALOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this);
533    mService->cleanupConnection(this);
534}
535
536void SensorService::SensorEventConnection::onFirstRef()
537{
538}
539
540bool SensorService::SensorEventConnection::addSensor(int32_t handle) {
541    Mutex::Autolock _l(mConnectionLock);
542    if (mSensorInfo.indexOf(handle) <= 0) {
543        mSensorInfo.add(handle);
544        return true;
545    }
546    return false;
547}
548
549bool SensorService::SensorEventConnection::removeSensor(int32_t handle) {
550    Mutex::Autolock _l(mConnectionLock);
551    if (mSensorInfo.remove(handle) >= 0) {
552        return true;
553    }
554    return false;
555}
556
557bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const {
558    Mutex::Autolock _l(mConnectionLock);
559    return mSensorInfo.indexOf(handle) >= 0;
560}
561
562bool SensorService::SensorEventConnection::hasAnySensor() const {
563    Mutex::Autolock _l(mConnectionLock);
564    return mSensorInfo.size() ? true : false;
565}
566
567status_t SensorService::SensorEventConnection::sendEvents(
568        sensors_event_t const* buffer, size_t numEvents,
569        sensors_event_t* scratch)
570{
571    // filter out events not for this connection
572    size_t count = 0;
573    if (scratch) {
574        Mutex::Autolock _l(mConnectionLock);
575        size_t i=0;
576        while (i<numEvents) {
577            const int32_t curr = buffer[i].sensor;
578            if (mSensorInfo.indexOf(curr) >= 0) {
579                do {
580                    scratch[count++] = buffer[i++];
581                } while ((i<numEvents) && (buffer[i].sensor == curr));
582            } else {
583                i++;
584            }
585        }
586    } else {
587        scratch = const_cast<sensors_event_t *>(buffer);
588        count = numEvents;
589    }
590
591    // NOTE: ASensorEvent and sensors_event_t are the same type
592    ssize_t size = SensorEventQueue::write(mChannel,
593            reinterpret_cast<ASensorEvent const*>(scratch), count);
594    if (size == -EAGAIN) {
595        // the destination doesn't accept events anymore, it's probably
596        // full. For now, we just drop the events on the floor.
597        //ALOGW("dropping %d events on the floor", count);
598        return size;
599    }
600
601    return size < 0 ? status_t(size) : status_t(NO_ERROR);
602}
603
604sp<BitTube> SensorService::SensorEventConnection::getSensorChannel() const
605{
606    return mChannel;
607}
608
609status_t SensorService::SensorEventConnection::enableDisable(
610        int handle, bool enabled)
611{
612    status_t err;
613    if (enabled) {
614        err = mService->enable(this, handle);
615    } else {
616        err = mService->disable(this, handle);
617    }
618    return err;
619}
620
621status_t SensorService::SensorEventConnection::setEventRate(
622        int handle, nsecs_t ns)
623{
624    return mService->setEventRate(this, handle, ns);
625}
626
627// ---------------------------------------------------------------------------
628}; // namespace android
629
630