diff --git a/include/gui/ISensorServer.h b/include/gui/ISensorServer.h
index 4feb6db..3dca2a3 100644
--- a/include/gui/ISensorServer.h
+++ b/include/gui/ISensorServer.h
@@ -40,7 +40,7 @@
     virtual Vector<Sensor> getSensorList(const String16& opPackageName) = 0;
     virtual sp<ISensorEventConnection> createSensorEventConnection(const String8& packageName,
              int mode, const String16& opPackageName) = 0;
-    virtual status_t enableDataInjection(int enable) = 0;
+    virtual int32_t isDataInjectionEnabled() = 0;
 };
 
 // ----------------------------------------------------------------------------
diff --git a/include/gui/SensorManager.h b/include/gui/SensorManager.h
index 4c34e12..3796067 100644
--- a/include/gui/SensorManager.h
+++ b/include/gui/SensorManager.h
@@ -107,7 +107,7 @@
     ssize_t getSensorList(Sensor const* const** list) const;
     Sensor const* getDefaultSensor(int type);
     sp<SensorEventQueue> createEventQueue(String8 packageName = String8(""), int mode = 0);
-    status_t enableDataInjection(bool enable);
+    bool isDataInjectionEnabled();
 
 private:
     // DeathRecipient interface
diff --git a/libs/binder/Parcel.cpp b/libs/binder/Parcel.cpp
index 31667d9..7a4ddc4 100644
--- a/libs/binder/Parcel.cpp
+++ b/libs/binder/Parcel.cpp
@@ -735,9 +735,9 @@
     }
 
     if (!val) {
-        return writeAligned(-1);
+        return writeInt32(-1);
     }
-    status_t ret = writeAligned(len);
+    status_t ret = writeInt32(static_cast<uint32_t>(len));
     if (ret == NO_ERROR) {
         ret = write(val, len * sizeof(*val));
     }
@@ -751,9 +751,9 @@
     }
 
     if (!val) {
-        return writeAligned(-1);
+        return writeInt32(-1);
     }
-    status_t ret = writeAligned(len);
+    status_t ret = writeInt32(static_cast<uint32_t>(len));
     if (ret == NO_ERROR) {
         ret = write(val, len * sizeof(*val));
     }
diff --git a/libs/gui/ISensorServer.cpp b/libs/gui/ISensorServer.cpp
index 5dde9f9..f581b5c 100644
--- a/libs/gui/ISensorServer.cpp
+++ b/libs/gui/ISensorServer.cpp
@@ -77,10 +77,9 @@
         return interface_cast<ISensorEventConnection>(reply.readStrongBinder());
     }
 
-    virtual status_t enableDataInjection(int enable) {
+    virtual int isDataInjectionEnabled() {
         Parcel data, reply;
         data.writeInterfaceToken(ISensorServer::getInterfaceDescriptor());
-        data.writeInt32(enable);
         remote()->transact(ENABLE_DATA_INJECTION, data, &reply);
         return reply.readInt32();
     }
@@ -121,8 +120,7 @@
         }
         case ENABLE_DATA_INJECTION: {
             CHECK_INTERFACE(ISensorServer, data, reply);
-            int32_t enable = data.readInt32();
-            status_t ret = enableDataInjection(enable);
+            int32_t ret = isDataInjectionEnabled();
             reply->writeInt32(static_cast<int32_t>(ret));
             return NO_ERROR;
         }
diff --git a/libs/gui/SensorEventQueue.cpp b/libs/gui/SensorEventQueue.cpp
index 8b2018f..4b7986e 100644
--- a/libs/gui/SensorEventQueue.cpp
+++ b/libs/gui/SensorEventQueue.cpp
@@ -20,6 +20,7 @@
 #include <stdint.h>
 #include <sys/types.h>
 #include <sys/socket.h>
+#include <linux/errno.h>
 
 #include <utils/Errors.h>
 #include <utils/RefBase.h>
@@ -150,13 +151,20 @@
 }
 
 status_t SensorEventQueue::injectSensorEvent(const ASensorEvent& event) {
-   // Blocking call.
-   ssize_t size = ::send(mSensorChannel->getFd(), &event, sizeof(event), MSG_NOSIGNAL);
-   if (size < 0) {
-       ALOGE("injectSensorEvent failure %zd %d", size, mSensorChannel->getFd());
-       return INVALID_OPERATION;
-   }
-   return NO_ERROR;
+    do {
+        // Blocking call.
+        ssize_t size = ::send(mSensorChannel->getFd(), &event, sizeof(event), MSG_NOSIGNAL);
+        if (size >= 0) {
+            return NO_ERROR;
+        } else if (size < 0 && errno == EAGAIN) {
+            // If send is returning a "Try again" error, sleep for 100ms and try again. In all
+            // other cases log a failure and exit.
+            usleep(100000);
+        } else {
+            ALOGE("injectSensorEvent failure %s %zd", strerror(errno), size);
+            return INVALID_OPERATION;
+        }
+    } while (true);
 }
 
 void SensorEventQueue::sendAck(const ASensorEvent* events, int count) {
diff --git a/libs/gui/SensorManager.cpp b/libs/gui/SensorManager.cpp
index 8c9f95b..dd37781 100644
--- a/libs/gui/SensorManager.cpp
+++ b/libs/gui/SensorManager.cpp
@@ -153,12 +153,12 @@
     return queue;
 }
 
-status_t SensorManager::enableDataInjection(bool enable) {
+bool SensorManager::isDataInjectionEnabled() {
     Mutex::Autolock _l(mLock);
     if (assertStateLocked() == NO_ERROR) {
-        return mSensorServer->enableDataInjection(enable);
+        return mSensorServer->isDataInjectionEnabled();
     }
-    return INVALID_OPERATION;
+    return false;
 }
 
 // ----------------------------------------------------------------------------
diff --git a/services/sensorservice/SensorService.cpp b/services/sensorservice/SensorService.cpp
index 550107c..71aa160 100644
--- a/services/sensorservice/SensorService.cpp
+++ b/services/sensorservice/SensorService.cpp
@@ -66,7 +66,6 @@
 
 const char* SensorService::WAKE_LOCK_NAME = "SensorService";
 // Permissions.
-static const String16 sDataInjectionPermission("android.permission.LOCATION_HARDWARE");
 static const String16 sDump("android.permission.DUMP");
 
 SensorService::SensorService()
@@ -246,12 +245,12 @@
                 IPCThreadState::self()->getCallingPid(),
                 IPCThreadState::self()->getCallingUid());
     } else {
-        if (args.size() > 1) {
+        if (args.size() > 2) {
            return INVALID_OPERATION;
         }
         Mutex::Autolock _l(mLock);
         SensorDevice& dev(SensorDevice::getInstance());
-        if (args.size() == 1 && args[0] == String16("restrict")) {
+        if (args.size() == 2 && args[0] == String16("restrict")) {
             // If already in restricted mode. Ignore.
             if (mCurrentOperatingMode == RESTRICTED) {
                 return status_t(NO_ERROR);
@@ -268,6 +267,7 @@
             for (size_t i=0 ; i< mActiveSensors.size(); ++i) {
                 mActiveSensors.valueAt(i)->clearAllPendingFlushConnections();
             }
+            mWhiteListedPackage.setTo(String8(args[1]));
             return status_t(NO_ERROR);
         } else if (args.size() == 1 && args[0] == String16("enable")) {
             // If currently in restricted mode, reset back to NORMAL mode else ignore.
@@ -275,7 +275,30 @@
                 mCurrentOperatingMode = NORMAL;
                 dev.enableAllSensors();
             }
+            if (mCurrentOperatingMode == DATA_INJECTION) {
+               resetToNormalModeLocked();
+            }
+            mWhiteListedPackage.clear();
             return status_t(NO_ERROR);
+        } else if (args.size() == 2 && args[0] == String16("data_injection")) {
+            if (mCurrentOperatingMode == NORMAL) {
+                dev.disableAllSensors();
+                status_t err = dev.setMode(DATA_INJECTION);
+                if (err == NO_ERROR) {
+                    mCurrentOperatingMode = DATA_INJECTION;
+                } else {
+                    // Re-enable sensors.
+                    dev.enableAllSensors();
+                }
+                mWhiteListedPackage.setTo(String8(args[1]));
+                return NO_ERROR;
+            } else if (mCurrentOperatingMode == DATA_INJECTION) {
+                // Already in DATA_INJECTION mode. Treat this as a no_op.
+                return NO_ERROR;
+            } else {
+                // Transition to data injection mode supported only from NORMAL mode.
+                return INVALID_OPERATION;
+            }
         } else if (mSensorList.size() == 0) {
             result.append("No Sensors on the device\n");
         } else {
@@ -362,10 +385,10 @@
                    result.appendFormat(" NORMAL\n");
                    break;
                case RESTRICTED:
-                   result.appendFormat(" RESTRICTED\n");
+                   result.appendFormat(" RESTRICTED : %s\n", mWhiteListedPackage.string());
                    break;
                case DATA_INJECTION:
-                   result.appendFormat(" DATA_INJECTION\n");
+                   result.appendFormat(" DATA_INJECTION : %s\n", mWhiteListedPackage.string());
             }
             result.appendFormat("%zd active connections\n", mActiveConnections.size());
 
@@ -712,12 +735,15 @@
     if (requestedMode != NORMAL && requestedMode != DATA_INJECTION) {
         return NULL;
     }
-    // DATA_INJECTION mode needs to have the required permissions set.
-    if (requestedMode == DATA_INJECTION && !hasDataInjectionPermissions()) {
-        return NULL;
-    }
 
     Mutex::Autolock _l(mLock);
+    // To create a client in DATA_INJECTION mode to inject data, SensorService should already be
+    // operating in DI mode.
+    if (requestedMode == DATA_INJECTION) {
+        if (mCurrentOperatingMode != DATA_INJECTION) return NULL;
+        if (!isWhiteListedPackage(packageName)) return NULL;
+    }
+
     uid_t uid = IPCThreadState::self()->getCallingUid();
     sp<SensorEventConnection> result(new SensorEventConnection(this, uid, packageName,
             requestedMode == DATA_INJECTION, opPackageName));
@@ -732,35 +758,9 @@
     return result;
 }
 
-status_t SensorService::enableDataInjection(int requestedMode) {
-    if (!hasDataInjectionPermissions()) {
-        return INVALID_OPERATION;
-    }
+int SensorService::isDataInjectionEnabled() {
     Mutex::Autolock _l(mLock);
-    ALOGD_IF(DEBUG_CONNECTIONS, "SensorService::enableDataInjection %d", requestedMode);
-    SensorDevice& dev(SensorDevice::getInstance());
-    status_t err(NO_ERROR);
-    if (requestedMode == DATA_INJECTION) {
-        if (mCurrentOperatingMode == NORMAL) {
-           dev.disableAllSensors();
-           err = dev.setMode(requestedMode);
-           if (err == NO_ERROR) {
-               mCurrentOperatingMode = DATA_INJECTION;
-           } else {
-               // Re-enable sensors.
-               dev.enableAllSensors();
-           }
-       } else if (mCurrentOperatingMode == DATA_INJECTION) {
-           // Already in DATA_INJECTION mode. Treat this as a no_op.
-           return NO_ERROR;
-       } else {
-           // Transition to data injection mode supported only from NORMAL mode.
-           return INVALID_OPERATION;
-       }
-    } else if (requestedMode == NORMAL && mCurrentOperatingMode != NORMAL) {
-       err = resetToNormalModeLocked();
-    }
-    return err;
+    return (mCurrentOperatingMode == DATA_INJECTION);
 }
 
 status_t SensorService::resetToNormalMode() {
@@ -838,7 +838,8 @@
     }
 
     Mutex::Autolock _l(mLock);
-    if (mCurrentOperatingMode == RESTRICTED && !isWhiteListedPackage(connection->getPackageName())) {
+    if ((mCurrentOperatingMode == RESTRICTED || mCurrentOperatingMode == DATA_INJECTION)
+           && !isWhiteListedPackage(connection->getPackageName())) {
         return INVALID_OPERATION;
     }
 
@@ -1106,15 +1107,6 @@
     return true;
 }
 
-bool SensorService::hasDataInjectionPermissions() {
-    if (!PermissionCache::checkCallingPermission(sDataInjectionPermission)) {
-        ALOGE("Permission Denial trying to activate data injection without"
-              " the required permission");
-        return false;
-    }
-    return true;
-}
-
 void SensorService::checkWakeLockState() {
     Mutex::Autolock _l(mLock);
     checkWakeLockStateLocked();
@@ -1159,8 +1151,7 @@
 }
 
 bool SensorService::isWhiteListedPackage(const String8& packageName) {
-    // TODO: Come up with a list of packages.
-    return (packageName.find(".cts.") != -1);
+    return (packageName.contains(mWhiteListedPackage.string()));
 }
 
 int SensorService::getNumEventsForSensorType(int sensor_event_type) {
diff --git a/services/sensorservice/SensorService.h b/services/sensorservice/SensorService.h
index 0a7abe8..9a573ae 100644
--- a/services/sensorservice/SensorService.h
+++ b/services/sensorservice/SensorService.h
@@ -100,6 +100,18 @@
       // State Transitions supported.
       //     RESTRICTED   <---  NORMAL   ---> DATA_INJECTION
       //                  --->           <---
+
+      // Shell commands to switch modes in SensorService.
+      // 1) Put SensorService in RESTRICTED mode with packageName .cts. If it is already in
+      // restricted mode it is treated as a NO_OP (and packageName is NOT changed).
+      // $ adb shell dumpsys sensorservice restrict .cts.
+      //
+      // 2) Put SensorService in DATA_INJECTION mode with packageName .xts. If it is already in
+      // data_injection mode it is treated as a NO_OP (and packageName is NOT changed).
+      // $ adb shell dumpsys sensorservice data_injection .xts.
+      //
+      // 3) Reset sensorservice back to NORMAL mode.
+      // $ adb shell dumpsys sensorservice enable
     };
 
     static const char* WAKE_LOCK_NAME;
@@ -117,7 +129,7 @@
     virtual Vector<Sensor> getSensorList(const String16& opPackageName);
     virtual sp<ISensorEventConnection> createSensorEventConnection(const String8& packageName,
              int requestedMode, const String16& opPackageName);
-    virtual status_t enableDataInjection(int enable);
+    virtual int isDataInjectionEnabled();
     virtual status_t dump(int fd, const Vector<String16>& args);
 
     class SensorEventConnection : public BnSensorEventConnection, public LooperCallback {
@@ -334,7 +346,6 @@
             sensors_event_t const* buffer, const int count);
     static bool canAccessSensor(const Sensor& sensor, const char* operation,
             const String16& opPackageName);
-    static bool hasDataInjectionPermissions();
     // SensorService acquires a partial wakelock for delivering events from wake up sensors. This
     // method checks whether all the events from these wake up sensors have been delivered to the
     // corresponding applications, if yes the wakelock is released.
@@ -394,6 +405,11 @@
     sensors_event_t *mSensorEventBuffer, *mSensorEventScratch;
     SensorEventConnection const **mMapFlushEventsToConnections;
     Mode mCurrentOperatingMode;
+    // This packagaName is set when SensorService is in RESTRICTED or DATA_INJECTION mode. Only
+    // applications with this packageName are allowed to activate/deactivate or call flush on
+    // sensors. To run CTS this is can be set to ".cts." and only CTS tests will get access to
+    // sensors.
+    String8 mWhiteListedPackage;
 
     // The size of this vector is constant, only the items are mutable
     KeyedVector<int32_t, CircularBuffer *> mLastEventSeen;