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