Home | History | Annotate | Download | only in sensorservice 
      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 
     49 namespace 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 
     61 SensorService::SensorService()
     62     : mInitCheck(NO_INIT)
     63 {
     64 }
     65 
     66 void 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 
    146 void 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 
    160 void SensorService::registerVirtualSensor(SensorInterface* s)
    161 {
    162     registerSensor(s);
    163     mVirtualSensorList.add( s );
    164 }
    165 
    166 SensorService::~SensorService()
    167 {
    168     for (size_t i=0 ; i<mSensorMap.size() ; i++)
    169         delete mSensorMap.valueAt(i);
    170 }
    171 
    172 static const String16 sDump("android.permission.DUMP");
    173 
    174 status_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 
    223 bool 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!");
    290     return false;
    291 }
    292 
    293 void SensorService::recordLastValue(
    294         sensors_event_t const * buffer, size_t count)
    295 {
    296     Mutex::Autolock _l(mLock);
    297 
    298     // record the last event for each sensor
    299     int32_t prev = buffer[0].sensor;
    300     for (size_t i=1 ; i<count ; i++) {
    301         // record the last event of each sensor type in this buffer
    302         int32_t curr = buffer[i].sensor;
    303         if (curr != prev) {
    304             mLastEventSeen.editValueFor(prev) = buffer[i-1];
    305             prev = curr;
    306         }
    307     }
    308     mLastEventSeen.editValueFor(prev) = buffer[count-1];
    309 }
    310 
    311 void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count)
    312 {
    313     struct compar {
    314         static int cmp(void const* lhs, void const* rhs) {
    315             sensors_event_t const* l = static_cast<sensors_event_t const*>(lhs);
    316             sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs);
    317             return r->timestamp - l->timestamp;
    318         }
    319     };
    320     qsort(buffer, count, sizeof(sensors_event_t), compar::cmp);
    321 }
    322 
    323 SortedVector< wp<SensorService::SensorEventConnection> >
    324 SensorService::getActiveConnections() const
    325 {
    326     Mutex::Autolock _l(mLock);
    327     return mActiveConnections;
    328 }
    329 
    330 DefaultKeyedVector<int, SensorInterface*>
    331 SensorService::getActiveVirtualSensors() const
    332 {
    333     Mutex::Autolock _l(mLock);
    334     return mActiveVirtualSensors;
    335 }
    336 
    337 String8 SensorService::getSensorName(int handle) const {
    338     size_t count = mUserSensorList.size();
    339     for (size_t i=0 ; i<count ; i++) {
    340         const Sensor& sensor(mUserSensorList[i]);
    341         if (sensor.getHandle() == handle) {
    342             return sensor.getName();
    343         }
    344     }
    345     String8 result("unknown");
    346     return result;
    347 }
    348 
    349 Vector<Sensor> SensorService::getSensorList()
    350 {
    351     return mUserSensorList;
    352 }
    353 
    354 sp<ISensorEventConnection> SensorService::createSensorEventConnection()
    355 {
    356     sp<SensorEventConnection> result(new SensorEventConnection(this));
    357     return result;
    358 }
    359 
    360 void SensorService::cleanupConnection(SensorEventConnection* c)
    361 {
    362     Mutex::Autolock _l(mLock);
    363     const wp<SensorEventConnection> connection(c);
    364     size_t size = mActiveSensors.size();
    365     LOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size);
    366     for (size_t i=0 ; i<size ; ) {
    367         int handle = mActiveSensors.keyAt(i);
    368         if (c->hasSensor(handle)) {
    369             LOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle);
    370             SensorInterface* sensor = mSensorMap.valueFor( handle );
    371             LOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle);
    372             if (sensor) {
    373                 sensor->activate(c, false);
    374             }
    375         }
    376         SensorRecord* rec = mActiveSensors.valueAt(i);
    377         LOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle);
    378         LOGD_IF(DEBUG_CONNECTIONS,
    379                 "removing connection %p for sensor[%d].handle=0x%08x",
    380                 c, i, handle);
    381 
    382         if (rec && rec->removeConnection(connection)) {
    383             LOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection");
    384             mActiveSensors.removeItemsAt(i, 1);
    385             mActiveVirtualSensors.removeItem(handle);
    386             delete rec;
    387             size--;
    388         } else {
    389             i++;
    390         }
    391     }
    392     mActiveConnections.remove(connection);
    393 }
    394 
    395 status_t SensorService::enable(const sp<SensorEventConnection>& connection,
    396         int handle)
    397 {
    398     if (mInitCheck != NO_ERROR)
    399         return mInitCheck;
    400 
    401     Mutex::Autolock _l(mLock);
    402     SensorInterface* sensor = mSensorMap.valueFor(handle);
    403     status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE);
    404     if (err == NO_ERROR) {
    405         SensorRecord* rec = mActiveSensors.valueFor(handle);
    406         if (rec == 0) {
    407             rec = new SensorRecord(connection);
    408             mActiveSensors.add(handle, rec);
    409             if (sensor->isVirtual()) {
    410                 mActiveVirtualSensors.add(handle, sensor);
    411             }
    412         } else {
    413             if (rec->addConnection(connection)) {
    414                 // this sensor is already activated, but we are adding a
    415                 // connection that uses it. Immediately send down the last
    416                 // known value of the requested sensor if it's not a
    417                 // "continuous" sensor.
    418                 if (sensor->getSensor().getMinDelay() == 0) {
    419                     sensors_event_t scratch;
    420                     sensors_event_t& event(mLastEventSeen.editValueFor(handle));
    421                     if (event.version == sizeof(sensors_event_t)) {
    422                         connection->sendEvents(&event, 1);
    423                     }
    424                 }
    425             }
    426         }
    427         if (err == NO_ERROR) {
    428             // connection now active
    429             if (connection->addSensor(handle)) {
    430                 // the sensor was added (which means it wasn't already there)
    431                 // so, see if this connection becomes active
    432                 if (mActiveConnections.indexOf(connection) < 0) {
    433                     mActiveConnections.add(connection);
    434                 }
    435             }
    436         }
    437     }
    438     return err;
    439 }
    440 
    441 status_t SensorService::disable(const sp<SensorEventConnection>& connection,
    442         int handle)
    443 {
    444     if (mInitCheck != NO_ERROR)
    445         return mInitCheck;
    446 
    447     status_t err = NO_ERROR;
    448     Mutex::Autolock _l(mLock);
    449     SensorRecord* rec = mActiveSensors.valueFor(handle);
    450     if (rec) {
    451         // see if this connection becomes inactive
    452         connection->removeSensor(handle);
    453         if (connection->hasAnySensor() == false) {
    454             mActiveConnections.remove(connection);
    455         }
    456         // see if this sensor becomes inactive
    457         if (rec->removeConnection(connection)) {
    458             mActiveSensors.removeItem(handle);
    459             mActiveVirtualSensors.removeItem(handle);
    460             delete rec;
    461         }
    462         SensorInterface* sensor = mSensorMap.valueFor(handle);
    463         err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
    464     }
    465     return err;
    466 }
    467 
    468 status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection,
    469         int handle, nsecs_t ns)
    470 {
    471     if (mInitCheck != NO_ERROR)
    472         return mInitCheck;
    473 
    474     if (ns < 0)
    475         return BAD_VALUE;
    476 
    477     if (ns < MINIMUM_EVENTS_PERIOD)
    478         ns = MINIMUM_EVENTS_PERIOD;
    479 
    480     SensorInterface* sensor = mSensorMap.valueFor(handle);
    481     if (!sensor) return BAD_VALUE;
    482     return sensor->setDelay(connection.get(), handle, ns);
    483 }
    484 
    485 // ---------------------------------------------------------------------------
    486 
    487 SensorService::SensorRecord::SensorRecord(
    488         const sp<SensorEventConnection>& connection)
    489 {
    490     mConnections.add(connection);
    491 }
    492 
    493 bool SensorService::SensorRecord::addConnection(
    494         const sp<SensorEventConnection>& connection)
    495 {
    496     if (mConnections.indexOf(connection) < 0) {
    497         mConnections.add(connection);
    498         return true;
    499     }
    500     return false;
    501 }
    502 
    503 bool SensorService::SensorRecord::removeConnection(
    504         const wp<SensorEventConnection>& connection)
    505 {
    506     ssize_t index = mConnections.indexOf(connection);
    507     if (index >= 0) {
    508         mConnections.removeItemsAt(index, 1);
    509     }
    510     return mConnections.size() ? false : true;
    511 }
    512 
    513 // ---------------------------------------------------------------------------
    514 
    515 SensorService::SensorEventConnection::SensorEventConnection(
    516         const sp<SensorService>& service)
    517     : mService(service), mChannel(new SensorChannel())
    518 {
    519 }
    520 
    521 SensorService::SensorEventConnection::~SensorEventConnection()
    522 {
    523     LOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this);
    524     mService->cleanupConnection(this);
    525 }
    526 
    527 void SensorService::SensorEventConnection::onFirstRef()
    528 {
    529 }
    530 
    531 bool SensorService::SensorEventConnection::addSensor(int32_t handle) {
    532     Mutex::Autolock _l(mConnectionLock);
    533     if (mSensorInfo.indexOf(handle) <= 0) {
    534         mSensorInfo.add(handle);
    535         return true;
    536     }
    537     return false;
    538 }
    539 
    540 bool SensorService::SensorEventConnection::removeSensor(int32_t handle) {
    541     Mutex::Autolock _l(mConnectionLock);
    542     if (mSensorInfo.remove(handle) >= 0) {
    543         return true;
    544     }
    545     return false;
    546 }
    547 
    548 bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const {
    549     Mutex::Autolock _l(mConnectionLock);
    550     return mSensorInfo.indexOf(handle) >= 0;
    551 }
    552 
    553 bool SensorService::SensorEventConnection::hasAnySensor() const {
    554     Mutex::Autolock _l(mConnectionLock);
    555     return mSensorInfo.size() ? true : false;
    556 }
    557 
    558 status_t SensorService::SensorEventConnection::sendEvents(
    559         sensors_event_t const* buffer, size_t numEvents,
    560         sensors_event_t* scratch)
    561 {
    562     // filter out events not for this connection
    563     size_t count = 0;
    564     if (scratch) {
    565         Mutex::Autolock _l(mConnectionLock);
    566         size_t i=0;
    567         while (i<numEvents) {
    568             const int32_t curr = buffer[i].sensor;
    569             if (mSensorInfo.indexOf(curr) >= 0) {
    570                 do {
    571                     scratch[count++] = buffer[i++];
    572                 } while ((i<numEvents) && (buffer[i].sensor == curr));
    573             } else {
    574                 i++;
    575             }
    576         }
    577     } else {
    578         scratch = const_cast<sensors_event_t *>(buffer);
    579         count = numEvents;
    580     }
    581 
    582     if (count == 0)
    583         return 0;
    584 
    585     ssize_t size = mChannel->write(scratch, count*sizeof(sensors_event_t));
    586     if (size == -EAGAIN) {
    587         // the destination doesn't accept events anymore, it's probably
    588         // full. For now, we just drop the events on the floor.
    589         //LOGW("dropping %d events on the floor", count);
    590         return size;
    591     }
    592 
    593     //LOGE_IF(size<0, "dropping %d events on the floor (%s)",
    594     //        count, strerror(-size));
    595 
    596     return size < 0 ? status_t(size) : status_t(NO_ERROR);
    597 }
    598 
    599 sp<SensorChannel> SensorService::SensorEventConnection::getSensorChannel() const
    600 {
    601     return mChannel;
    602 }
    603 
    604 status_t SensorService::SensorEventConnection::enableDisable(
    605         int handle, bool enabled)
    606 {
    607     status_t err;
    608     if (enabled) {
    609         err = mService->enable(this, handle);
    610     } else {
    611         err = mService->disable(this, handle);
    612     }
    613     return err;
    614 }
    615 
    616 status_t SensorService::SensorEventConnection::setEventRate(
    617         int handle, nsecs_t ns)
    618 {
    619     return mService->setEventRate(this, handle, ns);
    620 }
    621 
    622 // ---------------------------------------------------------------------------
    623 }; // namespace android
    624 
    625