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