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