Set up Multi-HAL for Sensors HAL 2.1

Bug: 149758467
Test: Load onto device and verify VTS passes
Test: Verify new fake subhals load properly and that unit tests pass
that have been updated in this topic

Change-Id: Icefae8c1261b29d704beedd51de5b3f53b374cc4

13 files changed, 852 insertions, 180 deletions

diff --git a/sensors/2.0/multihal/Android.bp b/sensors/2.0/multihal/Android.bp
index 3ce33906cc..bf51fcdbbc 100644
--- a/sensors/2.0/multihal/Android.bp
+++ b/sensors/2.0/multihal/Android.bp
@@ -25,6 +25,9 @@ cc_binary {
     ],
     init_rc: ["android.hardware.sensors@2.0-service-multihal.rc"],
     vintf_fragments: ["android.hardware.sensors@2.0-multihal.xml"],
+    header_libs: [
+        "android.hardware.sensors@2.X-shared-utils",
+    ],
     shared_libs: [
         "android.hardware.sensors@2.0",
         "android.hardware.sensors@2.0-ScopedWakelock",
@@ -37,5 +40,8 @@ cc_binary {
         "libpower",
         "libutils",
     ],
-    static_libs: ["android.hardware.sensors@2.X-multihal"],
+    static_libs: [
+        "android.hardware.sensors@1.0-convert",
+        "android.hardware.sensors@2.X-multihal",
+    ],
 }
diff --git a/sensors/2.0/multihal/service.cpp b/sensors/2.0/multihal/service.cpp
index ef77048020..f50ad7e16a 100644
--- a/sensors/2.0/multihal/service.cpp
+++ b/sensors/2.0/multihal/service.cpp
@@ -23,12 +23,12 @@
 using android::hardware::configureRpcThreadpool;
 using android::hardware::joinRpcThreadpool;
 using android::hardware::sensors::V2_0::ISensors;
-using android::hardware::sensors::V2_0::implementation::HalProxy;
+using android::hardware::sensors::V2_1::implementation::HalProxyV2_0;
 
 int main(int /* argc */, char** /* argv */) {
     configureRpcThreadpool(1, true);
 
-    android::sp<ISensors> halProxy = new HalProxy();
+    android::sp<ISensors> halProxy = new HalProxyV2_0();
     if (halProxy->registerAsService() != ::android::OK) {
         ALOGE("Failed to register Sensors HAL instance");
         return -1;
diff --git a/sensors/common/default/2.X/multihal/Android.bp b/sensors/common/default/2.X/multihal/Android.bp
index 6122323fd8..c80c47a4bf 100644
--- a/sensors/common/default/2.X/multihal/Android.bp
+++ b/sensors/common/default/2.X/multihal/Android.bp
@@ -17,6 +17,7 @@ cc_defaults {
     name: "android.hardware.sensors@2.X-multihal-defaults",
     header_libs: [
         "android.hardware.sensors@2.X-multihal.header",
+        "android.hardware.sensors@2.X-shared-utils",
     ],
     shared_libs: [
         "android.hardware.sensors@1.0",
@@ -30,6 +31,9 @@ cc_defaults {
         "libpower",
         "libutils",
     ],
+    static_libs: [
+        "android.hardware.sensors@1.0-convert",
+    ],
     cflags: ["-DLOG_TAG=\"SensorsMultiHal\""],
 }
 
@@ -62,6 +66,7 @@ cc_library_static {
     ],
     srcs: [
         "HalProxy.cpp",
+        "HalProxyCallback.cpp",
     ],
     vendor_available: true,
     export_header_lib_headers: [
diff --git a/sensors/common/default/2.X/multihal/HalProxy.cpp b/sensors/common/default/2.X/multihal/HalProxy.cpp
index 869c0330f4..a09e9e938e 100644
--- a/sensors/common/default/2.X/multihal/HalProxy.cpp
+++ b/sensors/common/default/2.X/multihal/HalProxy.cpp
@@ -32,15 +32,17 @@
 namespace android {
 namespace hardware {
 namespace sensors {
-namespace V2_0 {
+namespace V2_1 {
 namespace implementation {
 
+using ::android::hardware::sensors::V1_0::Result;
 using ::android::hardware::sensors::V2_0::EventQueueFlagBits;
 using ::android::hardware::sensors::V2_0::WakeLockQueueFlagBits;
 using ::android::hardware::sensors::V2_0::implementation::getTimeNow;
 using ::android::hardware::sensors::V2_0::implementation::kWakelockTimeoutNs;
 
-typedef ISensorsSubHal*(SensorsHalGetSubHalFunc)(uint32_t*);
+typedef V2_0::implementation::ISensorsSubHal*(SensorsHalGetSubHalFunc)(uint32_t*);
+typedef V2_1::implementation::ISensorsSubHal*(SensorsHalGetSubHalV2_1Func)(uint32_t*);
 
 static constexpr int32_t kBitsAfterSubHalIndex = 24;
 
@@ -85,7 +87,24 @@ HalProxy::HalProxy() {
     init();
 }
 
-HalProxy::HalProxy(std::vector<ISensorsSubHal*>& subHalList) : mSubHalList(subHalList) {
+HalProxy::HalProxy(std::vector<ISensorsSubHalV2_0*>& subHalList) {
+    for (ISensorsSubHalV2_0* subHal : subHalList) {
+        mSubHalList.push_back(std::make_unique<SubHalWrapperV2_0>(subHal));
+    }
+
+    init();
+}
+
+HalProxy::HalProxy(std::vector<ISensorsSubHalV2_0*>& subHalList,
+                   std::vector<ISensorsSubHalV2_1*>& subHalListV2_1) {
+    for (ISensorsSubHalV2_0* subHal : subHalList) {
+        mSubHalList.push_back(std::make_unique<SubHalWrapperV2_0>(subHal));
+    }
+
+    for (ISensorsSubHalV2_1* subHal : subHalListV2_1) {
+        mSubHalList.push_back(std::make_unique<SubHalWrapperV2_1>(subHal));
+    }
+
     init();
 }
 
@@ -93,8 +112,8 @@ HalProxy::~HalProxy() {
     stopThreads();
 }
 
-Return<void> HalProxy::getSensorsList(getSensorsList_cb _hidl_cb) {
-    std::vector<SensorInfo> sensors;
+Return<void> HalProxy::getSensorsList_2_1(ISensorsV2_1::getSensorsList_2_1_cb _hidl_cb) {
+    std::vector<V2_1::SensorInfo> sensors;
     for (const auto& iter : mSensors) {
         sensors.push_back(iter.second);
     }
@@ -102,22 +121,31 @@ Return<void> HalProxy::getSensorsList(getSensorsList_cb _hidl_cb) {
     return Void();
 }
 
+Return<void> HalProxy::getSensorsList(ISensorsV2_0::getSensorsList_cb _hidl_cb) {
+    std::vector<V1_0::SensorInfo> sensors;
+    for (const auto& iter : mSensors) {
+        sensors.push_back(convertToOldSensorInfo(iter.second));
+    }
+    _hidl_cb(sensors);
+    return Void();
+}
+
 Return<Result> HalProxy::setOperationMode(OperationMode mode) {
     Result result = Result::OK;
     size_t subHalIndex;
     for (subHalIndex = 0; subHalIndex < mSubHalList.size(); subHalIndex++) {
-        ISensorsSubHal* subHal = mSubHalList[subHalIndex];
-        result = subHal->setOperationMode(mode);
+        result = mSubHalList[subHalIndex]->setOperationMode(mode);
         if (result != Result::OK) {
-            ALOGE("setOperationMode failed for SubHal: %s", subHal->getName().c_str());
+            ALOGE("setOperationMode failed for SubHal: %s",
+                  mSubHalList[subHalIndex]->getName().c_str());
             break;
         }
     }
+
     if (result != Result::OK) {
         // Reset the subhal operation modes that have been flipped
         for (size_t i = 0; i < subHalIndex; i++) {
-            ISensorsSubHal* subHal = mSubHalList[i];
-            subHal->setOperationMode(mCurrentOperationMode);
+            mSubHalList[i]->setOperationMode(mCurrentOperationMode);
         }
     } else {
         mCurrentOperationMode = mode;
@@ -133,10 +161,42 @@ Return<Result> HalProxy::activate(int32_t sensorHandle, bool enabled) {
             ->activate(clearSubHalIndex(sensorHandle), enabled);
 }
 
+Return<Result> HalProxy::initialize_2_1(
+        const ::android::hardware::MQDescriptorSync<V2_1::Event>& eventQueueDescriptor,
+        const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
+        const sp<V2_1::ISensorsCallback>& sensorsCallback) {
+    sp<ISensorsCallbackWrapperBase> dynamicCallback =
+            new ISensorsCallbackWrapperV2_1(sensorsCallback);
+
+    // Create the Event FMQ from the eventQueueDescriptor. Reset the read/write positions.
+    auto eventQueue =
+            std::make_unique<EventMessageQueueV2_1>(eventQueueDescriptor, true /* resetPointers */);
+    std::unique_ptr<EventMessageQueueWrapperBase> queue =
+            std::make_unique<EventMessageQueueWrapperV2_1>(eventQueue);
+
+    return initializeCommon(queue, wakeLockDescriptor, dynamicCallback);
+}
+
 Return<Result> HalProxy::initialize(
-        const ::android::hardware::MQDescriptorSync<Event>& eventQueueDescriptor,
+        const ::android::hardware::MQDescriptorSync<V1_0::Event>& eventQueueDescriptor,
         const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
-        const sp<ISensorsCallback>& sensorsCallback) {
+        const sp<V2_0::ISensorsCallback>& sensorsCallback) {
+    sp<ISensorsCallbackWrapperBase> dynamicCallback =
+            new ISensorsCallbackWrapperV2_0(sensorsCallback);
+
+    // Create the Event FMQ from the eventQueueDescriptor. Reset the read/write positions.
+    auto eventQueue =
+            std::make_unique<EventMessageQueueV2_0>(eventQueueDescriptor, true /* resetPointers */);
+    std::unique_ptr<EventMessageQueueWrapperBase> queue =
+            std::make_unique<EventMessageQueueWrapperV1_0>(eventQueue);
+
+    return initializeCommon(queue, wakeLockDescriptor, dynamicCallback);
+}
+
+Return<Result> HalProxy::initializeCommon(
+        std::unique_ptr<EventMessageQueueWrapperBase>& eventQueue,
+        const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
+        const sp<ISensorsCallbackWrapperBase>& sensorsCallback) {
     Result result = Result::OK;
 
     stopThreads();
@@ -147,7 +207,7 @@ Return<Result> HalProxy::initialize(
     disableAllSensors();
 
     // Clears the queue if any events were pending write before.
-    mPendingWriteEventsQueue = std::queue<std::pair<std::vector<Event>, size_t>>();
+    mPendingWriteEventsQueue = std::queue<std::pair<std::vector<V2_1::Event>, size_t>>();
     mSizePendingWriteEventsQueue = 0;
 
     // Clears previously connected dynamic sensors
@@ -156,8 +216,7 @@ Return<Result> HalProxy::initialize(
     mDynamicSensorsCallback = sensorsCallback;
 
     // Create the Event FMQ from the eventQueueDescriptor. Reset the read/write positions.
-    mEventQueue =
-            std::make_unique<EventMessageQueue>(eventQueueDescriptor, true /* resetPointers */);
+    mEventQueue = std::move(eventQueue);
 
     // Create the Wake Lock FMQ that is used by the framework to communicate whenever WAKE_UP
     // events have been successfully read and handled by the framework.
@@ -186,12 +245,10 @@ Return<Result> HalProxy::initialize(
     mWakelockThread = std::thread(startWakelockThread, this);
 
     for (size_t i = 0; i < mSubHalList.size(); i++) {
-        auto subHal = mSubHalList[i];
-        const auto& subHalCallback = mSubHalCallbacks[i];
-        Result currRes = subHal->initialize(subHalCallback);
+        Result currRes = mSubHalList[i]->initialize(this, this, i);
         if (currRes != Result::OK) {
             result = currRes;
-            ALOGE("Subhal '%s' failed to initialize.", subHal->getName().c_str());
+            ALOGE("Subhal '%s' failed to initialize.", mSubHalList[i]->getName().c_str());
             break;
         }
     }
@@ -217,7 +274,11 @@ Return<Result> HalProxy::flush(int32_t sensorHandle) {
     return getSubHalForSensorHandle(sensorHandle)->flush(clearSubHalIndex(sensorHandle));
 }
 
-Return<Result> HalProxy::injectSensorData(const Event& event) {
+Return<Result> HalProxy::injectSensorData_2_1(const V2_1::Event& event) {
+    return injectSensorData(convertToOldEvent(event));
+}
+
+Return<Result> HalProxy::injectSensorData(const V1_0::Event& event) {
     Result result = Result::OK;
     if (mCurrentOperationMode == OperationMode::NORMAL &&
         event.sensorType != V1_0::SensorType::ADDITIONAL_INFO) {
@@ -226,18 +287,19 @@ Return<Result> HalProxy::injectSensorData(const Event& event) {
         result = Result::BAD_VALUE;
     }
     if (result == Result::OK) {
-        Event subHalEvent = event;
+        V1_0::Event subHalEvent = event;
         if (!isSubHalIndexValid(event.sensorHandle)) {
             return Result::BAD_VALUE;
         }
         subHalEvent.sensorHandle = clearSubHalIndex(event.sensorHandle);
-        result = getSubHalForSensorHandle(event.sensorHandle)->injectSensorData(subHalEvent);
+        result = getSubHalForSensorHandle(event.sensorHandle)
+                         ->injectSensorData(convertToNewEvent(subHalEvent));
     }
     return result;
 }
 
 Return<void> HalProxy::registerDirectChannel(const SharedMemInfo& mem,
-                                             registerDirectChannel_cb _hidl_cb) {
+                                             ISensorsV2_0::registerDirectChannel_cb _hidl_cb) {
     if (mDirectChannelSubHal == nullptr) {
         _hidl_cb(Result::INVALID_OPERATION, -1 /* channelHandle */);
     } else {
@@ -257,7 +319,8 @@ Return<Result> HalProxy::unregisterDirectChannel(int32_t channelHandle) {
 }
 
 Return<void> HalProxy::configDirectReport(int32_t sensorHandle, int32_t channelHandle,
-                                          RateLevel rate, configDirectReport_cb _hidl_cb) {
+                                          RateLevel rate,
+                                          ISensorsV2_0::configDirectReport_cb _hidl_cb) {
     if (mDirectChannelSubHal == nullptr) {
         _hidl_cb(Result::INVALID_OPERATION, -1 /* reportToken */);
     } else if (sensorHandle == -1 && rate != RateLevel::STOP) {
@@ -302,7 +365,7 @@ Return<void> HalProxy::debug(const hidl_handle& fd, const hidl_vec<hidl_string>&
     stream << "  # of non-dynamic sensors across all subhals: " << mSensors.size() << std::endl;
     stream << "  # of dynamic sensors across all subhals: " << mDynamicSensors.size() << std::endl;
     stream << "SubHals (" << mSubHalList.size() << "):" << std::endl;
-    for (ISensorsSubHal* subHal : mSubHalList) {
+    for (auto& subHal : mSubHalList) {
         stream << "  Name: " << subHal->getName() << std::endl;
         stream << "  Debug dump: " << std::endl;
         android::base::WriteStringToFd(stream.str(), writeFd);
@@ -369,20 +432,37 @@ void HalProxy::initializeSubHalListFromConfigFile(const char* configFileName) {
             } else {
                 SensorsHalGetSubHalFunc* sensorsHalGetSubHalPtr =
                         (SensorsHalGetSubHalFunc*)dlsym(handle, "sensorsHalGetSubHal");
-                if (sensorsHalGetSubHalPtr == nullptr) {
-                    ALOGE("Failed to locate sensorsHalGetSubHal function for library: %s",
-                          subHalLibraryFile.c_str());
-                } else {
+                if (sensorsHalGetSubHalPtr != nullptr) {
                     std::function<SensorsHalGetSubHalFunc> sensorsHalGetSubHal =
                             *sensorsHalGetSubHalPtr;
                     uint32_t version;
-                    ISensorsSubHal* subHal = sensorsHalGetSubHal(&version);
+                    ISensorsSubHalV2_0* subHal = sensorsHalGetSubHal(&version);
                     if (version != SUB_HAL_2_0_VERSION) {
                         ALOGE("SubHal version was not 2.0 for library: %s",
                               subHalLibraryFile.c_str());
                     } else {
                         ALOGV("Loaded SubHal from library: %s", subHalLibraryFile.c_str());
-                        mSubHalList.push_back(subHal);
+                        mSubHalList.push_back(std::make_unique<SubHalWrapperV2_0>(subHal));
+                    }
+                } else {
+                    SensorsHalGetSubHalV2_1Func* getSubHalV2_1Ptr =
+                            (SensorsHalGetSubHalV2_1Func*)dlsym(handle, "sensorsHalGetSubHal_2_1");
+
+                    if (getSubHalV2_1Ptr == nullptr) {
+                        ALOGE("Failed to locate sensorsHalGetSubHal function for library: %s",
+                              subHalLibraryFile.c_str());
+                    } else {
+                        std::function<SensorsHalGetSubHalV2_1Func> sensorsHalGetSubHal_2_1 =
+                                *getSubHalV2_1Ptr;
+                        uint32_t version;
+                        ISensorsSubHalV2_1* subHal = sensorsHalGetSubHal_2_1(&version);
+                        if (version != SUB_HAL_2_1_VERSION) {
+                            ALOGE("SubHal version was not 2.1 for library: %s",
+                                  subHalLibraryFile.c_str());
+                        } else {
+                            ALOGV("Loaded SubHal from library: %s", subHalLibraryFile.c_str());
+                            mSubHalList.push_back(std::make_unique<SubHalWrapperV2_1>(subHal));
+                        }
                     }
                 }
             }
@@ -390,36 +470,28 @@ void HalProxy::initializeSubHalListFromConfigFile(const char* configFileName) {
     }
 }
 
-void HalProxy::initializeSubHalCallbacks() {
-    for (size_t subHalIndex = 0; subHalIndex < mSubHalList.size(); subHalIndex++) {
-        sp<IHalProxyCallback> callback = new HalProxyCallback(this, subHalIndex);
-        mSubHalCallbacks.push_back(callback);
-    }
-}
-
 void HalProxy::initializeSensorList() {
     for (size_t subHalIndex = 0; subHalIndex < mSubHalList.size(); subHalIndex++) {
-        ISensorsSubHal* subHal = mSubHalList[subHalIndex];
-        auto result = subHal->getSensorsList([&](const auto& list) {
+        auto result = mSubHalList[subHalIndex]->getSensorsList([&](const auto& list) {
             for (SensorInfo sensor : list) {
                 if (!subHalIndexIsClear(sensor.sensorHandle)) {
                     ALOGE("SubHal sensorHandle's first byte was not 0");
                 } else {
                     ALOGV("Loaded sensor: %s", sensor.name.c_str());
                     sensor.sensorHandle = setSubHalIndex(sensor.sensorHandle, subHalIndex);
-                    setDirectChannelFlags(&sensor, subHal);
+                    setDirectChannelFlags(&sensor, mSubHalList[subHalIndex]);
                     mSensors[sensor.sensorHandle] = sensor;
                 }
             }
         });
         if (!result.isOk()) {
-            ALOGE("getSensorsList call failed for SubHal: %s", subHal->getName().c_str());
+            ALOGE("getSensorsList call failed for SubHal: %s",
+                  mSubHalList[subHalIndex]->getName().c_str());
         }
     }
 }
 
 void HalProxy::init() {
-    initializeSubHalCallbacks();
     initializeSensorList();
 }
 
@@ -552,7 +624,7 @@ void HalProxy::resetSharedWakelock() {
 }
 
 void HalProxy::postEventsToMessageQueue(const std::vector<Event>& events, size_t numWakeupEvents,
-                                        ScopedWakelock wakelock) {
+                                        V2_0::implementation::ScopedWakelock wakelock) {
     size_t numToWrite = 0;
     std::lock_guard<std::mutex> lock(mEventQueueWriteMutex);
     if (wakelock.isLocked()) {
@@ -610,7 +682,8 @@ void HalProxy::decrementRefCountAndMaybeReleaseWakelock(size_t delta,
     }
 }
 
-void HalProxy::setDirectChannelFlags(SensorInfo* sensorInfo, ISensorsSubHal* subHal) {
+void HalProxy::setDirectChannelFlags(SensorInfo* sensorInfo,
+                                     std::shared_ptr<ISubHalWrapperBase> subHal) {
     bool sensorSupportsDirectChannel =
             (sensorInfo->flags & (V1_0::SensorFlagBits::MASK_DIRECT_REPORT |
                                   V1_0::SensorFlagBits::MASK_DIRECT_CHANNEL)) != 0;
@@ -624,7 +697,7 @@ void HalProxy::setDirectChannelFlags(SensorInfo* sensorInfo, ISensorsSubHal* sub
     }
 }
 
-ISensorsSubHal* HalProxy::getSubHalForSensorHandle(int32_t sensorHandle) {
+std::shared_ptr<ISubHalWrapperBase> HalProxy::getSubHalForSensorHandle(int32_t sensorHandle) {
     return mSubHalList[extractSubHalIndex(sensorHandle)];
 }
 
@@ -651,46 +724,8 @@ bool HalProxy::subHalIndexIsClear(int32_t sensorHandle) {
     return (sensorHandle & kSensorHandleSubHalIndexMask) == 0;
 }
 
-void HalProxyCallback::postEvents(const std::vector<Event>& events, ScopedWakelock wakelock) {
-    if (events.empty() || !mHalProxy->areThreadsRunning()) return;
-    size_t numWakeupEvents;
-    std::vector<Event> processedEvents = processEvents(events, &numWakeupEvents);
-    if (numWakeupEvents > 0) {
-        ALOG_ASSERT(wakelock.isLocked(),
-                    "Wakeup events posted while wakelock unlocked for subhal"
-                    " w/ index %" PRId32 ".",
-                    mSubHalIndex);
-    } else {
-        ALOG_ASSERT(!wakelock.isLocked(),
-                    "No Wakeup events posted but wakelock locked for subhal"
-                    " w/ index %" PRId32 ".",
-                    mSubHalIndex);
-    }
-    mHalProxy->postEventsToMessageQueue(processedEvents, numWakeupEvents, std::move(wakelock));
-}
-
-ScopedWakelock HalProxyCallback::createScopedWakelock(bool lock) {
-    ScopedWakelock wakelock(mHalProxy, lock);
-    return wakelock;
-}
-
-std::vector<Event> HalProxyCallback::processEvents(const std::vector<Event>& events,
-                                                   size_t* numWakeupEvents) const {
-    *numWakeupEvents = 0;
-    std::vector<Event> eventsOut;
-    for (Event event : events) {
-        event.sensorHandle = setSubHalIndex(event.sensorHandle, mSubHalIndex);
-        eventsOut.push_back(event);
-        const SensorInfo& sensor = mHalProxy->getSensorInfo(event.sensorHandle);
-        if ((sensor.flags & V1_0::SensorFlagBits::WAKE_UP) != 0) {
-            (*numWakeupEvents)++;
-        }
-    }
-    return eventsOut;
-}
-
 }  // namespace implementation
-}  // namespace V2_0
+}  // namespace V2_1
 }  // namespace sensors
 }  // namespace hardware
 }  // namespace android
diff --git a/sensors/common/default/2.X/multihal/HalProxyCallback.cpp b/sensors/common/default/2.X/multihal/HalProxyCallback.cpp
new file mode 100644
index 0000000000..a0e0c6b751
--- /dev/null
+++ b/sensors/common/default/2.X/multihal/HalProxyCallback.cpp
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2019 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 "HalProxyCallback.h"
+
+namespace android {
+namespace hardware {
+namespace sensors {
+namespace V2_0 {
+namespace implementation {
+
+static constexpr int32_t kBitsAfterSubHalIndex = 24;
+
+/**
+ * Set the subhal index as first byte of sensor handle and return this modified version.
+ *
+ * @param sensorHandle The sensor handle to modify.
+ * @param subHalIndex The index in the hal proxy of the sub hal this sensor belongs to.
+ *
+ * @return The modified sensor handle.
+ */
+int32_t setSubHalIndex(int32_t sensorHandle, size_t subHalIndex) {
+    return sensorHandle | (static_cast<int32_t>(subHalIndex) << kBitsAfterSubHalIndex);
+}
+
+void HalProxyCallbackBase::postEvents(const std::vector<V2_1::Event>& events,
+                                      ScopedWakelock wakelock) {
+    if (events.empty() || !mCallback->areThreadsRunning()) return;
+    size_t numWakeupEvents;
+    std::vector<V2_1::Event> processedEvents = processEvents(events, &numWakeupEvents);
+    if (numWakeupEvents > 0) {
+        ALOG_ASSERT(wakelock.isLocked(),
+                    "Wakeup events posted while wakelock unlocked for subhal"
+                    " w/ index %" PRId32 ".",
+                    mSubHalIndex);
+    } else {
+        ALOG_ASSERT(!wakelock.isLocked(),
+                    "No Wakeup events posted but wakelock locked for subhal"
+                    " w/ index %" PRId32 ".",
+                    mSubHalIndex);
+    }
+    mCallback->postEventsToMessageQueue(processedEvents, numWakeupEvents, std::move(wakelock));
+}
+
+ScopedWakelock HalProxyCallbackBase::createScopedWakelock(bool lock) {
+    ScopedWakelock wakelock(mRefCounter, lock);
+    return wakelock;
+}
+
+std::vector<V2_1::Event> HalProxyCallbackBase::processEvents(const std::vector<V2_1::Event>& events,
+                                                             size_t* numWakeupEvents) const {
+    *numWakeupEvents = 0;
+    std::vector<V2_1::Event> eventsOut;
+    for (V2_1::Event event : events) {
+        event.sensorHandle = setSubHalIndex(event.sensorHandle, mSubHalIndex);
+        eventsOut.push_back(event);
+        const V2_1::SensorInfo& sensor = mCallback->getSensorInfo(event.sensorHandle);
+        if ((sensor.flags & V1_0::SensorFlagBits::WAKE_UP) != 0) {
+            (*numWakeupEvents)++;
+        }
+    }
+    return eventsOut;
+}
+
+}  // namespace implementation
+}  // namespace V2_0
+}  // namespace sensors
+}  // namespace hardware
+}  // namespace android
diff --git a/sensors/common/default/2.X/multihal/include/HalProxy.h b/sensors/common/default/2.X/multihal/include/HalProxy.h
index d7e8795903..fb0b806bab 100644
--- a/sensors/common/default/2.X/multihal/include/HalProxy.h
+++ b/sensors/common/default/2.X/multihal/include/HalProxy.h
@@ -16,12 +16,17 @@
 
 #pragma once
 
+#include "EventMessageQueueWrapper.h"
+#include "HalProxyCallback.h"
+#include "ISensorsCallbackWrapper.h"
+#include "SubHalWrapper.h"
 #include "V2_0/ScopedWakelock.h"
 #include "V2_0/SubHal.h"
 #include "V2_1/SubHal.h"
+#include "convertV2_1.h"
 
-#include <android/hardware/sensors/2.0/ISensors.h>
-#include <android/hardware/sensors/2.0/types.h>
+#include <android/hardware/sensors/2.1/ISensors.h>
+#include <android/hardware/sensors/2.1/types.h>
 #include <fmq/MessageQueue.h>
 #include <hardware_legacy/power.h>
 #include <hidl/MQDescriptor.h>
@@ -38,96 +43,97 @@
 namespace android {
 namespace hardware {
 namespace sensors {
-namespace V2_0 {
+namespace V2_1 {
 namespace implementation {
 
-using ::android::sp;
-using ::android::hardware::EventFlag;
-using ::android::hardware::hidl_string;
-using ::android::hardware::hidl_vec;
-using ::android::hardware::MessageQueue;
-using ::android::hardware::MQDescriptor;
-using ::android::hardware::Return;
-using ::android::hardware::Void;
-
-class HalProxy : public ISensors, public IScopedWakelockRefCounter {
+/**
+ * HalProxy is the main interface for Multi-HAL. It is responsible for managing  subHALs and
+ * proxying function calls to/from the subHAL APIs from the sensors framework. It also manages any
+ * wakelocks allocated through the IHalProxyCallback and manages posting events to the sensors
+ * framework.
+ */
+class HalProxy : public V2_0::implementation::IScopedWakelockRefCounter,
+                 public V2_0::implementation::ISubHalCallback {
   public:
-    using Event = ::android::hardware::sensors::V1_0::Event;
+    using Event = ::android::hardware::sensors::V2_1::Event;
     using OperationMode = ::android::hardware::sensors::V1_0::OperationMode;
     using RateLevel = ::android::hardware::sensors::V1_0::RateLevel;
     using Result = ::android::hardware::sensors::V1_0::Result;
-    using SensorInfo = ::android::hardware::sensors::V1_0::SensorInfo;
+    using SensorInfo = ::android::hardware::sensors::V2_1::SensorInfo;
     using SharedMemInfo = ::android::hardware::sensors::V1_0::SharedMemInfo;
-    using ISensorsSubHal = ::android::hardware::sensors::V2_0::implementation::ISensorsSubHal;
+    using IHalProxyCallbackV2_0 = V2_0::implementation::IHalProxyCallback;
+    using IHalProxyCallbackV2_1 = V2_1::implementation::IHalProxyCallback;
+    using ISensorsSubHalV2_0 = V2_0::implementation::ISensorsSubHal;
+    using ISensorsSubHalV2_1 = V2_1::implementation::ISensorsSubHal;
+    using ISensorsV2_0 = V2_0::ISensors;
+    using ISensorsV2_1 = V2_1::ISensors;
+    using HalProxyCallbackBase = V2_0::implementation::HalProxyCallbackBase;
 
     explicit HalProxy();
     // Test only constructor.
-    explicit HalProxy(std::vector<ISensorsSubHal*>& subHalList);
+    explicit HalProxy(std::vector<ISensorsSubHalV2_0*>& subHalList);
+    explicit HalProxy(std::vector<ISensorsSubHalV2_0*>& subHalList,
+                      std::vector<ISensorsSubHalV2_1*>& subHalListV2_1);
     ~HalProxy();
 
+    // Methods from ::android::hardware::sensors::V2_1::ISensors follow.
+    Return<void> getSensorsList_2_1(ISensorsV2_1::getSensorsList_2_1_cb _hidl_cb);
+
+    Return<Result> initialize_2_1(
+            const ::android::hardware::MQDescriptorSync<V2_1::Event>& eventQueueDescriptor,
+            const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
+            const sp<V2_1::ISensorsCallback>& sensorsCallback);
+
+    Return<Result> injectSensorData_2_1(const Event& event);
+
     // Methods from ::android::hardware::sensors::V2_0::ISensors follow.
-    Return<void> getSensorsList(getSensorsList_cb _hidl_cb) override;
+    Return<void> getSensorsList(ISensorsV2_0::getSensorsList_cb _hidl_cb);
 
-    Return<Result> setOperationMode(OperationMode mode) override;
+    Return<Result> setOperationMode(OperationMode mode);
 
-    Return<Result> activate(int32_t sensorHandle, bool enabled) override;
+    Return<Result> activate(int32_t sensorHandle, bool enabled);
 
     Return<Result> initialize(
-            const ::android::hardware::MQDescriptorSync<Event>& eventQueueDescriptor,
+            const ::android::hardware::MQDescriptorSync<V1_0::Event>& eventQueueDescriptor,
+            const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
+            const sp<V2_0::ISensorsCallback>& sensorsCallback);
+
+    Return<Result> initializeCommon(
+            std::unique_ptr<EventMessageQueueWrapperBase>& eventQueue,
             const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
-            const sp<ISensorsCallback>& sensorsCallback) override;
+            const sp<ISensorsCallbackWrapperBase>& sensorsCallback);
 
     Return<Result> batch(int32_t sensorHandle, int64_t samplingPeriodNs,
-                         int64_t maxReportLatencyNs) override;
+                         int64_t maxReportLatencyNs);
 
-    Return<Result> flush(int32_t sensorHandle) override;
+    Return<Result> flush(int32_t sensorHandle);
 
-    Return<Result> injectSensorData(const Event& event) override;
+    Return<Result> injectSensorData(const V1_0::Event& event);
 
     Return<void> registerDirectChannel(const SharedMemInfo& mem,
-                                       registerDirectChannel_cb _hidl_cb) override;
+                                       ISensorsV2_0::registerDirectChannel_cb _hidl_cb);
 
-    Return<Result> unregisterDirectChannel(int32_t channelHandle) override;
+    Return<Result> unregisterDirectChannel(int32_t channelHandle);
 
     Return<void> configDirectReport(int32_t sensorHandle, int32_t channelHandle, RateLevel rate,
-                                    configDirectReport_cb _hidl_cb) override;
+                                    ISensorsV2_0::configDirectReport_cb _hidl_cb);
 
-    Return<void> debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args) override;
+    Return<void> debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args);
 
-    // Below methods from ::android::hardware::sensors::V2_0::ISensorsCallback with a minor change
-    // to pass in the sub-HAL index. While the above methods are invoked from the sensors framework
-    // via the binder, these methods are invoked from a callback provided to sub-HALs inside the
-    // same process as the HalProxy, but potentially running on different threads.
     Return<void> onDynamicSensorsConnected(const hidl_vec<SensorInfo>& dynamicSensorsAdded,
-                                           int32_t subHalIndex);
+                                           int32_t subHalIndex) override;
 
     Return<void> onDynamicSensorsDisconnected(const hidl_vec<int32_t>& dynamicSensorHandlesRemoved,
-                                              int32_t subHalIndex);
+                                              int32_t subHalIndex) override;
 
-    // Below methods are for HalProxyCallback
-
-    /**
-     * Post events to the event message queue if there is room to write them. Otherwise post the
-     * remaining events to a background thread for a blocking write with a kPendingWriteTimeoutNs
-     * timeout.
-     *
-     * @param events The list of events to post to the message queue.
-     * @param numWakeupEvents The number of wakeup events in events.
-     * @param wakelock The wakelock associated with this post of events.
-     */
     void postEventsToMessageQueue(const std::vector<Event>& events, size_t numWakeupEvents,
-                                  ScopedWakelock wakelock);
+                                  V2_0::implementation::ScopedWakelock wakelock) override;
 
-    /**
-     * Get the sensor info associated with that sensorHandle.
-     *
-     * @param sensorHandle The sensor handle.
-     *
-     * @return The sensor info object in the mapping.
-     */
-    const SensorInfo& getSensorInfo(int32_t sensorHandle) { return mSensors[sensorHandle]; }
+    const SensorInfo& getSensorInfo(int32_t sensorHandle) override {
+        return mSensors[sensorHandle];
+    }
 
-    bool areThreadsRunning() { return mThreadsRun.load(); }
+    bool areThreadsRunning() override { return mThreadsRun.load(); }
 
     // Below methods are from IScopedWakelockRefCounter interface
     bool incrementRefCountAndMaybeAcquireWakelock(size_t delta,
@@ -136,13 +142,14 @@ class HalProxy : public ISensors, public IScopedWakelockRefCounter {
     void decrementRefCountAndMaybeReleaseWakelock(size_t delta, int64_t timeoutStart = -1) override;
 
   private:
-    using EventMessageQueue = MessageQueue<Event, kSynchronizedReadWrite>;
+    using EventMessageQueueV2_1 = MessageQueue<V2_1::Event, kSynchronizedReadWrite>;
+    using EventMessageQueueV2_0 = MessageQueue<V1_0::Event, kSynchronizedReadWrite>;
     using WakeLockMessageQueue = MessageQueue<uint32_t, kSynchronizedReadWrite>;
 
     /**
      * The Event FMQ where sensor events are written
      */
-    std::unique_ptr<EventMessageQueue> mEventQueue;
+    std::unique_ptr<EventMessageQueueWrapperBase> mEventQueue;
 
     /**
      * The Wake Lock FMQ that is read to determine when the framework has handled WAKE_UP events
@@ -161,15 +168,12 @@ class HalProxy : public ISensors, public IScopedWakelockRefCounter {
     /**
      * Callback to the sensors framework to inform it that new sensors have been added or removed.
      */
-    sp<ISensorsCallback> mDynamicSensorsCallback;
+    sp<ISensorsCallbackWrapperBase> mDynamicSensorsCallback;
 
     /**
-     * SubHal object pointers that have been saved from vendor dynamic libraries.
+     * SubHal objects that have been saved from vendor dynamic libraries.
      */
-    std::vector<ISensorsSubHal*> mSubHalList;
-
-    //! The list of subhal callbacks for each subhal where the indices correlate with mSubHalList
-    std::vector<const sp<IHalProxyCallback>> mSubHalCallbacks;
+    std::vector<std::shared_ptr<ISubHalWrapperBase>> mSubHalList;
 
     /**
      * Map of sensor handles to SensorInfo objects that contains the sensor info from subhals as
@@ -187,7 +191,7 @@ class HalProxy : public ISensors, public IScopedWakelockRefCounter {
     OperationMode mCurrentOperationMode = OperationMode::NORMAL;
 
     //! The single subHal that supports directChannel reporting.
-    ISensorsSubHal* mDirectChannelSubHal = nullptr;
+    std::shared_ptr<ISubHalWrapperBase> mDirectChannelSubHal;
 
     //! The timeout for each pending write on background thread for events.
     static const int64_t kPendingWriteTimeoutNs = 5 * INT64_C(1000000000) /* 5 seconds */;
@@ -239,9 +243,9 @@ class HalProxy : public ISensors, public IScopedWakelockRefCounter {
     //! The refcount of how many ScopedWakelocks and pending wakeup events are active
     size_t mWakelockRefCount = 0;
 
-    int64_t mWakelockTimeoutStartTime = getTimeNow();
+    int64_t mWakelockTimeoutStartTime = V2_0::implementation::getTimeNow();
 
-    int64_t mWakelockTimeoutResetTime = getTimeNow();
+    int64_t mWakelockTimeoutResetTime = V2_0::implementation::getTimeNow();
 
     const char* kWakelockName = "SensorsHAL_WAKEUP";
 
@@ -321,7 +325,7 @@ class HalProxy : public ISensors, public IScopedWakelockRefCounter {
      *    disabled.
      * @param subHal The subhal pointer that the current sensorInfo object came from.
      */
-    void setDirectChannelFlags(SensorInfo* sensorInfo, ISensorsSubHal* subHal);
+    void setDirectChannelFlags(SensorInfo* sensorInfo, std::shared_ptr<ISubHalWrapperBase> subHal);
 
     /*
      * Get the subhal pointer which can be found by indexing into the mSubHalList vector
@@ -329,7 +333,7 @@ class HalProxy : public ISensors, public IScopedWakelockRefCounter {
      *
      * @param sensorHandle The handle used to identify a sensor in one of the subhals.
      */
-    ISensorsSubHal* getSubHalForSensorHandle(int32_t sensorHandle);
+    std::shared_ptr<ISubHalWrapperBase> getSubHalForSensorHandle(int32_t sensorHandle);
 
     /**
      * Checks that sensorHandle's subhal index byte is within bounds of mSubHalList.
@@ -368,39 +372,81 @@ class HalProxy : public ISensors, public IScopedWakelockRefCounter {
 };
 
 /**
- * Callback class used to provide the HalProxy with the index of which subHal is invoking
+ * Since a newer HAL can't masquerade as a older HAL, IHalProxy enables the HalProxy to be compiled
+ * either for HAL 2.0 or HAL 2.1 depending on the build configuration.
  */
-class HalProxyCallback : public IHalProxyCallback {
-    using SensorInfo = ::android::hardware::sensors::V1_0::SensorInfo;
+template <class ISensorsVersion>
+class IHalProxy : public HalProxy, public ISensorsVersion {
+    Return<void> getSensorsList(ISensorsV2_0::getSensorsList_cb _hidl_cb) override {
+        return HalProxy::getSensorsList(_hidl_cb);
+    }
 
-  public:
-    HalProxyCallback(HalProxy* halProxy, int32_t subHalIndex)
-        : mHalProxy(halProxy), mSubHalIndex(subHalIndex) {}
+    Return<Result> setOperationMode(OperationMode mode) override {
+        return HalProxy::setOperationMode(mode);
+    }
+
+    Return<Result> activate(int32_t sensorHandle, bool enabled) override {
+        return HalProxy::activate(sensorHandle, enabled);
+    }
 
-    Return<void> onDynamicSensorsConnected(
-            const hidl_vec<SensorInfo>& dynamicSensorsAdded) override {
-        return mHalProxy->onDynamicSensorsConnected(dynamicSensorsAdded, mSubHalIndex);
+    Return<Result> initialize(
+            const ::android::hardware::MQDescriptorSync<V1_0::Event>& eventQueueDescriptor,
+            const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
+            const sp<V2_0::ISensorsCallback>& sensorsCallback) override {
+        return HalProxy::initialize(eventQueueDescriptor, wakeLockDescriptor, sensorsCallback);
     }
 
-    Return<void> onDynamicSensorsDisconnected(
-            const hidl_vec<int32_t>& dynamicSensorHandlesRemoved) override {
-        return mHalProxy->onDynamicSensorsDisconnected(dynamicSensorHandlesRemoved, mSubHalIndex);
+    Return<Result> batch(int32_t sensorHandle, int64_t samplingPeriodNs,
+                         int64_t maxReportLatencyNs) override {
+        return HalProxy::batch(sensorHandle, samplingPeriodNs, maxReportLatencyNs);
     }
 
-    void postEvents(const std::vector<Event>& events, ScopedWakelock wakelock);
+    Return<Result> flush(int32_t sensorHandle) override { return HalProxy::flush(sensorHandle); }
 
-    ScopedWakelock createScopedWakelock(bool lock);
+    Return<Result> injectSensorData(const V1_0::Event& event) override {
+        return HalProxy::injectSensorData(event);
+    }
 
-  private:
-    HalProxy* mHalProxy;
-    int32_t mSubHalIndex;
+    Return<void> registerDirectChannel(const SharedMemInfo& mem,
+                                       ISensorsV2_0::registerDirectChannel_cb _hidl_cb) override {
+        return HalProxy::registerDirectChannel(mem, _hidl_cb);
+    }
+
+    Return<Result> unregisterDirectChannel(int32_t channelHandle) override {
+        return HalProxy::unregisterDirectChannel(channelHandle);
+    }
 
-    std::vector<Event> processEvents(const std::vector<Event>& events,
-                                     size_t* numWakeupEvents) const;
+    Return<void> configDirectReport(int32_t sensorHandle, int32_t channelHandle, RateLevel rate,
+                                    ISensorsV2_0::configDirectReport_cb _hidl_cb) override {
+        return HalProxy::configDirectReport(sensorHandle, channelHandle, rate, _hidl_cb);
+    }
+
+    Return<void> debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args) override {
+        return HalProxy::debug(fd, args);
+    }
+};
+
+class HalProxyV2_0 : public IHalProxy<V2_0::ISensors> {};
+
+class HalProxyV2_1 : public IHalProxy<V2_1::ISensors> {
+    Return<void> getSensorsList_2_1(ISensorsV2_1::getSensorsList_2_1_cb _hidl_cb) override {
+        return HalProxy::getSensorsList_2_1(_hidl_cb);
+    }
+
+    Return<Result> initialize_2_1(
+            const ::android::hardware::MQDescriptorSync<V2_1::Event>& eventQueueDescriptor,
+            const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
+            const sp<V2_1::ISensorsCallback>& sensorsCallback) override {
+        return HalProxy::initialize_2_1(eventQueueDescriptor, wakeLockDescriptor, sensorsCallback);
+    }
+
+    Return<Result> injectSensorData_2_1(const Event& event) override {
+        return HalProxy::injectSensorData_2_1(event);
+    }
 };
 
 }  // namespace implementation
-}  // namespace V2_0
+}  // namespace V2_1
 }  // namespace sensors
 }  // namespace hardware
 }  // namespace android
diff --git a/sensors/common/default/2.X/multihal/include/HalProxyCallback.h b/sensors/common/default/2.X/multihal/include/HalProxyCallback.h
new file mode 100644
index 0000000000..e62b7d1c00
--- /dev/null
+++ b/sensors/common/default/2.X/multihal/include/HalProxyCallback.h
@@ -0,0 +1,171 @@
+/*
+ * Copyright (C) 2019 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.
+ */
+
+#pragma once
+
+#include "V2_0/ScopedWakelock.h"
+#include "V2_0/SubHal.h"
+#include "V2_1/SubHal.h"
+#include "convertV2_1.h"
+
+#include <android/hardware/sensors/2.1/ISensors.h>
+#include <android/hardware/sensors/2.1/types.h>
+#include <log/log.h>
+
+namespace android {
+namespace hardware {
+namespace sensors {
+namespace V2_0 {
+namespace implementation {
+
+/**
+ * Interface used to communicate with the HalProxy when subHals interact with their provided
+ * callback.
+ */
+class ISubHalCallback {
+  public:
+    virtual ~ISubHalCallback() {}
+
+    // Below methods from ::android::hardware::sensors::V2_0::ISensorsCallback with a minor change
+    // to pass in the sub-HAL index. While the above methods are invoked from the sensors framework
+    // via the binder, these methods are invoked from a callback provided to sub-HALs inside the
+    // same process as the HalProxy, but potentially running on different threads.
+    virtual Return<void> onDynamicSensorsConnected(
+            const hidl_vec<V2_1::SensorInfo>& dynamicSensorsAdded, int32_t subHalIndex) = 0;
+
+    virtual Return<void> onDynamicSensorsDisconnected(
+            const hidl_vec<int32_t>& dynamicSensorHandlesRemoved, int32_t subHalIndex) = 0;
+
+    /**
+     * Post events to the event message queue if there is room to write them. Otherwise post the
+     * remaining events to a background thread for a blocking write with a kPendingWriteTimeoutNs
+     * timeout.
+     *
+     * @param events The list of events to post to the message queue.
+     * @param numWakeupEvents The number of wakeup events in events.
+     * @param wakelock The wakelock associated with this post of events.
+     */
+    virtual void postEventsToMessageQueue(const std::vector<V2_1::Event>& events,
+                                          size_t numWakeupEvents,
+                                          V2_0::implementation::ScopedWakelock wakelock) = 0;
+
+    /**
+     * Get the sensor info associated with that sensorHandle.
+     *
+     * @param sensorHandle The sensor handle.
+     *
+     * @return The sensor info object in the mapping.
+     */
+    virtual const V2_1::SensorInfo& getSensorInfo(int32_t sensorHandle) = 0;
+
+    virtual bool areThreadsRunning() = 0;
+};
+
+/**
+ * Callback class given to subhals that allows the HalProxy to know which subhal a given invocation
+ * is coming from.
+ */
+class HalProxyCallbackBase : public VirtualLightRefBase {
+  public:
+    HalProxyCallbackBase(ISubHalCallback* callback,
+                         V2_0::implementation::IScopedWakelockRefCounter* refCounter,
+                         int32_t subHalIndex)
+        : mCallback(callback), mRefCounter(refCounter), mSubHalIndex(subHalIndex) {}
+
+    void postEvents(const std::vector<V2_1::Event>& events,
+                    V2_0::implementation::ScopedWakelock wakelock);
+
+    V2_0::implementation::ScopedWakelock createScopedWakelock(bool lock);
+
+  protected:
+    ISubHalCallback* mCallback;
+    V2_0::implementation::IScopedWakelockRefCounter* mRefCounter;
+    int32_t mSubHalIndex;
+
+  private:
+    std::vector<V2_1::Event> processEvents(const std::vector<V2_1::Event>& events,
+                                           size_t* numWakeupEvents) const;
+};
+
+class HalProxyCallbackV2_0 : public HalProxyCallbackBase,
+                             public V2_0::implementation::IHalProxyCallback {
+  public:
+    HalProxyCallbackV2_0(ISubHalCallback* callback,
+                         V2_0::implementation::IScopedWakelockRefCounter* refCounter,
+                         int32_t subHalIndex)
+        : HalProxyCallbackBase(callback, refCounter, subHalIndex) {}
+
+    Return<void> onDynamicSensorsConnected(
+            const hidl_vec<V1_0::SensorInfo>& dynamicSensorsAdded) override {
+        return mCallback->onDynamicSensorsConnected(
+                V2_1::implementation::convertToNewSensorInfos(dynamicSensorsAdded), mSubHalIndex);
+    }
+
+    Return<void> onDynamicSensorsDisconnected(
+            const hidl_vec<int32_t>& dynamicSensorHandlesRemoved) override {
+        return mCallback->onDynamicSensorsDisconnected(dynamicSensorHandlesRemoved, mSubHalIndex);
+    }
+
+    void postEvents(const std::vector<V1_0::Event>& events,
+                    V2_0::implementation::ScopedWakelock wakelock) override {
+        HalProxyCallbackBase::postEvents(V2_1::implementation::convertToNewEvents(events),
+                                         std::move(wakelock));
+    }
+
+    V2_0::implementation::ScopedWakelock createScopedWakelock(bool lock) override {
+        return HalProxyCallbackBase::createScopedWakelock(lock);
+    }
+};
+
+class HalProxyCallbackV2_1 : public HalProxyCallbackBase,
+                             public V2_1::implementation::IHalProxyCallback {
+  public:
+    HalProxyCallbackV2_1(ISubHalCallback* callback,
+                         V2_0::implementation::IScopedWakelockRefCounter* refCounter,
+                         int32_t subHalIndex)
+        : HalProxyCallbackBase(callback, refCounter, subHalIndex) {}
+
+    Return<void> onDynamicSensorsConnected_2_1(
+            const hidl_vec<V2_1::SensorInfo>& dynamicSensorsAdded) override {
+        return mCallback->onDynamicSensorsConnected(dynamicSensorsAdded, mSubHalIndex);
+    }
+
+    Return<void> onDynamicSensorsConnected(
+            const hidl_vec<V1_0::SensorInfo>& /* dynamicSensorsAdded */) override {
+        LOG_ALWAYS_FATAL("Old dynamic sensors method can't be used");
+        return Void();
+    }
+
+    Return<void> onDynamicSensorsDisconnected(
+            const hidl_vec<int32_t>& dynamicSensorHandlesRemoved) override {
+        return mCallback->onDynamicSensorsDisconnected(dynamicSensorHandlesRemoved, mSubHalIndex);
+    }
+
+    void postEvents(const std::vector<V2_1::Event>& events,
+                    V2_0::implementation::ScopedWakelock wakelock) override {
+        return HalProxyCallbackBase::postEvents(events, std::move(wakelock));
+    }
+
+    V2_0::implementation::ScopedWakelock createScopedWakelock(bool lock) override {
+        return HalProxyCallbackBase::createScopedWakelock(lock);
+    }
+};
+
+}  // namespace implementation
+}  // namespace V2_0
+}  // namespace sensors
+}  // namespace hardware
+}  // namespace android
\ No newline at end of file
diff --git a/sensors/common/default/2.X/multihal/include/SubHalWrapper.h b/sensors/common/default/2.X/multihal/include/SubHalWrapper.h
new file mode 100644
index 0000000000..149bb5ea5b
--- /dev/null
+++ b/sensors/common/default/2.X/multihal/include/SubHalWrapper.h
@@ -0,0 +1,188 @@
+/*
+ * Copyright (C) 2020 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.
+ */
+
+#pragma once
+
+#include "HalProxyCallback.h"
+#include "V2_0/SubHal.h"
+#include "V2_1/SubHal.h"
+
+#include "android/hardware/sensors/1.0/ISensors.h"
+#include "android/hardware/sensors/1.0/types.h"
+#include "android/hardware/sensors/2.0/ISensors.h"
+#include "android/hardware/sensors/2.0/ISensorsCallback.h"
+#include "android/hardware/sensors/2.1/ISensors.h"
+#include "android/hardware/sensors/2.1/ISensorsCallback.h"
+#include "android/hardware/sensors/2.1/types.h"
+
+#include <utils/LightRefBase.h>
+
+#include <cassert>
+
+namespace android {
+namespace hardware {
+namespace sensors {
+namespace V2_1 {
+namespace implementation {
+
+/**
+ * The following subHal wrapper classes abstract away common functionality across V2.0 and V2.1
+ * subHal interfaces. Much of the logic is common between the two versions and this allows users of
+ * the classes to only care about the type used at initialization and then interact with either
+ * version of the subHal interface without worrying about the type.
+ */
+class ISubHalWrapperBase {
+  protected:
+    using Event = ::android::hardware::sensors::V2_1::Event;
+    using OperationMode = ::android::hardware::sensors::V1_0::OperationMode;
+    using RateLevel = ::android::hardware::sensors::V1_0::RateLevel;
+    using Result = ::android::hardware::sensors::V1_0::Result;
+    using SensorInfo = ::android::hardware::sensors::V2_1::SensorInfo;
+    using SharedMemInfo = ::android::hardware::sensors::V1_0::SharedMemInfo;
+
+  public:
+    virtual ~ISubHalWrapperBase() {}
+
+    virtual bool supportsNewEvents() = 0;
+
+    virtual Return<Result> initialize(V2_0::implementation::ISubHalCallback* callback,
+                                      V2_0::implementation::IScopedWakelockRefCounter* refCounter,
+                                      int32_t subHalIndex) = 0;
+
+    virtual Return<void> getSensorsList(
+            ::android::hardware::sensors::V2_1::ISensors::getSensorsList_2_1_cb _hidl_cb) = 0;
+
+    virtual Return<Result> setOperationMode(OperationMode mode) = 0;
+
+    virtual Return<Result> activate(int32_t sensorHandle, bool enabled) = 0;
+
+    virtual Return<Result> batch(int32_t sensorHandle, int64_t samplingPeriodNs,
+                                 int64_t maxReportLatencyNs) = 0;
+
+    virtual Return<Result> flush(int32_t sensorHandle) = 0;
+
+    virtual Return<Result> injectSensorData(const Event& event) = 0;
+
+    virtual Return<void> registerDirectChannel(const SharedMemInfo& mem,
+                                               ISensors::registerDirectChannel_cb _hidl_cb) = 0;
+
+    virtual Return<Result> unregisterDirectChannel(int32_t channelHandle) = 0;
+
+    virtual Return<void> configDirectReport(int32_t sensorHandle, int32_t channelHandle,
+                                            RateLevel rate,
+                                            ISensors::configDirectReport_cb _hidl_cb) = 0;
+
+    virtual Return<void> debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args) = 0;
+
+    virtual const std::string getName() = 0;
+};
+
+template <typename T>
+class SubHalWrapperBase : public ISubHalWrapperBase {
+  public:
+    SubHalWrapperBase(T* subHal) : mSubHal(subHal){};
+
+    virtual bool supportsNewEvents() override { return false; }
+
+    virtual Return<void> getSensorsList(
+            ::android::hardware::sensors::V2_1::ISensors::getSensorsList_2_1_cb _hidl_cb) override {
+        return mSubHal->getSensorsList(
+                [&](const auto& list) { _hidl_cb(convertToNewSensorInfos(list)); });
+    }
+
+    Return<Result> setOperationMode(OperationMode mode) override {
+        return mSubHal->setOperationMode(mode);
+    }
+
+    Return<Result> activate(int32_t sensorHandle, bool enabled) override {
+        return mSubHal->activate(sensorHandle, enabled);
+    }
+
+    Return<Result> batch(int32_t sensorHandle, int64_t samplingPeriodNs,
+                         int64_t maxReportLatencyNs) override {
+        return mSubHal->batch(sensorHandle, samplingPeriodNs, maxReportLatencyNs);
+    }
+
+    Return<Result> flush(int32_t sensorHandle) override { return mSubHal->flush(sensorHandle); }
+
+    virtual Return<Result> injectSensorData(const Event& event) override {
+        return mSubHal->injectSensorData(convertToOldEvent(event));
+    }
+
+    Return<void> registerDirectChannel(const SharedMemInfo& mem,
+                                       ISensors::registerDirectChannel_cb _hidl_cb) override {
+        return mSubHal->registerDirectChannel(mem, _hidl_cb);
+    }
+
+    Return<Result> unregisterDirectChannel(int32_t channelHandle) override {
+        return mSubHal->unregisterDirectChannel(channelHandle);
+    }
+
+    Return<void> configDirectReport(int32_t sensorHandle, int32_t channelHandle, RateLevel rate,
+                                    ISensors::configDirectReport_cb _hidl_cb) override {
+        return mSubHal->configDirectReport(sensorHandle, channelHandle, rate, _hidl_cb);
+    }
+
+    Return<void> debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args) override {
+        return mSubHal->debug(fd, args);
+    }
+
+    const std::string getName() override { return mSubHal->getName(); }
+
+  protected:
+    T* mSubHal;
+};
+
+class SubHalWrapperV2_0 : public SubHalWrapperBase<V2_0::implementation::ISensorsSubHal> {
+  public:
+    SubHalWrapperV2_0(V2_0::implementation::ISensorsSubHal* subHal) : SubHalWrapperBase(subHal){};
+
+    Return<Result> initialize(V2_0::implementation::ISubHalCallback* callback,
+                              V2_0::implementation::IScopedWakelockRefCounter* refCounter,
+                              int32_t subHalIndex) override {
+        return mSubHal->initialize(
+                new V2_0::implementation::HalProxyCallbackV2_0(callback, refCounter, subHalIndex));
+    }
+};
+
+class SubHalWrapperV2_1 : public SubHalWrapperBase<V2_1::implementation::ISensorsSubHal> {
+  public:
+    SubHalWrapperV2_1(V2_1::implementation::ISensorsSubHal* subHal) : SubHalWrapperBase(subHal) {}
+
+    bool supportsNewEvents() override { return true; }
+
+    virtual Return<void> getSensorsList(
+            ::android::hardware::sensors::V2_1::ISensors::getSensorsList_2_1_cb _hidl_cb) override {
+        return mSubHal->getSensorsList_2_1([&](const auto& list) { _hidl_cb(list); });
+    }
+
+    virtual Return<Result> injectSensorData(const Event& event) override {
+        return mSubHal->injectSensorData_2_1(event);
+    }
+
+    Return<Result> initialize(V2_0::implementation::ISubHalCallback* callback,
+                              V2_0::implementation::IScopedWakelockRefCounter* refCounter,
+                              int32_t subHalIndex) override {
+        return mSubHal->initialize(
+                new V2_0::implementation::HalProxyCallbackV2_1(callback, refCounter, subHalIndex));
+    }
+};
+
+}  // namespace implementation
+}  // namespace V2_1
+}  // namespace sensors
+}  // namespace hardware
+}  // namespace android
diff --git a/sensors/common/default/2.X/multihal/include/V2_0/ScopedWakelock.h b/sensors/common/default/2.X/multihal/include/V2_0/ScopedWakelock.h
index aa6d9db3d4..1cc5cd5e9e 100644
--- a/sensors/common/default/2.X/multihal/include/V2_0/ScopedWakelock.h
+++ b/sensors/common/default/2.X/multihal/include/V2_0/ScopedWakelock.h
@@ -88,7 +88,7 @@ class ScopedWakelock {
     bool isLocked() const { return mLocked; }
 
   private:
-    friend class HalProxyCallback;
+    friend class HalProxyCallbackBase;
     IScopedWakelockRefCounter* mRefCounter;
     int64_t mCreatedAtTimeNs;
     bool mLocked;
diff --git a/sensors/common/default/2.X/multihal/tests/Android.bp b/sensors/common/default/2.X/multihal/tests/Android.bp
index 7692b51de0..cfd9e78a6f 100644
--- a/sensors/common/default/2.X/multihal/tests/Android.bp
+++ b/sensors/common/default/2.X/multihal/tests/Android.bp
@@ -20,6 +20,7 @@ cc_defaults {
     ],
     header_libs: [
         "android.hardware.sensors@2.0-multihal.header",
+        "android.hardware.sensors@2.X-shared-utils",
     ],
     export_include_dirs: ["fake_subhal"],
     shared_libs: [
@@ -36,6 +37,7 @@ cc_defaults {
         "libutils",
     ],
     static_libs: [
+        "android.hardware.sensors@1.0-convert",
         "android.hardware.sensors@2.X-multihal",
     ],
     cflags: [
@@ -78,7 +80,11 @@ cc_test {
     name: "android.hardware.sensors@2.X-halproxy-unit-tests",
     srcs: ["HalProxy_test.cpp"],
     vendor: true,
+    header_libs: [
+        "android.hardware.sensors@2.X-shared-utils",
+    ],
     static_libs: [
+        "android.hardware.sensors@1.0-convert",
         "android.hardware.sensors@2.0-ScopedWakelock.testlib",
         "android.hardware.sensors@2.X-multihal",
         "android.hardware.sensors@2.X-fakesubhal-unittest",
diff --git a/sensors/common/default/2.X/multihal/tests/HalProxy_test.cpp b/sensors/common/default/2.X/multihal/tests/HalProxy_test.cpp
index 867c4a149d..ce65c3cd80 100644
--- a/sensors/common/default/2.X/multihal/tests/HalProxy_test.cpp
+++ b/sensors/common/default/2.X/multihal/tests/HalProxy_test.cpp
@@ -40,8 +40,8 @@ using ::android::hardware::sensors::V1_0::SensorType;
 using ::android::hardware::sensors::V2_0::EventQueueFlagBits;
 using ::android::hardware::sensors::V2_0::ISensorsCallback;
 using ::android::hardware::sensors::V2_0::WakeLockQueueFlagBits;
-using ::android::hardware::sensors::V2_0::implementation::HalProxy;
-using ::android::hardware::sensors::V2_0::implementation::HalProxyCallback;
+using ::android::hardware::sensors::V2_0::implementation::HalProxyCallbackBase;
+using ::android::hardware::sensors::V2_0::implementation::ScopedWakelock;
 using ::android::hardware::sensors::V2_0::subhal::implementation::AddAndRemoveDynamicSensorsSubHal;
 using ::android::hardware::sensors::V2_0::subhal::implementation::AllSensorsSubHal;
 using ::android::hardware::sensors::V2_0::subhal::implementation::
@@ -53,6 +53,7 @@ using ::android::hardware::sensors::V2_0::subhal::implementation::OnChangeSensor
 using ::android::hardware::sensors::V2_0::subhal::implementation::SensorsSubHal;
 using ::android::hardware::sensors::V2_0::subhal::implementation::
         SetOperationModeFailingSensorsSubHal;
+using ::android::hardware::sensors::V2_1::implementation::HalProxy;
 
 using EventMessageQueue = MessageQueue<Event, ::android::hardware::kSynchronizedReadWrite>;
 using WakeupMessageQueue = MessageQueue<uint32_t, ::android::hardware::kSynchronizedReadWrite>;
diff --git a/sensors/common/utils/EventMessageQueueWrapper.h b/sensors/common/utils/EventMessageQueueWrapper.h
index bf3261ffbc..c4f92c8386 100644
--- a/sensors/common/utils/EventMessageQueueWrapper.h
+++ b/sensors/common/utils/EventMessageQueueWrapper.h
@@ -39,8 +39,14 @@ class EventMessageQueueWrapperBase : public RefBase {
 
     virtual std::atomic<uint32_t>* getEventFlagWord() = 0;
     virtual size_t availableToRead() = 0;
+    virtual size_t availableToWrite() = 0;
     virtual bool read(V2_1::Event* events, size_t numToRead) = 0;
+    virtual bool write(const V2_1::Event* events, size_t numToWrite) = 0;
     virtual bool write(const std::vector<V2_1::Event>& events) = 0;
+    virtual bool writeBlocking(const V2_1::Event* events, size_t count, uint32_t readNotification,
+                               uint32_t writeNotification, int64_t timeOutNanos,
+                               android::hardware::EventFlag* evFlag) = 0;
+    virtual size_t getQuantumCount() = 0;
 };
 
 class EventMessageQueueWrapperV1_0 : public EventMessageQueueWrapperBase {
@@ -60,15 +66,30 @@ class EventMessageQueueWrapperV1_0 : public EventMessageQueueWrapperBase {
 
     virtual size_t availableToRead() override { return mQueue->availableToRead(); }
 
+    size_t availableToWrite() override { return mQueue->availableToWrite(); }
+
     virtual bool read(V2_1::Event* events, size_t numToRead) override {
         return mQueue->read(reinterpret_cast<V1_0::Event*>(events), numToRead);
     }
 
+    bool write(const V2_1::Event* events, size_t numToWrite) override {
+        return mQueue->write(reinterpret_cast<const V1_0::Event*>(events), numToWrite);
+    }
+
     virtual bool write(const std::vector<V2_1::Event>& events) override {
         const std::vector<V1_0::Event>& oldEvents = convertToOldEvents(events);
         return mQueue->write(oldEvents.data(), oldEvents.size());
     }
 
+    bool writeBlocking(const V2_1::Event* events, size_t count, uint32_t readNotification,
+                       uint32_t writeNotification, int64_t timeOutNanos,
+                       android::hardware::EventFlag* evFlag) override {
+        return mQueue->writeBlocking(reinterpret_cast<const V1_0::Event*>(events), count,
+                                     readNotification, writeNotification, timeOutNanos, evFlag);
+    }
+
+    size_t getQuantumCount() override { return mQueue->getQuantumCount(); }
+
   private:
     std::unique_ptr<EventMessageQueue> mQueue;
 };
@@ -88,14 +109,29 @@ class EventMessageQueueWrapperV2_1 : public EventMessageQueueWrapperBase {
 
     virtual size_t availableToRead() override { return mQueue->availableToRead(); }
 
+    size_t availableToWrite() override { return mQueue->availableToWrite(); }
+
     virtual bool read(V2_1::Event* events, size_t numToRead) override {
         return mQueue->read(events, numToRead);
     }
 
+    bool write(const V2_1::Event* events, size_t numToWrite) override {
+        return mQueue->write(events, numToWrite);
+    }
+
     bool write(const std::vector<V2_1::Event>& events) override {
         return mQueue->write(events.data(), events.size());
     }
 
+    bool writeBlocking(const V2_1::Event* events, size_t count, uint32_t readNotification,
+                       uint32_t writeNotification, int64_t timeOutNanos,
+                       android::hardware::EventFlag* evFlag) override {
+        return mQueue->writeBlocking(events, count, readNotification, writeNotification,
+                                     timeOutNanos, evFlag);
+    }
+
+    size_t getQuantumCount() override { return mQueue->getQuantumCount(); }
+
   private:
     std::unique_ptr<EventMessageQueue> mQueue;
 };
diff --git a/sensors/common/utils/ISensorsCallbackWrapper.h b/sensors/common/utils/ISensorsCallbackWrapper.h
new file mode 100644
index 0000000000..816b225806
--- /dev/null
+++ b/sensors/common/utils/ISensorsCallbackWrapper.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2020 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.
+ */
+
+#ifndef ANDROID_HARDWARE_SENSORS_V2_1_ISENSORSCALLBACKWRAPPER_H
+#define ANDROID_HARDWARE_SENSORS_V2_1_ISENSORSCALLBACKWRAPPER_H
+
+#include "convertV2_1.h"
+
+#include "android/hardware/sensors/1.0/ISensors.h"
+#include "android/hardware/sensors/1.0/types.h"
+#include "android/hardware/sensors/2.0/ISensors.h"
+#include "android/hardware/sensors/2.0/ISensorsCallback.h"
+#include "android/hardware/sensors/2.1/ISensors.h"
+#include "android/hardware/sensors/2.1/ISensorsCallback.h"
+#include "android/hardware/sensors/2.1/types.h"
+
+#include <utils/LightRefBase.h>
+
+#include <cassert>
+
+namespace android {
+namespace hardware {
+namespace sensors {
+namespace V2_1 {
+namespace implementation {
+
+/**
+ * The ISensorsCallbackWrapper classes below abstract away the common logic between both the V2.0
+ * and V2.1 versions of the Sensors HAL interface. This allows users of these classes to only care
+ * about the HAL version at init time and then interact with either version of the callback without
+ * worrying about the class type by utilizing the base class.
+ */
+class ISensorsCallbackWrapperBase : public VirtualLightRefBase {
+  public:
+    virtual Return<void> onDynamicSensorsConnected(
+            const hidl_vec<V2_1::SensorInfo>& sensorInfos) = 0;
+
+    virtual Return<void> onDynamicSensorsDisconnected(const hidl_vec<int32_t>& sensorHandles) = 0;
+};
+
+template <typename T>
+class SensorsCallbackWrapperBase : public ISensorsCallbackWrapperBase {
+  public:
+    SensorsCallbackWrapperBase(sp<T> sensorsCallback) : mSensorsCallback(sensorsCallback){};
+
+    virtual Return<void> onDynamicSensorsConnected(
+            const hidl_vec<V2_1::SensorInfo>& sensorInfos) override {
+        return mSensorsCallback->onDynamicSensorsConnected(convertToOldSensorInfos(sensorInfos));
+    }
+
+    Return<void> onDynamicSensorsDisconnected(const hidl_vec<int32_t>& sensorHandles) {
+        return mSensorsCallback->onDynamicSensorsDisconnected(sensorHandles);
+    }
+
+  protected:
+    sp<T> mSensorsCallback;
+};
+
+class ISensorsCallbackWrapperV2_0
+    : public SensorsCallbackWrapperBase<hardware::sensors::V2_0::ISensorsCallback> {
+  public:
+    ISensorsCallbackWrapperV2_0(sp<hardware::sensors::V2_0::ISensorsCallback> sensorsCallback)
+        : SensorsCallbackWrapperBase(sensorsCallback){};
+};
+
+class ISensorsCallbackWrapperV2_1
+    : public SensorsCallbackWrapperBase<hardware::sensors::V2_1::ISensorsCallback> {
+  public:
+    ISensorsCallbackWrapperV2_1(sp<hardware::sensors::V2_1::ISensorsCallback> sensorsCallback)
+        : SensorsCallbackWrapperBase(sensorsCallback) {}
+
+    Return<void> onDynamicSensorsConnected(const hidl_vec<V2_1::SensorInfo>& sensorInfos) override {
+        return mSensorsCallback->onDynamicSensorsConnected_2_1(sensorInfos);
+    }
+};
+
+}  // namespace implementation
+}  // namespace V2_1
+}  // namespace sensors
+}  // namespace hardware
+}  // namespace android
+
+#endif  // ANDROID_HARDWARE_SENSORS_V2_1_ISENSORSCALLBACKWRAPPER_H
\ No newline at end of file