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