diff --git a/automotive/audiocontrol/2.0/default/AudioControl.cpp b/automotive/audiocontrol/2.0/default/AudioControl.cpp
index 5bde839..b7c11cd 100644
--- a/automotive/audiocontrol/2.0/default/AudioControl.cpp
+++ b/automotive/audiocontrol/2.0/default/AudioControl.cpp
@@ -69,7 +69,7 @@
 }
 
 Return<void> AudioControl::setFadeTowardFront(float value) {
-    if (!isValidValue(value)) {
+    if (isValidValue(value)) {
         // Just log in this default mock implementation
         LOG(INFO) << "Fader set to " << value;
     } else {
diff --git a/automotive/vehicle/2.0/default/Android.bp b/automotive/vehicle/2.0/default/Android.bp
index 21b410a..872b35b 100644
--- a/automotive/vehicle/2.0/default/Android.bp
+++ b/automotive/vehicle/2.0/default/Android.bp
@@ -75,7 +75,10 @@
     ],
     local_include_dirs: ["common/include/vhal_v2_0"],
     export_include_dirs: ["impl"],
-    whole_static_libs: ["android.hardware.automotive.vehicle@2.0-manager-lib"],
+    whole_static_libs: [
+        "android.hardware.automotive.vehicle@2.0-emulated-user-hal-lib",
+        "android.hardware.automotive.vehicle@2.0-manager-lib",
+    ],
     shared_libs: [
         "libbase",
         "libjsoncpp",
@@ -87,6 +90,16 @@
     ],
 }
 
+// Library used  to emulate User HAL behavior through lshal debug requests.
+cc_library_static {
+    name: "android.hardware.automotive.vehicle@2.0-emulated-user-hal-lib",
+    vendor: true,
+    defaults: ["vhal_v2_0_defaults"],
+    srcs: [
+        "impl/vhal_v2_0/EmulatedUserHal.cpp",
+    ],
+}
+
 cc_test {
     name: "android.hardware.automotive.vehicle@2.0-manager-unit-tests",
     vendor: true,
diff --git a/automotive/vehicle/2.0/default/impl/vhal_v2_0/DefaultConfig.h b/automotive/vehicle/2.0/default/impl/vhal_v2_0/DefaultConfig.h
index 83546e2..b8a606a 100644
--- a/automotive/vehicle/2.0/default/impl/vhal_v2_0/DefaultConfig.h
+++ b/automotive/vehicle/2.0/default/impl/vhal_v2_0/DefaultConfig.h
@@ -79,8 +79,6 @@
 constexpr int WHEEL_FRONT_RIGHT = (int)VehicleAreaWheel::RIGHT_FRONT;
 constexpr int WHEEL_REAR_LEFT = (int)VehicleAreaWheel::LEFT_REAR;
 constexpr int WHEEL_REAR_RIGHT = (int)VehicleAreaWheel::RIGHT_REAR;
-constexpr int INITIAL_USER_INFO = (int)VehicleProperty::INITIAL_USER_INFO;
-constexpr int SWITCH_USER = (int)VehicleProperty::SWITCH_USER;
 
 /**
  * This property is used for test purpose to generate fake events. Here is the test package that
diff --git a/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedUserHal.cpp b/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedUserHal.cpp
new file mode 100644
index 0000000..c49fadc
--- /dev/null
+++ b/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedUserHal.cpp
@@ -0,0 +1,186 @@
+/*
+ * 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.
+ */
+#define LOG_TAG "EmulatedUserHal"
+
+#include <cutils/log.h>
+#include <utils/SystemClock.h>
+
+#include "EmulatedUserHal.h"
+
+namespace android {
+namespace hardware {
+namespace automotive {
+namespace vehicle {
+namespace V2_0 {
+
+namespace impl {
+
+constexpr int INITIAL_USER_INFO = static_cast<int>(VehicleProperty::INITIAL_USER_INFO);
+constexpr int SWITCH_USER = static_cast<int>(VehicleProperty::SWITCH_USER);
+
+bool EmulatedUserHal::isSupported(int32_t prop) {
+    switch (prop) {
+        case INITIAL_USER_INFO:
+        case SWITCH_USER:
+            return true;
+        default:
+            return false;
+    }
+}
+
+android::base::Result<std::unique_ptr<VehiclePropValue>> EmulatedUserHal::onSetProperty(
+        const VehiclePropValue& value) {
+    ALOGV("onSetProperty(): %s", toString(value).c_str());
+
+    switch (value.prop) {
+        case INITIAL_USER_INFO:
+            return onSetInitialUserInfoResponse(value);
+        case SWITCH_USER:
+            return onSetSwitchUserResponse(value);
+        default:
+            return android::base::Error(static_cast<int>(StatusCode::INVALID_ARG))
+                   << "Unsupported property: " << toString(value);
+    }
+}
+
+android::base::Result<std::unique_ptr<VehiclePropValue>>
+EmulatedUserHal::onSetInitialUserInfoResponse(const VehiclePropValue& value) {
+    if (value.value.int32Values.size() == 0) {
+        ALOGE("set(INITIAL_USER_INFO): no int32values, ignoring it: %s", toString(value).c_str());
+        return android::base::Error(static_cast<int>(StatusCode::INVALID_ARG))
+               << "no int32values on " << toString(value);
+    }
+
+    if (value.areaId != 0) {
+        ALOGD("set(INITIAL_USER_INFO) called from lshal; storing it: %s", toString(value).c_str());
+        mInitialUserResponseFromCmd.reset(new VehiclePropValue(value));
+        return {};
+    }
+
+    ALOGD("set(INITIAL_USER_INFO) called from Android: %s", toString(value).c_str());
+
+    int32_t requestId = value.value.int32Values[0];
+    if (mInitialUserResponseFromCmd != nullptr) {
+        ALOGI("replying INITIAL_USER_INFO with lshal value:  %s",
+              toString(*mInitialUserResponseFromCmd).c_str());
+        return sendUserHalResponse(std::move(mInitialUserResponseFromCmd), requestId);
+    }
+
+    // Returns default response
+    auto updatedValue = std::unique_ptr<VehiclePropValue>(new VehiclePropValue);
+    updatedValue->prop = INITIAL_USER_INFO;
+    updatedValue->timestamp = elapsedRealtimeNano();
+    updatedValue->value.int32Values.resize(2);
+    updatedValue->value.int32Values[0] = requestId;
+    updatedValue->value.int32Values[1] = (int32_t)InitialUserInfoResponseAction::DEFAULT;
+
+    ALOGI("no lshal response; replying with InitialUserInfoResponseAction::DEFAULT: %s",
+          toString(*updatedValue).c_str());
+
+    return updatedValue;
+}
+
+android::base::Result<std::unique_ptr<VehiclePropValue>> EmulatedUserHal::onSetSwitchUserResponse(
+        const VehiclePropValue& value) {
+    if (value.value.int32Values.size() == 0) {
+        ALOGE("set(SWITCH_USER): no int32values, ignoring it: %s", toString(value).c_str());
+        return android::base::Error(static_cast<int>(StatusCode::INVALID_ARG))
+               << "no int32values on " << toString(value);
+    }
+
+    if (value.areaId != 0) {
+        ALOGD("set(SWITCH_USER) called from lshal; storing it: %s", toString(value).c_str());
+        mSwitchUserResponseFromCmd.reset(new VehiclePropValue(value));
+        return {};
+    }
+    ALOGD("set(SWITCH_USER) called from Android: %s", toString(value).c_str());
+
+    int32_t requestId = value.value.int32Values[0];
+    if (mSwitchUserResponseFromCmd != nullptr) {
+        ALOGI("replying SWITCH_USER with lshal value:  %s",
+              toString(*mSwitchUserResponseFromCmd).c_str());
+        return sendUserHalResponse(std::move(mSwitchUserResponseFromCmd), requestId);
+    }
+
+    // Returns default response
+    auto updatedValue = std::unique_ptr<VehiclePropValue>(new VehiclePropValue);
+    updatedValue->prop = SWITCH_USER;
+    updatedValue->timestamp = elapsedRealtimeNano();
+    updatedValue->value.int32Values.resize(3);
+    updatedValue->value.int32Values[0] = requestId;
+    updatedValue->value.int32Values[1] = (int32_t)SwitchUserMessageType::VEHICLE_RESPONSE;
+    updatedValue->value.int32Values[2] = (int32_t)SwitchUserStatus::SUCCESS;
+
+    ALOGI("no lshal response; replying with VEHICLE_RESPONSE / SUCCESS: %s",
+          toString(*updatedValue).c_str());
+
+    return updatedValue;
+}
+
+android::base::Result<std::unique_ptr<VehiclePropValue>> EmulatedUserHal::sendUserHalResponse(
+        std::unique_ptr<VehiclePropValue> response, int32_t requestId) {
+    switch (response->areaId) {
+        case 1:
+            ALOGD("returning response with right request id");
+            response->value.int32Values[0] = requestId;
+            break;
+        case 2:
+            ALOGD("returning response with wrong request id");
+            response->value.int32Values[0] = -requestId;
+            break;
+        case 3:
+            ALOGD("not generating a property change event because of lshal prop: %s",
+                  toString(*response).c_str());
+            return android::base::Error(static_cast<int>(StatusCode::NOT_AVAILABLE))
+                   << "not generating a property change event because of lshal prop: "
+                   << toString(*response);
+        default:
+            ALOGE("invalid action on lshal response: %s", toString(*response).c_str());
+            return android::base::Error(static_cast<int>(StatusCode::INTERNAL_ERROR))
+                   << "invalid action on lshal response: " << toString(*response);
+    }
+
+    ALOGD("updating property to: %s", toString(*response).c_str());
+
+    return response;
+}
+
+void EmulatedUserHal::showDumpHelp(int fd) {
+    dprintf(fd, "%s: dumps state used for user management\n", kUserHalDumpOption);
+}
+
+void EmulatedUserHal::dump(int fd, std::string indent) {
+    if (mInitialUserResponseFromCmd != nullptr) {
+        dprintf(fd, "%sInitialUserInfo response: %s\n", indent.c_str(),
+                toString(*mInitialUserResponseFromCmd).c_str());
+    } else {
+        dprintf(fd, "%sNo InitialUserInfo response\n", indent.c_str());
+    }
+    if (mSwitchUserResponseFromCmd != nullptr) {
+        dprintf(fd, "%sSwitchUser response: %s\n", indent.c_str(),
+                toString(*mSwitchUserResponseFromCmd).c_str());
+    } else {
+        dprintf(fd, "%sNo SwitchUser response\n", indent.c_str());
+    }
+}
+
+}  // namespace impl
+
+}  // namespace V2_0
+}  // namespace vehicle
+}  // namespace automotive
+}  // namespace hardware
+}  // namespace android
diff --git a/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedUserHal.h b/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedUserHal.h
new file mode 100644
index 0000000..b25efcb
--- /dev/null
+++ b/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedUserHal.h
@@ -0,0 +1,115 @@
+/*
+ * 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_automotive_vehicle_V2_0_impl_EmulatedUserHal_H_
+#define android_hardware_automotive_vehicle_V2_0_impl_EmulatedUserHal_H_
+
+#include <android-base/result.h>
+
+#include <android/hardware/automotive/vehicle/2.0/types.h>
+
+namespace android {
+namespace hardware {
+namespace automotive {
+namespace vehicle {
+namespace V2_0 {
+
+namespace impl {
+
+constexpr char kUserHalDumpOption[] = "--user-hal";
+
+/**
+ * Class used to emulate User HAL behavior through lshal debug requests.
+ */
+class EmulatedUserHal {
+  public:
+    EmulatedUserHal() {}
+
+    ~EmulatedUserHal() = default;
+
+    /**
+     * Checks if the emulator can handle the property.
+     */
+    bool isSupported(int32_t prop);
+
+    /**
+     * Lets the emulator handle the property.
+     *
+     * @return updated property and StatusCode
+     */
+    android::base::Result<std::unique_ptr<VehiclePropValue>> onSetProperty(
+            const VehiclePropValue& value);
+
+    /**
+     * Shows the User HAL emulation help.
+     */
+    void showDumpHelp(int fd);
+
+    /**
+     * Dump its contents.
+     */
+    void dump(int fd, std::string indent);
+
+  private:
+    /**
+     * INITIAL_USER_INFO is called by Android when it starts, and it's expecting a property change
+     * indicating what the initial user should be.
+     *
+     * During normal circumstances, the emulator will reply right away, passing a response if
+     * InitialUserInfoResponseAction::DEFAULT (so Android could use its own logic to decide which
+     * user to boot).
+     *
+     * But during development / testing, the behavior can be changed using lshal dump, which must
+     * use the areaId to indicate what should happen next.
+     *
+     * So, the behavior of set(INITIAL_USER_INFO) is:
+     *
+     * - if it has an areaId, store the property into mInitialUserResponseFromCmd (as it was called
+     * by lshal).
+     * - else if mInitialUserResponseFromCmd is not set, return a response with the same request id
+     * and InitialUserInfoResponseAction::DEFAULT
+     * - else the behavior is defined by the areaId on mInitialUserResponseFromCmd:
+     * - if it's 1, reply with mInitialUserResponseFromCmd and the right request id
+     * - if it's 2, reply with mInitialUserResponseFromCmd but a wrong request id (so Android can
+     * test this error scenario)
+     * - if it's 3, then don't send a property change (so Android can emulate a timeout)
+     *
+     */
+    android::base::Result<std::unique_ptr<VehiclePropValue>> onSetInitialUserInfoResponse(
+            const VehiclePropValue& value);
+
+    /**
+     * Used to emulate SWITCH_USER - see onSetInitialUserInfoResponse() for usage.
+     */
+    android::base::Result<std::unique_ptr<VehiclePropValue>> onSetSwitchUserResponse(
+            const VehiclePropValue& value);
+
+    android::base::Result<std::unique_ptr<VehiclePropValue>> sendUserHalResponse(
+            std::unique_ptr<VehiclePropValue> response, int32_t requestId);
+
+    std::unique_ptr<VehiclePropValue> mInitialUserResponseFromCmd;
+    std::unique_ptr<VehiclePropValue> mSwitchUserResponseFromCmd;
+};
+
+}  // namespace impl
+
+}  // namespace V2_0
+}  // namespace vehicle
+}  // namespace automotive
+}  // namespace hardware
+}  // namespace android
+
+#endif  // android_hardware_automotive_vehicle_V2_0_impl_EmulatedUserHal_H_
diff --git a/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedVehicleConnector.cpp b/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedVehicleConnector.cpp
index ce7dc65..7f9362f 100644
--- a/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedVehicleConnector.cpp
+++ b/automotive/vehicle/2.0/default/impl/vhal_v2_0/EmulatedVehicleConnector.cpp
@@ -38,9 +38,6 @@
 class EmulatedPassthroughConnector : public PassthroughConnector {
   public:
     bool onDump(const hidl_handle& fd, const hidl_vec<hidl_string>& options) override;
-
-  private:
-    void dumpUserHal(int fd, std::string indent);
 };
 
 bool EmulatedPassthroughConnector::onDump(const hidl_handle& handle,
@@ -50,12 +47,12 @@
     if (options.size() > 0) {
         if (options[0] == "--help") {
             dprintf(fd, "Emulator-specific usage:\n");
-            dprintf(fd, "--user-hal: dumps state used for user management \n");
+            mEmulatedUserHal.showDumpHelp(fd);
             dprintf(fd, "\n");
             // Include caller's help options
             return true;
-        } else if (options[0] == "--user-hal") {
-            dumpUserHal(fd, "");
+        } else if (options[0] == kUserHalDumpOption) {
+            mEmulatedUserHal.dump(fd, "");
             return false;
 
         } else {
@@ -65,27 +62,12 @@
     }
 
     dprintf(fd, "Emulator-specific state:\n");
-    dumpUserHal(fd, "  ");
+    mEmulatedUserHal.dump(fd, "  ");
     dprintf(fd, "\n");
 
     return true;
 }
 
-void EmulatedPassthroughConnector::dumpUserHal(int fd, std::string indent) {
-    if (mInitialUserResponseFromCmd != nullptr) {
-        dprintf(fd, "%sInitialUserInfo response: %s\n", indent.c_str(),
-                toString(*mInitialUserResponseFromCmd).c_str());
-    } else {
-        dprintf(fd, "%sNo InitialUserInfo response\n", indent.c_str());
-    }
-    if (mSwitchUserResponseFromCmd != nullptr) {
-        dprintf(fd, "%sSwitchUser response: %s\n", indent.c_str(),
-                toString(*mSwitchUserResponseFromCmd).c_str());
-    } else {
-        dprintf(fd, "%sNo SwitchUser response\n", indent.c_str());
-    }
-}
-
 PassthroughConnectorPtr makeEmulatedPassthroughConnector() {
     return std::make_unique<EmulatedPassthroughConnector>();
 }
diff --git a/automotive/vehicle/2.0/default/impl/vhal_v2_0/VehicleHalServer.cpp b/automotive/vehicle/2.0/default/impl/vhal_v2_0/VehicleHalServer.cpp
index 70e39eb..ad5096e 100644
--- a/automotive/vehicle/2.0/default/impl/vhal_v2_0/VehicleHalServer.cpp
+++ b/automotive/vehicle/2.0/default/impl/vhal_v2_0/VehicleHalServer.cpp
@@ -181,6 +181,23 @@
 }
 
 StatusCode VehicleHalServer::onSetProperty(const VehiclePropValue& value, bool updateStatus) {
+    if (mEmulatedUserHal.isSupported(value.prop)) {
+        LOG(INFO) << "onSetProperty(): property " << value.prop << " will be handled by UserHal";
+
+        const auto& ret = mEmulatedUserHal.onSetProperty(value);
+        if (!ret.ok()) {
+            LOG(ERROR) << "onSetProperty(): HAL returned error: " << ret.error().message();
+            return StatusCode(ret.error().code());
+        }
+        auto updatedValue = ret.value().get();
+        if (updatedValue != nullptr) {
+            LOG(INFO) << "onSetProperty(): updating property returned by HAL: "
+                      << toString(*updatedValue);
+            onPropertyValueFromCar(*updatedValue, updateStatus);
+        }
+        return StatusCode::OK;
+    }
+
     // Some properties need to be treated non-trivially
     switch (value.prop) {
         case kGenerateFakeDataControllingProperty:
@@ -245,10 +262,6 @@
                     break;
             }
             break;
-        case INITIAL_USER_INFO:
-            return onSetInitialUserInfoResponse(value, updateStatus);
-        case SWITCH_USER:
-            return onSetSwitchUserResponse(value, updateStatus);
         default:
             break;
     }
@@ -262,165 +275,4 @@
     return StatusCode::OK;
 }
 
-/**
- * INITIAL_USER_INFO is called by Android when it starts, and it's expecting a property change
- * indicating what the initial user should be.
- *
- * During normal circumstances, the emulator will reply right away, passing a response if
- * InitialUserInfoResponseAction::DEFAULT (so Android could use its own logic to decide which user
- * to boot).
- *
- * But during development / testing, the behavior can be changed using lshal dump, which must use
- * the areaId to indicate what should happen next.
- *
- * So, the behavior of set(INITIAL_USER_INFO) is:
- *
- * - if it has an areaId, store the property into mInitialUserResponseFromCmd (as it was called by
- *   lshal).
- * - else if mInitialUserResponseFromCmd is not set, return a response with the same request id and
- *   InitialUserInfoResponseAction::DEFAULT
- * - else the behavior is defined by the areaId on mInitialUserResponseFromCmd:
- * - if it's 1, reply with mInitialUserResponseFromCmd and the right request id
- * - if it's 2, reply with mInitialUserResponseFromCmd but a wrong request id (so Android can test
- *   this error scenario)
- * - if it's 3, then don't send a property change (so Android can emulate a timeout)
- *
- */
-StatusCode VehicleHalServer::onSetInitialUserInfoResponse(const VehiclePropValue& value,
-                                                          bool updateStatus) {
-    // TODO: LOG calls below might be more suited to be DEBUG, but those are not being logged
-    // (even when explicitly calling setprop log.tag. As this class should be using ALOG instead of
-    // LOG, it's not worth investigating why...
-
-    if (value.value.int32Values.size() == 0) {
-        LOG(ERROR) << "set(INITIAL_USER_INFO): no int32values, ignoring it: " << toString(value);
-        return StatusCode::INVALID_ARG;
-    }
-
-    if (value.areaId != 0) {
-        LOG(INFO) << "set(INITIAL_USER_INFO) called from lshal; storing it: " << toString(value);
-        mInitialUserResponseFromCmd.reset(new VehiclePropValue(value));
-        return StatusCode::OK;
-    }
-    LOG(INFO) << "set(INITIAL_USER_INFO) called from Android: " << toString(value);
-
-    int32_t requestId = value.value.int32Values[0];
-
-    // Create the update property and set common values
-    auto updatedValue = createVehiclePropValue(VehiclePropertyType::MIXED, 0);
-    updatedValue->prop = INITIAL_USER_INFO;
-    updatedValue->timestamp = elapsedRealtimeNano();
-
-    if (mInitialUserResponseFromCmd == nullptr) {
-        updatedValue->value.int32Values.resize(2);
-        updatedValue->value.int32Values[0] = requestId;
-        updatedValue->value.int32Values[1] = (int32_t)InitialUserInfoResponseAction::DEFAULT;
-        LOG(INFO) << "no lshal response; returning InitialUserInfoResponseAction::DEFAULT: "
-                  << toString(*updatedValue);
-        onPropertyValueFromCar(*updatedValue, updateStatus);
-        return StatusCode::OK;
-    }
-
-    // mInitialUserResponseFromCmd is used for just one request
-    std::unique_ptr<VehiclePropValue> response = std::move(mInitialUserResponseFromCmd);
-
-    // TODO(b/150409377): rather than populate the raw values directly, it should use the
-    // libraries that convert a InitialUserInfoResponse into a VehiclePropValue)
-
-    switch (response->areaId) {
-        case 1:
-            LOG(INFO) << "returning response with right request id";
-            *updatedValue = *response;
-            updatedValue->areaId = 0;
-            updatedValue->value.int32Values[0] = requestId;
-            break;
-        case 2:
-            LOG(INFO) << "returning response with wrong request id";
-            *updatedValue = *response;
-            updatedValue->areaId = 0;
-            updatedValue->value.int32Values[0] = -requestId;
-            break;
-        case 3:
-            LOG(INFO) << "not generating a property change event because of lshal prop: "
-                      << toString(*response);
-            return StatusCode::OK;
-        default:
-            LOG(ERROR) << "invalid action on lshal response: " << toString(*response);
-            return StatusCode::INTERNAL_ERROR;
-    }
-
-    LOG(INFO) << "updating property to: " << toString(*updatedValue);
-    onPropertyValueFromCar(*updatedValue, updateStatus);
-    return StatusCode::OK;
-}
-
-/**
- * Used to emulate SWITCH_USER - see onSetInitialUserInfoResponse() for usage.
- */
-StatusCode VehicleHalServer::onSetSwitchUserResponse(const VehiclePropValue& value,
-                                                     bool updateStatus) {
-    if (value.value.int32Values.size() == 0) {
-        LOG(ERROR) << "set(SWITCH_USER): no int32values, ignoring it: " << toString(value);
-        return StatusCode::INVALID_ARG;
-    }
-
-    if (value.areaId != 0) {
-        LOG(INFO) << "set(SWITCH_USER) called from lshal; storing it: " << toString(value);
-        mSwitchUserResponseFromCmd.reset(new VehiclePropValue(value));
-        return StatusCode::OK;
-    }
-    LOG(INFO) << "set(SWITCH_USER) called from Android: " << toString(value);
-
-    int32_t requestId = value.value.int32Values[0];
-
-    // Create the update property and set common values
-    auto updatedValue = createVehiclePropValue(VehiclePropertyType::MIXED, 0);
-    updatedValue->prop = SWITCH_USER;
-    updatedValue->timestamp = elapsedRealtimeNano();
-
-    if (mSwitchUserResponseFromCmd == nullptr) {
-        updatedValue->value.int32Values.resize(3);
-        updatedValue->value.int32Values[0] = requestId;
-        updatedValue->value.int32Values[1] = (int32_t)SwitchUserMessageType::VEHICLE_RESPONSE;
-        updatedValue->value.int32Values[2] = (int32_t)SwitchUserStatus::SUCCESS;
-        LOG(INFO) << "no lshal response; returning VEHICLE_RESPONSE / SUCCESS: "
-                  << toString(*updatedValue);
-        onPropertyValueFromCar(*updatedValue, updateStatus);
-        return StatusCode::OK;
-    }
-
-    // mSwitchUserResponseFromCmd is used for just one request
-    std::unique_ptr<VehiclePropValue> response = std::move(mSwitchUserResponseFromCmd);
-
-    // TODO(b/150409377): move code below to a local function like sendUserHalResponse(),
-    // as it's the same for all (like onSetInitialUserInfoResponse)
-
-    switch (response->areaId) {
-        case 1:
-            LOG(INFO) << "returning response with right request id";
-            *updatedValue = *response;
-            updatedValue->areaId = 0;
-            updatedValue->value.int32Values[0] = requestId;
-            break;
-        case 2:
-            LOG(INFO) << "returning response with wrong request id";
-            *updatedValue = *response;
-            updatedValue->areaId = 0;
-            updatedValue->value.int32Values[0] = -requestId;
-            break;
-        case 3:
-            LOG(INFO) << "not generating a property change event because of lshal prop: "
-                      << toString(*response);
-            return StatusCode::OK;
-        default:
-            LOG(ERROR) << "invalid action on lshal response: " << toString(*response);
-            return StatusCode::INTERNAL_ERROR;
-    }
-
-    LOG(INFO) << "updating property to: " << toString(*updatedValue);
-    onPropertyValueFromCar(*updatedValue, updateStatus);
-
-    return StatusCode::OK;
-}
-
 }  // namespace android::hardware::automotive::vehicle::V2_0::impl
diff --git a/automotive/vehicle/2.0/default/impl/vhal_v2_0/VehicleHalServer.h b/automotive/vehicle/2.0/default/impl/vhal_v2_0/VehicleHalServer.h
index 20e094a..2841fbe 100644
--- a/automotive/vehicle/2.0/default/impl/vhal_v2_0/VehicleHalServer.h
+++ b/automotive/vehicle/2.0/default/impl/vhal_v2_0/VehicleHalServer.h
@@ -19,6 +19,7 @@
 #include <vhal_v2_0/VehicleObjectPool.h>
 #include <vhal_v2_0/VehicleServer.h>
 
+#include "EmulatedUserHal.h"
 #include "GeneratorHub.h"
 
 namespace android::hardware::automotive::vehicle::V2_0::impl {
@@ -53,15 +54,10 @@
     VehiclePropValuePtr createHwInputKeyProp(VehicleHwKeyInputAction action, int32_t keyCode,
                                              int32_t targetDisplay);
 
-    StatusCode onSetInitialUserInfoResponse(const VehiclePropValue& value, bool updateStatus);
-    StatusCode onSetSwitchUserResponse(const VehiclePropValue& value, bool updateStatus);
-
     // data members
 
   protected:
-    // TODO(b/146207078): it might be clearer to move members below to an EmulatedUserHal class
-    std::unique_ptr<VehiclePropValue> mInitialUserResponseFromCmd;
-    std::unique_ptr<VehiclePropValue> mSwitchUserResponseFromCmd;
+    EmulatedUserHal mEmulatedUserHal;
 
   private:
     GeneratorHub mGeneratorHub{
diff --git a/automotive/vehicle/2.0/types.hal b/automotive/vehicle/2.0/types.hal
index 82f938c..ee34e42 100644
--- a/automotive/vehicle/2.0/types.hal
+++ b/automotive/vehicle/2.0/types.hal
@@ -606,8 +606,23 @@
     /**
      * Tire pressure
      *
-     * min/max value indicates tire pressure sensor range.  Each tire will have a separate min/max
-     * value denoted by its areaConfig.areaId.
+     * Each tires is identified by its areaConfig.areaId config and their
+     * minFloatValue/maxFloatValue are used to store OEM recommended pressure
+     * range.
+     * The Min value in the areaConfig data represents the lower bound of
+     * the recommended tire pressure.
+     * The Max value in the areaConfig data represents the upper bound of
+     * the recommended tire pressure.
+     * For example:
+     * The following areaConfig indicates the recommended tire pressure
+     * of left_front tire is from 200.0 KILOPASCAL to 240.0 KILOPASCAL.
+     * .areaConfigs = {
+     *      VehicleAreaConfig {
+     *          .areaId = VehicleAreaWheel::LEFT_FRONT,
+     *          .minFloatValue = 200.0,
+     *          .maxFloatValue = 240.0,
+     *      }
+     * },
      *
      * @change_mode VehiclePropertyChangeMode:CONTINUOUS
      * @access VehiclePropertyAccess:READ
@@ -786,7 +801,8 @@
     /*
      * HVAC Properties
      *
-     * Additional rules for mapping a zoned HVAC property to AreaIDs:
+     * Additional rules for mapping a zoned HVAC property (except
+     * HVAC_MAX_DEFROST_ON) to AreaIDs:
      *  - Every seat in VehicleAreaSeat that is available in the car, must be
      *    part of an AreaID in the AreaID array.
      *
@@ -919,6 +935,11 @@
      * possible.  Any parameters modified as a side effect of turning on/off
      * the MAX DEFROST parameter shall generate onPropertyEvent() callbacks to
      * the VHAL.
+     * The AreaIDs for HVAC_MAX_DEFROST_ON indicate MAX DEFROST can be controlled
+     * in the area.
+     * For example:
+     * areaConfig.areaId = {ROW_1_LEFT | ROW_1_RIGHT} indicates HVAC_MAX_DEFROST_ON
+     * only can be controlled for the front rows.
      *
      * @change_mode VehiclePropertyChangeMode:ON_CHANGE
      * @access VehiclePropertyAccess:READ_WRITE
diff --git a/boot/1.1/vts/functional/VtsHalBootV1_1TargetTest.cpp b/boot/1.1/vts/functional/VtsHalBootV1_1TargetTest.cpp
index 7c58ef3..30b965d 100644
--- a/boot/1.1/vts/functional/VtsHalBootV1_1TargetTest.cpp
+++ b/boot/1.1/vts/functional/VtsHalBootV1_1TargetTest.cpp
@@ -76,7 +76,11 @@
     for (const auto value : ValidMergeStatusValues()) {
         EXPECT_TRUE(boot->setSnapshotMergeStatus(value).withDefault(false));
         auto status = boot->getSnapshotMergeStatus();
-        EXPECT_EQ(status, value);
+        if (value == MergeStatus::SNAPSHOTTED) {
+            EXPECT_TRUE(status == MergeStatus::SNAPSHOTTED || status == MergeStatus::NONE);
+        } else {
+            EXPECT_EQ(status, value);
+        }
     }
 }
 
diff --git a/camera/provider/2.4/vts/functional/VtsHalCameraProviderV2_4TargetTest.cpp b/camera/provider/2.4/vts/functional/VtsHalCameraProviderV2_4TargetTest.cpp
index 05b8b47..bf5fbfe 100644
--- a/camera/provider/2.4/vts/functional/VtsHalCameraProviderV2_4TargetTest.cpp
+++ b/camera/provider/2.4/vts/functional/VtsHalCameraProviderV2_4TargetTest.cpp
@@ -18,12 +18,13 @@
 
 #include <algorithm>
 #include <chrono>
+#include <condition_variable>
+#include <list>
 #include <mutex>
 #include <regex>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
-#include <condition_variable>
 
 #include <inttypes.h>
 
@@ -165,6 +166,26 @@
     YUV_REPROCESS,
 };
 
+enum SystemCameraKind {
+    /**
+     * These camera devices are visible to all apps and system components alike
+     */
+    PUBLIC = 0,
+
+    /**
+     * These camera devices are visible only to processes having the
+     * android.permission.SYSTEM_CAMERA permission. They are not exposed to 3P
+     * apps.
+     */
+    SYSTEM_ONLY_CAMERA,
+
+    /**
+     * These camera devices are visible only to HAL clients (that try to connect
+     * on a hwbinder thread).
+     */
+    HIDDEN_SECURE_CAMERA
+};
+
 namespace {
     // "device@<version>/legacy/<id>"
     const char *kDeviceNameRE = "device@([0-9]+\\.[0-9]+)/%s/(.+)";
@@ -851,6 +872,8 @@
     static Status isAutoFocusModeAvailable(
             CameraParameters &cameraParams, const char *mode) ;
     static Status isMonochromeCamera(const camera_metadata_t *staticMeta);
+    static Status getSystemCameraKind(const camera_metadata_t* staticMeta,
+                                      SystemCameraKind* systemCameraKind);
 
     // Used by switchToOffline where a new result queue is created for offline reqs
     void updateInflightResultQueue(std::shared_ptr<ResultMetadataQueue> resultQueue);
@@ -2555,6 +2578,90 @@
     }
 }
 
+TEST_P(CameraHidlTest, systemCameraTest) {
+    hidl_vec<hidl_string> cameraDeviceNames = getCameraDeviceNames(mProvider);
+    std::map<std::string, std::list<SystemCameraKind>> hiddenPhysicalIdToLogicalMap;
+    for (const auto& name : cameraDeviceNames) {
+        int deviceVersion = getCameraDeviceVersion(name, mProviderType);
+        switch (deviceVersion) {
+            case CAMERA_DEVICE_API_VERSION_3_6:
+            case CAMERA_DEVICE_API_VERSION_3_5:
+            case CAMERA_DEVICE_API_VERSION_3_4:
+            case CAMERA_DEVICE_API_VERSION_3_3:
+            case CAMERA_DEVICE_API_VERSION_3_2: {
+                ::android::sp<::android::hardware::camera::device::V3_2::ICameraDevice> device3_x;
+                ALOGI("getCameraCharacteristics: Testing camera device %s", name.c_str());
+                Return<void> ret;
+                ret = mProvider->getCameraDeviceInterface_V3_x(
+                        name, [&](auto status, const auto& device) {
+                            ALOGI("getCameraDeviceInterface_V3_x returns status:%d", (int)status);
+                            ASSERT_EQ(Status::OK, status);
+                            ASSERT_NE(device, nullptr);
+                            device3_x = device;
+                        });
+                ASSERT_TRUE(ret.isOk());
+
+                ret = device3_x->getCameraCharacteristics([&](auto status, const auto& chars) {
+                    ASSERT_EQ(status, Status::OK);
+                    const camera_metadata_t* staticMeta =
+                            reinterpret_cast<const camera_metadata_t*>(chars.data());
+                    ASSERT_NE(staticMeta, nullptr);
+                    Status rc = isLogicalMultiCamera(staticMeta);
+                    ASSERT_TRUE(Status::OK == rc || Status::METHOD_NOT_SUPPORTED == rc);
+                    if (Status::METHOD_NOT_SUPPORTED == rc) {
+                        return;
+                    }
+                    std::unordered_set<std::string> physicalIds;
+                    ASSERT_EQ(Status::OK, getPhysicalCameraIds(staticMeta, &physicalIds));
+                    SystemCameraKind systemCameraKind = SystemCameraKind::PUBLIC;
+                    rc = getSystemCameraKind(staticMeta, &systemCameraKind);
+                    ASSERT_EQ(rc, Status::OK);
+                    for (auto physicalId : physicalIds) {
+                        bool isPublicId = false;
+                        for (auto& deviceName : cameraDeviceNames) {
+                            std::string publicVersion, publicId;
+                            ASSERT_TRUE(::matchDeviceName(deviceName, mProviderType, &publicVersion,
+                                                          &publicId));
+                            if (physicalId == publicId) {
+                                isPublicId = true;
+                                break;
+                            }
+                        }
+                        // For hidden physical cameras, collect their associated logical cameras
+                        // and store the system camera kind.
+                        if (!isPublicId) {
+                            auto it = hiddenPhysicalIdToLogicalMap.find(physicalId);
+                            if (it == hiddenPhysicalIdToLogicalMap.end()) {
+                                hiddenPhysicalIdToLogicalMap.insert(std::make_pair(
+                                        physicalId, std::list<SystemCameraKind>(systemCameraKind)));
+                            } else {
+                                it->second.push_back(systemCameraKind);
+                            }
+                        }
+                    }
+                });
+                ASSERT_TRUE(ret.isOk());
+            } break;
+            case CAMERA_DEVICE_API_VERSION_1_0: {
+                // Not applicable
+            } break;
+            default: {
+                ALOGE("%s: Unsupported device version %d", __func__, deviceVersion);
+                ADD_FAILURE();
+            } break;
+        }
+    }
+
+    // Check that the system camera kind of the logical cameras associated with
+    // each hidden physical camera is the same.
+    for (const auto& it : hiddenPhysicalIdToLogicalMap) {
+        SystemCameraKind neededSystemCameraKind = it.second.front();
+        for (auto foundSystemCamera : it.second) {
+            ASSERT_EQ(neededSystemCameraKind, foundSystemCamera);
+        }
+    }
+}
+
 // Verify that the static camera characteristics can be retrieved
 // successfully.
 TEST_P(CameraHidlTest, getCameraCharacteristics) {
@@ -5671,6 +5778,39 @@
     return ret;
 }
 
+Status CameraHidlTest::getSystemCameraKind(const camera_metadata_t* staticMeta,
+                                           SystemCameraKind* systemCameraKind) {
+    Status ret = Status::OK;
+    if (nullptr == staticMeta || nullptr == systemCameraKind) {
+        return Status::ILLEGAL_ARGUMENT;
+    }
+
+    camera_metadata_ro_entry entry;
+    int rc = find_camera_metadata_ro_entry(staticMeta, ANDROID_REQUEST_AVAILABLE_CAPABILITIES,
+                                           &entry);
+    if (0 != rc) {
+        return Status::ILLEGAL_ARGUMENT;
+    }
+
+    if (entry.count == 1 &&
+        entry.data.u8[0] == ANDROID_REQUEST_AVAILABLE_CAPABILITIES_SECURE_IMAGE_DATA) {
+        *systemCameraKind = SystemCameraKind::HIDDEN_SECURE_CAMERA;
+        return ret;
+    }
+
+    // Go through the capabilities and check if it has
+    // ANDROID_REQUEST_AVAILABLE_CAPABILITIES_SYSTEM_CAMERA
+    for (size_t i = 0; i < entry.count; ++i) {
+        uint8_t capability = entry.data.u8[i];
+        if (capability == ANDROID_REQUEST_AVAILABLE_CAPABILITIES_SYSTEM_CAMERA) {
+            *systemCameraKind = SystemCameraKind::SYSTEM_ONLY_CAMERA;
+            return ret;
+        }
+    }
+    *systemCameraKind = SystemCameraKind::PUBLIC;
+    return ret;
+}
+
 // Check whether this is a monochrome camera using the static camera characteristics.
 Status CameraHidlTest::isMonochromeCamera(const camera_metadata_t *staticMeta) {
     Status ret = Status::METHOD_NOT_SUPPORTED;
@@ -6391,8 +6531,10 @@
         const hidl_vec<hidl_string>& deviceNames) {
     const camera_metadata_t* metadata = (camera_metadata_t*)chars.data();
     ASSERT_NE(nullptr, metadata);
-
-    Status rc = isLogicalMultiCamera(metadata);
+    SystemCameraKind systemCameraKind = SystemCameraKind::PUBLIC;
+    Status rc = getSystemCameraKind(metadata, &systemCameraKind);
+    ASSERT_EQ(rc, Status::OK);
+    rc = isLogicalMultiCamera(metadata);
     ASSERT_TRUE(Status::OK == rc || Status::METHOD_NOT_SUPPORTED == rc);
     if (Status::METHOD_NOT_SUPPORTED == rc) {
         return;
@@ -6411,6 +6553,7 @@
         ASSERT_NE(physicalId, cameraId);
         bool isPublicId = false;
         std::string fullPublicId;
+        SystemCameraKind physSystemCameraKind = SystemCameraKind::PUBLIC;
         for (auto& deviceName : deviceNames) {
             std::string publicVersion, publicId;
             ASSERT_TRUE(::matchDeviceName(deviceName, mProviderType, &publicVersion, &publicId));
@@ -6434,9 +6577,16 @@
             ret = subDevice->getCameraCharacteristics(
                     [&](auto status, const auto& chars) {
                 ASSERT_EQ(Status::OK, status);
-                retcode = find_camera_metadata_ro_entry(
-                        (const camera_metadata_t *)chars.data(),
-                        ANDROID_CONTROL_ZOOM_RATIO_RANGE, &entry);
+                const camera_metadata_t* staticMeta =
+                        reinterpret_cast<const camera_metadata_t*>(chars.data());
+                rc = getSystemCameraKind(staticMeta, &physSystemCameraKind);
+                ASSERT_EQ(rc, Status::OK);
+                // Make sure that the system camera kind of a non-hidden
+                // physical cameras is the same as the logical camera associated
+                // with it.
+                ASSERT_EQ(physSystemCameraKind, systemCameraKind);
+                retcode = find_camera_metadata_ro_entry(staticMeta,
+                                                        ANDROID_CONTROL_ZOOM_RATIO_RANGE, &entry);
                 bool subCameraHasZoomRatioRange = (0 == retcode && entry.count == 2);
                 ASSERT_EQ(hasZoomRatioRange, subCameraHasZoomRatioRange);
             });
@@ -6452,17 +6602,16 @@
         ASSERT_NE(device3_5, nullptr);
 
         // Check camera characteristics for hidden camera id
-        Return<void> ret = device3_5->getPhysicalCameraCharacteristics(physicalId,
-                [&](auto status, const auto& chars) {
-            verifyCameraCharacteristics(status, chars);
-            verifyMonochromeCharacteristics(chars, deviceVersion);
-
-            retcode = find_camera_metadata_ro_entry(
-                    (const camera_metadata_t *)chars.data(),
-                    ANDROID_CONTROL_ZOOM_RATIO_RANGE, &entry);
-            bool subCameraHasZoomRatioRange = (0 == retcode && entry.count == 2);
-            ASSERT_EQ(hasZoomRatioRange, subCameraHasZoomRatioRange);
-        });
+        Return<void> ret = device3_5->getPhysicalCameraCharacteristics(
+                physicalId, [&](auto status, const auto& chars) {
+                    verifyCameraCharacteristics(status, chars);
+                    verifyMonochromeCharacteristics(chars, deviceVersion);
+                    retcode =
+                            find_camera_metadata_ro_entry((const camera_metadata_t*)chars.data(),
+                                                          ANDROID_CONTROL_ZOOM_RATIO_RANGE, &entry);
+                    bool subCameraHasZoomRatioRange = (0 == retcode && entry.count == 2);
+                    ASSERT_EQ(hasZoomRatioRange, subCameraHasZoomRatioRange);
+                });
         ASSERT_TRUE(ret.isOk());
 
         // Check calling getCameraDeviceInterface_V3_x() on hidden camera id returns
diff --git a/compatibility_matrices/compatibility_matrix.5.xml b/compatibility_matrices/compatibility_matrix.5.xml
index 1b051f5..e772b6f 100644
--- a/compatibility_matrices/compatibility_matrix.5.xml
+++ b/compatibility_matrices/compatibility_matrix.5.xml
@@ -371,6 +371,7 @@
     </hal>
     <hal format="hidl" optional="true">
         <name>android.hardware.radio</name>
+        <version>1.4</version>
         <version>1.5</version>
         <interface>
             <name>IRadio</name>
diff --git a/identity/aidl/android/hardware/identity/IWritableIdentityCredential.aidl b/identity/aidl/android/hardware/identity/IWritableIdentityCredential.aidl
index 9673821..07486e6 100644
--- a/identity/aidl/android/hardware/identity/IWritableIdentityCredential.aidl
+++ b/identity/aidl/android/hardware/identity/IWritableIdentityCredential.aidl
@@ -140,7 +140,8 @@
      * with STATUS_INVALID_DATA.
      *
      * @param id a numeric identifier that must be unique within the context of a Credential and may
-     *     be used to reference the profile. If this is not satisfied the call fails with
+     *     be used to reference the profile. This id must be non-negative and less than 32 (allowing
+     *     for a total of 32 profiles). If this is not satisfied the call fails with
      *     STATUS_INVALID_DATA.
      *
      * @param readerCertificate if non-empty, specifies a single X.509 certificate (not a chain of
diff --git a/identity/aidl/default/WritableIdentityCredential.cpp b/identity/aidl/default/WritableIdentityCredential.cpp
index 89f7f35..52cd496 100644
--- a/identity/aidl/default/WritableIdentityCredential.cpp
+++ b/identity/aidl/default/WritableIdentityCredential.cpp
@@ -44,6 +44,8 @@
         return false;
     }
     storageKey_ = random.value();
+    startPersonalizationCalled_ = false;
+    firstEntry_ = true;
 
     return true;
 }
@@ -105,6 +107,12 @@
 
 ndk::ScopedAStatus WritableIdentityCredential::startPersonalization(
         int32_t accessControlProfileCount, const vector<int32_t>& entryCounts) {
+    if (startPersonalizationCalled_) {
+        return ndk::ScopedAStatus(AStatus_fromServiceSpecificErrorWithMessage(
+                IIdentityCredentialStore::STATUS_FAILED, "startPersonalization called already"));
+    }
+
+    startPersonalizationCalled_ = true;
     numAccessControlProfileRemaining_ = accessControlProfileCount;
     remainingEntryCounts_ = entryCounts;
     entryNameSpace_ = "";
@@ -128,6 +136,19 @@
                 "numAccessControlProfileRemaining_ is 0 and expected non-zero"));
     }
 
+    if (accessControlProfileIds_.find(id) != accessControlProfileIds_.end()) {
+        return ndk::ScopedAStatus(AStatus_fromServiceSpecificErrorWithMessage(
+                IIdentityCredentialStore::STATUS_INVALID_DATA,
+                "Access Control Profile id must be unique"));
+    }
+    accessControlProfileIds_.insert(id);
+
+    if (id < 0 || id >= 32) {
+        return ndk::ScopedAStatus(AStatus_fromServiceSpecificErrorWithMessage(
+                IIdentityCredentialStore::STATUS_INVALID_DATA,
+                "Access Control Profile id must be non-negative and less than 32"));
+    }
+
     // Spec requires if |userAuthenticationRequired| is false, then |timeoutMillis| must also
     // be zero.
     if (!userAuthenticationRequired && timeoutMillis != 0) {
@@ -184,12 +205,20 @@
     }
 
     // Handle initial beginEntry() call.
-    if (entryNameSpace_ == "") {
+    if (firstEntry_) {
+        firstEntry_ = false;
         entryNameSpace_ = nameSpace;
+        allNameSpaces_.insert(nameSpace);
     }
 
     // If the namespace changed...
     if (nameSpace != entryNameSpace_) {
+        if (allNameSpaces_.find(nameSpace) != allNameSpaces_.end()) {
+            return ndk::ScopedAStatus(AStatus_fromServiceSpecificErrorWithMessage(
+                    IIdentityCredentialStore::STATUS_INVALID_DATA,
+                    "Name space cannot be added in interleaving fashion"));
+        }
+
         // Then check that all entries in the previous namespace have been added..
         if (remainingEntryCounts_[0] != 0) {
             return ndk::ScopedAStatus(AStatus_fromServiceSpecificErrorWithMessage(
@@ -197,6 +226,8 @@
                     "New namespace but a non-zero number of entries remain to be added"));
         }
         remainingEntryCounts_.erase(remainingEntryCounts_.begin());
+        remainingEntryCounts_[0] -= 1;
+        allNameSpaces_.insert(nameSpace);
 
         if (signedDataCurrentNamespace_.size() > 0) {
             signedDataNamespaces_.add(entryNameSpace_, std::move(signedDataCurrentNamespace_));
@@ -330,6 +361,18 @@
 
 ndk::ScopedAStatus WritableIdentityCredential::finishAddingEntries(
         vector<int8_t>* outCredentialData, vector<int8_t>* outProofOfProvisioningSignature) {
+    if (numAccessControlProfileRemaining_ != 0) {
+        return ndk::ScopedAStatus(AStatus_fromServiceSpecificErrorWithMessage(
+                IIdentityCredentialStore::STATUS_INVALID_DATA,
+                "numAccessControlProfileRemaining_ is not 0 and expected zero"));
+    }
+
+    if (remainingEntryCounts_.size() > 1 || remainingEntryCounts_[0] != 0) {
+        return ndk::ScopedAStatus(AStatus_fromServiceSpecificErrorWithMessage(
+                IIdentityCredentialStore::STATUS_INVALID_DATA,
+                "More entry spaces remain than startPersonalization configured"));
+    }
+
     if (signedDataCurrentNamespace_.size() > 0) {
         signedDataNamespaces_.add(entryNameSpace_, std::move(signedDataCurrentNamespace_));
     }
diff --git a/identity/aidl/default/WritableIdentityCredential.h b/identity/aidl/default/WritableIdentityCredential.h
index b182862..cb91f7b 100644
--- a/identity/aidl/default/WritableIdentityCredential.h
+++ b/identity/aidl/default/WritableIdentityCredential.h
@@ -21,9 +21,11 @@
 #include <android/hardware/identity/support/IdentityCredentialSupport.h>
 
 #include <cppbor.h>
+#include <set>
 
 namespace aidl::android::hardware::identity {
 
+using ::std::set;
 using ::std::string;
 using ::std::vector;
 
@@ -66,6 +68,8 @@
 
     // This is set in initialize().
     vector<uint8_t> storageKey_;
+    bool startPersonalizationCalled_;
+    bool firstEntry_;
 
     // These are set in getAttestationCertificate().
     vector<uint8_t> credentialPrivKey_;
@@ -79,6 +83,9 @@
     cppbor::Map signedDataNamespaces_;
     cppbor::Array signedDataCurrentNamespace_;
 
+    // This field is initialized in addAccessControlProfile
+    set<int32_t> accessControlProfileIds_;
+
     // These fields are initialized during beginAddEntry()
     size_t entryRemainingBytes_;
     vector<uint8_t> entryAdditionalData_;
@@ -86,6 +93,7 @@
     string entryName_;
     vector<int32_t> entryAccessControlProfileIds_;
     vector<uint8_t> entryBytes_;
+    set<string> allNameSpaces_;
 };
 
 }  // namespace aidl::android::hardware::identity
diff --git a/identity/aidl/vts/Android.bp b/identity/aidl/vts/Android.bp
index ef8beb4..e4780bf 100644
--- a/identity/aidl/vts/Android.bp
+++ b/identity/aidl/vts/Android.bp
@@ -4,7 +4,11 @@
         "VtsHalTargetTestDefaults",
         "use_libaidlvintf_gtest_helper_static",
     ],
-    srcs: ["VtsHalIdentityTargetTest.cpp"],
+    srcs: [
+        "VtsHalIdentityEndToEndTest.cpp",
+        "VtsIWritableIdentityCredentialTests.cpp",
+        "VtsIdentityTestUtils.cpp",
+    ],
     shared_libs: [
         "libbinder",
         "libcrypto",
diff --git a/identity/aidl/vts/VtsHalIdentityTargetTest.cpp b/identity/aidl/vts/VtsHalIdentityEndToEndTest.cpp
similarity index 69%
rename from identity/aidl/vts/VtsHalIdentityTargetTest.cpp
rename to identity/aidl/vts/VtsHalIdentityEndToEndTest.cpp
index ea37fdc..8a4e8a7 100644
--- a/identity/aidl/vts/VtsHalIdentityTargetTest.cpp
+++ b/identity/aidl/vts/VtsHalIdentityEndToEndTest.cpp
@@ -28,8 +28,11 @@
 #include <future>
 #include <map>
 
+#include "VtsIdentityTestUtils.h"
+
 namespace android::hardware::identity {
 
+using std::endl;
 using std::map;
 using std::optional;
 using std::string;
@@ -41,51 +44,6 @@
 
 using ::android::hardware::keymaster::HardwareAuthToken;
 
-// ---------------------------------------------------------------------------
-// Test Data.
-// ---------------------------------------------------------------------------
-
-struct TestEntryData {
-    TestEntryData(string nameSpace, string name, vector<int32_t> profileIds)
-        : nameSpace(nameSpace), name(name), profileIds(profileIds) {}
-
-    TestEntryData(string nameSpace, string name, const string& value, vector<int32_t> profileIds)
-        : TestEntryData(nameSpace, name, profileIds) {
-        valueCbor = cppbor::Tstr(((const char*)value.data())).encode();
-    }
-    TestEntryData(string nameSpace, string name, const vector<uint8_t>& value,
-                  vector<int32_t> profileIds)
-        : TestEntryData(nameSpace, name, profileIds) {
-        valueCbor = cppbor::Bstr(value).encode();
-    }
-    TestEntryData(string nameSpace, string name, bool value, vector<int32_t> profileIds)
-        : TestEntryData(nameSpace, name, profileIds) {
-        valueCbor = cppbor::Bool(value).encode();
-    }
-    TestEntryData(string nameSpace, string name, int64_t value, vector<int32_t> profileIds)
-        : TestEntryData(nameSpace, name, profileIds) {
-        if (value >= 0) {
-            valueCbor = cppbor::Uint(value).encode();
-        } else {
-            valueCbor = cppbor::Nint(-value).encode();
-        }
-    }
-
-    string nameSpace;
-    string name;
-    vector<uint8_t> valueCbor;
-    vector<int32_t> profileIds;
-};
-
-struct TestProfile {
-    uint16_t id;
-    vector<uint8_t> readerCertificate;
-    bool userAuthenticationRequired;
-    uint64_t timeoutMillis;
-};
-
-// ----------------------------------------------------------------
-
 class IdentityAidl : public testing::TestWithParam<std::string> {
   public:
     virtual void SetUp() override {
@@ -108,39 +66,26 @@
 TEST_P(IdentityAidl, createAndRetrieveCredential) {
     // First, generate a key-pair for the reader since its public key will be
     // part of the request data.
-    optional<vector<uint8_t>> readerKeyPKCS8 = support::createEcKeyPair();
-    ASSERT_TRUE(readerKeyPKCS8);
-    optional<vector<uint8_t>> readerPublicKey =
-            support::ecKeyPairGetPublicKey(readerKeyPKCS8.value());
-    optional<vector<uint8_t>> readerKey = support::ecKeyPairGetPrivateKey(readerKeyPKCS8.value());
-    string serialDecimal = "1234";
-    string issuer = "Android Open Source Project";
-    string subject = "Android IdentityCredential VTS Test";
-    time_t validityNotBefore = time(nullptr);
-    time_t validityNotAfter = validityNotBefore + 365 * 24 * 3600;
-    optional<vector<uint8_t>> readerCertificate = support::ecPublicKeyGenerateCertificate(
-            readerPublicKey.value(), readerKey.value(), serialDecimal, issuer, subject,
-            validityNotBefore, validityNotAfter);
+    vector<uint8_t> readerKey;
+    optional<vector<uint8_t>> readerCertificate =
+            test_utils::GenerateReaderCertificate("1234", readerKey);
     ASSERT_TRUE(readerCertificate);
 
     // Make the portrait image really big (just shy of 256 KiB) to ensure that
     // the chunking code gets exercised.
     vector<uint8_t> portraitImage;
-    portraitImage.resize(256 * 1024 - 10);
-    for (size_t n = 0; n < portraitImage.size(); n++) {
-        portraitImage[n] = (uint8_t)n;
-    }
+    test_utils::SetImageData(portraitImage);
 
     // Access control profiles:
-    const vector<TestProfile> testProfiles = {// Profile 0 (reader authentication)
-                                              {0, readerCertificate.value(), false, 0},
-                                              // Profile 1 (no authentication)
-                                              {1, {}, false, 0}};
+    const vector<test_utils::TestProfile> testProfiles = {// Profile 0 (reader authentication)
+                                                          {0, readerCertificate.value(), false, 0},
+                                                          // Profile 1 (no authentication)
+                                                          {1, {}, false, 0}};
 
     HardwareAuthToken authToken;
 
     // Here's the actual test data:
-    const vector<TestEntryData> testEntries = {
+    const vector<test_utils::TestEntryData> testEntries = {
             {"PersonalData", "Last name", string("Turing"), vector<int32_t>{0, 1}},
             {"PersonalData", "Birth date", string("19120623"), vector<int32_t>{0, 1}},
             {"PersonalData", "First name", string("Alan"), vector<int32_t>{0, 1}},
@@ -155,67 +100,33 @@
 
     string cborPretty;
     sp<IWritableIdentityCredential> writableCredential;
-    string docType = "org.iso.18013-5.2019.mdl";
-    bool testCredential = true;
-    ASSERT_TRUE(credentialStore_->createCredential(docType, testCredential, &writableCredential)
-                        .isOk());
-    ASSERT_NE(writableCredential, nullptr);
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
 
     string challenge = "attestationChallenge";
+    test_utils::AttestationData attData(writableCredential, challenge, {});
+    ASSERT_TRUE(attData.result.isOk())
+            << attData.result.exceptionCode() << "; " << attData.result.exceptionMessage() << endl;
+    ASSERT_EQ(binder::Status::EX_NONE, attData.result.exceptionCode());
+    ASSERT_EQ(IIdentityCredentialStore::STATUS_OK, attData.result.serviceSpecificErrorCode());
+
     // TODO: set it to something random and check it's in the cert chain
-    vector<uint8_t> attestationApplicationId = {};
-    vector<uint8_t> attestationChallenge(challenge.begin(), challenge.end());
-    vector<Certificate> attestationCertificates;
-    ASSERT_TRUE(writableCredential
-                        ->getAttestationCertificate(attestationApplicationId, attestationChallenge,
-                                                    &attestationCertificates)
-                        .isOk());
-    ASSERT_GE(attestationCertificates.size(), 2);
+    ASSERT_GE(attData.attestationCertificate.size(), 2);
 
     ASSERT_TRUE(
             writableCredential->startPersonalization(testProfiles.size(), testEntriesEntryCounts)
                     .isOk());
 
-    vector<SecureAccessControlProfile> returnedSecureProfiles;
-    for (const auto& testProfile : testProfiles) {
-        SecureAccessControlProfile profile;
-        Certificate cert;
-        cert.encodedCertificate = testProfile.readerCertificate;
-        ASSERT_TRUE(writableCredential
-                            ->addAccessControlProfile(testProfile.id, cert,
-                                                      testProfile.userAuthenticationRequired,
-                                                      testProfile.timeoutMillis,
-                                                      0,  // secureUserId
-                                                      &profile)
-                            .isOk());
-        ASSERT_EQ(testProfile.id, profile.id);
-        ASSERT_EQ(testProfile.readerCertificate, profile.readerCertificate.encodedCertificate);
-        ASSERT_EQ(testProfile.userAuthenticationRequired, profile.userAuthenticationRequired);
-        ASSERT_EQ(testProfile.timeoutMillis, profile.timeoutMillis);
-        ASSERT_EQ(support::kAesGcmTagSize + support::kAesGcmIvSize, profile.mac.size());
-        returnedSecureProfiles.push_back(profile);
-    }
+    optional<vector<SecureAccessControlProfile>> secureProfiles =
+            test_utils::AddAccessControlProfiles(writableCredential, testProfiles);
+    ASSERT_TRUE(secureProfiles);
 
     // Uses TestEntryData* pointer as key and values are the encrypted blobs. This
     // is a little hacky but it works well enough.
-    map<const TestEntryData*, vector<vector<uint8_t>>> encryptedBlobs;
+    map<const test_utils::TestEntryData*, vector<vector<uint8_t>>> encryptedBlobs;
 
     for (const auto& entry : testEntries) {
-        vector<vector<uint8_t>> chunks =
-                support::chunkVector(entry.valueCbor, hwInfo.dataChunkSize);
-
-        ASSERT_TRUE(writableCredential
-                            ->beginAddEntry(entry.profileIds, entry.nameSpace, entry.name,
-                                            entry.valueCbor.size())
-                            .isOk());
-
-        vector<vector<uint8_t>> encryptedChunks;
-        for (const auto& chunk : chunks) {
-            vector<uint8_t> encryptedChunk;
-            ASSERT_TRUE(writableCredential->addEntryValue(chunk, &encryptedChunk).isOk());
-            encryptedChunks.push_back(encryptedChunk);
-        }
-        encryptedBlobs[&entry] = encryptedChunks;
+        ASSERT_TRUE(test_utils::AddEntry(writableCredential, entry, hwInfo.dataChunkSize,
+                                         encryptedBlobs, true));
     }
 
     vector<uint8_t> credentialData;
@@ -276,8 +187,8 @@
             "]",
             cborPretty);
 
-    optional<vector<uint8_t>> credentialPubKey =
-            support::certificateChainGetTopMostKey(attestationCertificates[0].encodedCertificate);
+    optional<vector<uint8_t>> credentialPubKey = support::certificateChainGetTopMostKey(
+            attData.attestationCertificate[0].encodedCertificate);
     ASSERT_TRUE(credentialPubKey);
     EXPECT_TRUE(support::coseCheckEcDsaSignature(proofOfProvisioningSignature,
                                                  {},  // Additional data
@@ -347,8 +258,8 @@
                                          .add(cppbor::Semantic(24, itemsRequestBytes))
                                          .encode();
     optional<vector<uint8_t>> readerSignature =
-            support::coseSignEcDsa(readerKey.value(), {},  // content
-                                   dataToSign,             // detached content
+            support::coseSignEcDsa(readerKey, {},  // content
+                                   dataToSign,     // detached content
                                    readerCertificate.value());
     ASSERT_TRUE(readerSignature);
 
@@ -358,7 +269,7 @@
     ASSERT_TRUE(credential->generateSigningKeyPair(&signingKeyBlob, &signingKeyCertificate).isOk());
 
     ASSERT_TRUE(credential
-                        ->startRetrieval(returnedSecureProfiles, authToken, itemsRequestBytes,
+                        ->startRetrieval(secureProfiles.value(), authToken, itemsRequestBytes,
                                          signingKeyBlob, sessionTranscriptBytes,
                                          readerSignature.value(), testEntriesEntryCounts)
                         .isOk());
@@ -405,6 +316,8 @@
     cppbor::Array deviceAuthentication;
     deviceAuthentication.add("DeviceAuthentication");
     deviceAuthentication.add(sessionTranscript.clone());
+
+    string docType = "org.iso.18013-5.2019.mdl";
     deviceAuthentication.add(docType);
     deviceAuthentication.add(cppbor::Semantic(24, deviceNameSpacesBytes));
     vector<uint8_t> encodedDeviceAuthentication = deviceAuthentication.encode();
diff --git a/identity/aidl/vts/VtsIWritableIdentityCredentialTests.cpp b/identity/aidl/vts/VtsIWritableIdentityCredentialTests.cpp
new file mode 100644
index 0000000..b68fbb5
--- /dev/null
+++ b/identity/aidl/vts/VtsIWritableIdentityCredentialTests.cpp
@@ -0,0 +1,683 @@
+/*
+ * 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.
+ */
+
+#define LOG_TAG "VtsIWritableIdentityCredentialTests"
+
+#include <aidl/Gtest.h>
+#include <aidl/Vintf.h>
+#include <android-base/logging.h>
+#include <android/hardware/identity/IIdentityCredentialStore.h>
+#include <android/hardware/identity/support/IdentityCredentialSupport.h>
+#include <binder/IServiceManager.h>
+#include <binder/ProcessState.h>
+#include <cppbor.h>
+#include <cppbor_parse.h>
+#include <gtest/gtest.h>
+#include <future>
+#include <map>
+
+#include "VtsIdentityTestUtils.h"
+
+namespace android::hardware::identity {
+
+using std::endl;
+using std::map;
+using std::optional;
+using std::string;
+using std::vector;
+
+using ::android::sp;
+using ::android::String16;
+using ::android::binder::Status;
+
+class IdentityCredentialTests : public testing::TestWithParam<string> {
+  public:
+    virtual void SetUp() override {
+        credentialStore_ = android::waitForDeclaredService<IIdentityCredentialStore>(
+                String16(GetParam().c_str()));
+        ASSERT_NE(credentialStore_, nullptr);
+    }
+
+    sp<IIdentityCredentialStore> credentialStore_;
+};
+
+TEST_P(IdentityCredentialTests, verifyAttestationWithEmptyChallenge) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    vector<uint8_t> attestationChallenge;
+    vector<Certificate> attestationCertificate;
+    vector<uint8_t> attestationApplicationId = {};
+    result = writableCredential->getAttestationCertificate(
+            attestationApplicationId, attestationChallenge, &attestationCertificate);
+
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    EXPECT_TRUE(test_utils::ValidateAttestationCertificate(attestationCertificate));
+}
+
+TEST_P(IdentityCredentialTests, verifyAttestationSuccessWithChallenge) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    string challenge = "NotSoRandomChallenge1NotSoRandomChallenge1NotSoRandomChallenge1";
+    vector<uint8_t> attestationChallenge(challenge.begin(), challenge.end());
+    vector<Certificate> attestationCertificate;
+    vector<uint8_t> attestationApplicationId = {};
+
+    result = writableCredential->getAttestationCertificate(
+            attestationApplicationId, attestationChallenge, &attestationCertificate);
+
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    EXPECT_TRUE(test_utils::ValidateAttestationCertificate(attestationCertificate));
+}
+
+TEST_P(IdentityCredentialTests, verifyAttestationDoubleCallFails) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    string challenge = "NotSoRandomChallenge1";
+    test_utils::AttestationData attData(writableCredential, challenge, {});
+    ASSERT_TRUE(test_utils::ValidateAttestationCertificate(attData.attestationCertificate));
+
+    string challenge2 = "NotSoRandomChallenge2";
+    test_utils::AttestationData attData2(writableCredential, challenge2, {});
+    EXPECT_FALSE(attData2.result.isOk()) << attData2.result.exceptionCode() << "; "
+                                         << attData2.result.exceptionMessage() << endl;
+    EXPECT_EQ(binder::Status::EX_SERVICE_SPECIFIC, attData2.result.exceptionCode());
+    EXPECT_EQ(IIdentityCredentialStore::STATUS_FAILED, attData2.result.serviceSpecificErrorCode());
+}
+
+TEST_P(IdentityCredentialTests, verifyStartPersonalization) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    // First call should go through
+    const vector<int32_t> entryCounts = {2, 4};
+    result = writableCredential->startPersonalization(5, entryCounts);
+    ASSERT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    // Call personalization again to check if repeat call is allowed.
+    result = writableCredential->startPersonalization(7, entryCounts);
+
+    // Second call to startPersonalization should have failed.
+    EXPECT_FALSE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                                << endl;
+    EXPECT_EQ(binder::Status::EX_SERVICE_SPECIFIC, result.exceptionCode());
+    EXPECT_EQ(IIdentityCredentialStore::STATUS_FAILED, result.serviceSpecificErrorCode());
+}
+
+TEST_P(IdentityCredentialTests, verifyStartPersonalizationMin) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    // Verify minimal number of profile count and entry count
+    const vector<int32_t> entryCounts = {1, 1};
+    writableCredential->startPersonalization(1, entryCounts);
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+}
+
+TEST_P(IdentityCredentialTests, verifyStartPersonalizationZero) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    const vector<int32_t> entryCounts = {0};
+    writableCredential->startPersonalization(0, entryCounts);
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+}
+
+TEST_P(IdentityCredentialTests, verifyStartPersonalizationOne) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    // Verify minimal number of profile count and entry count
+    const vector<int32_t> entryCounts = {1};
+    writableCredential->startPersonalization(1, entryCounts);
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+}
+
+TEST_P(IdentityCredentialTests, verifyStartPersonalizationLarge) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    // Verify set a large number of profile count and entry count is ok
+    const vector<int32_t> entryCounts = {3000};
+    writableCredential->startPersonalization(3500, entryCounts);
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+}
+
+TEST_P(IdentityCredentialTests, verifyProfileNumberMismatchShouldFail) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    // Enter mismatched entry and profile numbers
+    const vector<int32_t> entryCounts = {5, 6};
+    writableCredential->startPersonalization(5, entryCounts);
+    ASSERT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    optional<vector<uint8_t>> readerCertificate = test_utils::GenerateReaderCertificate("12345");
+    ASSERT_TRUE(readerCertificate);
+
+    const vector<test_utils::TestProfile> testProfiles = {// Profile 0 (reader authentication)
+                                                          {1, readerCertificate.value(), false, 0},
+                                                          {2, readerCertificate.value(), true, 1},
+                                                          // Profile 4 (no authentication)
+                                                          {4, {}, false, 0}};
+
+    optional<vector<SecureAccessControlProfile>> secureProfiles =
+            test_utils::AddAccessControlProfiles(writableCredential, testProfiles);
+    ASSERT_TRUE(secureProfiles);
+
+    vector<uint8_t> credentialData;
+    vector<uint8_t> proofOfProvisioningSignature;
+    result =
+            writableCredential->finishAddingEntries(&credentialData, &proofOfProvisioningSignature);
+
+    // finishAddingEntries should fail because the number of addAccessControlProfile mismatched with
+    // startPersonalization, and begintest_utils::AddEntry was not called.
+    EXPECT_FALSE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                                << endl;
+    EXPECT_EQ(binder::Status::EX_SERVICE_SPECIFIC, result.exceptionCode());
+    EXPECT_EQ(IIdentityCredentialStore::STATUS_INVALID_DATA, result.serviceSpecificErrorCode());
+}
+
+TEST_P(IdentityCredentialTests, verifyDuplicateProfileId) {
+    Status result;
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    const vector<int32_t> entryCounts = {3, 6};
+    writableCredential->startPersonalization(3, entryCounts);
+    ASSERT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    const vector<test_utils::TestProfile> testProfiles = {// first profile should go though
+                                                          {1, {}, true, 2},
+                                                          // same id, different
+                                                          // authentication requirement
+                                                          {1, {}, true, 1},
+                                                          // same id, different certificate
+                                                          {1, {}, false, 0}};
+
+    bool expectOk = true;
+    for (const auto& testProfile : testProfiles) {
+        SecureAccessControlProfile profile;
+        Certificate cert;
+        cert.encodedCertificate = testProfile.readerCertificate;
+        result = writableCredential->addAccessControlProfile(
+                testProfile.id, cert, testProfile.userAuthenticationRequired,
+                testProfile.timeoutMillis, 0, &profile);
+
+        if (expectOk) {
+            expectOk = false;
+            // for profile should be allowed though as there are no duplications
+            // yet.
+            ASSERT_TRUE(result.isOk())
+                    << result.exceptionCode() << "; " << result.exceptionMessage()
+                    << "test profile id = " << testProfile.id << endl;
+
+            ASSERT_EQ(testProfile.id, profile.id);
+            ASSERT_EQ(testProfile.readerCertificate, profile.readerCertificate.encodedCertificate);
+            ASSERT_EQ(testProfile.userAuthenticationRequired, profile.userAuthenticationRequired);
+            ASSERT_EQ(testProfile.timeoutMillis, profile.timeoutMillis);
+            ASSERT_EQ(support::kAesGcmTagSize + support::kAesGcmIvSize, profile.mac.size());
+        } else {
+            // should not allow duplicate id profiles.
+            ASSERT_FALSE(result.isOk())
+                    << result.exceptionCode() << "; " << result.exceptionMessage()
+                    << ". Test profile id = " << testProfile.id
+                    << ", timeout=" << testProfile.timeoutMillis << endl;
+            ASSERT_EQ(binder::Status::EX_SERVICE_SPECIFIC, result.exceptionCode());
+            ASSERT_EQ(IIdentityCredentialStore::STATUS_INVALID_DATA,
+                      result.serviceSpecificErrorCode());
+        }
+    }
+}
+
+TEST_P(IdentityCredentialTests, verifyOneProfileAndEntryPass) {
+    Status result;
+
+    HardwareInformation hwInfo;
+    ASSERT_TRUE(credentialStore_->getHardwareInformation(&hwInfo).isOk());
+
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    string challenge = "NotSoRandomChallenge1";
+    test_utils::AttestationData attData(writableCredential, challenge, {});
+    EXPECT_TRUE(attData.result.isOk())
+            << attData.result.exceptionCode() << "; " << attData.result.exceptionMessage() << endl;
+
+    const vector<int32_t> entryCounts = {1u};
+    writableCredential->startPersonalization(1, entryCounts);
+    ASSERT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    optional<vector<uint8_t>> readerCertificate1 = test_utils::GenerateReaderCertificate("123456");
+    ASSERT_TRUE(readerCertificate1);
+
+    const vector<test_utils::TestProfile> testProfiles = {{1, readerCertificate1.value(), true, 1}};
+
+    optional<vector<SecureAccessControlProfile>> secureProfiles =
+            test_utils::AddAccessControlProfiles(writableCredential, testProfiles);
+    ASSERT_TRUE(secureProfiles);
+
+    const vector<test_utils::TestEntryData> testEntries1 = {
+            {"Name Space", "Last name", string("Turing"), vector<int32_t>{0, 1}},
+    };
+
+    map<const test_utils::TestEntryData*, vector<vector<uint8_t>>> encryptedBlobs;
+    for (const auto& entry : testEntries1) {
+        ASSERT_TRUE(test_utils::AddEntry(writableCredential, entry, hwInfo.dataChunkSize,
+                                         encryptedBlobs, true));
+    }
+
+    vector<uint8_t> credentialData;
+    vector<uint8_t> proofOfProvisioningSignature;
+    result =
+            writableCredential->finishAddingEntries(&credentialData, &proofOfProvisioningSignature);
+
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    optional<vector<uint8_t>> proofOfProvisioning =
+            support::coseSignGetPayload(proofOfProvisioningSignature);
+    ASSERT_TRUE(proofOfProvisioning);
+    string cborPretty =
+            support::cborPrettyPrint(proofOfProvisioning.value(), 32, {"readerCertificate"});
+    EXPECT_EQ(
+            "[\n"
+            "  'ProofOfProvisioning',\n"
+            "  'org.iso.18013-5.2019.mdl',\n"
+            "  [\n"
+            "    {\n"
+            "      'id' : 1,\n"
+            "      'readerCertificate' : <not printed>,\n"
+            "      'userAuthenticationRequired' : true,\n"
+            "      'timeoutMillis' : 1,\n"
+            "    },\n"
+            "  ],\n"
+            "  {\n"
+            "    'Name Space' : [\n"
+            "      {\n"
+            "        'name' : 'Last name',\n"
+            "        'value' : 'Turing',\n"
+            "        'accessControlProfiles' : [0, 1, ],\n"
+            "      },\n"
+            "    ],\n"
+            "  },\n"
+            "  true,\n"
+            "]",
+            cborPretty);
+
+    optional<vector<uint8_t>> credentialPubKey = support::certificateChainGetTopMostKey(
+            attData.attestationCertificate[0].encodedCertificate);
+    ASSERT_TRUE(credentialPubKey);
+    EXPECT_TRUE(support::coseCheckEcDsaSignature(proofOfProvisioningSignature,
+                                                 {},  // Additional data
+                                                 credentialPubKey.value()));
+}
+
+TEST_P(IdentityCredentialTests, verifyManyProfilesAndEntriesPass) {
+    Status result;
+
+    HardwareInformation hwInfo;
+    ASSERT_TRUE(credentialStore_->getHardwareInformation(&hwInfo).isOk());
+
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    string challenge = "NotSoRandomChallenge";
+    test_utils::AttestationData attData(writableCredential, challenge, {});
+    EXPECT_TRUE(attData.result.isOk())
+            << attData.result.exceptionCode() << "; " << attData.result.exceptionMessage() << endl;
+
+    optional<vector<uint8_t>> readerCertificate1 = test_utils::GenerateReaderCertificate("123456");
+    ASSERT_TRUE(readerCertificate1);
+
+    optional<vector<uint8_t>> readerCertificate2 = test_utils::GenerateReaderCertificate("1256");
+    ASSERT_TRUE(readerCertificate2);
+
+    const vector<test_utils::TestProfile> testProfiles = {
+            {1, readerCertificate1.value(), true, 1},
+            {2, readerCertificate2.value(), true, 2},
+    };
+    const vector<int32_t> entryCounts = {1u, 3u, 1u, 1u, 2u};
+    writableCredential->startPersonalization(testProfiles.size(), entryCounts);
+    ASSERT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    optional<vector<SecureAccessControlProfile>> secureProfiles =
+            test_utils::AddAccessControlProfiles(writableCredential, testProfiles);
+    ASSERT_TRUE(secureProfiles);
+
+    vector<uint8_t> portraitImage1;
+    test_utils::SetImageData(portraitImage1);
+
+    vector<uint8_t> portraitImage2;
+    test_utils::SetImageData(portraitImage2);
+
+    const vector<test_utils::TestEntryData> testEntries1 = {
+            {"Name Space 1", "Last name", string("Turing"), vector<int32_t>{1, 2}},
+            {"Name Space2", "Home address", string("Maida Vale, London, England"),
+             vector<int32_t>{1}},
+            {"Name Space2", "Work address", string("Maida Vale2, London, England"),
+             vector<int32_t>{2}},
+            {"Name Space2", "Trailer address", string("Maida, London, England"),
+             vector<int32_t>{1}},
+            {"Image", "Portrait image", portraitImage1, vector<int32_t>{1}},
+            {"Image2", "Work image", portraitImage2, vector<int32_t>{1, 2}},
+            {"Name Space3", "xyzw", string("random stuff"), vector<int32_t>{1, 2}},
+            {"Name Space3", "Something", string("Some string"), vector<int32_t>{2}},
+    };
+
+    map<const test_utils::TestEntryData*, vector<vector<uint8_t>>> encryptedBlobs;
+    for (const auto& entry : testEntries1) {
+        EXPECT_TRUE(test_utils::AddEntry(writableCredential, entry, hwInfo.dataChunkSize,
+                                         encryptedBlobs, true));
+    }
+
+    vector<uint8_t> credentialData;
+    vector<uint8_t> proofOfProvisioningSignature;
+    result =
+            writableCredential->finishAddingEntries(&credentialData, &proofOfProvisioningSignature);
+
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    optional<vector<uint8_t>> proofOfProvisioning =
+            support::coseSignGetPayload(proofOfProvisioningSignature);
+    ASSERT_TRUE(proofOfProvisioning);
+    string cborPretty = support::cborPrettyPrint(proofOfProvisioning.value(),
+                                                 32,  //
+                                                 {"readerCertificate"});
+    EXPECT_EQ(
+            "[\n"
+            "  'ProofOfProvisioning',\n"
+            "  'org.iso.18013-5.2019.mdl',\n"
+            "  [\n"
+            "    {\n"
+            "      'id' : 1,\n"
+            "      'readerCertificate' : <not printed>,\n"
+            "      'userAuthenticationRequired' : true,\n"
+            "      'timeoutMillis' : 1,\n"
+            "    },\n"
+            "    {\n"
+            "      'id' : 2,\n"
+            "      'readerCertificate' : <not printed>,\n"
+            "      'userAuthenticationRequired' : true,\n"
+            "      'timeoutMillis' : 2,\n"
+            "    },\n"
+            "  ],\n"
+            "  {\n"
+            "    'Name Space 1' : [\n"
+            "      {\n"
+            "        'name' : 'Last name',\n"
+            "        'value' : 'Turing',\n"
+            "        'accessControlProfiles' : [1, 2, ],\n"
+            "      },\n"
+            "    ],\n"
+            "    'Name Space2' : [\n"
+            "      {\n"
+            "        'name' : 'Home address',\n"
+            "        'value' : 'Maida Vale, London, England',\n"
+            "        'accessControlProfiles' : [1, ],\n"
+            "      },\n"
+            "      {\n"
+            "        'name' : 'Work address',\n"
+            "        'value' : 'Maida Vale2, London, England',\n"
+            "        'accessControlProfiles' : [2, ],\n"
+            "      },\n"
+            "      {\n"
+            "        'name' : 'Trailer address',\n"
+            "        'value' : 'Maida, London, England',\n"
+            "        'accessControlProfiles' : [1, ],\n"
+            "      },\n"
+            "    ],\n"
+            "    'Image' : [\n"
+            "      {\n"
+            "        'name' : 'Portrait image',\n"
+            "        'value' : <bstr size=262134 sha1=941e372f654d86c32d88fae9e41b706afbfd02bb>,\n"
+            "        'accessControlProfiles' : [1, ],\n"
+            "      },\n"
+            "    ],\n"
+            "    'Image2' : [\n"
+            "      {\n"
+            "        'name' : 'Work image',\n"
+            "        'value' : <bstr size=262134 sha1=941e372f654d86c32d88fae9e41b706afbfd02bb>,\n"
+            "        'accessControlProfiles' : [1, 2, ],\n"
+            "      },\n"
+            "    ],\n"
+            "    'Name Space3' : [\n"
+            "      {\n"
+            "        'name' : 'xyzw',\n"
+            "        'value' : 'random stuff',\n"
+            "        'accessControlProfiles' : [1, 2, ],\n"
+            "      },\n"
+            "      {\n"
+            "        'name' : 'Something',\n"
+            "        'value' : 'Some string',\n"
+            "        'accessControlProfiles' : [2, ],\n"
+            "      },\n"
+            "    ],\n"
+            "  },\n"
+            "  true,\n"
+            "]",
+            cborPretty);
+
+    optional<vector<uint8_t>> credentialPubKey = support::certificateChainGetTopMostKey(
+            attData.attestationCertificate[0].encodedCertificate);
+    ASSERT_TRUE(credentialPubKey);
+    EXPECT_TRUE(support::coseCheckEcDsaSignature(proofOfProvisioningSignature,
+                                                 {},  // Additional data
+                                                 credentialPubKey.value()));
+}
+
+TEST_P(IdentityCredentialTests, verifyEmptyNameSpaceMixedWithNonEmptyWorks) {
+    Status result;
+
+    HardwareInformation hwInfo;
+    ASSERT_TRUE(credentialStore_->getHardwareInformation(&hwInfo).isOk());
+
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    string challenge = "NotSoRandomChallenge";
+    test_utils::AttestationData attData(writableCredential, challenge, {});
+    ASSERT_TRUE(attData.result.isOk())
+            << attData.result.exceptionCode() << "; " << attData.result.exceptionMessage() << endl;
+
+    const vector<int32_t> entryCounts = {2u, 2u};
+    writableCredential->startPersonalization(3, entryCounts);
+    ASSERT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    optional<vector<uint8_t>> readerCertificate1 = test_utils::GenerateReaderCertificate("123456");
+    ASSERT_TRUE(readerCertificate1);
+
+    optional<vector<uint8_t>> readerCertificate2 =
+            test_utils::GenerateReaderCertificate("123456987987987987987987");
+    ASSERT_TRUE(readerCertificate2);
+
+    const vector<test_utils::TestProfile> testProfiles = {{0, readerCertificate1.value(), false, 0},
+                                                          {1, readerCertificate2.value(), true, 1},
+                                                          {2, {}, false, 0}};
+
+    optional<vector<SecureAccessControlProfile>> secureProfiles =
+            test_utils::AddAccessControlProfiles(writableCredential, testProfiles);
+    ASSERT_TRUE(secureProfiles);
+
+    const vector<test_utils::TestEntryData> testEntries1 = {
+            // test empty name space
+            {"", "t name", string("Turing"), vector<int32_t>{2}},
+            {"", "Birth", string("19120623"), vector<int32_t>{2}},
+            {"Name Space", "Last name", string("Turing"), vector<int32_t>{0, 1}},
+            {"Name Space", "Birth date", string("19120623"), vector<int32_t>{0, 1}},
+    };
+
+    map<const test_utils::TestEntryData*, vector<vector<uint8_t>>> encryptedBlobs;
+    for (const auto& entry : testEntries1) {
+        EXPECT_TRUE(test_utils::AddEntry(writableCredential, entry, hwInfo.dataChunkSize,
+                                         encryptedBlobs, true));
+    }
+
+    vector<uint8_t> credentialData;
+    vector<uint8_t> proofOfProvisioningSignature;
+    result =
+            writableCredential->finishAddingEntries(&credentialData, &proofOfProvisioningSignature);
+
+    EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+}
+
+TEST_P(IdentityCredentialTests, verifyInterleavingEntryNameSpaceOrderingFails) {
+    Status result;
+
+    HardwareInformation hwInfo;
+    ASSERT_TRUE(credentialStore_->getHardwareInformation(&hwInfo).isOk());
+
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    string challenge = "NotSoRandomChallenge";
+    test_utils::AttestationData attData(writableCredential, challenge, {});
+    ASSERT_TRUE(attData.result.isOk())
+            << attData.result.exceptionCode() << "; " << attData.result.exceptionMessage() << endl;
+
+    // Enter mismatched entry and profile numbers.
+    // Technically the 2nd name space of "Name Space" occurs intermittently, 2
+    // before "Image" and 2 after image, which is not correct.  All of same name
+    // space should occur together.  Let's see if this fails.
+    const vector<int32_t> entryCounts = {2u, 1u, 2u};
+    writableCredential->startPersonalization(3, entryCounts);
+    ASSERT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    optional<vector<uint8_t>> readerCertificate1 = test_utils::GenerateReaderCertificate("123456");
+    ASSERT_TRUE(readerCertificate1);
+
+    optional<vector<uint8_t>> readerCertificate2 =
+            test_utils::GenerateReaderCertificate("123456987987987987987987");
+    ASSERT_TRUE(readerCertificate2);
+
+    const vector<test_utils::TestProfile> testProfiles = {{0, readerCertificate1.value(), false, 0},
+                                                          {1, readerCertificate2.value(), true, 1},
+                                                          {2, {}, false, 0}};
+
+    optional<vector<SecureAccessControlProfile>> secureProfiles =
+            test_utils::AddAccessControlProfiles(writableCredential, testProfiles);
+    ASSERT_TRUE(secureProfiles);
+
+    const vector<test_utils::TestEntryData> testEntries1 = {
+            // test empty name space
+            {"Name Space", "Last name", string("Turing"), vector<int32_t>{0, 1}},
+            {"Name Space", "Birth date", string("19120623"), vector<int32_t>{0, 1}},
+    };
+
+    map<const test_utils::TestEntryData*, vector<vector<uint8_t>>> encryptedBlobs;
+    for (const auto& entry : testEntries1) {
+        EXPECT_TRUE(test_utils::AddEntry(writableCredential, entry, hwInfo.dataChunkSize,
+                                         encryptedBlobs, true));
+    }
+    const test_utils::TestEntryData testEntry2 = {"Image", "Portrait image", string("asdfs"),
+                                                  vector<int32_t>{0, 1}};
+
+    EXPECT_TRUE(test_utils::AddEntry(writableCredential, testEntry2, hwInfo.dataChunkSize,
+                                     encryptedBlobs, true));
+
+    // We expect this to fail because the namespace is out of order, all "Name Space"
+    // should have been called together
+    const vector<test_utils::TestEntryData> testEntries3 = {
+            {"Name Space", "First name", string("Alan"), vector<int32_t>{0, 1}},
+            {"Name Space", "Home address", string("Maida Vale, London, England"),
+             vector<int32_t>{0}},
+    };
+
+    for (const auto& entry : testEntries3) {
+        EXPECT_FALSE(test_utils::AddEntry(writableCredential, entry, hwInfo.dataChunkSize,
+                                          encryptedBlobs, false));
+    }
+
+    vector<uint8_t> credentialData;
+    vector<uint8_t> proofOfProvisioningSignature;
+    result =
+            writableCredential->finishAddingEntries(&credentialData, &proofOfProvisioningSignature);
+
+    // should fail because test_utils::AddEntry should have failed earlier.
+    EXPECT_FALSE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                                << endl;
+    EXPECT_EQ(binder::Status::EX_SERVICE_SPECIFIC, result.exceptionCode());
+    EXPECT_EQ(IIdentityCredentialStore::STATUS_INVALID_DATA, result.serviceSpecificErrorCode());
+}
+
+TEST_P(IdentityCredentialTests, verifyAccessControlProfileIdOutOfRange) {
+    sp<IWritableIdentityCredential> writableCredential;
+    ASSERT_TRUE(test_utils::SetupWritableCredential(writableCredential, credentialStore_));
+
+    const vector<int32_t> entryCounts = {1};
+    Status result = writableCredential->startPersonalization(1, entryCounts);
+    ASSERT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                               << endl;
+
+    SecureAccessControlProfile profile;
+
+    // This should fail because the id is >= 32
+    result = writableCredential->addAccessControlProfile(32,     // id
+                                                         {},     // readerCertificate
+                                                         false,  // userAuthenticationRequired
+                                                         0,      // timeoutMillis
+                                                         42,     // secureUserId
+                                                         &profile);
+    ASSERT_FALSE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage();
+    ASSERT_EQ(binder::Status::EX_SERVICE_SPECIFIC, result.exceptionCode());
+    ASSERT_EQ(IIdentityCredentialStore::STATUS_INVALID_DATA, result.serviceSpecificErrorCode());
+
+    // This should fail because the id is < 0
+    result = writableCredential->addAccessControlProfile(-1,     // id
+                                                         {},     // readerCertificate
+                                                         false,  // userAuthenticationRequired
+                                                         0,      // timeoutMillis
+                                                         42,     // secureUserId
+                                                         &profile);
+    ASSERT_FALSE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage();
+    ASSERT_EQ(binder::Status::EX_SERVICE_SPECIFIC, result.exceptionCode());
+    ASSERT_EQ(IIdentityCredentialStore::STATUS_INVALID_DATA, result.serviceSpecificErrorCode());
+}
+
+INSTANTIATE_TEST_SUITE_P(
+        Identity, IdentityCredentialTests,
+        testing::ValuesIn(android::getAidlHalInstanceNames(IIdentityCredentialStore::descriptor)),
+        android::PrintInstanceNameToString);
+
+}  // namespace android::hardware::identity
diff --git a/identity/aidl/vts/VtsIdentityTestUtils.cpp b/identity/aidl/vts/VtsIdentityTestUtils.cpp
new file mode 100644
index 0000000..3aeebc6
--- /dev/null
+++ b/identity/aidl/vts/VtsIdentityTestUtils.cpp
@@ -0,0 +1,179 @@
+/*
+ * Copyright 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 "VtsIdentityTestUtils.h"
+
+#include <aidl/Gtest.h>
+#include <map>
+
+namespace android::hardware::identity::test_utils {
+
+using std::endl;
+using std::map;
+using std::optional;
+using std::string;
+using std::vector;
+
+using ::android::sp;
+using ::android::String16;
+using ::android::binder::Status;
+
+bool SetupWritableCredential(sp<IWritableIdentityCredential>& writableCredential,
+                             sp<IIdentityCredentialStore>& credentialStore) {
+    if (credentialStore == nullptr) {
+        return false;
+    }
+
+    string docType = "org.iso.18013-5.2019.mdl";
+    bool testCredential = true;
+    Status result = credentialStore->createCredential(docType, testCredential, &writableCredential);
+
+    if (result.isOk() && writableCredential != nullptr) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+optional<vector<uint8_t>> GenerateReaderCertificate(string serialDecimal) {
+    vector<uint8_t> privKey;
+    return GenerateReaderCertificate(serialDecimal, privKey);
+}
+
+optional<vector<uint8_t>> GenerateReaderCertificate(string serialDecimal,
+                                                    vector<uint8_t>& readerPrivateKey) {
+    optional<vector<uint8_t>> readerKeyPKCS8 = support::createEcKeyPair();
+    if (!readerKeyPKCS8) {
+        return {};
+    }
+
+    optional<vector<uint8_t>> readerPublicKey =
+            support::ecKeyPairGetPublicKey(readerKeyPKCS8.value());
+    optional<vector<uint8_t>> readerKey = support::ecKeyPairGetPrivateKey(readerKeyPKCS8.value());
+    if (!readerPublicKey || !readerKey) {
+        return {};
+    }
+
+    readerPrivateKey = readerKey.value();
+
+    string issuer = "Android Open Source Project";
+    string subject = "Android IdentityCredential VTS Test";
+    time_t validityNotBefore = time(nullptr);
+    time_t validityNotAfter = validityNotBefore + 365 * 24 * 3600;
+
+    return support::ecPublicKeyGenerateCertificate(readerPublicKey.value(), readerKey.value(),
+                                                   serialDecimal, issuer, subject,
+                                                   validityNotBefore, validityNotAfter);
+}
+
+optional<vector<SecureAccessControlProfile>> AddAccessControlProfiles(
+        sp<IWritableIdentityCredential>& writableCredential,
+        const vector<TestProfile>& testProfiles) {
+    Status result;
+
+    vector<SecureAccessControlProfile> secureProfiles;
+
+    for (const auto& testProfile : testProfiles) {
+        SecureAccessControlProfile profile;
+        Certificate cert;
+        cert.encodedCertificate = testProfile.readerCertificate;
+        result = writableCredential->addAccessControlProfile(
+                testProfile.id, cert, testProfile.userAuthenticationRequired,
+                testProfile.timeoutMillis, 0, &profile);
+
+        // Don't use assert so all errors can be outputed.  Then return
+        // instead of exit even on errors so caller can decide.
+        EXPECT_TRUE(result.isOk()) << result.exceptionCode() << "; " << result.exceptionMessage()
+                                   << "test profile id = " << testProfile.id << endl;
+        EXPECT_EQ(testProfile.id, profile.id);
+        EXPECT_EQ(testProfile.readerCertificate, profile.readerCertificate.encodedCertificate);
+        EXPECT_EQ(testProfile.userAuthenticationRequired, profile.userAuthenticationRequired);
+        EXPECT_EQ(testProfile.timeoutMillis, profile.timeoutMillis);
+        EXPECT_EQ(support::kAesGcmTagSize + support::kAesGcmIvSize, profile.mac.size());
+
+        if (!result.isOk() || testProfile.id != profile.id ||
+            testProfile.readerCertificate != profile.readerCertificate.encodedCertificate ||
+            testProfile.userAuthenticationRequired != profile.userAuthenticationRequired ||
+            testProfile.timeoutMillis != profile.timeoutMillis ||
+            support::kAesGcmTagSize + support::kAesGcmIvSize != profile.mac.size()) {
+            return {};
+        }
+
+        secureProfiles.push_back(profile);
+    }
+
+    return secureProfiles;
+}
+
+// Most test expects this function to pass. So we will print out additional
+// value if failed so more debug data can be provided.
+bool AddEntry(sp<IWritableIdentityCredential>& writableCredential, const TestEntryData& entry,
+              int dataChunkSize, map<const TestEntryData*, vector<vector<uint8_t>>>& encryptedBlobs,
+              bool expectSuccess) {
+    Status result;
+    vector<vector<uint8_t>> chunks = support::chunkVector(entry.valueCbor, dataChunkSize);
+
+    result = writableCredential->beginAddEntry(entry.profileIds, entry.nameSpace, entry.name,
+                                               entry.valueCbor.size());
+
+    if (expectSuccess) {
+        EXPECT_TRUE(result.isOk())
+                << result.exceptionCode() << "; " << result.exceptionMessage() << endl
+                << "entry name = " << entry.name << ", name space=" << entry.nameSpace << endl;
+    }
+
+    if (!result.isOk()) {
+        return false;
+    }
+
+    vector<vector<uint8_t>> encryptedChunks;
+    for (const auto& chunk : chunks) {
+        vector<uint8_t> encryptedContent;
+        result = writableCredential->addEntryValue(chunk, &encryptedContent);
+        if (expectSuccess) {
+            EXPECT_TRUE(result.isOk())
+                    << result.exceptionCode() << "; " << result.exceptionMessage() << endl
+                    << "entry name = " << entry.name << ", name space = " << entry.nameSpace
+                    << endl;
+
+            EXPECT_GT(encryptedContent.size(), 0u) << "entry name = " << entry.name
+                                                   << ", name space = " << entry.nameSpace << endl;
+        }
+
+        if (!result.isOk() || encryptedContent.size() <= 0u) {
+            return false;
+        }
+
+        encryptedChunks.push_back(encryptedContent);
+    }
+
+    encryptedBlobs[&entry] = encryptedChunks;
+    return true;
+}
+
+bool ValidateAttestationCertificate(vector<Certificate>& inputCertificates) {
+    return (inputCertificates.size() >= 2);
+    // TODO: add parsing of the certificate and make sure it is genuine.
+}
+
+void SetImageData(vector<uint8_t>& image) {
+    image.resize(256 * 1024 - 10);
+    for (size_t n = 0; n < image.size(); n++) {
+        image[n] = (uint8_t)n;
+    }
+}
+
+}  // namespace android::hardware::identity::test_utils
diff --git a/identity/aidl/vts/VtsIdentityTestUtils.h b/identity/aidl/vts/VtsIdentityTestUtils.h
new file mode 100644
index 0000000..043ccd6
--- /dev/null
+++ b/identity/aidl/vts/VtsIdentityTestUtils.h
@@ -0,0 +1,118 @@
+/*
+ * Copyright 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.
+ */
+
+#ifndef VTS_IDENTITY_TEST_UTILS_H
+#define VTS_IDENTITY_TEST_UTILS_H
+
+#include <android/hardware/identity/IIdentityCredentialStore.h>
+#include <android/hardware/identity/support/IdentityCredentialSupport.h>
+#include <cppbor.h>
+#include <cppbor_parse.h>
+
+namespace android::hardware::identity::test_utils {
+
+using ::std::map;
+using ::std::optional;
+using ::std::string;
+using ::std::vector;
+
+using ::android::sp;
+using ::android::binder::Status;
+
+struct AttestationData {
+    AttestationData(sp<IWritableIdentityCredential>& writableCredential, string challenge,
+                    vector<uint8_t> applicationId)
+        : attestationApplicationId(applicationId) {
+        // ASSERT_NE(writableCredential, nullptr);
+
+        if (!challenge.empty()) {
+            attestationChallenge.assign(challenge.begin(), challenge.end());
+        }
+
+        result = writableCredential->getAttestationCertificate(
+                attestationApplicationId, attestationChallenge, &attestationCertificate);
+    }
+
+    AttestationData() {}
+
+    vector<uint8_t> attestationChallenge;
+    vector<uint8_t> attestationApplicationId;
+    vector<Certificate> attestationCertificate;
+    Status result;
+};
+
+struct TestEntryData {
+    TestEntryData(string nameSpace, string name, vector<int32_t> profileIds)
+        : nameSpace(nameSpace), name(name), profileIds(profileIds) {}
+
+    TestEntryData(string nameSpace, string name, const string& value, vector<int32_t> profileIds)
+        : TestEntryData(nameSpace, name, profileIds) {
+        valueCbor = cppbor::Tstr(((const char*)value.data())).encode();
+    }
+    TestEntryData(string nameSpace, string name, const vector<uint8_t>& value,
+                  vector<int32_t> profileIds)
+        : TestEntryData(nameSpace, name, profileIds) {
+        valueCbor = cppbor::Bstr(value).encode();
+    }
+    TestEntryData(string nameSpace, string name, bool value, vector<int32_t> profileIds)
+        : TestEntryData(nameSpace, name, profileIds) {
+        valueCbor = cppbor::Bool(value).encode();
+    }
+    TestEntryData(string nameSpace, string name, int64_t value, vector<int32_t> profileIds)
+        : TestEntryData(nameSpace, name, profileIds) {
+        if (value >= 0) {
+            valueCbor = cppbor::Uint(value).encode();
+        } else {
+            valueCbor = cppbor::Nint(-value).encode();
+        }
+    }
+
+    string nameSpace;
+    string name;
+    vector<uint8_t> valueCbor;
+    vector<int32_t> profileIds;
+};
+
+struct TestProfile {
+    uint16_t id;
+    vector<uint8_t> readerCertificate;
+    bool userAuthenticationRequired;
+    uint64_t timeoutMillis;
+};
+
+bool SetupWritableCredential(sp<IWritableIdentityCredential>& writableCredential,
+                             sp<IIdentityCredentialStore>& credentialStore);
+
+optional<vector<uint8_t>> GenerateReaderCertificate(string serialDecimal);
+
+optional<vector<uint8_t>> GenerateReaderCertificate(string serialDecimal,
+                                                    vector<uint8_t>& readerPrivateKey);
+
+optional<vector<SecureAccessControlProfile>> AddAccessControlProfiles(
+        sp<IWritableIdentityCredential>& writableCredential,
+        const vector<TestProfile>& testProfiles);
+
+bool AddEntry(sp<IWritableIdentityCredential>& writableCredential, const TestEntryData& entry,
+              int dataChunkSize, map<const TestEntryData*, vector<vector<uint8_t>>>& encryptedBlobs,
+              bool expectSuccess);
+
+bool ValidateAttestationCertificate(vector<Certificate>& inputCertificates);
+
+void SetImageData(vector<uint8_t>& image);
+
+}  // namespace android::hardware::identity::test_utils
+
+#endif  // VTS_IDENTITY_TEST_UTILS_H
diff --git a/identity/support/src/IdentityCredentialSupport.cpp b/identity/support/src/IdentityCredentialSupport.cpp
index bf6a5c3..dc49ddc 100644
--- a/identity/support/src/IdentityCredentialSupport.cpp
+++ b/identity/support/src/IdentityCredentialSupport.cpp
@@ -958,12 +958,17 @@
 optional<vector<uint8_t>> createEcKeyPair() {
     auto ec_key = EC_KEY_Ptr(EC_KEY_new());
     auto pkey = EVP_PKEY_Ptr(EVP_PKEY_new());
-    auto group = EC_GROUP_Ptr(EC_GROUP_new_by_curve_name(NID_X9_62_prime256v1));
     if (ec_key.get() == nullptr || pkey.get() == nullptr) {
         LOG(ERROR) << "Memory allocation failed";
         return {};
     }
 
+    auto group = EC_GROUP_Ptr(EC_GROUP_new_by_curve_name(NID_X9_62_prime256v1));
+    if (group.get() == nullptr) {
+        LOG(ERROR) << "Error creating EC group by curve name";
+        return {};
+    }
+
     if (EC_KEY_set_group(ec_key.get(), group.get()) != 1 ||
         EC_KEY_generate_key(ec_key.get()) != 1 || EC_KEY_check_key(ec_key.get()) < 0) {
         LOG(ERROR) << "Error generating key";
diff --git a/media/omx/1.0/vts/functional/master/Android.bp b/media/omx/1.0/vts/functional/master/Android.bp
index 8e58821..5953eb5 100644
--- a/media/omx/1.0/vts/functional/master/Android.bp
+++ b/media/omx/1.0/vts/functional/master/Android.bp
@@ -19,6 +19,7 @@
     defaults: ["VtsHalMediaOmxV1_0Defaults"],
     srcs: ["VtsHalMediaOmxV1_0TargetMasterTest.cpp"],
     test_suites: [
+        "general-tests",
         "vts",
     ],
 }
diff --git a/media/omx/1.0/vts/functional/master/VtsHalMediaOmxV1_0TargetMasterTest.cpp b/media/omx/1.0/vts/functional/master/VtsHalMediaOmxV1_0TargetMasterTest.cpp
index c14f1da..9b4722e 100644
--- a/media/omx/1.0/vts/functional/master/VtsHalMediaOmxV1_0TargetMasterTest.cpp
+++ b/media/omx/1.0/vts/functional/master/VtsHalMediaOmxV1_0TargetMasterTest.cpp
@@ -20,6 +20,7 @@
 #endif
 
 #include <android-base/logging.h>
+#include <android-base/strings.h>
 
 #include <android/hardware/media/omx/1.0/IOmx.h>
 #include <android/hardware/media/omx/1.0/IOmxNode.h>
@@ -33,21 +34,22 @@
 #include <hidl/GtestPrinter.h>
 #include <hidl/ServiceManagement.h>
 
-using ::android::hardware::media::omx::V1_0::IOmx;
-using ::android::hardware::media::omx::V1_0::IOmxObserver;
-using ::android::hardware::media::omx::V1_0::IOmxNode;
-using ::android::hardware::media::omx::V1_0::IOmxStore;
-using ::android::hardware::media::omx::V1_0::Message;
-using ::android::hardware::media::omx::V1_0::CodecBuffer;
-using ::android::hardware::media::omx::V1_0::PortMode;
-using ::android::hidl::allocator::V1_0::IAllocator;
-using ::android::hidl::memory::V1_0::IMemory;
-using ::android::hidl::memory::V1_0::IMapper;
+using ::android::sp;
+using ::android::base::Join;
+using ::android::hardware::hidl_string;
+using ::android::hardware::hidl_vec;
 using ::android::hardware::Return;
 using ::android::hardware::Void;
-using ::android::hardware::hidl_vec;
-using ::android::hardware::hidl_string;
-using ::android::sp;
+using ::android::hardware::media::omx::V1_0::CodecBuffer;
+using ::android::hardware::media::omx::V1_0::IOmx;
+using ::android::hardware::media::omx::V1_0::IOmxNode;
+using ::android::hardware::media::omx::V1_0::IOmxObserver;
+using ::android::hardware::media::omx::V1_0::IOmxStore;
+using ::android::hardware::media::omx::V1_0::Message;
+using ::android::hardware::media::omx::V1_0::PortMode;
+using ::android::hidl::allocator::V1_0::IAllocator;
+using ::android::hidl::memory::V1_0::IMapper;
+using ::android::hidl::memory::V1_0::IMemory;
 
 #include <getopt.h>
 #include <media_hidl_test_common.h>
@@ -70,6 +72,11 @@
     }
 };
 
+struct AttributePattern {
+    const testing::internal::RE key;
+    const testing::internal::RE value;
+};
+
 void displayComponentInfo(hidl_vec<IOmx::ComponentInfo>& nodeList) {
     for (size_t i = 0; i < nodeList.size(); i++) {
         printf("%s | ", nodeList[i].mName.c_str());
@@ -80,6 +87,109 @@
     }
 }
 
+/*
+ * Returns the role based on is_encoder and mime.
+ *
+ * The mapping from a pair (is_encoder, mime) to a role string is
+ * defined in frameworks/av/media/libmedia/MediaDefs.cpp and
+ * frameworks/av/media/libstagefright/omx/OMXUtils.cpp. This function
+ * does essentially the same work as GetComponentRole() in
+ * OMXUtils.cpp.
+ *
+ * Args:
+ *   is_encoder: A boolean indicating whether the role is for an
+ *       encoder or a decoder.
+ *   mime: A string of the desired mime type.
+ *
+ * Returns:
+ *   A const string for the requested role name, empty if mime is not
+ *   recognized.
+ */
+const std::string getComponentRole(bool isEncoder, const std::string mime) {
+    // Mapping from mime types to roles.
+    // These values come from MediaDefs.cpp and OMXUtils.cpp
+    const std::map<const std::string, const std::string> audioMimeToRole = {
+            {"3gpp", "amrnb"},         {"ac3", "ac3"},     {"amr-wb", "amrwb"},
+            {"eac3", "eac3"},          {"flac", "flac"},   {"g711-alaw", "g711alaw"},
+            {"g711-mlaw", "g711mlaw"}, {"gsm", "gsm"},     {"mp4a-latm", "aac"},
+            {"mpeg", "mp3"},           {"mpeg-L1", "mp1"}, {"mpeg-L2", "mp2"},
+            {"opus", "opus"},          {"raw", "raw"},     {"vorbis", "vorbis"},
+    };
+    const std::map<const std::string, const std::string> videoMimeToRole = {
+            {"3gpp", "h263"},         {"avc", "avc"},           {"dolby-vision", "dolby-vision"},
+            {"hevc", "hevc"},         {"mp4v-es", "mpeg4"},     {"mpeg2", "mpeg2"},
+            {"x-vnd.on2.vp8", "vp8"}, {"x-vnd.on2.vp9", "vp9"},
+    };
+    const std::map<const std::string, const std::string> imageMimeToRole = {
+            {"vnd.android.heic", "heic"},
+    };
+
+    // Suffix begins after the mime prefix.
+    const size_t prefixEnd = mime.find("/");
+    if (prefixEnd == std::string::npos || prefixEnd == mime.size()) return "";
+    const std::string mime_suffix = mime.substr(prefixEnd + 1, mime.size() - 1);
+    const std::string middle = isEncoder ? "encoder." : "decoder.";
+    std::string prefix;
+    std::string suffix;
+    if (mime.rfind("audio/", 0) != std::string::npos) {
+        const auto it = audioMimeToRole.find(mime_suffix);
+        if (it == audioMimeToRole.end()) return "";
+        prefix = "audio_";
+        suffix = it->second;
+    } else if (mime.rfind("video/", 0) != std::string::npos) {
+        const auto it = videoMimeToRole.find(mime_suffix);
+        if (it == videoMimeToRole.end()) return "";
+        prefix = "video_";
+        suffix = it->second;
+    } else if (mime.rfind("image/", 0) != std::string::npos) {
+        const auto it = imageMimeToRole.find(mime_suffix);
+        if (it == imageMimeToRole.end()) return "";
+        prefix = "image_";
+        suffix = it->second;
+    } else {
+        return "";
+    }
+    return prefix + middle + suffix;
+}
+
+void validateAttributes(
+        const std::map<const std::string, const testing::internal::RE>& knownPatterns,
+        const std::vector<const struct AttributePattern>& unknownPatterns,
+        hidl_vec<IOmxStore::Attribute> attributes) {
+    std::set<const std::string> attributeKeys;
+    for (const auto& attr : attributes) {
+        // Make sure there are no duplicates
+        const auto [nodeIter, inserted] = attributeKeys.insert(attr.key);
+        EXPECT_EQ(inserted, true) << "Attribute \"" << attr.key << "\" has duplicates.";
+
+        // Check the value against the corresponding regular
+        // expression.
+        const auto knownPattern = knownPatterns.find(attr.key);
+        if (knownPattern != knownPatterns.end()) {
+            EXPECT_EQ(testing::internal::RE::FullMatch(attr.value, knownPattern->second), true)
+                    << "Attribute \"" << attr.key << "\" has invalid value \"" << attr.value << ".";
+            ;
+        } else {
+            // Failed to find exact attribute, check against
+            // possible patterns.
+            bool keyFound = false;
+            for (const auto& unknownPattern : unknownPatterns) {
+                if (testing::internal::RE::PartialMatch(attr.key, unknownPattern.key)) {
+                    keyFound = true;
+                    EXPECT_EQ(testing::internal::RE::FullMatch(attr.value, unknownPattern.value),
+                              true)
+                            << "Attribute \"" << attr.key << "\" has invalid value \"" << attr.value
+                            << ".";
+                }
+            }
+            if (!keyFound) {
+                std::cout << "Warning, Unrecognized attribute \"" << attr.key << "\" with value \""
+                          << attr.value << "\"." << std::endl;
+            }
+        }
+    }
+}
+
 // Make sure IOmx and IOmxStore have the same set of instances.
 TEST(MasterHidlTest, instanceMatchValidation) {
     auto omxInstances = android::hardware::getAllHalInstanceNames(IOmx::descriptor);
@@ -91,7 +201,7 @@
     }
 }
 
-// list service attributes
+// list service attributes and verify expected formats
 TEST_P(MasterHidlTest, ListServiceAttr) {
     description("list service attributes");
     android::hardware::media::omx::V1_0::Status status;
@@ -105,7 +215,36 @@
                     })
                     .isOk());
     ASSERT_EQ(status, android::hardware::media::omx::V1_0::Status::OK);
-    if (attributes.size() == 0) ALOGV("Warning, Attribute list empty");
+    if (attributes.size() == 0) {
+        std::cout << "Warning, Attribute list empty" << std::endl;
+    } else {
+        /*
+         * knownPatterns is a map whose keys are the known "key" for a service
+         * attribute pair (see IOmxStore::Attribute), and whose values are the
+         * corresponding regular expressions that will have to match with the
+         * "value" of the attribute pair. If listServiceAttributes() returns an
+         * attribute that has a matching key but an unmatched value, the test
+         * will fail.
+         */
+        const std::map<const std::string, const testing::internal::RE> knownPatterns = {
+                {"max-video-encoder-input-buffers", "0|[1-9][0-9]*"},
+                {"supports-multiple-secure-codecs", "0|1"},
+                {"supports-secure-with-non-secure-codec", "0|1"},
+        };
+        /*
+         * unknownPatterns is a vector of pairs of regular expressions.
+         * For each attribute whose key is not known (i.e., does not match any
+         * of the keys in the "knownPatterns" variable defined above), that key will be
+         * tried for a match with the first element of each pair of the variable
+         * "unknownPatterns". If a match occurs, the value of that same attribute will be
+         * tried for a match with the second element of the pair. If this second
+         * match fails, the test will fail.
+         */
+        const std::vector<const struct AttributePattern> unknownPatterns = {
+                {"supports-[a-z0-9-]*", "0|1"}};
+
+        validateAttributes(knownPatterns, unknownPatterns, attributes);
+    }
 }
 
 // get node prefix
@@ -114,17 +253,183 @@
     hidl_string prefix;
     omxStore->getNodePrefix(
         [&prefix](hidl_string const& _nl) { prefix = _nl; });
-    if (prefix.empty()) ALOGV("Warning, Node Prefix empty");
+    if (prefix.empty()) std::cout << "Warning, Node Prefix empty" << std::endl;
 }
 
-// list roles
+// list roles and validate all RoleInfo objects
 TEST_P(MasterHidlTest, ListRoles) {
     description("list roles");
     hidl_vec<IOmxStore::RoleInfo> roleList;
     omxStore->listRoles([&roleList](hidl_vec<IOmxStore::RoleInfo> const& _nl) {
         roleList = _nl;
     });
-    if (roleList.size() == 0) ALOGV("Warning, RoleInfo list empty");
+    if (roleList.size() == 0) {
+        GTEST_SKIP() << "Warning, RoleInfo list empty";
+        return;
+    }
+
+    // Basic patterns for matching
+    const std::string toggle = "(0|1)";
+    const std::string string = "(.*)";
+    const std::string num = "(0|([1-9][0-9]*))";
+    const std::string size = "(" + num + "x" + num + ")";
+    const std::string ratio = "(" + num + ":" + num + ")";
+    const std::string range_num = "((" + num + "-" + num + ")|" + num + ")";
+    const std::string range_size = "((" + size + "-" + size + ")|" + size + ")";
+    const std::string range_ratio = "((" + ratio + "-" + ratio + ")|" + ratio + ")";
+    const std::string list_range_num = "(" + range_num + "(," + range_num + ")*)";
+
+    // Matching rules for node attributes with fixed keys
+    const std::map<const std::string, const testing::internal::RE> knownPatterns = {
+            {"alignment", size},
+            {"bitrate-range", range_num},
+            {"block-aspect-ratio-range", range_ratio},
+            {"block-count-range", range_num},
+            {"block-size", size},
+            {"blocks-per-second-range", range_num},
+            {"complexity-default", num},
+            {"complexity-range", range_num},
+            {"feature-adaptive-playback", toggle},
+            {"feature-bitrate-control", "(VBR|CBR|CQ)[,(VBR|CBR|CQ)]*"},
+            {"feature-can-swap-width-height", toggle},
+            {"feature-intra-refresh", toggle},
+            {"feature-partial-frame", toggle},
+            {"feature-secure-playback", toggle},
+            {"feature-tunneled-playback", toggle},
+            {"frame-rate-range", range_num},
+            {"max-channel-count", num},
+            {"max-concurrent-instances", num},
+            {"max-supported-instances", num},
+            {"pixel-aspect-ratio-range", range_ratio},
+            {"quality-default", num},
+            {"quality-range", range_num},
+            {"quality-scale", string},
+            {"sample-rate-ranges", list_range_num},
+            {"size-range", range_size},
+    };
+
+    // Strings for matching rules for node attributes with key patterns
+    const std::vector<const struct AttributePattern> unknownPatterns = {
+            {"measured-frame-rate-" + size + "-range", range_num},
+            {"feature-[a-zA-Z0-9_-]+", string},
+    };
+
+    // Matching rules for node names and owners
+    const testing::internal::RE nodeNamePattern = "[a-zA-Z0-9.-]+";
+    const testing::internal::RE nodeOwnerPattern = "[a-zA-Z0-9._-]+";
+
+    std::set<const std::string> roleKeys;
+    std::map<const std::string, std::set<const std::string>> nodeToRoles;
+    std::map<const std::string, std::set<const std::string>> ownerToNodes;
+    for (const IOmxStore::RoleInfo& role : roleList) {
+        // Make sure there are no duplicates
+        const auto [roleIter, inserted] = roleKeys.insert(role.role);
+        EXPECT_EQ(inserted, true) << "Role \"" << role.role << "\" has duplicates.";
+
+        // Make sure role name follows expected format based on type and
+        // isEncoder
+        const std::string role_name = getComponentRole(role.isEncoder, role.type);
+        EXPECT_EQ(role_name, role.role) << "Role \"" << role.role << "\" does not match "
+                                        << (role.isEncoder ? "an encoder " : "a decoder ")
+                                        << "for mime type \"" << role.type << ".";
+
+        // Check the nodes for this role
+        std::set<const std::string> nodeKeys;
+        for (const IOmxStore::NodeInfo& node : role.nodes) {
+            // Make sure there are no duplicates
+            const auto [nodeIter, inserted] = nodeKeys.insert(node.name);
+            EXPECT_EQ(inserted, true) << "Node \"" << node.name << "\" has duplicates.";
+
+            // Check the format of node name
+            EXPECT_EQ(testing::internal::RE::FullMatch(node.name, nodeNamePattern), true)
+                    << "Node name \"" << node.name << " is invalid.";
+            // Check the format of node owner
+            EXPECT_EQ(testing::internal::RE::FullMatch(node.owner, nodeOwnerPattern), true)
+                    << "Node owner \"" << node.owner << " is invalid.";
+
+            validateAttributes(knownPatterns, unknownPatterns, node.attributes);
+
+            ownerToNodes[node.owner].insert(node.name);
+            nodeToRoles[node.name].insert(role.role);
+        }
+    }
+
+    // Verify the information with IOmx::listNodes().
+    // IOmxStore::listRoles() and IOmx::listNodes() should give consistent
+    // information about nodes and roles.
+    for (const auto& [owner, nodes] : ownerToNodes) {
+        // Obtain the IOmx instance for each "owner"
+        const sp<IOmx> omx = omxStore->getOmx(owner);
+        EXPECT_NE(nullptr, omx);
+
+        // Invoke IOmx::listNodes()
+        android::hardware::media::omx::V1_0::Status status;
+        hidl_vec<IOmx::ComponentInfo> nodeList;
+        EXPECT_TRUE(
+                omx->listNodes([&status, &nodeList](android::hardware::media::omx::V1_0::Status _s,
+                                                    hidl_vec<IOmx::ComponentInfo> const& _nl) {
+                       status = _s;
+                       nodeList = _nl;
+                   }).isOk());
+        ASSERT_EQ(status, android::hardware::media::omx::V1_0::Status::OK);
+
+        // Verify that roles for each node match with the information from
+        // IOmxStore::listRoles().
+        std::set<const std::string> nodeKeys;
+        for (IOmx::ComponentInfo node : nodeList) {
+            // Make sure there are no duplicates
+            const auto [nodeIter, inserted] = nodeKeys.insert(node.mName);
+            EXPECT_EQ(inserted, true)
+                    << "IOmx::listNodes() lists duplicate nodes \"" << node.mName << "\".";
+
+            // Skip "hidden" nodes, i.e. those that are not advertised by
+            // IOmxStore::listRoles().
+            if (nodes.find(node.mName) == nodes.end()) {
+                std::cout << "Warning, IOmx::listNodes() lists unknown node \"" << node.mName
+                          << "\" for IOmx instance \"" << owner << "\"." << std::endl;
+                continue;
+            }
+
+            // All the roles advertised by IOmxStore::listRoles() for this
+            // node must be included in roleKeys.
+            std::set<const std::string> difference;
+            std::set_difference(nodeToRoles[node.mName].begin(), nodeToRoles[node.mName].end(),
+                                roleKeys.begin(), roleKeys.end(),
+                                std::inserter(difference, difference.begin()));
+            EXPECT_EQ(difference.empty(), true) << "IOmx::listNodes() for IOmx "
+                                                   "instance \""
+                                                << owner
+                                                << "\" does not report some "
+                                                   "expected nodes: "
+                                                << android::base::Join(difference, ", ") << ".";
+        }
+        // Check that all nodes obtained from IOmxStore::listRoles() are
+        // supported by the their corresponding IOmx instances.
+        std::set<const std::string> difference;
+        std::set_difference(nodes.begin(), nodes.end(), nodeKeys.begin(), nodeKeys.end(),
+                            std::inserter(difference, difference.begin()));
+        EXPECT_EQ(difference.empty(), true) << "IOmx::listNodes() for IOmx "
+                                               "instance \""
+                                            << owner
+                                            << "\" does not report some "
+                                               "expected nodes: "
+                                            << android::base::Join(difference, ", ") << ".";
+    }
+
+    if (!nodeToRoles.empty()) {
+        // Check that the prefix is a sensible string.
+        hidl_string prefix;
+        omxStore->getNodePrefix([&prefix](hidl_string const& _nl) { prefix = _nl; });
+        EXPECT_EQ(testing::internal::RE::PartialMatch(prefix, nodeNamePattern), true)
+                << "\"" << prefix << "\" is not a valid prefix for node names.";
+
+        // Check that all node names have the said prefix.
+        for (const auto& node : nodeToRoles) {
+            EXPECT_NE(node.first.rfind(prefix, 0), std::string::npos)
+                    << "Node \"" << node.first << "\" does not start with prefix \"" << prefix
+                    << "\".";
+        }
+    }
 }
 
 // list components and roles.
@@ -143,7 +448,7 @@
             .isOk());
     ASSERT_EQ(status, android::hardware::media::omx::V1_0::Status::OK);
     if (nodeList.size() == 0)
-        ALOGV("Warning, ComponentInfo list empty");
+        std::cout << "Warning, ComponentInfo list empty" << std::endl;
     else {
         // displayComponentInfo(nodeList);
         for (size_t i = 0; i < nodeList.size(); i++) {
diff --git a/sensors/2.0/multihal/Android.bp b/sensors/2.0/multihal/Android.bp
index 3ce3390..bf51fcd 100644
--- a/sensors/2.0/multihal/Android.bp
+++ b/sensors/2.0/multihal/Android.bp
@@ -25,6 +25,9 @@
     ],
     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 @@
         "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 ef77048..f50ad7e 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/2.1/multihal/Android.bp b/sensors/2.1/multihal/Android.bp
new file mode 100644
index 0000000..6a7cac9
--- /dev/null
+++ b/sensors/2.1/multihal/Android.bp
@@ -0,0 +1,47 @@
+//
+// 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.
+
+cc_binary {
+    name: "android.hardware.sensors@2.1-service.multihal",
+    defaults: [
+        "hidl_defaults",
+    ],
+    vendor: true,
+    relative_install_path: "hw",
+    srcs: [
+        "service.cpp",
+    ],
+    init_rc: ["android.hardware.sensors@2.1-service-multihal.rc"],
+    vintf_fragments: ["android.hardware.sensors@2.1-multihal.xml"],
+    header_libs: [
+        "android.hardware.sensors@2.X-shared-utils",
+    ],
+    shared_libs: [
+        "android.hardware.sensors@2.0",
+        "android.hardware.sensors@2.0-ScopedWakelock",
+        "android.hardware.sensors@2.1",
+        "libbase",
+        "libcutils",
+        "libfmq",
+        "libhidlbase",
+        "liblog",
+        "libpower",
+        "libutils",
+    ],
+    static_libs: [
+        "android.hardware.sensors@1.0-convert",
+        "android.hardware.sensors@2.X-multihal",
+    ],
+}
diff --git a/sensors/2.1/multihal/OWNERS b/sensors/2.1/multihal/OWNERS
new file mode 100644
index 0000000..e955670
--- /dev/null
+++ b/sensors/2.1/multihal/OWNERS
@@ -0,0 +1,3 @@
+arthuri@google.com
+bduddie@google.com
+stange@google.com
\ No newline at end of file
diff --git a/sensors/2.1/multihal/android.hardware.sensors@2.1-multihal.xml b/sensors/2.1/multihal/android.hardware.sensors@2.1-multihal.xml
new file mode 100644
index 0000000..18bd3ae
--- /dev/null
+++ b/sensors/2.1/multihal/android.hardware.sensors@2.1-multihal.xml
@@ -0,0 +1,11 @@
+<manifest version="1.0" type="device">
+    <hal format="hidl">
+        <name>android.hardware.sensors</name>
+        <transport>hwbinder</transport>
+        <version>2.1</version>
+        <interface>
+            <name>ISensors</name>
+            <instance>default</instance>
+        </interface>
+    </hal>
+</manifest>
diff --git a/sensors/2.1/multihal/android.hardware.sensors@2.1-service-multihal.rc b/sensors/2.1/multihal/android.hardware.sensors@2.1-service-multihal.rc
new file mode 100644
index 0000000..fc99ee7
--- /dev/null
+++ b/sensors/2.1/multihal/android.hardware.sensors@2.1-service-multihal.rc
@@ -0,0 +1,7 @@
+service vendor.sensors-hal-2-1-multihal /vendor/bin/hw/android.hardware.sensors@2.1-service.multihal
+    class hal
+    user system
+    group system wakelock context_hub
+    writepid /dev/cpuset/system-background/tasks
+    capabilities BLOCK_SUSPEND
+    rlimit rtprio 10 10
diff --git a/sensors/2.1/multihal/service.cpp b/sensors/2.1/multihal/service.cpp
new file mode 100644
index 0000000..d68d9a3
--- /dev/null
+++ b/sensors/2.1/multihal/service.cpp
@@ -0,0 +1,39 @@
+/*
+ * 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.
+ */
+
+#include <android/hardware/sensors/2.1/ISensors.h>
+#include <hidl/HidlTransportSupport.h>
+#include <log/log.h>
+#include <utils/StrongPointer.h>
+#include "HalProxy.h"
+
+using android::hardware::configureRpcThreadpool;
+using android::hardware::joinRpcThreadpool;
+using android::hardware::sensors::V2_1::ISensors;
+using android::hardware::sensors::V2_1::implementation::HalProxyV2_1;
+
+int main(int /* argc */, char** /* argv */) {
+    configureRpcThreadpool(1, true);
+
+    android::sp<ISensors> halProxy = new HalProxyV2_1();
+    if (halProxy->registerAsService() != ::android::OK) {
+        ALOGE("Failed to register Sensors HAL instance");
+        return -1;
+    }
+
+    joinRpcThreadpool();
+    return 1;  // joinRpcThreadpool shouldn't exit
+}
diff --git a/sensors/common/default/2.X/multihal/Android.bp b/sensors/common/default/2.X/multihal/Android.bp
index 6122323..c80c47a 100644
--- a/sensors/common/default/2.X/multihal/Android.bp
+++ b/sensors/common/default/2.X/multihal/Android.bp
@@ -17,6 +17,7 @@
     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 @@
         "libpower",
         "libutils",
     ],
+    static_libs: [
+        "android.hardware.sensors@1.0-convert",
+    ],
     cflags: ["-DLOG_TAG=\"SensorsMultiHal\""],
 }
 
@@ -62,6 +66,7 @@
     ],
     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 869c033..a09e9e9 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 @@
     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 @@
     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();
 }
 
+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 @@
             ->activate(clearSubHalIndex(sensorHandle), enabled);
 }
 
-Return<Result> HalProxy::initialize(
-        const ::android::hardware::MQDescriptorSync<Event>& eventQueueDescriptor,
+Return<Result> HalProxy::initialize_2_1(
+        const ::android::hardware::MQDescriptorSync<V2_1::Event>& eventQueueDescriptor,
         const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
-        const sp<ISensorsCallback>& sensorsCallback) {
+        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<V1_0::Event>& eventQueueDescriptor,
+        const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
+        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 @@
     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 @@
     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 @@
     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 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 @@
         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<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 @@
     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 @@
             } 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::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::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::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 @@
     }
 }
 
-ISensorsSubHal* HalProxy::getSubHalForSensorHandle(int32_t sensorHandle) {
+std::shared_ptr<ISubHalWrapperBase> HalProxy::getSubHalForSensorHandle(int32_t sensorHandle) {
     return mSubHalList[extractSubHalIndex(sensorHandle)];
 }
 
@@ -651,46 +724,8 @@
     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 0000000..3c1b17c
--- /dev/null
+++ b/sensors/common/default/2.X/multihal/HalProxyCallback.cpp
@@ -0,0 +1,84 @@
+/*
+ * 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"
+
+#include <cinttypes>
+
+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 d7e8795..fb0b806 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<ISensorsCallback>& sensorsCallback) override;
+            const sp<V2_0::ISensorsCallback>& sensorsCallback);
+
+    Return<Result> initializeCommon(
+            std::unique_ptr<EventMessageQueueWrapperBase>& eventQueue,
+            const ::android::hardware::MQDescriptorSync<uint32_t>& wakeLockDescriptor,
+            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 @@
     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 @@
     /**
      * 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 @@
     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 @@
     //! 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 @@
      *    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 @@
      *
      * @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 @@
 };
 
 /**
- * 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;
-
-  public:
-    HalProxyCallback(HalProxy* halProxy, int32_t subHalIndex)
-        : mHalProxy(halProxy), mSubHalIndex(subHalIndex) {}
-
-    Return<void> onDynamicSensorsConnected(
-            const hidl_vec<SensorInfo>& dynamicSensorsAdded) override {
-        return mHalProxy->onDynamicSensorsConnected(dynamicSensorsAdded, mSubHalIndex);
+template <class ISensorsVersion>
+class IHalProxy : public HalProxy, public ISensorsVersion {
+    Return<void> getSensorsList(ISensorsV2_0::getSensorsList_cb _hidl_cb) override {
+        return HalProxy::getSensorsList(_hidl_cb);
     }
 
-    Return<void> onDynamicSensorsDisconnected(
-            const hidl_vec<int32_t>& dynamicSensorHandlesRemoved) override {
-        return mHalProxy->onDynamicSensorsDisconnected(dynamicSensorHandlesRemoved, mSubHalIndex);
+    Return<Result> setOperationMode(OperationMode mode) override {
+        return HalProxy::setOperationMode(mode);
     }
 
-    void postEvents(const std::vector<Event>& events, ScopedWakelock wakelock);
+    Return<Result> activate(int32_t sensorHandle, bool enabled) override {
+        return HalProxy::activate(sensorHandle, enabled);
+    }
 
-    ScopedWakelock createScopedWakelock(bool lock);
+    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);
+    }
 
-  private:
-    HalProxy* mHalProxy;
-    int32_t mSubHalIndex;
+    Return<Result> batch(int32_t sensorHandle, int64_t samplingPeriodNs,
+                         int64_t maxReportLatencyNs) override {
+        return HalProxy::batch(sensorHandle, samplingPeriodNs, maxReportLatencyNs);
+    }
 
-    std::vector<Event> processEvents(const std::vector<Event>& events,
-                                     size_t* numWakeupEvents) const;
+    Return<Result> flush(int32_t sensorHandle) override { return HalProxy::flush(sensorHandle); }
+
+    Return<Result> injectSensorData(const V1_0::Event& event) override {
+        return HalProxy::injectSensorData(event);
+    }
+
+    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);
+    }
+
+    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 0000000..e62b7d1
--- /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 0000000..149bb5e
--- /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 aa6d9db..1cc5cd5 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 @@
     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 7692b51..a15faed 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 @@
     ],
     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 @@
         "libutils",
     ],
     static_libs: [
+        "android.hardware.sensors@1.0-convert",
         "android.hardware.sensors@2.X-multihal",
     ],
     cflags: [
@@ -48,6 +50,7 @@
     vendor: true,
     defaults: ["android.hardware.sensors@2.X-fakesubhal-defaults"],
     cflags: [
+        "-DSUB_HAL_VERSION_2_0",
         "-DSUPPORT_CONTINUOUS_SENSORS",
         "-DSUB_HAL_NAME=\"FakeSubHal-Continuous\"",
     ],
@@ -58,6 +61,17 @@
     vendor: true,
     defaults: ["android.hardware.sensors@2.X-fakesubhal-defaults"],
     cflags: [
+        "-DSUB_HAL_VERSION_2_0",
+        "-DSUPPORT_ON_CHANGE_SENSORS",
+        "-DSUB_HAL_NAME=\"FakeSubHal-OnChange\"",
+    ],
+}
+
+cc_library {
+    name: "android.hardware.sensors@2.X-fakesubhal-config3",
+    vendor: true,
+    defaults: ["android.hardware.sensors@2.X-fakesubhal-defaults"],
+    cflags: [
         "-DSUPPORT_ON_CHANGE_SENSORS",
         "-DSUB_HAL_NAME=\"FakeSubHal-OnChange\"",
     ],
@@ -78,7 +92,11 @@
     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",
@@ -86,7 +104,6 @@
     shared_libs: [
         "android.hardware.sensors@1.0",
         "android.hardware.sensors@2.0",
-        "android.hardware.sensors@2.0-ScopedWakelock",
         "android.hardware.sensors@2.1",
         "libbase",
         "libcutils",
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 867c4a1..858786a 100644
--- a/sensors/common/default/2.X/multihal/tests/HalProxy_test.cpp
+++ b/sensors/common/default/2.X/multihal/tests/HalProxy_test.cpp
@@ -15,12 +15,15 @@
 
 #include <gtest/gtest.h>
 
+#include <android/hardware/sensors/1.0/types.h>
 #include <android/hardware/sensors/2.0/types.h>
+#include <android/hardware/sensors/2.1/types.h>
 #include <fmq/MessageQueue.h>
 
 #include "HalProxy.h"
 #include "SensorsSubHal.h"
 #include "V2_0/ScopedWakelock.h"
+#include "convertV2_1.h"
 
 #include <chrono>
 #include <set>
@@ -38,27 +41,35 @@
 using ::android::hardware::sensors::V1_0::SensorInfo;
 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::subhal::implementation::AddAndRemoveDynamicSensorsSubHal;
-using ::android::hardware::sensors::V2_0::subhal::implementation::AllSensorsSubHal;
-using ::android::hardware::sensors::V2_0::subhal::implementation::
+using ::android::hardware::sensors::V2_0::implementation::HalProxyCallbackBase;
+using ::android::hardware::sensors::V2_0::implementation::ScopedWakelock;
+using ::android::hardware::sensors::V2_1::implementation::convertToNewEvents;
+using ::android::hardware::sensors::V2_1::implementation::convertToNewSensorInfos;
+using ::android::hardware::sensors::V2_1::implementation::HalProxy;
+using ::android::hardware::sensors::V2_1::subhal::implementation::AddAndRemoveDynamicSensorsSubHal;
+using ::android::hardware::sensors::V2_1::subhal::implementation::AllSensorsSubHal;
+using ::android::hardware::sensors::V2_1::subhal::implementation::
         AllSupportDirectChannelSensorsSubHal;
-using ::android::hardware::sensors::V2_0::subhal::implementation::ContinuousSensorsSubHal;
-using ::android::hardware::sensors::V2_0::subhal::implementation::
+using ::android::hardware::sensors::V2_1::subhal::implementation::ContinuousSensorsSubHal;
+using ::android::hardware::sensors::V2_1::subhal::implementation::
         DoesNotSupportDirectChannelSensorsSubHal;
-using ::android::hardware::sensors::V2_0::subhal::implementation::OnChangeSensorsSubHal;
-using ::android::hardware::sensors::V2_0::subhal::implementation::SensorsSubHal;
-using ::android::hardware::sensors::V2_0::subhal::implementation::
+using ::android::hardware::sensors::V2_1::subhal::implementation::OnChangeSensorsSubHal;
+using ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_0;
+using ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_1;
+using ::android::hardware::sensors::V2_1::subhal::implementation::
         SetOperationModeFailingSensorsSubHal;
 
-using EventMessageQueue = MessageQueue<Event, ::android::hardware::kSynchronizedReadWrite>;
+using ISensorsCallbackV2_0 = ::android::hardware::sensors::V2_0::ISensorsCallback;
+using ISensorsCallbackV2_1 = ::android::hardware::sensors::V2_1::ISensorsCallback;
+using EventV1_0 = ::android::hardware::sensors::V1_0::Event;
+using EventV2_1 = ::android::hardware::sensors::V2_1::Event;
+using EventMessageQueueV2_1 = MessageQueue<EventV2_1, ::android::hardware::kSynchronizedReadWrite>;
+using EventMessageQueueV2_0 = MessageQueue<EventV1_0, ::android::hardware::kSynchronizedReadWrite>;
 using WakeupMessageQueue = MessageQueue<uint32_t, ::android::hardware::kSynchronizedReadWrite>;
 
 // The barebones sensors callback class passed into halproxy initialize calls
-class SensorsCallback : public ISensorsCallback {
+class SensorsCallback : public ISensorsCallbackV2_0 {
   public:
     Return<void> onDynamicSensorsConnected(
             const hidl_vec<SensorInfo>& /*dynamicSensorsAdded*/) override {
@@ -73,8 +84,30 @@
     }
 };
 
+class SensorsCallbackV2_1 : public ISensorsCallbackV2_1 {
+  public:
+    Return<void> onDynamicSensorsConnected_2_1(
+            const hidl_vec<::android::hardware::sensors::V2_1::SensorInfo>& /*dynamicSensorsAdded*/)
+            override {
+        // Nothing yet
+        return Return<void>();
+    }
+
+    Return<void> onDynamicSensorsConnected(
+            const hidl_vec<SensorInfo>& /*dynamicSensorsAdded*/) override {
+        // Nothing yet
+        return Return<void>();
+    }
+
+    Return<void> onDynamicSensorsDisconnected(
+            const hidl_vec<int32_t>& /*dynamicSensorHandlesRemoved*/) override {
+        // Nothing yet
+        return Return<void>();
+    }
+};
+
 // The sensors callback that expects a variable list of sensors to be added
-class TestSensorsCallback : public ISensorsCallback {
+class TestSensorsCallback : public ISensorsCallbackV2_0 {
   public:
     Return<void> onDynamicSensorsConnected(
             const hidl_vec<SensorInfo>& dynamicSensorsAdded) override {
@@ -129,10 +162,10 @@
 void ackWakeupEventsToHalProxy(size_t numEvents, std::unique_ptr<WakeupMessageQueue>& wakelockQueue,
                                EventFlag* wakelockQueueFlag);
 
-bool readEventsOutOfQueue(size_t numEvents, std::unique_ptr<EventMessageQueue>& eventQueue,
+bool readEventsOutOfQueue(size_t numEvents, std::unique_ptr<EventMessageQueueV2_0>& eventQueue,
                           EventFlag* eventQueueFlag);
 
-std::unique_ptr<EventMessageQueue> makeEventFMQ(size_t size);
+std::unique_ptr<EventMessageQueueV2_0> makeEventFMQ(size_t size);
 
 std::unique_ptr<WakeupMessageQueue> makeWakelockFMQ(size_t size);
 
@@ -142,7 +175,7 @@
  *
  * @return A proximity event.
  */
-Event makeProximityEvent();
+EventV1_0 makeProximityEvent();
 
 /**
  * Construct and return a HIDL Event type thats sensorHandle refers to a proximity sensor
@@ -150,7 +183,7 @@
  *
  * @return A proximity event.
  */
-Event makeAccelerometerEvent();
+EventV1_0 makeAccelerometerEvent();
 
 /**
  * Make a certain number of proximity type events with the sensorHandle field set to
@@ -160,7 +193,7 @@
  *
  * @return The created list of events.
  */
-std::vector<Event> makeMultipleProximityEvents(size_t numEvents);
+std::vector<EventV1_0> makeMultipleProximityEvents(size_t numEvents);
 
 /**
  * Make a certain number of accelerometer type events with the sensorHandle field set to
@@ -170,7 +203,7 @@
  *
  * @return The created list of events.
  */
-std::vector<Event> makeMultipleAccelerometerEvents(size_t numEvents);
+std::vector<EventV1_0> makeMultipleAccelerometerEvents(size_t numEvents);
 
 /**
  * Given a SensorInfo vector and a sensor handles vector populate 'sensors' with SensorInfo
@@ -188,7 +221,7 @@
 
 // Tests follow
 TEST(HalProxyTest, GetSensorsListOneSubHalTest) {
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> fakeSubHals{&subHal};
     HalProxy proxy(fakeSubHals);
 
@@ -200,8 +233,8 @@
 }
 
 TEST(HalProxyTest, GetSensorsListTwoSubHalTest) {
-    ContinuousSensorsSubHal continuousSubHal;
-    OnChangeSensorsSubHal onChangeSubHal;
+    ContinuousSensorsSubHal<SensorsSubHalV2_0> continuousSubHal;
+    OnChangeSensorsSubHal<SensorsSubHalV2_0> onChangeSubHal;
     std::vector<ISensorsSubHal*> fakeSubHals;
     fakeSubHals.push_back(&continuousSubHal);
     fakeSubHals.push_back(&onChangeSubHal);
@@ -221,8 +254,8 @@
 }
 
 TEST(HalProxyTest, SetOperationModeTwoSubHalSuccessTest) {
-    ContinuousSensorsSubHal subHal1;
-    OnChangeSensorsSubHal subHal2;
+    ContinuousSensorsSubHal<SensorsSubHalV2_0> subHal1;
+    OnChangeSensorsSubHal<SensorsSubHalV2_0> subHal2;
 
     std::vector<ISensorsSubHal*> fakeSubHals{&subHal1, &subHal2};
     HalProxy proxy(fakeSubHals);
@@ -238,7 +271,7 @@
 }
 
 TEST(HalProxyTest, SetOperationModeTwoSubHalFailTest) {
-    AllSensorsSubHal subHal1;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal1;
     SetOperationModeFailingSensorsSubHal subHal2;
 
     std::vector<ISensorsSubHal*> fakeSubHals{&subHal1, &subHal2};
@@ -279,16 +312,16 @@
 
 TEST(HalProxyTest, PostSingleNonWakeupEvent) {
     constexpr size_t kQueueSize = 5;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
-    std::vector<Event> events{makeAccelerometerEvent()};
-    subHal.postEvents(events, false /* wakeup */);
+    std::vector<EventV1_0> events{makeAccelerometerEvent()};
+    subHal.postEvents(convertToNewEvents(events), false /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), 1);
 }
@@ -296,28 +329,28 @@
 TEST(HalProxyTest, PostMultipleNonWakeupEvent) {
     constexpr size_t kQueueSize = 5;
     constexpr size_t kNumEvents = 3;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
-    std::vector<Event> events = makeMultipleAccelerometerEvents(kNumEvents);
-    subHal.postEvents(events, false /* wakeup */);
+    std::vector<EventV1_0> events = makeMultipleAccelerometerEvents(kNumEvents);
+    subHal.postEvents(convertToNewEvents(events), false /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), kNumEvents);
 }
 
 TEST(HalProxyTest, PostSingleWakeupEvent) {
     constexpr size_t kQueueSize = 5;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
     EventFlag* eventQueueFlag;
@@ -326,8 +359,8 @@
     EventFlag* wakelockQueueFlag;
     EventFlag::createEventFlag(wakeLockQueue->getEventFlagWord(), &wakelockQueueFlag);
 
-    std::vector<Event> events{makeProximityEvent()};
-    subHal.postEvents(events, true /* wakeup */);
+    std::vector<EventV1_0> events{makeProximityEvent()};
+    subHal.postEvents(convertToNewEvents(events), true /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), 1);
 
@@ -338,12 +371,12 @@
 TEST(HalProxyTest, PostMultipleWakeupEvents) {
     constexpr size_t kQueueSize = 5;
     constexpr size_t kNumEvents = 3;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
     EventFlag* eventQueueFlag;
@@ -352,8 +385,8 @@
     EventFlag* wakelockQueueFlag;
     EventFlag::createEventFlag(wakeLockQueue->getEventFlagWord(), &wakelockQueueFlag);
 
-    std::vector<Event> events = makeMultipleProximityEvents(kNumEvents);
-    subHal.postEvents(events, true /* wakeup */);
+    std::vector<EventV1_0> events = makeMultipleProximityEvents(kNumEvents);
+    subHal.postEvents(convertToNewEvents(events), true /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), kNumEvents);
 
@@ -364,20 +397,20 @@
 TEST(HalProxyTest, PostEventsMultipleSubhals) {
     constexpr size_t kQueueSize = 5;
     constexpr size_t kNumEvents = 2;
-    AllSensorsSubHal subHal1, subHal2;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal1, subHal2;
     std::vector<ISensorsSubHal*> subHals{&subHal1, &subHal2};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
-    std::vector<Event> events = makeMultipleAccelerometerEvents(kNumEvents);
-    subHal1.postEvents(events, false /* wakeup */);
+    std::vector<EventV1_0> events = makeMultipleAccelerometerEvents(kNumEvents);
+    subHal1.postEvents(convertToNewEvents(events), false /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), kNumEvents);
 
-    subHal2.postEvents(events, false /* wakeup */);
+    subHal2.postEvents(convertToNewEvents(events), false /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), kNumEvents * 2);
 }
@@ -385,19 +418,19 @@
 TEST(HalProxyTest, PostEventsDelayedWrite) {
     constexpr size_t kQueueSize = 5;
     constexpr size_t kNumEvents = 6;
-    AllSensorsSubHal subHal1, subHal2;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal1, subHal2;
     std::vector<ISensorsSubHal*> subHals{&subHal1, &subHal2};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
     EventFlag* eventQueueFlag;
     EventFlag::createEventFlag(eventQueue->getEventFlagWord(), &eventQueueFlag);
 
-    std::vector<Event> events = makeMultipleAccelerometerEvents(kNumEvents);
-    subHal1.postEvents(events, false /* wakeup */);
+    std::vector<EventV1_0> events = makeMultipleAccelerometerEvents(kNumEvents);
+    subHal1.postEvents(convertToNewEvents(events), false /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), kQueueSize);
 
@@ -413,18 +446,20 @@
 TEST(HalProxyTest, PostEventsMultipleSubhalsThreaded) {
     constexpr size_t kQueueSize = 5;
     constexpr size_t kNumEvents = 2;
-    AllSensorsSubHal subHal1, subHal2;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal1, subHal2;
     std::vector<ISensorsSubHal*> subHals{&subHal1, &subHal2};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
-    std::vector<Event> events = makeMultipleAccelerometerEvents(kNumEvents);
+    std::vector<EventV1_0> events = makeMultipleAccelerometerEvents(kNumEvents);
 
-    std::thread t1(&AllSensorsSubHal::postEvents, &subHal1, events, false);
-    std::thread t2(&AllSensorsSubHal::postEvents, &subHal2, events, false);
+    std::thread t1(&AllSensorsSubHal<SensorsSubHalV2_0>::postEvents, &subHal1,
+                   convertToNewEvents(events), false);
+    std::thread t2(&AllSensorsSubHal<SensorsSubHalV2_0>::postEvents, &subHal2,
+                   convertToNewEvents(events), false);
 
     t1.join();
     t2.join();
@@ -435,34 +470,34 @@
 TEST(HalProxyTest, DestructingWithEventsPendingOnBackgroundThread) {
     constexpr size_t kQueueSize = 5;
     constexpr size_t kNumEvents = 6;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
 
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     HalProxy proxy(subHals);
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
-    std::vector<Event> events = makeMultipleAccelerometerEvents(kNumEvents);
-    subHal.postEvents(events, false /* wakeup */);
+    std::vector<EventV1_0> events = makeMultipleAccelerometerEvents(kNumEvents);
+    subHal.postEvents(convertToNewEvents(events), false /* wakeup */);
 
     // Destructing HalProxy object with events on the background thread
 }
 
 TEST(HalProxyTest, DestructingWithUnackedWakeupEventsPosted) {
     constexpr size_t kQueueSize = 5;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
 
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     HalProxy proxy(subHals);
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
-    std::vector<Event> events{makeProximityEvent()};
-    subHal.postEvents(events, true /* wakeup */);
+    std::vector<EventV1_0> events{makeProximityEvent()};
+    subHal.postEvents(convertToNewEvents(events), true /* wakeup */);
 
     // Not sending any acks back through wakeLockQueue
 
@@ -472,17 +507,17 @@
 TEST(HalProxyTest, ReinitializeWithEventsPendingOnBackgroundThread) {
     constexpr size_t kQueueSize = 5;
     constexpr size_t kNumEvents = 10;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
 
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     HalProxy proxy(subHals);
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
-    std::vector<Event> events = makeMultipleAccelerometerEvents(kNumEvents);
-    subHal.postEvents(events, false /* wakeup */);
+    std::vector<EventV1_0> events = makeMultipleAccelerometerEvents(kNumEvents);
+    subHal.postEvents(convertToNewEvents(events), false /* wakeup */);
 
     eventQueue = makeEventFMQ(kQueueSize);
     wakeLockQueue = makeWakelockFMQ(kQueueSize);
@@ -492,23 +527,23 @@
     EXPECT_EQ(secondInitResult, Result::OK);
     // Small sleep so that pending writes thread has a change to hit writeBlocking call.
     std::this_thread::sleep_for(std::chrono::milliseconds(5));
-    Event eventOut;
+    EventV1_0 eventOut;
     EXPECT_FALSE(eventQueue->read(&eventOut));
 }
 
 TEST(HalProxyTest, ReinitializingWithUnackedWakeupEventsPosted) {
     constexpr size_t kQueueSize = 5;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
 
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     HalProxy proxy(subHals);
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
-    std::vector<Event> events{makeProximityEvent()};
-    subHal.postEvents(events, true /* wakeup */);
+    std::vector<EventV1_0> events{makeProximityEvent()};
+    subHal.postEvents(convertToNewEvents(events), true /* wakeup */);
 
     // Not sending any acks back through wakeLockQueue
 
@@ -523,12 +558,12 @@
 TEST(HalProxyTest, InitializeManyTimesInARow) {
     constexpr size_t kQueueSize = 5;
     constexpr size_t kNumTimesToInit = 100;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
 
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     HalProxy proxy(subHals);
 
     for (size_t i = 0; i < kNumTimesToInit; i++) {
@@ -540,15 +575,15 @@
 
 TEST(HalProxyTest, OperationModeResetOnInitialize) {
     constexpr size_t kQueueSize = 5;
-    AllSensorsSubHal subHal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     HalProxy proxy(subHals);
     proxy.setOperationMode(OperationMode::DATA_INJECTION);
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
-    Event event = makeAccelerometerEvent();
+    EventV1_0 event = makeAccelerometerEvent();
     // Should not be able to inject a non AdditionInfo type event because operation mode should
     // have been reset to NORMAL
     EXPECT_EQ(proxy.injectSensorData(event), Result::BAD_VALUE);
@@ -559,7 +594,7 @@
     constexpr size_t kNumSensors = 5;
     AddAndRemoveDynamicSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     HalProxy proxy(subHals);
 
@@ -574,9 +609,9 @@
     }
 
     TestSensorsCallback* callback = new TestSensorsCallback();
-    ::android::sp<ISensorsCallback> callbackPtr = callback;
+    ::android::sp<ISensorsCallbackV2_0> callbackPtr = callback;
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callbackPtr);
-    subHal.addDynamicSensors(sensorsToConnect);
+    subHal.addDynamicSensors(convertToNewSensorInfos(sensorsToConnect));
 
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callbackPtr);
     subHal.removeDynamicSensors(sensorHandlesToAttemptToRemove);
@@ -593,7 +628,7 @@
     AddAndRemoveDynamicSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(0);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(0);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(0);
 
     std::vector<SensorInfo> sensorsToConnect;
@@ -602,9 +637,9 @@
                                                  sensorHandlesToExpect);
 
     TestSensorsCallback* callback = new TestSensorsCallback();
-    ::android::sp<ISensorsCallback> callbackPtr = callback;
+    ::android::sp<ISensorsCallbackV2_0> callbackPtr = callback;
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callbackPtr);
-    subHal.addDynamicSensors(sensorsToConnect);
+    subHal.addDynamicSensors(convertToNewSensorInfos(sensorsToConnect));
 
     std::vector<SensorInfo> sensorsSeen = callback->getSensorsConnected();
     EXPECT_EQ(kNumSensors, sensorsSeen.size());
@@ -621,7 +656,7 @@
     AddAndRemoveDynamicSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(0);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(0);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(0);
 
     std::vector<SensorInfo> sensorsToConnect;
@@ -646,9 +681,9 @@
                                           nonDynamicSensorHandles.end());
 
     TestSensorsCallback* callback = new TestSensorsCallback();
-    ::android::sp<ISensorsCallback> callbackPtr = callback;
+    ::android::sp<ISensorsCallbackV2_0> callbackPtr = callback;
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callbackPtr);
-    subHal.addDynamicSensors(sensorsToConnect);
+    subHal.addDynamicSensors(convertToNewSensorInfos(sensorsToConnect));
     subHal.removeDynamicSensors(sensorHandlesToAttemptToRemove);
 
     std::vector<int32_t> sensorHandlesSeen = callback->getSensorHandlesDisconnected();
@@ -667,15 +702,15 @@
     constexpr size_t kNumSubHals = 3;
     constexpr size_t kQueueSize = 5;
     int32_t kNumSubHalsInt32 = static_cast<int32_t>(kNumSubHals);
-    std::vector<AllSensorsSubHal> subHalObjs(kNumSubHals);
+    std::vector<AllSensorsSubHal<SensorsSubHalV2_0>> subHalObjs(kNumSubHals);
     std::vector<ISensorsSubHal*> subHals;
     for (const auto& subHal : subHalObjs) {
         subHals.push_back((ISensorsSubHal*)(&subHal));
     }
 
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     HalProxy proxy(subHals);
     // Initialize for the injectSensorData call so callback postEvents is valid
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
@@ -687,7 +722,7 @@
     EXPECT_EQ(proxy.activate(0x00000001 | (kNumSubHalsInt32 << 24), true), Result::BAD_VALUE);
     EXPECT_EQ(proxy.batch(0x00000001 | (kNumSubHalsInt32 << 24), 0, 0), Result::BAD_VALUE);
     EXPECT_EQ(proxy.flush(0x00000001 | (kNumSubHalsInt32 << 24)), Result::BAD_VALUE);
-    Event event;
+    EventV1_0 event;
     event.sensorHandle = 0x00000001 | (kNumSubHalsInt32 << 24);
     EXPECT_EQ(proxy.injectSensorData(event), Result::BAD_VALUE);
 }
@@ -696,28 +731,28 @@
     constexpr size_t kQueueSize = 5;
     constexpr int32_t subhal1Index = 0;
     constexpr int32_t subhal2Index = 1;
-    AllSensorsSubHal subhal1;
-    AllSensorsSubHal subhal2;
+    AllSensorsSubHal<SensorsSubHalV2_0> subhal1;
+    AllSensorsSubHal<SensorsSubHalV2_0> subhal2;
     std::vector<ISensorsSubHal*> subHals{&subhal1, &subhal2};
 
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     HalProxy proxy(subHals);
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
     int32_t sensorHandleToPost = 0x00000001;
-    Event eventIn = makeAccelerometerEvent();
+    EventV1_0 eventIn = makeAccelerometerEvent();
     eventIn.sensorHandle = sensorHandleToPost;
-    std::vector<Event> eventsToPost{eventIn};
-    subhal1.postEvents(eventsToPost, false);
+    std::vector<EventV1_0> eventsToPost{eventIn};
+    subhal1.postEvents(convertToNewEvents(eventsToPost), false);
 
-    Event eventOut;
+    EventV1_0 eventOut;
     EXPECT_TRUE(eventQueue->read(&eventOut));
 
     EXPECT_EQ(eventOut.sensorHandle, (subhal1Index << 24) | sensorHandleToPost);
 
-    subhal2.postEvents(eventsToPost, false);
+    subhal2.postEvents(convertToNewEvents(eventsToPost), false);
 
     EXPECT_TRUE(eventQueue->read(&eventOut));
 
@@ -728,22 +763,22 @@
     constexpr size_t kQueueSize = 5;
     // TODO: Make this constant linked to same limit in HalProxy.h
     constexpr size_t kMaxPendingQueueSize = 100000;
-    AllSensorsSubHal subhal;
+    AllSensorsSubHal<SensorsSubHalV2_0> subhal;
     std::vector<ISensorsSubHal*> subHals{&subhal};
 
-    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<EventMessageQueueV2_0> eventQueue = makeEventFMQ(kQueueSize);
     std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
-    ::android::sp<ISensorsCallback> callback = new SensorsCallback();
+    ::android::sp<ISensorsCallbackV2_0> callback = new SensorsCallback();
     EventFlag* eventQueueFlag;
     EventFlag::createEventFlag(eventQueue->getEventFlagWord(), &eventQueueFlag);
     HalProxy proxy(subHals);
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
     // Fill pending queue
-    std::vector<Event> events = makeMultipleAccelerometerEvents(kQueueSize);
-    subhal.postEvents(events, false);
+    std::vector<EventV1_0> events = makeMultipleAccelerometerEvents(kQueueSize);
+    subhal.postEvents(convertToNewEvents(events), false);
     events = makeMultipleAccelerometerEvents(kMaxPendingQueueSize);
-    subhal.postEvents(events, false);
+    subhal.postEvents(convertToNewEvents(events), false);
 
     // Drain pending queue
     for (int i = 0; i < kMaxPendingQueueSize + kQueueSize; i += kQueueSize) {
@@ -752,9 +787,9 @@
 
     // Put one event on pending queue
     events = makeMultipleAccelerometerEvents(kQueueSize);
-    subhal.postEvents(events, false);
+    subhal.postEvents(convertToNewEvents(events), false);
     events = {makeAccelerometerEvent()};
-    subhal.postEvents(events, false);
+    subhal.postEvents(convertToNewEvents(events), false);
 
     // Read out to make room for one event on pending queue to write to FMQ
     ASSERT_TRUE(readEventsOutOfQueue(kQueueSize, eventQueue, eventQueueFlag));
@@ -763,6 +798,35 @@
     EXPECT_TRUE(readEventsOutOfQueue(1, eventQueue, eventQueueFlag));
 }
 
+TEST(HalProxyTest, PostEventsMultipleSubhalsThreadedV2_1) {
+    constexpr size_t kQueueSize = 5;
+    constexpr size_t kNumEvents = 2;
+    AllSensorsSubHal<SensorsSubHalV2_0> subHal1;
+    AllSensorsSubHal<SensorsSubHalV2_1> subHal2;
+    std::vector<::android::hardware::sensors::V2_0::implementation::ISensorsSubHal*> subHalsV2_0{
+            &subHal1};
+    std::vector<::android::hardware::sensors::V2_1::implementation::ISensorsSubHal*> subHalsV2_1{
+            &subHal2};
+    HalProxy proxy(subHalsV2_0, subHalsV2_1);
+    std::unique_ptr<EventMessageQueueV2_1> eventQueue =
+            std::make_unique<EventMessageQueueV2_1>(kQueueSize, true);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
+    ::android::sp<ISensorsCallbackV2_1> callback = new SensorsCallbackV2_1();
+    proxy.initialize_2_1(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
+
+    std::vector<EventV1_0> events = makeMultipleAccelerometerEvents(kNumEvents);
+
+    std::thread t1(&AllSensorsSubHal<SensorsSubHalV2_0>::postEvents, &subHal1,
+                   convertToNewEvents(events), false);
+    std::thread t2(&AllSensorsSubHal<SensorsSubHalV2_1>::postEvents, &subHal2,
+                   convertToNewEvents(events), false);
+
+    t1.join();
+    t2.join();
+
+    EXPECT_EQ(eventQueue->availableToRead(), kNumEvents * 2);
+}
+
 // Helper implementations follow
 void testSensorsListFromProxyAndSubHal(const std::vector<SensorInfo>& proxySensorsList,
                                        const std::vector<SensorInfo>& subHalSensorsList) {
@@ -801,26 +865,26 @@
     wakelockQueueFlag->wake(static_cast<uint32_t>(WakeLockQueueFlagBits::DATA_WRITTEN));
 }
 
-bool readEventsOutOfQueue(size_t numEvents, std::unique_ptr<EventMessageQueue>& eventQueue,
+bool readEventsOutOfQueue(size_t numEvents, std::unique_ptr<EventMessageQueueV2_0>& eventQueue,
                           EventFlag* eventQueueFlag) {
     constexpr int64_t kReadBlockingTimeout = INT64_C(500000000);
-    std::vector<Event> events(numEvents);
+    std::vector<EventV1_0> events(numEvents);
     return eventQueue->readBlocking(events.data(), numEvents,
                                     static_cast<uint32_t>(EventQueueFlagBits::EVENTS_READ),
                                     static_cast<uint32_t>(EventQueueFlagBits::READ_AND_PROCESS),
                                     kReadBlockingTimeout, eventQueueFlag);
 }
 
-std::unique_ptr<EventMessageQueue> makeEventFMQ(size_t size) {
-    return std::make_unique<EventMessageQueue>(size, true);
+std::unique_ptr<EventMessageQueueV2_0> makeEventFMQ(size_t size) {
+    return std::make_unique<EventMessageQueueV2_0>(size, true);
 }
 
 std::unique_ptr<WakeupMessageQueue> makeWakelockFMQ(size_t size) {
     return std::make_unique<WakeupMessageQueue>(size, true);
 }
 
-Event makeProximityEvent() {
-    Event event;
+EventV1_0 makeProximityEvent() {
+    EventV1_0 event;
     event.timestamp = 0xFF00FF00;
     // This is the sensorhandle of proximity, which is wakeup type
     event.sensorHandle = 0x00000008;
@@ -829,8 +893,8 @@
     return event;
 }
 
-Event makeAccelerometerEvent() {
-    Event event;
+EventV1_0 makeAccelerometerEvent() {
+    EventV1_0 event;
     event.timestamp = 0xFF00FF00;
     // This is the sensorhandle of proximity, which is wakeup type
     event.sensorHandle = 0x00000001;
@@ -839,16 +903,16 @@
     return event;
 }
 
-std::vector<Event> makeMultipleProximityEvents(size_t numEvents) {
-    std::vector<Event> events;
+std::vector<EventV1_0> makeMultipleProximityEvents(size_t numEvents) {
+    std::vector<EventV1_0> events;
     for (size_t i = 0; i < numEvents; i++) {
         events.push_back(makeProximityEvent());
     }
     return events;
 }
 
-std::vector<Event> makeMultipleAccelerometerEvents(size_t numEvents) {
-    std::vector<Event> events;
+std::vector<EventV1_0> makeMultipleAccelerometerEvents(size_t numEvents) {
+    std::vector<EventV1_0> events;
     for (size_t i = 0; i < numEvents; i++) {
         events.push_back(makeAccelerometerEvent());
     }
diff --git a/sensors/common/default/2.X/multihal/tests/fake_subhal/IHalProxyCallbackWrapper.h b/sensors/common/default/2.X/multihal/tests/fake_subhal/IHalProxyCallbackWrapper.h
new file mode 100644
index 0000000..4542bfd
--- /dev/null
+++ b/sensors/common/default/2.X/multihal/tests/fake_subhal/IHalProxyCallbackWrapper.h
@@ -0,0 +1,107 @@
+/*
+ * 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 "V2_0/SubHal.h"
+#include "V2_1/SubHal.h"
+#include "convertV2_1.h"
+
+namespace android {
+namespace hardware {
+namespace sensors {
+namespace V2_1 {
+namespace subhal {
+namespace implementation {
+
+/**
+ * The following callback wrapper classes abstract away common functionality across V2.0 and V2.1
+ * 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 callback interface without worrying about the type.
+ */
+class IHalProxyCallbackWrapperBase {
+  protected:
+    using ScopedWakelock = V2_0::implementation::ScopedWakelock;
+
+  public:
+    virtual ~IHalProxyCallbackWrapperBase() {}
+
+    virtual Return<void> onDynamicSensorsConnected(
+            const hidl_vec<V2_1::SensorInfo>& sensorInfos) = 0;
+
+    virtual Return<void> onDynamicSensorsDisconnected(const hidl_vec<int32_t>& sensorHandles) = 0;
+
+    virtual void postEvents(const std::vector<V2_1::Event>& events, ScopedWakelock wakelock) = 0;
+
+    virtual ScopedWakelock createScopedWakelock(bool lock) = 0;
+};
+
+template <typename T>
+class HalProxyCallbackWrapperBase : public IHalProxyCallbackWrapperBase {
+  public:
+    HalProxyCallbackWrapperBase(sp<T> callback) : mCallback(callback){};
+
+    Return<void> onDynamicSensorsDisconnected(const hidl_vec<int32_t>& sensorHandles) override {
+        return mCallback->onDynamicSensorsDisconnected(sensorHandles);
+    }
+
+    ScopedWakelock createScopedWakelock(bool lock) override {
+        return mCallback->createScopedWakelock(lock);
+    }
+
+  protected:
+    sp<T> mCallback;
+};
+
+class HalProxyCallbackWrapperV2_0
+    : public HalProxyCallbackWrapperBase<V2_0::implementation::IHalProxyCallback> {
+  public:
+    HalProxyCallbackWrapperV2_0(sp<V2_0::implementation::IHalProxyCallback> callback)
+        : HalProxyCallbackWrapperBase(callback){};
+
+    Return<void> onDynamicSensorsConnected(const hidl_vec<V2_1::SensorInfo>& sensorInfos) override {
+        return mCallback->onDynamicSensorsConnected(
+                V2_1::implementation::convertToOldSensorInfos(sensorInfos));
+    }
+
+    void postEvents(const std::vector<V2_1::Event>& events, ScopedWakelock wakelock) override {
+        return mCallback->postEvents(V2_1::implementation::convertToOldEvents(events),
+                                     std::move(wakelock));
+    }
+};
+
+class HalProxyCallbackWrapperV2_1
+    : public HalProxyCallbackWrapperBase<V2_1::implementation::IHalProxyCallback> {
+  public:
+    HalProxyCallbackWrapperV2_1(sp<V2_1::implementation::IHalProxyCallback> callback)
+        : HalProxyCallbackWrapperBase(callback){};
+
+    Return<void> onDynamicSensorsConnected(const hidl_vec<V2_1::SensorInfo>& sensorInfos) override {
+        return mCallback->onDynamicSensorsConnected_2_1(sensorInfos);
+    }
+
+    void postEvents(const std::vector<V2_1::Event>& events, ScopedWakelock wakelock) {
+        return mCallback->postEvents(events, std::move(wakelock));
+    }
+};
+
+}  // namespace implementation
+}  // namespace subhal
+}  // namespace V2_1
+}  // namespace sensors
+}  // namespace hardware
+}  // namespace android
diff --git a/sensors/common/default/2.X/multihal/tests/fake_subhal/Sensor.cpp b/sensors/common/default/2.X/multihal/tests/fake_subhal/Sensor.cpp
index de89a00..1efd971 100644
--- a/sensors/common/default/2.X/multihal/tests/fake_subhal/Sensor.cpp
+++ b/sensors/common/default/2.X/multihal/tests/fake_subhal/Sensor.cpp
@@ -24,13 +24,18 @@
 namespace android {
 namespace hardware {
 namespace sensors {
-namespace V2_0 {
+namespace V2_1 {
 namespace subhal {
 namespace implementation {
 
 using ::android::hardware::sensors::V1_0::MetaDataEventType;
+using ::android::hardware::sensors::V1_0::OperationMode;
+using ::android::hardware::sensors::V1_0::Result;
 using ::android::hardware::sensors::V1_0::SensorFlagBits;
 using ::android::hardware::sensors::V1_0::SensorStatus;
+using ::android::hardware::sensors::V2_1::Event;
+using ::android::hardware::sensors::V2_1::SensorInfo;
+using ::android::hardware::sensors::V2_1::SensorType;
 
 Sensor::Sensor(int32_t sensorHandle, ISensorsEventCallback* callback)
     : mIsEnabled(false),
@@ -343,7 +348,7 @@
 
 }  // namespace implementation
 }  // namespace subhal
-}  // namespace V2_0
+}  // namespace V2_1
 }  // namespace sensors
 }  // namespace hardware
 }  // namespace android
diff --git a/sensors/common/default/2.X/multihal/tests/fake_subhal/Sensor.h b/sensors/common/default/2.X/multihal/tests/fake_subhal/Sensor.h
index 60f5d3d..5cf9f83 100644
--- a/sensors/common/default/2.X/multihal/tests/fake_subhal/Sensor.h
+++ b/sensors/common/default/2.X/multihal/tests/fake_subhal/Sensor.h
@@ -16,7 +16,7 @@
 
 #pragma once
 
-#include <android/hardware/sensors/1.0/types.h>
+#include <android/hardware/sensors/2.1/types.h>
 
 #include <condition_variable>
 #include <memory>
@@ -24,16 +24,16 @@
 #include <thread>
 #include <vector>
 
-using ::android::hardware::sensors::V1_0::Event;
 using ::android::hardware::sensors::V1_0::OperationMode;
 using ::android::hardware::sensors::V1_0::Result;
-using ::android::hardware::sensors::V1_0::SensorInfo;
-using ::android::hardware::sensors::V1_0::SensorType;
+using ::android::hardware::sensors::V2_1::Event;
+using ::android::hardware::sensors::V2_1::SensorInfo;
+using ::android::hardware::sensors::V2_1::SensorType;
 
 namespace android {
 namespace hardware {
 namespace sensors {
-namespace V2_0 {
+namespace V2_1 {
 namespace subhal {
 namespace implementation {
 
@@ -151,7 +151,7 @@
 
 }  // namespace implementation
 }  // namespace subhal
-}  // namespace V2_0
+}  // namespace V2_1
 }  // namespace sensors
 }  // namespace hardware
 }  // namespace android
diff --git a/sensors/common/default/2.X/multihal/tests/fake_subhal/SensorsSubHal.cpp b/sensors/common/default/2.X/multihal/tests/fake_subhal/SensorsSubHal.cpp
index ff5ff38..20a4e9d 100644
--- a/sensors/common/default/2.X/multihal/tests/fake_subhal/SensorsSubHal.cpp
+++ b/sensors/common/default/2.X/multihal/tests/fake_subhal/SensorsSubHal.cpp
@@ -16,33 +16,66 @@
 
 #include "SensorsSubHal.h"
 
-#include <android/hardware/sensors/2.0/types.h>
+#include <android/hardware/sensors/2.1/types.h>
 #include <log/log.h>
 
-ISensorsSubHal* sensorsHalGetSubHal(uint32_t* version) {
+#ifdef SUB_HAL_VERSION_2_0
+::android::hardware::sensors::V2_0::implementation::ISensorsSubHal* sensorsHalGetSubHal(
+        uint32_t* version) {
 #if defined SUPPORT_CONTINUOUS_SENSORS && defined SUPPORT_ON_CHANGE_SENSORS
-    static ::android::hardware::sensors::V2_0::subhal::implementation::AllSensorsSubHal subHal;
+    static ::android::hardware::sensors::V2_1::subhal::implementation::AllSensorsSubHal<
+            ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_0>
+            subHal;
 #elif defined SUPPORT_CONTINUOUS_SENSORS
-    static ::android::hardware::sensors::V2_0::subhal::implementation::ContinuousSensorsSubHal
+    static ::android::hardware::sensors::V2_1::subhal::implementation::ContinuousSensorsSubHal<
+            ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_0>
             subHal;
 #elif defined SUPPORT_ON_CHANGE_SENSORS
-    static ::android::hardware::sensors::V2_0::subhal::implementation::OnChangeSensorsSubHal subHal;
+    static ::android::hardware::sensors::V2_1::subhal::implementation::OnChangeSensorsSubHal<
+            ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_0>
+            subHal;
 #else
-    static ::android::hardware::sensors::V2_0::subhal::implementation::SensorsSubHal subHal;
+    static ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHal<
+            ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_0>
+            subHal;
 #endif  // defined SUPPORT_CONTINUOUS_SENSORS && defined SUPPORT_ON_CHANGE_SENSORS
     *version = SUB_HAL_2_0_VERSION;
     return &subHal;
 }
 
+#else  // SUB_HAL_VERSION_2_0
+
+::android::hardware::sensors::V2_1::implementation::ISensorsSubHal* sensorsHalGetSubHal_2_1(
+        uint32_t* version) {
+#if defined SUPPORT_CONTINUOUS_SENSORS && defined SUPPORT_ON_CHANGE_SENSORS
+    static ::android::hardware::sensors::V2_1::subhal::implementation::AllSensorsSubHal<
+            ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_1>
+            subHal;
+#elif defined SUPPORT_CONTINUOUS_SENSORS
+    static ::android::hardware::sensors::V2_1::subhal::implementation::ContinuousSensorsSubHal<
+            ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_1>
+            subHal;
+#elif defined SUPPORT_ON_CHANGE_SENSORS
+    static ::android::hardware::sensors::V2_1::subhal::implementation::OnChangeSensorsSubHal<
+            ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_1>
+            subHal;
+#else
+    static ::android::hardware::sensors::V2_1::subhal::implementation::SensorsSubHalV2_1 subHal;
+#endif  // defined SUPPORT_CONTINUOUS_SENSORS && defined SUPPORT_ON_CHANGE_SENSORS
+    *version = SUB_HAL_2_1_VERSION;
+    return &subHal;
+}
+
+#endif  // SUB_HAL_VERSION_2_0
+
 namespace android {
 namespace hardware {
 namespace sensors {
-namespace V2_0 {
+namespace V2_1 {
 namespace subhal {
 namespace implementation {
 
 using ::android::hardware::Void;
-using ::android::hardware::sensors::V1_0::Event;
 using ::android::hardware::sensors::V1_0::OperationMode;
 using ::android::hardware::sensors::V1_0::RateLevel;
 using ::android::hardware::sensors::V1_0::Result;
@@ -50,11 +83,12 @@
 using ::android::hardware::sensors::V2_0::SensorTimeout;
 using ::android::hardware::sensors::V2_0::WakeLockQueueFlagBits;
 using ::android::hardware::sensors::V2_0::implementation::ScopedWakelock;
+using ::android::hardware::sensors::V2_1::Event;
 
-SensorsSubHal::SensorsSubHal() : mCallback(nullptr), mNextHandle(1) {}
+ISensorsSubHalBase::ISensorsSubHalBase() : mCallback(nullptr), mNextHandle(1) {}
 
 // Methods from ::android::hardware::sensors::V2_0::ISensors follow.
-Return<void> SensorsSubHal::getSensorsList(getSensorsList_cb _hidl_cb) {
+Return<void> ISensorsSubHalBase::getSensorsList(V2_1::ISensors::getSensorsList_2_1_cb _hidl_cb) {
     std::vector<SensorInfo> sensors;
     for (const auto& sensor : mSensors) {
         sensors.push_back(sensor.second->getSensorInfo());
@@ -64,7 +98,7 @@
     return Void();
 }
 
-Return<Result> SensorsSubHal::setOperationMode(OperationMode mode) {
+Return<Result> ISensorsSubHalBase::setOperationMode(OperationMode mode) {
     for (auto sensor : mSensors) {
         sensor.second->setOperationMode(mode);
     }
@@ -72,7 +106,7 @@
     return Result::OK;
 }
 
-Return<Result> SensorsSubHal::activate(int32_t sensorHandle, bool enabled) {
+Return<Result> ISensorsSubHalBase::activate(int32_t sensorHandle, bool enabled) {
     auto sensor = mSensors.find(sensorHandle);
     if (sensor != mSensors.end()) {
         sensor->second->activate(enabled);
@@ -81,8 +115,8 @@
     return Result::BAD_VALUE;
 }
 
-Return<Result> SensorsSubHal::batch(int32_t sensorHandle, int64_t samplingPeriodNs,
-                                    int64_t /* maxReportLatencyNs */) {
+Return<Result> ISensorsSubHalBase::batch(int32_t sensorHandle, int64_t samplingPeriodNs,
+                                         int64_t /* maxReportLatencyNs */) {
     auto sensor = mSensors.find(sensorHandle);
     if (sensor != mSensors.end()) {
         sensor->second->batch(samplingPeriodNs);
@@ -91,7 +125,7 @@
     return Result::BAD_VALUE;
 }
 
-Return<Result> SensorsSubHal::flush(int32_t sensorHandle) {
+Return<Result> ISensorsSubHalBase::flush(int32_t sensorHandle) {
     auto sensor = mSensors.find(sensorHandle);
     if (sensor != mSensors.end()) {
         return sensor->second->flush();
@@ -99,7 +133,7 @@
     return Result::BAD_VALUE;
 }
 
-Return<Result> SensorsSubHal::injectSensorData(const Event& event) {
+Return<Result> ISensorsSubHalBase::injectSensorData(const Event& event) {
     auto sensor = mSensors.find(event.sensorHandle);
     if (sensor != mSensors.end()) {
         return sensor->second->injectEvent(event);
@@ -108,24 +142,24 @@
     return Result::BAD_VALUE;
 }
 
-Return<void> SensorsSubHal::registerDirectChannel(const SharedMemInfo& /* mem */,
-                                                  registerDirectChannel_cb _hidl_cb) {
+Return<void> ISensorsSubHalBase::registerDirectChannel(
+        const SharedMemInfo& /* mem */, V2_0::ISensors::registerDirectChannel_cb _hidl_cb) {
     _hidl_cb(Result::INVALID_OPERATION, -1 /* channelHandle */);
     return Return<void>();
 }
 
-Return<Result> SensorsSubHal::unregisterDirectChannel(int32_t /* channelHandle */) {
+Return<Result> ISensorsSubHalBase::unregisterDirectChannel(int32_t /* channelHandle */) {
     return Result::INVALID_OPERATION;
 }
 
-Return<void> SensorsSubHal::configDirectReport(int32_t /* sensorHandle */,
-                                               int32_t /* channelHandle */, RateLevel /* rate */,
-                                               configDirectReport_cb _hidl_cb) {
+Return<void> ISensorsSubHalBase::configDirectReport(
+        int32_t /* sensorHandle */, int32_t /* channelHandle */, RateLevel /* rate */,
+        V2_0::ISensors::configDirectReport_cb _hidl_cb) {
     _hidl_cb(Result::INVALID_OPERATION, 0 /* reportToken */);
     return Return<void>();
 }
 
-Return<void> SensorsSubHal::debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args) {
+Return<void> ISensorsSubHalBase::debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args) {
     if (fd.getNativeHandle() == nullptr || fd->numFds < 1) {
         ALOGE("%s: missing fd for writing", __FUNCTION__);
         return Void();
@@ -156,44 +190,18 @@
     return Return<void>();
 }
 
-Return<Result> SensorsSubHal::initialize(const sp<IHalProxyCallback>& halProxyCallback) {
-    mCallback = halProxyCallback;
+Return<Result> ISensorsSubHalBase::initialize(
+        std::unique_ptr<IHalProxyCallbackWrapperBase>& halProxyCallback) {
+    mCallback = std::move(halProxyCallback);
     setOperationMode(OperationMode::NORMAL);
     return Result::OK;
 }
 
-void SensorsSubHal::postEvents(const std::vector<Event>& events, bool wakeup) {
+void ISensorsSubHalBase::postEvents(const std::vector<Event>& events, bool wakeup) {
     ScopedWakelock wakelock = mCallback->createScopedWakelock(wakeup);
     mCallback->postEvents(events, std::move(wakelock));
 }
 
-ContinuousSensorsSubHal::ContinuousSensorsSubHal() {
-    AddSensor<AccelSensor>();
-    AddSensor<GyroSensor>();
-    AddSensor<MagnetometerSensor>();
-    AddSensor<PressureSensor>();
-    AddSensor<DeviceTempSensor>();
-}
-
-OnChangeSensorsSubHal::OnChangeSensorsSubHal() {
-    AddSensor<AmbientTempSensor>();
-    AddSensor<LightSensor>();
-    AddSensor<ProximitySensor>();
-    AddSensor<RelativeHumiditySensor>();
-}
-
-AllSensorsSubHal::AllSensorsSubHal() {
-    AddSensor<AccelSensor>();
-    AddSensor<GyroSensor>();
-    AddSensor<MagnetometerSensor>();
-    AddSensor<PressureSensor>();
-    AddSensor<DeviceTempSensor>();
-    AddSensor<AmbientTempSensor>();
-    AddSensor<LightSensor>();
-    AddSensor<ProximitySensor>();
-    AddSensor<RelativeHumiditySensor>();
-}
-
 Return<Result> SetOperationModeFailingSensorsSubHal::setOperationMode(OperationMode /*mode*/) {
     return Result::BAD_VALUE;
 }
@@ -206,7 +214,7 @@
         sensorInfo.flags |= V1_0::SensorFlagBits::MASK_DIRECT_REPORT;
         sensors.push_back(sensorInfo);
     }
-    _hidl_cb(sensors);
+    _hidl_cb(V2_1::implementation::convertToOldSensorInfos(sensors));
     return Void();
 }
 
@@ -218,7 +226,7 @@
         sensorInfo.flags &= ~static_cast<uint32_t>(V1_0::SensorFlagBits::MASK_DIRECT_REPORT);
         sensors.push_back(sensorInfo);
     }
-    _hidl_cb(sensors);
+    _hidl_cb(V2_1::implementation::convertToOldSensorInfos(sensors));
     return Void();
 }
 
@@ -234,7 +242,7 @@
 
 }  // namespace implementation
 }  // namespace subhal
-}  // namespace V2_0
+}  // namespace V2_1
 }  // namespace sensors
 }  // namespace hardware
 }  // namespace android
diff --git a/sensors/common/default/2.X/multihal/tests/fake_subhal/SensorsSubHal.h b/sensors/common/default/2.X/multihal/tests/fake_subhal/SensorsSubHal.h
index 6da4404..1a78e84 100644
--- a/sensors/common/default/2.X/multihal/tests/fake_subhal/SensorsSubHal.h
+++ b/sensors/common/default/2.X/multihal/tests/fake_subhal/SensorsSubHal.h
@@ -17,7 +17,9 @@
 #pragma once
 
 #include "V2_0/SubHal.h"
+#include "V2_1/SubHal.h"
 
+#include "IHalProxyCallbackWrapper.h"
 #include "Sensor.h"
 
 #include <vector>
@@ -25,54 +27,54 @@
 namespace android {
 namespace hardware {
 namespace sensors {
-namespace V2_0 {
+namespace V2_1 {
 namespace subhal {
 namespace implementation {
 
 using ::android::hardware::sensors::V1_0::OperationMode;
 using ::android::hardware::sensors::V1_0::Result;
-using ::android::hardware::sensors::V2_0::implementation::IHalProxyCallback;
 
 /**
  * Implementation of a ISensorsSubHal that can be used to test the implementation of multihal 2.0.
  * See the README file for more details on how this class can be used for testing.
  */
-class SensorsSubHal : public ISensorsSubHal, public ISensorsEventCallback {
-    using Event = ::android::hardware::sensors::V1_0::Event;
+class ISensorsSubHalBase : public ISensorsEventCallback {
+  protected:
+    using Event = ::android::hardware::sensors::V2_1::Event;
     using RateLevel = ::android::hardware::sensors::V1_0::RateLevel;
     using SharedMemInfo = ::android::hardware::sensors::V1_0::SharedMemInfo;
 
   public:
-    SensorsSubHal();
+    ISensorsSubHalBase();
+
+    Return<void> getSensorsList(V2_1::ISensors::getSensorsList_2_1_cb _hidl_cb);
+    Return<Result> injectSensorData(const Event& event);
+    Return<Result> initialize(std::unique_ptr<IHalProxyCallbackWrapperBase>& halProxyCallback);
 
     // Methods from ::android::hardware::sensors::V2_0::ISensors follow.
-    virtual Return<void> getSensorsList(getSensorsList_cb _hidl_cb) override;
-
-    virtual Return<Result> setOperationMode(OperationMode mode) override;
+    virtual Return<Result> setOperationMode(OperationMode mode);
 
     OperationMode getOperationMode() const { return mCurrentOperationMode; }
 
-    Return<Result> activate(int32_t sensorHandle, bool enabled) override;
+    Return<Result> activate(int32_t sensorHandle, bool enabled);
 
     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> injectSensorData(const Event& event) override;
+    Return<Result> flush(int32_t sensorHandle);
 
     Return<void> registerDirectChannel(const SharedMemInfo& mem,
-                                       registerDirectChannel_cb _hidl_cb) override;
+                                       V2_0::ISensors::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;
+                                    V2_0::ISensors::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);
 
     // Methods from ::android::hardware::sensors::V2_0::implementation::ISensorsSubHal follow.
-    const std::string getName() override {
+    const std::string getName() {
 #ifdef SUB_HAL_NAME
         return SUB_HAL_NAME;
 #else   // SUB_HAL_NAME
@@ -80,8 +82,6 @@
 #endif  // SUB_HAL_NAME
     }
 
-    Return<Result> initialize(const sp<IHalProxyCallback>& halProxyCallback) override;
-
     // Method from ISensorsEventCallback.
     void postEvents(const std::vector<Event>& events, bool wakeup) override;
 
@@ -103,7 +103,7 @@
      * disconnected, sensor events need to be sent to the framework, and when a wakelock should be
      * acquired.
      */
-    sp<IHalProxyCallback> mCallback;
+    std::unique_ptr<IHalProxyCallbackWrapperBase> mCallback;
 
   private:
     /**
@@ -118,40 +118,143 @@
     int32_t mNextHandle;
 };
 
-// SubHal that has continuous sensors for testing purposes.
-class ContinuousSensorsSubHal : public SensorsSubHal {
+template <class SubHalClass>
+class SensorsSubHalBase : public ISensorsSubHalBase, public SubHalClass {
   public:
-    ContinuousSensorsSubHal();
+    Return<Result> setOperationMode(OperationMode mode) override {
+        return ISensorsSubHalBase::setOperationMode(mode);
+    }
+
+    Return<Result> activate(int32_t sensorHandle, bool enabled) override {
+        return ISensorsSubHalBase::activate(sensorHandle, enabled);
+    }
+
+    Return<Result> batch(int32_t sensorHandle, int64_t samplingPeriodNs,
+                         int64_t maxReportLatencyNs) override {
+        return ISensorsSubHalBase::batch(sensorHandle, samplingPeriodNs, maxReportLatencyNs);
+    }
+
+    Return<Result> flush(int32_t sensorHandle) override {
+        return ISensorsSubHalBase::flush(sensorHandle);
+    }
+
+    Return<void> registerDirectChannel(const SharedMemInfo& mem,
+                                       V2_0::ISensors::registerDirectChannel_cb _hidl_cb) override {
+        return ISensorsSubHalBase::registerDirectChannel(mem, _hidl_cb);
+    }
+
+    Return<Result> unregisterDirectChannel(int32_t channelHandle) override {
+        return ISensorsSubHalBase::unregisterDirectChannel(channelHandle);
+    }
+
+    Return<void> configDirectReport(int32_t sensorHandle, int32_t channelHandle, RateLevel rate,
+                                    V2_0::ISensors::configDirectReport_cb _hidl_cb) override {
+        return ISensorsSubHalBase::configDirectReport(sensorHandle, channelHandle, rate, _hidl_cb);
+    }
+
+    Return<void> debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args) override {
+        return ISensorsSubHalBase::debug(fd, args);
+    }
+
+    const std::string getName() override { return ISensorsSubHalBase::getName(); }
+};
+
+class SensorsSubHalV2_0 : public SensorsSubHalBase<V2_0::implementation::ISensorsSubHal> {
+  public:
+    virtual Return<void> getSensorsList(V2_0::ISensors::getSensorsList_cb _hidl_cb) override {
+        return ISensorsSubHalBase::getSensorsList([&](const auto& list) {
+            _hidl_cb(V2_1::implementation::convertToOldSensorInfos(list));
+        });
+    }
+
+    Return<Result> injectSensorData(const V1_0::Event& event) override {
+        return ISensorsSubHalBase::injectSensorData(V2_1::implementation::convertToNewEvent(event));
+    }
+
+    Return<Result> initialize(
+            const sp<V2_0::implementation::IHalProxyCallback>& halProxyCallback) override {
+        std::unique_ptr<IHalProxyCallbackWrapperBase> wrapper =
+                std::make_unique<HalProxyCallbackWrapperV2_0>(halProxyCallback);
+        return ISensorsSubHalBase::initialize(wrapper);
+    }
+};
+
+class SensorsSubHalV2_1 : public SensorsSubHalBase<V2_1::implementation::ISensorsSubHal> {
+  public:
+    Return<void> getSensorsList_2_1(V2_1::ISensors::getSensorsList_2_1_cb _hidl_cb) override {
+        return ISensorsSubHalBase::getSensorsList(_hidl_cb);
+    }
+
+    Return<Result> injectSensorData_2_1(const V2_1::Event& event) override {
+        return ISensorsSubHalBase::injectSensorData(event);
+    }
+
+    Return<Result> initialize(
+            const sp<V2_1::implementation::IHalProxyCallback>& halProxyCallback) override {
+        std::unique_ptr<IHalProxyCallbackWrapperBase> wrapper =
+                std::make_unique<HalProxyCallbackWrapperV2_1>(halProxyCallback);
+        return ISensorsSubHalBase::initialize(wrapper);
+    }
+};
+
+// SubHal that has continuous sensors for testing purposes.
+template <class SubHalVersion>
+class ContinuousSensorsSubHal : public SubHalVersion {
+  public:
+    ContinuousSensorsSubHal() {
+        ISensorsSubHalBase::AddSensor<AccelSensor>();
+        ISensorsSubHalBase::AddSensor<GyroSensor>();
+        ISensorsSubHalBase::AddSensor<MagnetometerSensor>();
+        ISensorsSubHalBase::AddSensor<PressureSensor>();
+        ISensorsSubHalBase::AddSensor<DeviceTempSensor>();
+    }
 };
 
 // SubHal that has on-change sensors for testing purposes.
-class OnChangeSensorsSubHal : public SensorsSubHal {
+template <class SubHalVersion>
+class OnChangeSensorsSubHal : public SubHalVersion {
   public:
-    OnChangeSensorsSubHal();
+    OnChangeSensorsSubHal() {
+        ISensorsSubHalBase::AddSensor<AmbientTempSensor>();
+        ISensorsSubHalBase::AddSensor<LightSensor>();
+        ISensorsSubHalBase::AddSensor<ProximitySensor>();
+        ISensorsSubHalBase::AddSensor<RelativeHumiditySensor>();
+    }
 };
 
 // SubHal that has both continuous and on-change sensors for testing purposes.
-class AllSensorsSubHal : public SensorsSubHal {
+template <class SubHalVersion>
+class AllSensorsSubHal : public SubHalVersion {
   public:
-    AllSensorsSubHal();
+    AllSensorsSubHal() {
+        ISensorsSubHalBase::AddSensor<AccelSensor>();
+        ISensorsSubHalBase::AddSensor<GyroSensor>();
+        ISensorsSubHalBase::AddSensor<MagnetometerSensor>();
+        ISensorsSubHalBase::AddSensor<PressureSensor>();
+        ISensorsSubHalBase::AddSensor<DeviceTempSensor>();
+        ISensorsSubHalBase::AddSensor<AmbientTempSensor>();
+        ISensorsSubHalBase::AddSensor<LightSensor>();
+        ISensorsSubHalBase::AddSensor<ProximitySensor>();
+        ISensorsSubHalBase::AddSensor<RelativeHumiditySensor>();
+    }
 };
 
-class SetOperationModeFailingSensorsSubHal : public AllSensorsSubHal {
+class SetOperationModeFailingSensorsSubHal : public AllSensorsSubHal<SensorsSubHalV2_0> {
   public:
     Return<Result> setOperationMode(OperationMode mode) override;
 };
 
-class AllSupportDirectChannelSensorsSubHal : public AllSensorsSubHal {
+class AllSupportDirectChannelSensorsSubHal : public AllSensorsSubHal<SensorsSubHalV2_0> {
   public:
-    Return<void> getSensorsList(getSensorsList_cb _hidl_cb) override;
+    Return<void> getSensorsList(V2_0::ISensors::getSensorsList_cb _hidl_cb) override;
 };
 
-class DoesNotSupportDirectChannelSensorsSubHal : public AllSensorsSubHal {
+class DoesNotSupportDirectChannelSensorsSubHal : public AllSensorsSubHal<SensorsSubHalV2_0> {
   public:
-    Return<void> getSensorsList(getSensorsList_cb _hidl_cb) override;
+    Return<void> getSensorsList(V2_0::ISensors::getSensorsList_cb _hidl_cb) override;
 };
 
-class AddAndRemoveDynamicSensorsSubHal : public AllSensorsSubHal {
+class AddAndRemoveDynamicSensorsSubHal : public AllSensorsSubHal<SensorsSubHalV2_0> {
   public:
     void addDynamicSensors(const std::vector<SensorInfo>& sensorsAdded);
     void removeDynamicSensors(const std::vector<int32_t>& sensorHandlesAdded);
@@ -159,7 +262,7 @@
 
 }  // namespace implementation
 }  // namespace subhal
-}  // namespace V2_0
+}  // namespace V2_1
 }  // namespace sensors
 }  // namespace hardware
 }  // namespace android
diff --git a/sensors/common/utils/EventMessageQueueWrapper.h b/sensors/common/utils/EventMessageQueueWrapper.h
index bf3261f..c4f92c8 100644
--- a/sensors/common/utils/EventMessageQueueWrapper.h
+++ b/sensors/common/utils/EventMessageQueueWrapper.h
@@ -39,8 +39,14 @@
 
     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 @@
 
     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 @@
 
     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 0000000..816b225
--- /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
diff --git a/wifi/1.4/default/Android.mk b/wifi/1.4/default/Android.mk
index 8573e8e..6be7dad 100644
--- a/wifi/1.4/default/Android.mk
+++ b/wifi/1.4/default/Android.mk
@@ -36,6 +36,9 @@
 ifdef WIFI_HIDL_FEATURE_DISABLE_AP_MAC_RANDOMIZATION
 LOCAL_CPPFLAGS += -DWIFI_HIDL_FEATURE_DISABLE_AP_MAC_RANDOMIZATION
 endif
+ifdef WIFI_AVOID_IFACE_RESET_MAC_CHANGE
+LOCAL_CPPFLAGS += -DWIFI_AVOID_IFACE_RESET_MAC_CHANGE
+endif
 # Allow implicit fallthroughs in wifi_legacy_hal.cpp until they are fixed.
 LOCAL_CFLAGS += -Wno-error=implicit-fallthrough
 LOCAL_SRC_FILES := \
diff --git a/wifi/1.4/default/wifi_chip.cpp b/wifi/1.4/default/wifi_chip.cpp
index 23dd13b..61912a5 100644
--- a/wifi/1.4/default/wifi_chip.cpp
+++ b/wifi/1.4/default/wifi_chip.cpp
@@ -101,6 +101,16 @@
     return "wlan" + std::to_string(idx);
 }
 
+// Returns the dedicated iface name if one is defined.
+std::string getApIfaceName() {
+    std::array<char, PROPERTY_VALUE_MAX> buffer;
+    if (property_get("ro.vendor.wifi.sap.interface", buffer.data(), nullptr) ==
+        0) {
+        return {};
+    }
+    return buffer.data();
+}
+
 std::string getP2pIfaceName() {
     std::array<char, PROPERTY_VALUE_MAX> buffer;
     property_get("wifi.direct.interface", buffer.data(), "p2p0");
@@ -1582,6 +1592,11 @@
 // AP iface names start with idx 1 for modes supporting
 // concurrent STA and not dual AP, else start with idx 0.
 std::string WifiChip::allocateApIfaceName() {
+    // Check if we have a dedicated iface for AP.
+    std::string ifname = getApIfaceName();
+    if (!ifname.empty()) {
+        return ifname;
+    }
     return allocateApOrStaIfaceName((isStaApConcurrencyAllowedInCurrentMode() &&
                                      !isDualApAllowedInCurrentMode())
                                         ? 1
diff --git a/wifi/1.4/default/wifi_iface_util.cpp b/wifi/1.4/default/wifi_iface_util.cpp
index 036c97b..13ba022 100644
--- a/wifi/1.4/default/wifi_iface_util.cpp
+++ b/wifi/1.4/default/wifi_iface_util.cpp
@@ -52,18 +52,22 @@
 
 bool WifiIfaceUtil::setMacAddress(const std::string& iface_name,
                                   const std::array<uint8_t, 6>& mac) {
+#ifndef WIFI_AVOID_IFACE_RESET_MAC_CHANGE
     if (!iface_tool_.lock()->SetUpState(iface_name.c_str(), false)) {
         LOG(ERROR) << "SetUpState(false) failed.";
         return false;
     }
+#endif
     if (!iface_tool_.lock()->SetMacAddress(iface_name.c_str(), mac)) {
         LOG(ERROR) << "SetMacAddress failed.";
         return false;
     }
+#ifndef WIFI_AVOID_IFACE_RESET_MAC_CHANGE
     if (!iface_tool_.lock()->SetUpState(iface_name.c_str(), true)) {
         LOG(ERROR) << "SetUpState(true) failed.";
         return false;
     }
+#endif
     IfaceEventHandlers event_handlers = {};
     const auto it = event_handlers_map_.find(iface_name);
     if (it != event_handlers_map_.end()) {