SensorService.cpp revision d1920ffede9e9bc107104ad94c291ca0f0f18bc3 
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;
229    const size_t minBufferSize = numEventMax * mVirtualSensorList.size();
230    sensors_event_t buffer[minBufferSize];
231    sensors_event_t scratch[minBufferSize];
232    SensorDevice& device(SensorDevice::getInstance());
233    const size_t vcount = mVirtualSensorList.size();
234
235    ssize_t count;
236    do {
237        count = device.poll(buffer, numEventMax);
238        if (count<0) {
239            ALOGE("sensor poll failed (%s)", strerror(-count));
240            break;
241        }
242
243        recordLastValue(buffer, count);
244
245        // handle virtual sensors
246        if (count && vcount) {
247            sensors_event_t const * const event = buffer;
248            const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
249                    getActiveVirtualSensors());
250            const size_t activeVirtualSensorCount = virtualSensors.size();
251            if (activeVirtualSensorCount) {
252                size_t k = 0;
253                SensorFusion& fusion(SensorFusion::getInstance());
254                if (fusion.isEnabled()) {
255                    for (size_t i=0 ; i<size_t(count) ; i++) {
256                        fusion.process(event[i]);
257                    }
258                }
259                for (size_t i=0 ; i<size_t(count) && k<minBufferSize ; i++) {
260                    for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
261                        if (count + k >= minBufferSize) {
262                            ALOGE("buffer too small to hold all events: "
263                                    "count=%u, k=%u, size=%u",
264                                    count, k, minBufferSize);
265                            break;
266                        }
267                        sensors_event_t out;
268                        SensorInterface* si = virtualSensors.valueAt(j);
269                        if (si->process(&out, event[i])) {
270                            buffer[count + k] = out;
271                            k++;
272                        }
273                    }
274                }
275                if (k) {
276                    // record the last synthesized values
277                    recordLastValue(&buffer[count], k);
278                    count += k;
279                    // sort the buffer by time-stamps
280                    sortEventBuffer(buffer, count);
281                }
282            }
283        }
284
285        // send our events to clients...
286        const SortedVector< wp<SensorEventConnection> > activeConnections(
287                getActiveConnections());
288        size_t numConnections = activeConnections.size();
289        for (size_t i=0 ; i<numConnections ; i++) {
290            sp<SensorEventConnection> connection(
291                    activeConnections[i].promote());
292            if (connection != 0) {
293                connection->sendEvents(buffer, count, scratch);
294            }
295        }
296    } while (count >= 0 || Thread::exitPending());
297
298    ALOGW("Exiting SensorService::threadLoop => aborting...");
299    abort();
300    return false;
301}
302
303void SensorService::recordLastValue(
304        sensors_event_t const * buffer, size_t count)
305{
306    Mutex::Autolock _l(mLock);
307
308    // record the last event for each sensor
309    int32_t prev = buffer[0].sensor;
310    for (size_t i=1 ; i<count ; i++) {
311        // record the last event of each sensor type in this buffer
312        int32_t curr = buffer[i].sensor;
313        if (curr != prev) {
314            mLastEventSeen.editValueFor(prev) = buffer[i-1];
315            prev = curr;
316        }
317    }
318    mLastEventSeen.editValueFor(prev) = buffer[count-1];
319}
320
321void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count)
322{
323    struct compar {
324        static int cmp(void const* lhs, void const* rhs) {
325            sensors_event_t const* l = static_cast<sensors_event_t const*>(lhs);
326            sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs);
327            return l->timestamp - r->timestamp;
328        }
329    };
330    qsort(buffer, count, sizeof(sensors_event_t), compar::cmp);
331}
332
333SortedVector< wp<SensorService::SensorEventConnection> >
334SensorService::getActiveConnections() const
335{
336    Mutex::Autolock _l(mLock);
337    return mActiveConnections;
338}
339
340DefaultKeyedVector<int, SensorInterface*>
341SensorService::getActiveVirtualSensors() const
342{
343    Mutex::Autolock _l(mLock);
344    return mActiveVirtualSensors;
345}
346
347String8 SensorService::getSensorName(int handle) const {
348    size_t count = mUserSensorList.size();
349    for (size_t i=0 ; i<count ; i++) {
350        const Sensor& sensor(mUserSensorList[i]);
351        if (sensor.getHandle() == handle) {
352            return sensor.getName();
353        }
354    }
355    String8 result("unknown");
356    return result;
357}
358
359Vector<Sensor> SensorService::getSensorList()
360{
361    return mUserSensorList;
362}
363
364sp<ISensorEventConnection> SensorService::createSensorEventConnection()
365{
366    sp<SensorEventConnection> result(new SensorEventConnection(this));
367    return result;
368}
369
370void SensorService::cleanupConnection(SensorEventConnection* c)
371{
372    Mutex::Autolock _l(mLock);
373    const wp<SensorEventConnection> connection(c);
374    size_t size = mActiveSensors.size();
375    ALOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size);
376    for (size_t i=0 ; i<size ; ) {
377        int handle = mActiveSensors.keyAt(i);
378        if (c->hasSensor(handle)) {
379            ALOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle);
380            SensorInterface* sensor = mSensorMap.valueFor( handle );
381            ALOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle);
382            if (sensor) {
383                sensor->activate(c, false);
384            }
385        }
386        SensorRecord* rec = mActiveSensors.valueAt(i);
387        ALOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle);
388        ALOGD_IF(DEBUG_CONNECTIONS,
389                "removing connection %p for sensor[%d].handle=0x%08x",
390                c, i, handle);
391
392        if (rec && rec->removeConnection(connection)) {
393            ALOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection");
394            mActiveSensors.removeItemsAt(i, 1);
395            mActiveVirtualSensors.removeItem(handle);
396            delete rec;
397            size--;
398        } else {
399            i++;
400        }
401    }
402    mActiveConnections.remove(connection);
403}
404
405status_t SensorService::enable(const sp<SensorEventConnection>& connection,
406        int handle)
407{
408    if (mInitCheck != NO_ERROR)
409        return mInitCheck;
410
411    Mutex::Autolock _l(mLock);
412    SensorInterface* sensor = mSensorMap.valueFor(handle);
413    status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE);
414    if (err == NO_ERROR) {
415        SensorRecord* rec = mActiveSensors.valueFor(handle);
416        if (rec == 0) {
417            rec = new SensorRecord(connection);
418            mActiveSensors.add(handle, rec);
419            if (sensor->isVirtual()) {
420                mActiveVirtualSensors.add(handle, sensor);
421            }
422        } else {
423            if (rec->addConnection(connection)) {
424                // this sensor is already activated, but we are adding a
425                // connection that uses it. Immediately send down the last
426                // known value of the requested sensor if it's not a
427                // "continuous" sensor.
428                if (sensor->getSensor().getMinDelay() == 0) {
429                    sensors_event_t scratch;
430                    sensors_event_t& event(mLastEventSeen.editValueFor(handle));
431                    if (event.version == sizeof(sensors_event_t)) {
432                        connection->sendEvents(&event, 1);
433                    }
434                }
435            }
436        }
437        if (err == NO_ERROR) {
438            // connection now active
439            if (connection->addSensor(handle)) {
440                // the sensor was added (which means it wasn't already there)
441                // so, see if this connection becomes active
442                if (mActiveConnections.indexOf(connection) < 0) {
443                    mActiveConnections.add(connection);
444                }
445            }
446        }
447    }
448    return err;
449}
450
451status_t SensorService::disable(const sp<SensorEventConnection>& connection,
452        int handle)
453{
454    if (mInitCheck != NO_ERROR)
455        return mInitCheck;
456
457    status_t err = NO_ERROR;
458    Mutex::Autolock _l(mLock);
459    SensorRecord* rec = mActiveSensors.valueFor(handle);
460    if (rec) {
461        // see if this connection becomes inactive
462        connection->removeSensor(handle);
463        if (connection->hasAnySensor() == false) {
464            mActiveConnections.remove(connection);
465        }
466        // see if this sensor becomes inactive
467        if (rec->removeConnection(connection)) {
468            mActiveSensors.removeItem(handle);
469            mActiveVirtualSensors.removeItem(handle);
470            delete rec;
471        }
472        SensorInterface* sensor = mSensorMap.valueFor(handle);
473        err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
474    }
475    return err;
476}
477
478status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection,
479        int handle, nsecs_t ns)
480{
481    if (mInitCheck != NO_ERROR)
482        return mInitCheck;
483
484    SensorInterface* sensor = mSensorMap.valueFor(handle);
485    if (!sensor)
486        return BAD_VALUE;
487
488    if (ns < 0)
489        return BAD_VALUE;
490
491    nsecs_t minDelayNs = sensor->getSensor().getMinDelayNs();
492    if (ns < minDelayNs) {
493        ns = minDelayNs;
494    }
495
496    if (ns < MINIMUM_EVENTS_PERIOD)
497        ns = MINIMUM_EVENTS_PERIOD;
498
499    return sensor->setDelay(connection.get(), handle, ns);
500}
501
502// ---------------------------------------------------------------------------
503
504SensorService::SensorRecord::SensorRecord(
505        const sp<SensorEventConnection>& connection)
506{
507    mConnections.add(connection);
508}
509
510bool SensorService::SensorRecord::addConnection(
511        const sp<SensorEventConnection>& connection)
512{
513    if (mConnections.indexOf(connection) < 0) {
514        mConnections.add(connection);
515        return true;
516    }
517    return false;
518}
519
520bool SensorService::SensorRecord::removeConnection(
521        const wp<SensorEventConnection>& connection)
522{
523    ssize_t index = mConnections.indexOf(connection);
524    if (index >= 0) {
525        mConnections.removeItemsAt(index, 1);
526    }
527    return mConnections.size() ? false : true;
528}
529
530// ---------------------------------------------------------------------------
531
532SensorService::SensorEventConnection::SensorEventConnection(
533        const sp<SensorService>& service)
534    : mService(service), mChannel(new BitTube())
535{
536}
537
538SensorService::SensorEventConnection::~SensorEventConnection()
539{
540    ALOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this);
541    mService->cleanupConnection(this);
542}
543
544void SensorService::SensorEventConnection::onFirstRef()
545{
546}
547
548bool SensorService::SensorEventConnection::addSensor(int32_t handle) {
549    Mutex::Autolock _l(mConnectionLock);
550    if (mSensorInfo.indexOf(handle) <= 0) {
551        mSensorInfo.add(handle);
552        return true;
553    }
554    return false;
555}
556
557bool SensorService::SensorEventConnection::removeSensor(int32_t handle) {
558    Mutex::Autolock _l(mConnectionLock);
559    if (mSensorInfo.remove(handle) >= 0) {
560        return true;
561    }
562    return false;
563}
564
565bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const {
566    Mutex::Autolock _l(mConnectionLock);
567    return mSensorInfo.indexOf(handle) >= 0;
568}
569
570bool SensorService::SensorEventConnection::hasAnySensor() const {
571    Mutex::Autolock _l(mConnectionLock);
572    return mSensorInfo.size() ? true : false;
573}
574
575status_t SensorService::SensorEventConnection::sendEvents(
576        sensors_event_t const* buffer, size_t numEvents,
577        sensors_event_t* scratch)
578{
579    // filter out events not for this connection
580    size_t count = 0;
581    if (scratch) {
582        Mutex::Autolock _l(mConnectionLock);
583        size_t i=0;
584        while (i<numEvents) {
585            const int32_t curr = buffer[i].sensor;
586            if (mSensorInfo.indexOf(curr) >= 0) {
587                do {
588                    scratch[count++] = buffer[i++];
589                } while ((i<numEvents) && (buffer[i].sensor == curr));
590            } else {
591                i++;
592            }
593        }
594    } else {
595        scratch = const_cast<sensors_event_t *>(buffer);
596        count = numEvents;
597    }
598
599    // NOTE: ASensorEvent and sensors_event_t are the same type
600    ssize_t size = SensorEventQueue::write(mChannel,
601            reinterpret_cast<ASensorEvent const*>(scratch), count);
602    if (size == -EAGAIN) {
603        // the destination doesn't accept events anymore, it's probably
604        // full. For now, we just drop the events on the floor.
605        //ALOGW("dropping %d events on the floor", count);
606        return size;
607    }
608
609    return size < 0 ? status_t(size) : status_t(NO_ERROR);
610}
611
612sp<BitTube> SensorService::SensorEventConnection::getSensorChannel() const
613{
614    return mChannel;
615}
616
617status_t SensorService::SensorEventConnection::enableDisable(
618        int handle, bool enabled)
619{
620    status_t err;
621    if (enabled) {
622        err = mService->enable(this, handle);
623    } else {
624        err = mService->disable(this, handle);
625    }
626    return err;
627}
628
629status_t SensorService::SensorEventConnection::setEventRate(
630        int handle, nsecs_t ns)
631{
632    return mService->setEventRate(this, handle, ns);
633}
634
635// ---------------------------------------------------------------------------
636}; // namespace android
637
638