SensorService.cpp revision f5a1230d322c14c42331d0a1536b50c87742973b
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 ALOGD("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 ALOGD("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 ALOGE("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 ALOGW("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 ALOGD_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 ALOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle); 371 SensorInterface* sensor = mSensorMap.valueFor( handle ); 372 ALOGE_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 ALOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle); 379 ALOGD_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 ALOGD_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 BitTube()) 526{ 527} 528 529SensorService::SensorEventConnection::~SensorEventConnection() 530{ 531 ALOGD_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 //ALOGW("dropping %d events on the floor", count); 598 return size; 599 } 600 601 //ALOGE_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<BitTube> 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