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 => aborting...");
    290     abort();
    291     return false;
    292 }
    293 
    294 void 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 
    312 void 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 
    324 SortedVector< wp<SensorService::SensorEventConnection> >
    325 SensorService::getActiveConnections() const
    326 {
    327     Mutex::Autolock _l(mLock);
    328     return mActiveConnections;
    329 }
    330 
    331 DefaultKeyedVector<int, SensorInterface*>
    332 SensorService::getActiveVirtualSensors() const
    333 {
    334     Mutex::Autolock _l(mLock);
    335     return mActiveVirtualSensors;
    336 }
    337 
    338 String8 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 
    350 Vector<Sensor> SensorService::getSensorList()
    351 {
    352     return mUserSensorList;
    353 }
    354 
    355 sp<ISensorEventConnection> SensorService::createSensorEventConnection()
    356 {
    357     sp<SensorEventConnection> result(new SensorEventConnection(this));
    358     return result;
    359 }
    360 
    361 void 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 
    396 status_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 
    442 status_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 
    469 status_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 
    495 SensorService::SensorRecord::SensorRecord(
    496         const sp<SensorEventConnection>& connection)
    497 {
    498     mConnections.add(connection);
    499 }
    500 
    501 bool 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 
    511 bool 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 
    523 SensorService::SensorEventConnection::SensorEventConnection(
    524         const sp<SensorService>& service)
    525     : mService(service), mChannel(new SensorChannel())
    526 {
    527 }
    528 
    529 SensorService::SensorEventConnection::~SensorEventConnection()
    530 {
    531     LOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this);
    532     mService->cleanupConnection(this);
    533 }
    534 
    535 void SensorService::SensorEventConnection::onFirstRef()
    536 {
    537 }
    538 
    539 bool 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 
    548 bool 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 
    556 bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const {
    557     Mutex::Autolock _l(mConnectionLock);
    558     return mSensorInfo.indexOf(handle) >= 0;
    559 }
    560 
    561 bool SensorService::SensorEventConnection::hasAnySensor() const {
    562     Mutex::Autolock _l(mConnectionLock);
    563     return mSensorInfo.size() ? true : false;
    564 }
    565 
    566 status_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 
    607 sp<SensorChannel> SensorService::SensorEventConnection::getSensorChannel() const
    608 {
    609     return mChannel;
    610 }
    611 
    612 status_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 
    624 status_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