services/sensorservice/SensorService.cpp - LeafOS-Project/android_frameworks_base - Gitiles

 /*
  * Copyright (C) 2010 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 #include <stdint.h>
 #include <math.h>
 #include <sys/types.h>

 #include <cutils/properties.h>

 #include <utils/SortedVector.h>
 #include <utils/KeyedVector.h>
 #include <utils/threads.h>
 #include <utils/Atomic.h>
 #include <utils/Errors.h>
 #include <utils/RefBase.h>
 #include <utils/Singleton.h>
 #include <utils/String16.h>

 #include <binder/BinderService.h>
 #include <binder/IServiceManager.h>
 #include <binder/PermissionCache.h>

 #include <gui/ISensorServer.h>
 #include <gui/ISensorEventConnection.h>

 #include <hardware/sensors.h>

 #include "CorrectedGyroSensor.h"
 #include "GravitySensor.h"
 #include "LinearAccelerationSensor.h"
 #include "OrientationSensor.h"
 #include "RotationVectorSensor.h"
 #include "SensorFusion.h"
 #include "SensorService.h"

 namespace android {
 // ---------------------------------------------------------------------------

 /*
  * Notes:
  *
  * - what about a gyro-corrected magnetic-field sensor?
  * - run mag sensor from time to time to force calibration
  * - gravity sensor length is wrong (=> drift in linear-acc sensor)
  *
  */

 SensorService::SensorService()
     : mInitCheck(NO_INIT)
 {
 }

 void SensorService::onFirstRef()
 {
     LOGD("nuSensorService starting...");

     SensorDevice& dev(SensorDevice::getInstance());

     if (dev.initCheck() == NO_ERROR) {
         sensor_t const* list;
         ssize_t count = dev.getSensorList(&list);
         if (count > 0) {
             ssize_t orientationIndex = -1;
             bool hasGyro = false;
             uint32_t virtualSensorsNeeds =
                     (1<<SENSOR_TYPE_GRAVITY) |
                     (1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
                     (1<<SENSOR_TYPE_ROTATION_VECTOR);

             mLastEventSeen.setCapacity(count);
             for (ssize_t i=0 ; i<count ; i++) {
                 registerSensor( new HardwareSensor(list[i]) );
                 switch (list[i].type) {
                     case SENSOR_TYPE_ORIENTATION:
                         orientationIndex = i;
                         break;
                     case SENSOR_TYPE_GYROSCOPE:
                         hasGyro = true;
                         break;
                     case SENSOR_TYPE_GRAVITY:
                     case SENSOR_TYPE_LINEAR_ACCELERATION:
                     case SENSOR_TYPE_ROTATION_VECTOR:
                         virtualSensorsNeeds &= ~(1<<list[i].type);
                         break;
                 }
             }

             // it's safe to instantiate the SensorFusion object here
             // (it wants to be instantiated after h/w sensors have been
             // registered)
             const SensorFusion& fusion(SensorFusion::getInstance());

             if (hasGyro) {
                 // Always instantiate Android's virtual sensors. Since they are
                 // instantiated behind sensors from the HAL, they won't
                 // interfere with applications, unless they looks specifically
                 // for them (by name).

                 registerVirtualSensor( new RotationVectorSensor() );
                 registerVirtualSensor( new GravitySensor(list, count) );
                 registerVirtualSensor( new LinearAccelerationSensor(list, count) );

                 // these are optional
                 registerVirtualSensor( new OrientationSensor() );
                 registerVirtualSensor( new CorrectedGyroSensor(list, count) );

                 // virtual debugging sensors...
                 char value[PROPERTY_VALUE_MAX];
                 property_get("debug.sensors", value, "0");
                 if (atoi(value)) {
                     registerVirtualSensor( new GyroDriftSensor() );
                 }
             }

             // build the sensor list returned to users
             mUserSensorList = mSensorList;
             if (hasGyro &&
                     (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR))) {
                 // if we have the fancy sensor fusion, and it's not provided by the
                 // HAL, use our own (fused) orientation sensor by removing the
                 // HAL supplied one form the user list.
                 if (orientationIndex >= 0) {
                     mUserSensorList.removeItemsAt(orientationIndex);
                 }
             }

             run("SensorService", PRIORITY_URGENT_DISPLAY);
             mInitCheck = NO_ERROR;
         }
     }
 }

 void SensorService::registerSensor(SensorInterface* s)
 {
     sensors_event_t event;
     memset(&event, 0, sizeof(event));

     const Sensor sensor(s->getSensor());
     // add to the sensor list (returned to clients)
     mSensorList.add(sensor);
     // add to our handle->SensorInterface mapping
     mSensorMap.add(sensor.getHandle(), s);
     // create an entry in the mLastEventSeen array
     mLastEventSeen.add(sensor.getHandle(), event);
 }

 void SensorService::registerVirtualSensor(SensorInterface* s)
 {
     registerSensor(s);
     mVirtualSensorList.add( s );
 }

 SensorService::~SensorService()
 {
     for (size_t i=0 ; i<mSensorMap.size() ; i++)
         delete mSensorMap.valueAt(i);
 }

 static const String16 sDump("android.permission.DUMP");

 status_t SensorService::dump(int fd, const Vector<String16>& args)
 {
     const size_t SIZE = 1024;
     char buffer[SIZE];
     String8 result;
     if (!PermissionCache::checkCallingPermission(sDump)) {
         snprintf(buffer, SIZE, "Permission Denial: "
                 "can't dump SurfaceFlinger from pid=%d, uid=%d\n",
                 IPCThreadState::self()->getCallingPid(),
                 IPCThreadState::self()->getCallingUid());
         result.append(buffer);
     } else {
         Mutex::Autolock _l(mLock);
         snprintf(buffer, SIZE, "Sensor List:\n");
         result.append(buffer);
         for (size_t i=0 ; i<mSensorList.size() ; i++) {
             const Sensor& s(mSensorList[i]);
             const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle()));
             snprintf(buffer, SIZE,
                     "%-48s| %-32s | 0x%08x | maxRate=%7.2fHz | "
                     "last=<%5.1f,%5.1f,%5.1f>\n",
                     s.getName().string(),
                     s.getVendor().string(),
                     s.getHandle(),
                     s.getMinDelay() ? (1000000.0f / s.getMinDelay()) : 0.0f,
                     e.data[0], e.data[1], e.data[2]);
             result.append(buffer);
         }
         SensorFusion::getInstance().dump(result, buffer, SIZE);
         SensorDevice::getInstance().dump(result, buffer, SIZE);

         snprintf(buffer, SIZE, "%d active connections\n",
                 mActiveConnections.size());
         result.append(buffer);
         snprintf(buffer, SIZE, "Active sensors:\n");
         result.append(buffer);
         for (size_t i=0 ; i<mActiveSensors.size() ; i++) {
             int handle = mActiveSensors.keyAt(i);
             snprintf(buffer, SIZE, "%s (handle=0x%08x, connections=%d)\n",
                     getSensorName(handle).string(),
                     handle,
                     mActiveSensors.valueAt(i)->getNumConnections());
             result.append(buffer);
         }
     }
     write(fd, result.string(), result.size());
     return NO_ERROR;
 }

 bool SensorService::threadLoop()
 {
     LOGD("nuSensorService thread starting...");

     const size_t numEventMax = 16 * (1 + mVirtualSensorList.size());
     sensors_event_t buffer[numEventMax];
     sensors_event_t scratch[numEventMax];
     SensorDevice& device(SensorDevice::getInstance());
     const size_t vcount = mVirtualSensorList.size();

     ssize_t count;
     do {
         count = device.poll(buffer, numEventMax);
         if (count<0) {
             LOGE("sensor poll failed (%s)", strerror(-count));
             break;
         }

         recordLastValue(buffer, count);

         // handle virtual sensors
         if (count && vcount) {
             sensors_event_t const * const event = buffer;
             const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
                     getActiveVirtualSensors());
             const size_t activeVirtualSensorCount = virtualSensors.size();
             if (activeVirtualSensorCount) {
                 size_t k = 0;
                 SensorFusion& fusion(SensorFusion::getInstance());
                 if (fusion.isEnabled()) {
                     for (size_t i=0 ; i<size_t(count) ; i++) {
                         fusion.process(event[i]);
                     }
                 }
                 for (size_t i=0 ; i<size_t(count) ; i++) {
                     for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
                         sensors_event_t out;
                         if (virtualSensors.valueAt(j)->process(&out, event[i])) {
                             buffer[count + k] = out;
                             k++;
                         }
                     }
                 }
                 if (k) {
                     // record the last synthesized values
                     recordLastValue(&buffer[count], k);
                     count += k;
                     // sort the buffer by time-stamps
                     sortEventBuffer(buffer, count);
                 }
             }
         }

         // send our events to clients...
         const SortedVector< wp<SensorEventConnection> > activeConnections(
                 getActiveConnections());
         size_t numConnections = activeConnections.size();
         for (size_t i=0 ; i<numConnections ; i++) {
             sp<SensorEventConnection> connection(
                     activeConnections[i].promote());
             if (connection != 0) {
                 connection->sendEvents(buffer, count, scratch);
             }
         }
     } while (count >= 0 || Thread::exitPending());

     LOGW("Exiting SensorService::threadLoop!");
     return false;
 }

 void SensorService::recordLastValue(
         sensors_event_t const * buffer, size_t count)
 {
     Mutex::Autolock _l(mLock);

     // record the last event for each sensor
     int32_t prev = buffer[0].sensor;
     for (size_t i=1 ; i<count ; i++) {
         // record the last event of each sensor type in this buffer
         int32_t curr = buffer[i].sensor;
         if (curr != prev) {
             mLastEventSeen.editValueFor(prev) = buffer[i-1];
             prev = curr;
         }
     }
     mLastEventSeen.editValueFor(prev) = buffer[count-1];
 }

 void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count)
 {
     struct compar {
         static int cmp(void const* lhs, void const* rhs) {
             sensors_event_t const* l = static_cast<sensors_event_t const*>(lhs);
             sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs);
             return r->timestamp - l->timestamp;
         }
     };
     qsort(buffer, count, sizeof(sensors_event_t), compar::cmp);
 }

 SortedVector< wp<SensorService::SensorEventConnection> >
 SensorService::getActiveConnections() const
 {
     Mutex::Autolock _l(mLock);
     return mActiveConnections;
 }

 DefaultKeyedVector<int, SensorInterface*>
 SensorService::getActiveVirtualSensors() const
 {
     Mutex::Autolock _l(mLock);
     return mActiveVirtualSensors;
 }

 String8 SensorService::getSensorName(int handle) const {
     size_t count = mUserSensorList.size();
     for (size_t i=0 ; i<count ; i++) {
         const Sensor& sensor(mUserSensorList[i]);
         if (sensor.getHandle() == handle) {
             return sensor.getName();
         }
     }
     String8 result("unknown");
     return result;
 }

 Vector<Sensor> SensorService::getSensorList()
 {
     return mUserSensorList;
 }

 sp<ISensorEventConnection> SensorService::createSensorEventConnection()
 {
     sp<SensorEventConnection> result(new SensorEventConnection(this));
     return result;
 }

 void SensorService::cleanupConnection(SensorEventConnection* c)
 {
     Mutex::Autolock _l(mLock);
     const wp<SensorEventConnection> connection(c);
     size_t size = mActiveSensors.size();
     LOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size);
     for (size_t i=0 ; i<size ; ) {
         int handle = mActiveSensors.keyAt(i);
         if (c->hasSensor(handle)) {
             LOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle);
             SensorInterface* sensor = mSensorMap.valueFor( handle );
             LOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle);
             if (sensor) {
                 sensor->activate(c, false);
             }
         }
         SensorRecord* rec = mActiveSensors.valueAt(i);
         LOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle);
         LOGD_IF(DEBUG_CONNECTIONS,
                 "removing connection %p for sensor[%d].handle=0x%08x",
                 c, i, handle);

         if (rec && rec->removeConnection(connection)) {
             LOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection");
             mActiveSensors.removeItemsAt(i, 1);
             mActiveVirtualSensors.removeItem(handle);
             delete rec;
             size--;
         } else {
             i++;
         }
     }
     mActiveConnections.remove(connection);
 }

 status_t SensorService::enable(const sp<SensorEventConnection>& connection,
         int handle)
 {
     if (mInitCheck != NO_ERROR)
         return mInitCheck;

     Mutex::Autolock _l(mLock);
     SensorInterface* sensor = mSensorMap.valueFor(handle);
     status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE);
     if (err == NO_ERROR) {
         SensorRecord* rec = mActiveSensors.valueFor(handle);
         if (rec == 0) {
             rec = new SensorRecord(connection);
             mActiveSensors.add(handle, rec);
             if (sensor->isVirtual()) {
                 mActiveVirtualSensors.add(handle, sensor);
             }
         } else {
             if (rec->addConnection(connection)) {
                 // this sensor is already activated, but we are adding a
                 // connection that uses it. Immediately send down the last
                 // known value of the requested sensor if it's not a
                 // "continuous" sensor.
                 if (sensor->getSensor().getMinDelay() == 0) {
                     sensors_event_t scratch;
                     sensors_event_t& event(mLastEventSeen.editValueFor(handle));
                     if (event.version == sizeof(sensors_event_t)) {
                         connection->sendEvents(&event, 1);
                     }
                 }
             }
         }
         if (err == NO_ERROR) {
             // connection now active
             if (connection->addSensor(handle)) {
                 // the sensor was added (which means it wasn't already there)
                 // so, see if this connection becomes active
                 if (mActiveConnections.indexOf(connection) < 0) {
                     mActiveConnections.add(connection);
                 }
             }
         }
     }
     return err;
 }

 status_t SensorService::disable(const sp<SensorEventConnection>& connection,
         int handle)
 {
     if (mInitCheck != NO_ERROR)
         return mInitCheck;

     status_t err = NO_ERROR;
     Mutex::Autolock _l(mLock);
     SensorRecord* rec = mActiveSensors.valueFor(handle);
     if (rec) {
         // see if this connection becomes inactive
         connection->removeSensor(handle);
         if (connection->hasAnySensor() == false) {
             mActiveConnections.remove(connection);
         }
         // see if this sensor becomes inactive
         if (rec->removeConnection(connection)) {
             mActiveSensors.removeItem(handle);
             mActiveVirtualSensors.removeItem(handle);
             delete rec;
         }
         SensorInterface* sensor = mSensorMap.valueFor(handle);
         err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
     }
     return err;
 }

 status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection,
         int handle, nsecs_t ns)
 {
     if (mInitCheck != NO_ERROR)
         return mInitCheck;

     if (ns < 0)
         return BAD_VALUE;

     if (ns < MINIMUM_EVENTS_PERIOD)
         ns = MINIMUM_EVENTS_PERIOD;

     SensorInterface* sensor = mSensorMap.valueFor(handle);
     if (!sensor) return BAD_VALUE;
     return sensor->setDelay(connection.get(), handle, ns);
 }

 // ---------------------------------------------------------------------------

 SensorService::SensorRecord::SensorRecord(
         const sp<SensorEventConnection>& connection)
 {
     mConnections.add(connection);
 }

 bool SensorService::SensorRecord::addConnection(
         const sp<SensorEventConnection>& connection)
 {
     if (mConnections.indexOf(connection) < 0) {
         mConnections.add(connection);
         return true;
     }
     return false;
 }

 bool SensorService::SensorRecord::removeConnection(
         const wp<SensorEventConnection>& connection)
 {
     ssize_t index = mConnections.indexOf(connection);
     if (index >= 0) {
         mConnections.removeItemsAt(index, 1);
     }
     return mConnections.size() ? false : true;
 }

 // ---------------------------------------------------------------------------

 SensorService::SensorEventConnection::SensorEventConnection(
         const sp<SensorService>& service)
     : mService(service), mChannel(new SensorChannel())
 {
 }

 SensorService::SensorEventConnection::~SensorEventConnection()
 {
     LOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this);
     mService->cleanupConnection(this);
 }

 void SensorService::SensorEventConnection::onFirstRef()
 {
 }

 bool SensorService::SensorEventConnection::addSensor(int32_t handle) {
     Mutex::Autolock _l(mConnectionLock);
     if (mSensorInfo.indexOf(handle) <= 0) {
         mSensorInfo.add(handle);
         return true;
     }
     return false;
 }

 bool SensorService::SensorEventConnection::removeSensor(int32_t handle) {
     Mutex::Autolock _l(mConnectionLock);
     if (mSensorInfo.remove(handle) >= 0) {
         return true;
     }
     return false;
 }

 bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const {
     Mutex::Autolock _l(mConnectionLock);
     return mSensorInfo.indexOf(handle) >= 0;
 }

 bool SensorService::SensorEventConnection::hasAnySensor() const {
     Mutex::Autolock _l(mConnectionLock);
     return mSensorInfo.size() ? true : false;
 }

 status_t SensorService::SensorEventConnection::sendEvents(
         sensors_event_t const* buffer, size_t numEvents,
         sensors_event_t* scratch)
 {
     // filter out events not for this connection
     size_t count = 0;
     if (scratch) {
         Mutex::Autolock _l(mConnectionLock);
         size_t i=0;
         while (i<numEvents) {
             const int32_t curr = buffer[i].sensor;
             if (mSensorInfo.indexOf(curr) >= 0) {
                 do {
                     scratch[count++] = buffer[i++];
                 } while ((i<numEvents) && (buffer[i].sensor == curr));
             } else {
                 i++;
             }
         }
     } else {
         scratch = const_cast<sensors_event_t *>(buffer);
         count = numEvents;
     }

     if (count == 0)
         return 0;

     ssize_t size = mChannel->write(scratch, count*sizeof(sensors_event_t));
     if (size == -EAGAIN) {
         // the destination doesn't accept events anymore, it's probably
         // full. For now, we just drop the events on the floor.
         //LOGW("dropping %d events on the floor", count);
         return size;
     }

     //LOGE_IF(size<0, "dropping %d events on the floor (%s)",
     //        count, strerror(-size));

     return size < 0 ? status_t(size) : status_t(NO_ERROR);
 }

 sp<SensorChannel> SensorService::SensorEventConnection::getSensorChannel() const
 {
     return mChannel;
 }

 status_t SensorService::SensorEventConnection::enableDisable(
         int handle, bool enabled)
 {
     status_t err;
     if (enabled) {
         err = mService->enable(this, handle);
     } else {
         err = mService->disable(this, handle);
     }
     return err;
 }

 status_t SensorService::SensorEventConnection::setEventRate(
         int handle, nsecs_t ns)
 {
     return mService->setEventRate(this, handle, ns);
 }

 // ---------------------------------------------------------------------------
 }; // namespace android
	/*
	* Copyright (C) 2010 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	#include <stdint.h>
	#include <math.h>
	#include <sys/types.h>

	#include <cutils/properties.h>

	#include <utils/SortedVector.h>
	#include <utils/KeyedVector.h>
	#include <utils/threads.h>
	#include <utils/Atomic.h>
	#include <utils/Errors.h>
	#include <utils/RefBase.h>
	#include <utils/Singleton.h>
	#include <utils/String16.h>

	#include <binder/BinderService.h>
	#include <binder/IServiceManager.h>
	#include <binder/PermissionCache.h>

	#include <gui/ISensorServer.h>
	#include <gui/ISensorEventConnection.h>

	#include <hardware/sensors.h>

	#include "CorrectedGyroSensor.h"
	#include "GravitySensor.h"
	#include "LinearAccelerationSensor.h"
	#include "OrientationSensor.h"
	#include "RotationVectorSensor.h"
	#include "SensorFusion.h"
	#include "SensorService.h"

	namespace android {
	// ---------------------------------------------------------------------------

	/*
	* Notes:
	*
	* - what about a gyro-corrected magnetic-field sensor?
	* - run mag sensor from time to time to force calibration
	* - gravity sensor length is wrong (=> drift in linear-acc sensor)
	*
	*/

	SensorService::SensorService()
	: mInitCheck(NO_INIT)
	{
	}

	void SensorService::onFirstRef()
	{
	LOGD("nuSensorService starting...");

	SensorDevice& dev(SensorDevice::getInstance());

	if (dev.initCheck() == NO_ERROR) {
	sensor_t const* list;
	ssize_t count = dev.getSensorList(&list);
	if (count > 0) {
	ssize_t orientationIndex = -1;
	bool hasGyro = false;
	uint32_t virtualSensorsNeeds =
	(1<<SENSOR_TYPE_GRAVITY) \|
	(1<<SENSOR_TYPE_LINEAR_ACCELERATION) \|
	(1<<SENSOR_TYPE_ROTATION_VECTOR);

	mLastEventSeen.setCapacity(count);
	for (ssize_t i=0 ; i<count ; i++) {
	registerSensor( new HardwareSensor(list[i]) );
	switch (list[i].type) {
	case SENSOR_TYPE_ORIENTATION:
	orientationIndex = i;
	break;
	case SENSOR_TYPE_GYROSCOPE:
	hasGyro = true;
	break;
	case SENSOR_TYPE_GRAVITY:
	case SENSOR_TYPE_LINEAR_ACCELERATION:
	case SENSOR_TYPE_ROTATION_VECTOR:
	virtualSensorsNeeds &= ~(1<<list[i].type);
	break;
	}
	}

	// it's safe to instantiate the SensorFusion object here
	// (it wants to be instantiated after h/w sensors have been
	// registered)
	const SensorFusion& fusion(SensorFusion::getInstance());

	if (hasGyro) {
	// Always instantiate Android's virtual sensors. Since they are
	// instantiated behind sensors from the HAL, they won't
	// interfere with applications, unless they looks specifically
	// for them (by name).

	registerVirtualSensor( new RotationVectorSensor() );
	registerVirtualSensor( new GravitySensor(list, count) );
	registerVirtualSensor( new LinearAccelerationSensor(list, count) );

	// these are optional
	registerVirtualSensor( new OrientationSensor() );
	registerVirtualSensor( new CorrectedGyroSensor(list, count) );

	// virtual debugging sensors...
	char value[PROPERTY_VALUE_MAX];
	property_get("debug.sensors", value, "0");
	if (atoi(value)) {
	registerVirtualSensor( new GyroDriftSensor() );
	}
	}

	// build the sensor list returned to users
	mUserSensorList = mSensorList;
	if (hasGyro &&
	(virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR))) {
	// if we have the fancy sensor fusion, and it's not provided by the
	// HAL, use our own (fused) orientation sensor by removing the
	// HAL supplied one form the user list.
	if (orientationIndex >= 0) {
	mUserSensorList.removeItemsAt(orientationIndex);
	}
	}

	run("SensorService", PRIORITY_URGENT_DISPLAY);
	mInitCheck = NO_ERROR;
	}
	}
	}

	void SensorService::registerSensor(SensorInterface* s)
	{
	sensors_event_t event;
	memset(&event, 0, sizeof(event));

	const Sensor sensor(s->getSensor());
	// add to the sensor list (returned to clients)
	mSensorList.add(sensor);
	// add to our handle->SensorInterface mapping
	mSensorMap.add(sensor.getHandle(), s);
	// create an entry in the mLastEventSeen array
	mLastEventSeen.add(sensor.getHandle(), event);
	}

	void SensorService::registerVirtualSensor(SensorInterface* s)
	{
	registerSensor(s);
	mVirtualSensorList.add( s );
	}

	SensorService::~SensorService()
	{
	for (size_t i=0 ; i<mSensorMap.size() ; i++)
	delete mSensorMap.valueAt(i);
	}

	static const String16 sDump("android.permission.DUMP");

	status_t SensorService::dump(int fd, const Vector<String16>& args)
	{
	const size_t SIZE = 1024;
	char buffer[SIZE];
	String8 result;
	if (!PermissionCache::checkCallingPermission(sDump)) {
	snprintf(buffer, SIZE, "Permission Denial: "
	"can't dump SurfaceFlinger from pid=%d, uid=%d\n",
	IPCThreadState::self()->getCallingPid(),
	IPCThreadState::self()->getCallingUid());
	result.append(buffer);
	} else {
	Mutex::Autolock _l(mLock);
	snprintf(buffer, SIZE, "Sensor List:\n");
	result.append(buffer);
	for (size_t i=0 ; i<mSensorList.size() ; i++) {
	const Sensor& s(mSensorList[i]);
	const sensors_event_t& e(mLastEventSeen.valueFor(s.getHandle()));
	snprintf(buffer, SIZE,
	"%-48s\| %-32s \| 0x%08x \| maxRate=%7.2fHz \| "
	"last=<%5.1f,%5.1f,%5.1f>\n",
	s.getName().string(),
	s.getVendor().string(),
	s.getHandle(),
	s.getMinDelay() ? (1000000.0f / s.getMinDelay()) : 0.0f,
	e.data[0], e.data[1], e.data[2]);
	result.append(buffer);
	}
	SensorFusion::getInstance().dump(result, buffer, SIZE);
	SensorDevice::getInstance().dump(result, buffer, SIZE);

	snprintf(buffer, SIZE, "%d active connections\n",
	mActiveConnections.size());
	result.append(buffer);
	snprintf(buffer, SIZE, "Active sensors:\n");
	result.append(buffer);
	for (size_t i=0 ; i<mActiveSensors.size() ; i++) {
	int handle = mActiveSensors.keyAt(i);
	snprintf(buffer, SIZE, "%s (handle=0x%08x, connections=%d)\n",
	getSensorName(handle).string(),
	handle,
	mActiveSensors.valueAt(i)->getNumConnections());
	result.append(buffer);
	}
	}
	write(fd, result.string(), result.size());
	return NO_ERROR;
	}

	bool SensorService::threadLoop()
	{
	LOGD("nuSensorService thread starting...");

	const size_t numEventMax = 16 * (1 + mVirtualSensorList.size());
	sensors_event_t buffer[numEventMax];
	sensors_event_t scratch[numEventMax];
	SensorDevice& device(SensorDevice::getInstance());
	const size_t vcount = mVirtualSensorList.size();

	ssize_t count;
	do {
	count = device.poll(buffer, numEventMax);
	if (count<0) {
	LOGE("sensor poll failed (%s)", strerror(-count));
	break;
	}

	recordLastValue(buffer, count);

	// handle virtual sensors
	if (count && vcount) {
	sensors_event_t const * const event = buffer;
	const DefaultKeyedVector<int, SensorInterface*> virtualSensors(
	getActiveVirtualSensors());
	const size_t activeVirtualSensorCount = virtualSensors.size();
	if (activeVirtualSensorCount) {
	size_t k = 0;
	SensorFusion& fusion(SensorFusion::getInstance());
	if (fusion.isEnabled()) {
	for (size_t i=0 ; i<size_t(count) ; i++) {
	fusion.process(event[i]);
	}
	}
	for (size_t i=0 ; i<size_t(count) ; i++) {
	for (size_t j=0 ; j<activeVirtualSensorCount ; j++) {
	sensors_event_t out;
	if (virtualSensors.valueAt(j)->process(&out, event[i])) {
	buffer[count + k] = out;
	k++;
	}
	}
	}
	if (k) {
	// record the last synthesized values
	recordLastValue(&buffer[count], k);
	count += k;
	// sort the buffer by time-stamps
	sortEventBuffer(buffer, count);
	}
	}
	}

	// send our events to clients...
	const SortedVector< wp<SensorEventConnection> > activeConnections(
	getActiveConnections());
	size_t numConnections = activeConnections.size();
	for (size_t i=0 ; i<numConnections ; i++) {
	sp<SensorEventConnection> connection(
	activeConnections[i].promote());
	if (connection != 0) {
	connection->sendEvents(buffer, count, scratch);
	}
	}
	} while (count >= 0 \|\| Thread::exitPending());

	LOGW("Exiting SensorService::threadLoop!");
	return false;
	}

	void SensorService::recordLastValue(
	sensors_event_t const * buffer, size_t count)
	{
	Mutex::Autolock _l(mLock);

	// record the last event for each sensor
	int32_t prev = buffer[0].sensor;
	for (size_t i=1 ; i<count ; i++) {
	// record the last event of each sensor type in this buffer
	int32_t curr = buffer[i].sensor;
	if (curr != prev) {
	mLastEventSeen.editValueFor(prev) = buffer[i-1];
	prev = curr;
	}
	}
	mLastEventSeen.editValueFor(prev) = buffer[count-1];
	}

	void SensorService::sortEventBuffer(sensors_event_t* buffer, size_t count)
	{
	struct compar {
	static int cmp(void const* lhs, void const* rhs) {
	sensors_event_t const* l = static_cast<sensors_event_t const*>(lhs);
	sensors_event_t const* r = static_cast<sensors_event_t const*>(rhs);
	return r->timestamp - l->timestamp;
	}
	};
	qsort(buffer, count, sizeof(sensors_event_t), compar::cmp);
	}

	SortedVector< wp<SensorService::SensorEventConnection> >
	SensorService::getActiveConnections() const
	{
	Mutex::Autolock _l(mLock);
	return mActiveConnections;
	}

	DefaultKeyedVector<int, SensorInterface*>
	SensorService::getActiveVirtualSensors() const
	{
	Mutex::Autolock _l(mLock);
	return mActiveVirtualSensors;
	}

	String8 SensorService::getSensorName(int handle) const {
	size_t count = mUserSensorList.size();
	for (size_t i=0 ; i<count ; i++) {
	const Sensor& sensor(mUserSensorList[i]);
	if (sensor.getHandle() == handle) {
	return sensor.getName();
	}
	}
	String8 result("unknown");
	return result;
	}

	Vector<Sensor> SensorService::getSensorList()
	{
	return mUserSensorList;
	}

	sp<ISensorEventConnection> SensorService::createSensorEventConnection()
	{
	sp<SensorEventConnection> result(new SensorEventConnection(this));
	return result;
	}

	void SensorService::cleanupConnection(SensorEventConnection* c)
	{
	Mutex::Autolock _l(mLock);
	const wp<SensorEventConnection> connection(c);
	size_t size = mActiveSensors.size();
	LOGD_IF(DEBUG_CONNECTIONS, "%d active sensors", size);
	for (size_t i=0 ; i<size ; ) {
	int handle = mActiveSensors.keyAt(i);
	if (c->hasSensor(handle)) {
	LOGD_IF(DEBUG_CONNECTIONS, "%i: disabling handle=0x%08x", i, handle);
	SensorInterface* sensor = mSensorMap.valueFor( handle );
	LOGE_IF(!sensor, "mSensorMap[handle=0x%08x] is null!", handle);
	if (sensor) {
	sensor->activate(c, false);
	}
	}
	SensorRecord* rec = mActiveSensors.valueAt(i);
	LOGE_IF(!rec, "mActiveSensors[%d] is null (handle=0x%08x)!", i, handle);
	LOGD_IF(DEBUG_CONNECTIONS,
	"removing connection %p for sensor[%d].handle=0x%08x",
	c, i, handle);

	if (rec && rec->removeConnection(connection)) {
	LOGD_IF(DEBUG_CONNECTIONS, "... and it was the last connection");
	mActiveSensors.removeItemsAt(i, 1);
	mActiveVirtualSensors.removeItem(handle);
	delete rec;
	size--;
	} else {
	i++;
	}
	}
	mActiveConnections.remove(connection);
	}

	status_t SensorService::enable(const sp<SensorEventConnection>& connection,
	int handle)
	{
	if (mInitCheck != NO_ERROR)
	return mInitCheck;

	Mutex::Autolock _l(mLock);
	SensorInterface* sensor = mSensorMap.valueFor(handle);
	status_t err = sensor ? sensor->activate(connection.get(), true) : status_t(BAD_VALUE);
	if (err == NO_ERROR) {
	SensorRecord* rec = mActiveSensors.valueFor(handle);
	if (rec == 0) {
	rec = new SensorRecord(connection);
	mActiveSensors.add(handle, rec);
	if (sensor->isVirtual()) {
	mActiveVirtualSensors.add(handle, sensor);
	}
	} else {
	if (rec->addConnection(connection)) {
	// this sensor is already activated, but we are adding a
	// connection that uses it. Immediately send down the last
	// known value of the requested sensor if it's not a
	// "continuous" sensor.
	if (sensor->getSensor().getMinDelay() == 0) {
	sensors_event_t scratch;
	sensors_event_t& event(mLastEventSeen.editValueFor(handle));
	if (event.version == sizeof(sensors_event_t)) {
	connection->sendEvents(&event, 1);
	}
	}
	}
	}
	if (err == NO_ERROR) {
	// connection now active
	if (connection->addSensor(handle)) {
	// the sensor was added (which means it wasn't already there)
	// so, see if this connection becomes active
	if (mActiveConnections.indexOf(connection) < 0) {
	mActiveConnections.add(connection);
	}
	}
	}
	}
	return err;
	}

	status_t SensorService::disable(const sp<SensorEventConnection>& connection,
	int handle)
	{
	if (mInitCheck != NO_ERROR)
	return mInitCheck;

	status_t err = NO_ERROR;
	Mutex::Autolock _l(mLock);
	SensorRecord* rec = mActiveSensors.valueFor(handle);
	if (rec) {
	// see if this connection becomes inactive
	connection->removeSensor(handle);
	if (connection->hasAnySensor() == false) {
	mActiveConnections.remove(connection);
	}
	// see if this sensor becomes inactive
	if (rec->removeConnection(connection)) {
	mActiveSensors.removeItem(handle);
	mActiveVirtualSensors.removeItem(handle);
	delete rec;
	}
	SensorInterface* sensor = mSensorMap.valueFor(handle);
	err = sensor ? sensor->activate(connection.get(), false) : status_t(BAD_VALUE);
	}
	return err;
	}

	status_t SensorService::setEventRate(const sp<SensorEventConnection>& connection,
	int handle, nsecs_t ns)
	{
	if (mInitCheck != NO_ERROR)
	return mInitCheck;

	if (ns < 0)
	return BAD_VALUE;

	if (ns < MINIMUM_EVENTS_PERIOD)
	ns = MINIMUM_EVENTS_PERIOD;

	SensorInterface* sensor = mSensorMap.valueFor(handle);
	if (!sensor) return BAD_VALUE;
	return sensor->setDelay(connection.get(), handle, ns);
	}

	// ---------------------------------------------------------------------------

	SensorService::SensorRecord::SensorRecord(
	const sp<SensorEventConnection>& connection)
	{
	mConnections.add(connection);
	}

	bool SensorService::SensorRecord::addConnection(
	const sp<SensorEventConnection>& connection)
	{
	if (mConnections.indexOf(connection) < 0) {
	mConnections.add(connection);
	return true;
	}
	return false;
	}

	bool SensorService::SensorRecord::removeConnection(
	const wp<SensorEventConnection>& connection)
	{
	ssize_t index = mConnections.indexOf(connection);
	if (index >= 0) {
	mConnections.removeItemsAt(index, 1);
	}
	return mConnections.size() ? false : true;
	}

	// ---------------------------------------------------------------------------

	SensorService::SensorEventConnection::SensorEventConnection(
	const sp<SensorService>& service)
	: mService(service), mChannel(new SensorChannel())
	{
	}

	SensorService::SensorEventConnection::~SensorEventConnection()
	{
	LOGD_IF(DEBUG_CONNECTIONS, "~SensorEventConnection(%p)", this);
	mService->cleanupConnection(this);
	}

	void SensorService::SensorEventConnection::onFirstRef()
	{
	}

	bool SensorService::SensorEventConnection::addSensor(int32_t handle) {
	Mutex::Autolock _l(mConnectionLock);
	if (mSensorInfo.indexOf(handle) <= 0) {
	mSensorInfo.add(handle);
	return true;
	}
	return false;
	}

	bool SensorService::SensorEventConnection::removeSensor(int32_t handle) {
	Mutex::Autolock _l(mConnectionLock);
	if (mSensorInfo.remove(handle) >= 0) {
	return true;
	}
	return false;
	}

	bool SensorService::SensorEventConnection::hasSensor(int32_t handle) const {
	Mutex::Autolock _l(mConnectionLock);
	return mSensorInfo.indexOf(handle) >= 0;
	}

	bool SensorService::SensorEventConnection::hasAnySensor() const {
	Mutex::Autolock _l(mConnectionLock);
	return mSensorInfo.size() ? true : false;
	}

	status_t SensorService::SensorEventConnection::sendEvents(
	sensors_event_t const* buffer, size_t numEvents,
	sensors_event_t* scratch)
	{
	// filter out events not for this connection
	size_t count = 0;
	if (scratch) {
	Mutex::Autolock _l(mConnectionLock);
	size_t i=0;
	while (i<numEvents) {
	const int32_t curr = buffer[i].sensor;
	if (mSensorInfo.indexOf(curr) >= 0) {
	do {
	scratch[count++] = buffer[i++];
	} while ((i<numEvents) && (buffer[i].sensor == curr));
	} else {
	i++;
	}
	}
	} else {
	scratch = const_cast<sensors_event_t *>(buffer);
	count = numEvents;
	}

	if (count == 0)
	return 0;

	ssize_t size = mChannel->write(scratch, count*sizeof(sensors_event_t));
	if (size == -EAGAIN) {
	// the destination doesn't accept events anymore, it's probably
	// full. For now, we just drop the events on the floor.
	//LOGW("dropping %d events on the floor", count);
	return size;
	}

	//LOGE_IF(size<0, "dropping %d events on the floor (%s)",
	// count, strerror(-size));

	return size < 0 ? status_t(size) : status_t(NO_ERROR);
	}

	sp<SensorChannel> SensorService::SensorEventConnection::getSensorChannel() const
	{
	return mChannel;
	}

	status_t SensorService::SensorEventConnection::enableDisable(
	int handle, bool enabled)
	{
	status_t err;
	if (enabled) {
	err = mService->enable(this, handle);
	} else {
	err = mService->disable(this, handle);
	}
	return err;
	}

	status_t SensorService::SensorEventConnection::setEventRate(
	int handle, nsecs_t ns)
	{
	return mService->setEventRate(this, handle, ns);
	}

	// ---------------------------------------------------------------------------
	}; // namespace android