diff --git a/fastboot/usb_osx.cpp b/fastboot/usb_osx.cpp
index 610eebf..a4b9307 100644
--- a/fastboot/usb_osx.cpp
+++ b/fastboot/usb_osx.cpp
@@ -191,16 +191,30 @@
 
         // Iterate over the endpoints for this interface and see if there
         // are any that do bulk in/out.
-        for (UInt8 endpoint = 1; endpoint <= interfaceNumEndpoints; endpoint++) {
+        for (UInt8 endpoint = 1; endpoint <= interfaceNumEndpoints; ++endpoint) {
             UInt8   transferType;
-            UInt16  maxPacketSize;
+            UInt16  endPointMaxPacketSize = 0;
             UInt8   interval;
+
+            // Attempt to retrieve the 'true' packet-size from supported interface.
+            kr = (*interface)
+                 ->GetEndpointProperties(interface, 0, endpoint,
+                                       kUSBOut,
+                                       &transferType,
+                                       &endPointMaxPacketSize, &interval);
+            if (kr == kIOReturnSuccess && !endPointMaxPacketSize) {
+                ERR("GetEndpointProperties() returned zero len packet-size");
+            }
+
+            UInt16  pipePropMaxPacketSize;
             UInt8   number;
             UInt8   direction;
 
+            // Proceed with extracting the transfer direction, so we can fill in the
+            // appropriate fields (bulkIn or bulkOut).
             kr = (*interface)->GetPipeProperties(interface, endpoint,
                     &direction,
-                    &number, &transferType, &maxPacketSize, &interval);
+                    &number, &transferType, &pipePropMaxPacketSize, &interval);
 
             if (kr == 0) {
                 if (transferType != kUSBBulk) {
@@ -216,7 +230,8 @@
                 }
 
                 if (handle->info.ifc_protocol == 0x01) {
-                    handle->zero_mask = maxPacketSize - 1;
+                    handle->zero_mask = (endPointMaxPacketSize == 0) ?
+                        pipePropMaxPacketSize - 1 : endPointMaxPacketSize - 1;
                 }
             } else {
                 ERR("could not get pipe properties for endpoint %u (%08x)\n", endpoint, kr);
diff --git a/fs_mgr/fs_mgr_remount.cpp b/fs_mgr/fs_mgr_remount.cpp
index 5fddf86..5a9f391 100644
--- a/fs_mgr/fs_mgr_remount.cpp
+++ b/fs_mgr/fs_mgr_remount.cpp
@@ -175,6 +175,8 @@
     }
     if (checkpointing) {
         LOG(ERROR) << "Cannot use remount when a checkpoint is in progress.";
+        LOG(ERROR) << "To force end checkpointing, call 'vdc checkpoint commitChanges'";
+        LOG(ERROR) << "Warning: this can lead to data corruption if rolled back.";
         return false;
     }
     return true;
diff --git a/fs_mgr/libsnapshot/include/libsnapshot/cow_writer.h b/fs_mgr/libsnapshot/include/libsnapshot/cow_writer.h
index 798bc73..c7b83a8 100644
--- a/fs_mgr/libsnapshot/include/libsnapshot/cow_writer.h
+++ b/fs_mgr/libsnapshot/include/libsnapshot/cow_writer.h
@@ -120,6 +120,12 @@
     void EnqueueCompressBlocks(const void* buffer, size_t num_blocks);
     bool GetCompressedBuffers(std::vector<std::basic_string<uint8_t>>* compressed_buf);
     void Finalize();
+    static std::basic_string<uint8_t> Compress(CowCompressionAlgorithm compression,
+                                               const void* data, size_t length);
+
+    static bool CompressBlocks(CowCompressionAlgorithm compression, size_t block_size,
+                               const void* buffer, size_t num_blocks,
+                               std::vector<std::basic_string<uint8_t>>* compressed_data);
 
   private:
     struct CompressWork {
diff --git a/fs_mgr/libsnapshot/include/libsnapshot/snapshot.h b/fs_mgr/libsnapshot/include/libsnapshot/snapshot.h
index cdff06e..9eb89b6 100644
--- a/fs_mgr/libsnapshot/include/libsnapshot/snapshot.h
+++ b/fs_mgr/libsnapshot/include/libsnapshot/snapshot.h
@@ -395,6 +395,10 @@
     // first-stage to decide whether to launch snapuserd.
     bool IsSnapuserdRequired();
 
+    // This is primarily used to device reboot. If OTA update is in progress,
+    // init will avoid killing processes
+    bool IsUserspaceSnapshotUpdateInProgress();
+
     enum class SnapshotDriver {
         DM_SNAPSHOT,
         DM_USER,
diff --git a/fs_mgr/libsnapshot/libsnapshot_cow/cow_compress.cpp b/fs_mgr/libsnapshot/libsnapshot_cow/cow_compress.cpp
index 4d9b748..9b50986 100644
--- a/fs_mgr/libsnapshot/libsnapshot_cow/cow_compress.cpp
+++ b/fs_mgr/libsnapshot/libsnapshot_cow/cow_compress.cpp
@@ -32,9 +32,13 @@
 
 namespace android {
 namespace snapshot {
-
 std::basic_string<uint8_t> CompressWorker::Compress(const void* data, size_t length) {
-    switch (compression_) {
+    return Compress(compression_, data, length);
+}
+
+std::basic_string<uint8_t> CompressWorker::Compress(CowCompressionAlgorithm compression,
+                                                    const void* data, size_t length) {
+    switch (compression) {
         case kCowCompressGz: {
             const auto bound = compressBound(length);
             std::basic_string<uint8_t> buffer(bound, '\0');
@@ -94,17 +98,22 @@
             return buffer;
         }
         default:
-            LOG(ERROR) << "unhandled compression type: " << compression_;
+            LOG(ERROR) << "unhandled compression type: " << compression;
             break;
     }
     return {};
 }
-
 bool CompressWorker::CompressBlocks(const void* buffer, size_t num_blocks,
                                     std::vector<std::basic_string<uint8_t>>* compressed_data) {
+    return CompressBlocks(compression_, block_size_, buffer, num_blocks, compressed_data);
+}
+
+bool CompressWorker::CompressBlocks(CowCompressionAlgorithm compression, size_t block_size,
+                                    const void* buffer, size_t num_blocks,
+                                    std::vector<std::basic_string<uint8_t>>* compressed_data) {
     const uint8_t* iter = reinterpret_cast<const uint8_t*>(buffer);
     while (num_blocks) {
-        auto data = Compress(iter, block_size_);
+        auto data = Compress(compression, iter, block_size);
         if (data.empty()) {
             PLOG(ERROR) << "CompressBlocks: Compression failed";
             return false;
@@ -116,7 +125,7 @@
 
         compressed_data->emplace_back(std::move(data));
         num_blocks -= 1;
-        iter += block_size_;
+        iter += block_size;
     }
     return true;
 }
diff --git a/fs_mgr/libsnapshot/libsnapshot_cow/cow_writer.cpp b/fs_mgr/libsnapshot/libsnapshot_cow/cow_writer.cpp
index 2d5e4bc..3932fad 100644
--- a/fs_mgr/libsnapshot/libsnapshot_cow/cow_writer.cpp
+++ b/fs_mgr/libsnapshot/libsnapshot_cow/cow_writer.cpp
@@ -275,6 +275,10 @@
 }
 
 void CowWriter::InitWorkers() {
+    if (num_compress_threads_ <= 1) {
+        LOG(INFO) << "Not creating new threads for compression.";
+        return;
+    }
     for (int i = 0; i < num_compress_threads_; i++) {
         auto wt = std::make_unique<CompressWorker>(compression_, header_.block_size);
         threads_.emplace_back(std::async(std::launch::async, &CompressWorker::RunThread, wt.get()));
@@ -447,6 +451,10 @@
     size_t num_blocks_per_thread = num_blocks / num_threads;
     const uint8_t* iter = reinterpret_cast<const uint8_t*>(data);
     compressed_buf_.clear();
+    if (num_threads <= 1) {
+        return CompressWorker::CompressBlocks(compression_, options_.block_size, data, num_blocks,
+                                              &compressed_buf_);
+    }
 
     // Submit the blocks per thread. The retrieval of
     // compressed buffers has to be done in the same order.
@@ -490,13 +498,12 @@
     while (num_blocks) {
         size_t pending_blocks = (std::min(kProcessingBlocks, num_blocks));
 
-        if (compression_) {
+        if (compression_ && num_compress_threads_ > 1) {
             if (!CompressBlocks(pending_blocks, iter)) {
                 return false;
             }
             buf_iter_ = compressed_buf_.begin();
             CHECK(pending_blocks == compressed_buf_.size());
-            iter += (pending_blocks * header_.block_size);
         }
 
         num_blocks -= pending_blocks;
@@ -512,7 +519,17 @@
             }
 
             if (compression_) {
-                auto data = std::move(*buf_iter_);
+                auto data = [&, this]() {
+                    if (num_compress_threads_ > 1) {
+                        auto data = std::move(*buf_iter_);
+                        buf_iter_++;
+                        return data;
+                    } else {
+                        auto data =
+                                CompressWorker::Compress(compression_, iter, header_.block_size);
+                        return data;
+                    }
+                }();
                 op.compression = compression_;
                 op.data_length = static_cast<uint16_t>(data.size());
 
@@ -520,15 +537,14 @@
                     PLOG(ERROR) << "AddRawBlocks: write failed";
                     return false;
                 }
-                buf_iter_++;
             } else {
                 op.data_length = static_cast<uint16_t>(header_.block_size);
                 if (!WriteOperation(op, iter, header_.block_size)) {
                     PLOG(ERROR) << "AddRawBlocks: write failed";
                     return false;
                 }
-                iter += header_.block_size;
             }
+            iter += header_.block_size;
 
             i += 1;
             pending_blocks -= 1;
diff --git a/fs_mgr/libsnapshot/snapshot.cpp b/fs_mgr/libsnapshot/snapshot.cpp
index 6fed09c..10d2f18 100644
--- a/fs_mgr/libsnapshot/snapshot.cpp
+++ b/fs_mgr/libsnapshot/snapshot.cpp
@@ -4349,5 +4349,16 @@
     return status.source_build_fingerprint();
 }
 
+bool SnapshotManager::IsUserspaceSnapshotUpdateInProgress() {
+    auto slot = GetCurrentSlot();
+    if (slot == Slot::Target) {
+        if (IsSnapuserdRequired()) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
 }  // namespace snapshot
 }  // namespace android
diff --git a/init/README.md b/init/README.md
index 957eb9e..4a5d0ec 100644
--- a/init/README.md
+++ b/init/README.md
@@ -244,6 +244,10 @@
   "r", "w" or "rw".  For native executables see libcutils
   android\_get\_control\_file().
 
+`gentle_kill`
+> This service will be sent SIGTERM instead of SIGKILL when stopped. After a 200 ms timeout, it will
+  be sent SIGKILL.
+
 `group <groupname> [ <groupname>\* ]`
 > Change to 'groupname' before exec'ing this service.  Additional
   groupnames beyond the (required) first one are used to set the
diff --git a/init/init_test.cpp b/init/init_test.cpp
index 1ab69ac..1e69ede 100644
--- a/init/init_test.cpp
+++ b/init/init_test.cpp
@@ -16,6 +16,7 @@
 
 #include <functional>
 #include <string_view>
+#include <thread>
 #include <type_traits>
 
 #include <android-base/file.h>
@@ -642,6 +643,91 @@
     ASSERT_LE(curr_limit.rlim_max, max_limit);
 }
 
+static std::vector<const char*> ConvertToArgv(const std::vector<std::string>& args) {
+    std::vector<const char*> argv;
+    argv.reserve(args.size() + 1);
+    for (const auto& arg : args) {
+        if (argv.empty()) {
+            LOG(DEBUG) << arg;
+        } else {
+            LOG(DEBUG) << "    " << arg;
+        }
+        argv.emplace_back(arg.data());
+    }
+    argv.emplace_back(nullptr);
+    return argv;
+}
+
+pid_t ForkExecvpAsync(const std::vector<std::string>& args) {
+    auto argv = ConvertToArgv(args);
+
+    pid_t pid = fork();
+    if (pid == 0) {
+        close(STDIN_FILENO);
+        close(STDOUT_FILENO);
+        close(STDERR_FILENO);
+
+        execvp(argv[0], const_cast<char**>(argv.data()));
+        PLOG(ERROR) << "exec in ForkExecvpAsync init test";
+        _exit(EXIT_FAILURE);
+    }
+    if (pid == -1) {
+        PLOG(ERROR) << "fork in ForkExecvpAsync init test";
+        return -1;
+    }
+    return pid;
+}
+
+TEST(init, GentleKill) {
+    std::string init_script = R"init(
+service test_gentle_kill /system/bin/sleep 1000
+    disabled
+    oneshot
+    gentle_kill
+    user root
+    group root
+    seclabel u:r:toolbox:s0
+)init";
+
+    ActionManager action_manager;
+    ServiceList service_list;
+    TestInitText(init_script, BuiltinFunctionMap(), {}, &action_manager, &service_list);
+    ASSERT_EQ(std::distance(service_list.begin(), service_list.end()), 1);
+
+    auto service = service_list.begin()->get();
+    ASSERT_NE(service, nullptr);
+    ASSERT_RESULT_OK(service->Start());
+    const pid_t pid = service->pid();
+    ASSERT_GT(pid, 0);
+    EXPECT_NE(getsid(pid), 0);
+
+    TemporaryFile logfile;
+    logfile.DoNotRemove();
+    ASSERT_TRUE(logfile.fd != -1);
+
+    std::vector<std::string> cmd;
+    cmd.push_back("system/bin/strace");
+    cmd.push_back("-o");
+    cmd.push_back(logfile.path);
+    cmd.push_back("-e");
+    cmd.push_back("signal");
+    cmd.push_back("-p");
+    cmd.push_back(std::to_string(pid));
+    pid_t strace_pid = ForkExecvpAsync(cmd);
+
+    // Give strace a moment to connect
+    std::this_thread::sleep_for(1s);
+    service->Stop();
+
+    int status;
+    waitpid(strace_pid, &status, 0);
+
+    std::string logs;
+    android::base::ReadFdToString(logfile.fd, &logs);
+    int pos = logs.find("killed by SIGTERM");
+    ASSERT_NE(pos, (int)std::string::npos);
+}
+
 class TestCaseLogger : public ::testing::EmptyTestEventListener {
     void OnTestStart(const ::testing::TestInfo& test_info) override {
 #ifdef __ANDROID__
diff --git a/init/reboot.cpp b/init/reboot.cpp
index a3fc534..27a7876 100644
--- a/init/reboot.cpp
+++ b/init/reboot.cpp
@@ -51,6 +51,7 @@
 #include <bootloader_message/bootloader_message.h>
 #include <cutils/android_reboot.h>
 #include <fs_mgr.h>
+#include <libsnapshot/snapshot.h>
 #include <logwrap/logwrap.h>
 #include <private/android_filesystem_config.h>
 #include <selinux/selinux.h>
@@ -422,11 +423,31 @@
     if (run_fsck && !FindPartitionsToUmount(&block_devices, &emulated_devices, false)) {
         return UMOUNT_STAT_ERROR;
     }
-
+    auto sm = snapshot::SnapshotManager::New();
+    bool ota_update_in_progress = false;
+    if (sm->IsUserspaceSnapshotUpdateInProgress()) {
+        LOG(INFO) << "OTA update in progress";
+        ota_update_in_progress = true;
+    }
     UmountStat stat = UmountPartitions(timeout - t.duration());
     if (stat != UMOUNT_STAT_SUCCESS) {
         LOG(INFO) << "umount timeout, last resort, kill all and try";
         if (DUMP_ON_UMOUNT_FAILURE) DumpUmountDebuggingInfo();
+        // Since umount timedout, we will try to kill all processes
+        // and do one more attempt to umount the partitions.
+        //
+        // However, if OTA update is in progress, we don't want
+        // to kill the snapuserd daemon as the daemon will
+        // be serving I/O requests. Killing the daemon will
+        // end up with I/O failures. If the update is in progress,
+        // we will just return the umount failure status immediately.
+        // This is ok, given the fact that killing the processes
+        // and doing an umount is just a last effort. We are
+        // still not doing fsck when all processes are killed.
+        //
+        if (ota_update_in_progress) {
+            return stat;
+        }
         KillAllProcesses();
         // even if it succeeds, still it is timeout and do not run fsck with all processes killed
         UmountStat st = UmountPartitions(0ms);
diff --git a/init/service.cpp b/init/service.cpp
index b9b3309..87d9c3a 100644
--- a/init/service.cpp
+++ b/init/service.cpp
@@ -26,6 +26,7 @@
 #include <sys/time.h>
 #include <termios.h>
 #include <unistd.h>
+#include <thread>
 
 #include <android-base/file.h>
 #include <android-base/logging.h>
@@ -887,6 +888,10 @@
     }
 
     if (pid_) {
+        if (flags_ & SVC_GENTLE_KILL) {
+            KillProcessGroup(SIGTERM);
+            if (!process_cgroup_empty()) std::this_thread::sleep_for(200ms);
+        }
         KillProcessGroup(SIGKILL);
         NotifyStateChange("stopping");
     } else {
diff --git a/init/service.h b/init/service.h
index f9749d2..3ef8902 100644
--- a/init/service.h
+++ b/init/service.h
@@ -56,6 +56,8 @@
                                      // should not be killed during shutdown
 #define SVC_TEMPORARY 0x1000  // This service was started by 'exec' and should be removed from the
                               // service list once it is reaped.
+#define SVC_GENTLE_KILL 0x2000  // This service should be stopped with SIGTERM instead of SIGKILL
+                                // Will still be SIGKILLed after timeout period of 200 ms
 
 #define NR_SVC_SUPP_GIDS 12    // twelve supplementary groups
 
diff --git a/init/service_parser.cpp b/init/service_parser.cpp
index 24a2024..3563084 100644
--- a/init/service_parser.cpp
+++ b/init/service_parser.cpp
@@ -151,6 +151,11 @@
     return {};
 }
 
+Result<void> ServiceParser::ParseGentleKill(std::vector<std::string>&& args) {
+    service_->flags_ |= SVC_GENTLE_KILL;
+    return {};
+}
+
 Result<void> ServiceParser::ParseGroup(std::vector<std::string>&& args) {
     auto gid = DecodeUid(args[1]);
     if (!gid.ok()) {
@@ -584,6 +589,7 @@
         {"disabled",                {0,     0,    &ServiceParser::ParseDisabled}},
         {"enter_namespace",         {2,     2,    &ServiceParser::ParseEnterNamespace}},
         {"file",                    {2,     2,    &ServiceParser::ParseFile}},
+        {"gentle_kill",             {0,     0,    &ServiceParser::ParseGentleKill}},
         {"group",                   {1,     NR_SVC_SUPP_GIDS + 1, &ServiceParser::ParseGroup}},
         {"interface",               {2,     2,    &ServiceParser::ParseInterface}},
         {"ioprio",                  {2,     2,    &ServiceParser::ParseIoprio}},
diff --git a/init/service_parser.h b/init/service_parser.h
index 54503dd..670a5c6 100644
--- a/init/service_parser.h
+++ b/init/service_parser.h
@@ -53,6 +53,7 @@
     Result<void> ParseDisabled(std::vector<std::string>&& args);
     Result<void> ParseEnterNamespace(std::vector<std::string>&& args);
     Result<void> ParseGroup(std::vector<std::string>&& args);
+    Result<void> ParseGentleKill(std::vector<std::string>&& args);
     Result<void> ParsePriority(std::vector<std::string>&& args);
     Result<void> ParseInterface(std::vector<std::string>&& args);
     Result<void> ParseIoprio(std::vector<std::string>&& args);
diff --git a/init/test_upgrade_mte/mte_upgrade_test_helper.cpp b/init/test_upgrade_mte/mte_upgrade_test_helper.cpp
index 3188337..6728cc6 100644
--- a/init/test_upgrade_mte/mte_upgrade_test_helper.cpp
+++ b/init/test_upgrade_mte/mte_upgrade_test_helper.cpp
@@ -60,6 +60,10 @@
         if (prctl(PR_SET_TAGGED_ADDR_CTRL, res & ~PR_MTE_TCF_SYNC, 0, 0, 0) == -1) abort();
     }
     std::unique_ptr<volatile char[]> f(new char[1]);
+    // This out-of-bounds is on purpose: we are testing MTE, which is designed to turn
+    // out-of-bound errors into segfaults.
+    // This binary gets run by src/com/android/tests/init/MteUpgradeTest.java, which
+    // asserts that it crashes as expected.
     f[17] = 'x';
     char buf[1];
     read(1, buf, 1);
diff --git a/storaged/tests/storaged_test.cpp b/storaged/tests/storaged_test.cpp
index bb71bf3..9a281c2 100644
--- a/storaged/tests/storaged_test.cpp
+++ b/storaged/tests/storaged_test.cpp
@@ -284,6 +284,8 @@
     dsm_detect.update_mean();
     dsm_detect.update_std();
 
+    // FixLater: avoid floating point loop counters
+    // NOLINTNEXTLINE(clang-analyzer-security.FloatLoopCounter,cert-flp30-c)
     for (double i = 0; i < 2 * dsm_detect.mSigma; i += 0.5) {
         struct disk_perf test_perf;
         struct disk_perf test_mean = dsm_detect.mMean;
diff --git a/trusty/keymaster/Android.bp b/trusty/keymaster/Android.bp
index 31f0a72..b249013 100644
--- a/trusty/keymaster/Android.bp
+++ b/trusty/keymaster/Android.bp
@@ -181,3 +181,30 @@
         "-Werror",
     ],
 }
+
+cc_binary {
+    name: "trusty_keymaster_set_attestation_ids",
+    vendor: true,
+
+    srcs: [
+        "set_attestation_ids/set_attestation_ids.cpp",
+        "ipc/trusty_keymaster_ipc.cpp",
+    ],
+
+    local_include_dirs: ["include"],
+
+    shared_libs: [
+        "libbase",
+        "libc",
+        "libcrypto",
+        "liblog",
+        "libtrusty",
+        "libhardware",
+        "libkeymaster_messages",
+        "libutils",
+    ],
+    cflags: [
+        "-Wall",
+        "-Werror",
+    ],
+}
diff --git a/trusty/keymaster/set_attestation_ids/set_attestation_ids.cpp b/trusty/keymaster/set_attestation_ids/set_attestation_ids.cpp
new file mode 100644
index 0000000..e944167
--- /dev/null
+++ b/trusty/keymaster/set_attestation_ids/set_attestation_ids.cpp
@@ -0,0 +1,162 @@
+/*
+ * 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 <getopt.h>
+
+#include <string>
+
+#include <android-base/properties.h>
+#include <trusty_keymaster/ipc/trusty_keymaster_ipc.h>
+
+namespace {
+
+const char* sopts = "hb:d:p:s:M:m:i:c:";
+const struct option lopts[] = {
+        {"help", no_argument, nullptr, 'h'},
+        {"brand", required_argument, nullptr, 'b'},
+        {"device", required_argument, nullptr, 'd'},
+        {"product", required_argument, nullptr, 'p'},
+        {"serial", required_argument, nullptr, 's'},
+        {"manufacturer", required_argument, nullptr, 'M'},
+        {"model", required_argument, nullptr, 'm'},
+        {"imei", required_argument, nullptr, 'i'},
+        {"meid", required_argument, nullptr, 'c'},
+        {0, 0, 0, 0},
+};
+
+std::string buf2string(const keymaster::Buffer& buf) {
+    return std::string(reinterpret_cast<const char*>(buf.peek_read()), buf.available_read());
+}
+
+void print_usage(const char* prog, const keymaster::SetAttestationIdsRequest& req) {
+    fprintf(stderr,
+            "Usage: %s [options]\n"
+            "\n"
+            "options:\n"
+            "  -h, --help                 prints this message and exit\n"
+            "  -b, --brand <val>          set brand (default '%s')\n"
+            "  -d, --device <val>         set device (default '%s')\n"
+            "  -p, --product <val>        set product (default '%s')\n"
+            "  -s, --serial <val>         set serial (default '%s')\n"
+            "  -M, --manufacturer <val>   set manufacturer (default '%s')\n"
+            "  -m, --model <val>          set model (default '%s')\n"
+            "  -i, --imei <val>           set IMEI (default '%s')\n"
+            "  -c, --meid <val>           set MEID (default '%s')\n"
+            "\n",
+            prog, buf2string(req.brand).c_str(), buf2string(req.device).c_str(),
+            buf2string(req.product).c_str(), buf2string(req.serial).c_str(),
+            buf2string(req.manufacturer).c_str(), buf2string(req.model).c_str(),
+            buf2string(req.imei).c_str(), buf2string(req.meid).c_str());
+}
+
+void set_from_prop(keymaster::Buffer* buf, const std::string& prop) {
+    std::string prop_value = ::android::base::GetProperty(prop, /* default_value = */ "");
+    if (!prop_value.empty()) {
+        buf->Reinitialize(prop_value.data(), prop_value.size());
+    }
+}
+
+void populate_ids(keymaster::SetAttestationIdsRequest* req) {
+    set_from_prop(&req->brand, "ro.product.brand");
+    set_from_prop(&req->device, "ro.product.device");
+    set_from_prop(&req->product, "ro.product.name");
+    set_from_prop(&req->serial, "ro.serialno");
+    set_from_prop(&req->manufacturer, "ro.product.manufacturer");
+    set_from_prop(&req->model, "ro.product.model");
+}
+
+}  // namespace
+
+int main(int argc, char** argv) {
+    // By default, set attestation IDs to the values in userspace properties.
+    keymaster::SetAttestationIdsRequest req(/* ver = */ 4);
+    populate_ids(&req);
+
+    while (true) {
+        int oidx = 0;
+        int c = getopt_long(argc, argv, sopts, lopts, &oidx);
+        if (c == -1) {
+            break; /* done */
+        }
+
+        switch (c) {
+            case 'b':
+                req.brand.Reinitialize(optarg, strlen(optarg));
+                break;
+            case 'd':
+                req.device.Reinitialize(optarg, strlen(optarg));
+                break;
+            case 'p':
+                req.product.Reinitialize(optarg, strlen(optarg));
+                break;
+            case 's':
+                req.serial.Reinitialize(optarg, strlen(optarg));
+                break;
+            case 'M':
+                req.manufacturer.Reinitialize(optarg, strlen(optarg));
+                break;
+            case 'm':
+                req.model.Reinitialize(optarg, strlen(optarg));
+                break;
+            case 'i':
+                req.imei.Reinitialize(optarg, strlen(optarg));
+                break;
+            case 'c':
+                req.meid.Reinitialize(optarg, strlen(optarg));
+                break;
+            case 'h':
+                print_usage(argv[0], req);
+                exit(EXIT_SUCCESS);
+            default:
+                print_usage(argv[0], req);
+                exit(EXIT_FAILURE);
+        }
+    }
+    if (optind != argc) {
+        print_usage(argv[0], req);
+        exit(EXIT_FAILURE);
+    }
+
+    int ret = trusty_keymaster_connect();
+    if (ret) {
+        fprintf(stderr, "trusty_keymaster_connect failed: %d\n", ret);
+        return EXIT_FAILURE;
+    }
+
+    printf("Setting:\n"
+           "  brand:        %s\n"
+           "  device:       %s\n"
+           "  product:      %s\n"
+           "  serial:       %s\n"
+           "  manufacturer: %s\n"
+           "  model:        %s\n"
+           "  IMEI:         %s\n"
+           "  MEID:         %s\n",
+           buf2string(req.brand).c_str(), buf2string(req.device).c_str(),
+           buf2string(req.product).c_str(), buf2string(req.serial).c_str(),
+           buf2string(req.manufacturer).c_str(), buf2string(req.model).c_str(),
+           buf2string(req.imei).c_str(), buf2string(req.meid).c_str());
+
+    keymaster::EmptyKeymasterResponse rsp(/* ver = */ 4);
+    ret = trusty_keymaster_send(KM_SET_ATTESTATION_IDS, req, &rsp);
+    if (ret) {
+        fprintf(stderr, "SET_ATTESTATION_IDS failed: %d\n", ret);
+        trusty_keymaster_disconnect();
+        return EXIT_FAILURE;
+    }
+
+    return EXIT_SUCCESS;
+}
diff --git a/usbd/Android.bp b/usbd/Android.bp
index 27db0fa..e67759c 100644
--- a/usbd/Android.bp
+++ b/usbd/Android.bp
@@ -8,10 +8,12 @@
     srcs: ["usbd.cpp"],
     shared_libs: [
         "libbase",
+        "libbinder_ndk",
         "libhidlbase",
         "liblog",
         "libutils",
         "libhardware",
         "android.hardware.usb.gadget@1.0",
+        "android.hardware.usb.gadget-V1-ndk",
     ],
 }
diff --git a/usbd/usbd.cpp b/usbd/usbd.cpp
index 6e24d8e..0616cfb 100644
--- a/usbd/usbd.cpp
+++ b/usbd/usbd.cpp
@@ -18,43 +18,78 @@
 
 #include <string>
 
+#include <aidl/android/hardware/usb/gadget/GadgetFunction.h>
+#include <aidl/android/hardware/usb/gadget/IUsbGadget.h>
 #include <android-base/logging.h>
 #include <android-base/properties.h>
+#include <android/binder_manager.h>
+#include <android/binder_process.h>
 #include <android/hardware/usb/gadget/1.0/IUsbGadget.h>
 
-#include <hidl/HidlTransportSupport.h>
-
+using aidl::android::hardware::usb::gadget::GadgetFunction;
 using android::base::GetProperty;
 using android::base::SetProperty;
-using android::hardware::configureRpcThreadpool;
-using android::hardware::usb::gadget::V1_0::GadgetFunction;
-using android::hardware::usb::gadget::V1_0::IUsbGadget;
 using android::hardware::Return;
+using ndk::ScopedAStatus;
+using std::shared_ptr;
+
+std::atomic<int> sUsbOperationCount{};
 
 int main(int /*argc*/, char** /*argv*/) {
     if (GetProperty("ro.bootmode", "") == "charger") exit(0);
+    int operationId = sUsbOperationCount++;
 
-    configureRpcThreadpool(1, true /*callerWillJoin*/);
-    android::sp<IUsbGadget> gadget = IUsbGadget::getService();
-    Return<void> ret;
+    ABinderProcess_setThreadPoolMaxThreadCount(1);
+    ABinderProcess_startThreadPool();
+    const std::string service_name =
+            std::string(aidl::android::hardware::usb::gadget::IUsbGadget::descriptor)
+                    .append("/default");
 
-    if (gadget != nullptr) {
-        LOG(INFO) << "Usb HAL found.";
-        std::string function = GetProperty("persist.sys.usb.config", "");
-        if (function == "adb") {
-            LOG(INFO) << "peristent prop is adb";
-            SetProperty("ctl.start", "adbd");
-            ret = gadget->setCurrentUsbFunctions(static_cast<uint64_t>(GadgetFunction::ADB),
-                                                 nullptr, 0);
+    std::string function = GetProperty("persist.sys.usb.config", "");
+    if (function == "adb") {
+        LOG(INFO) << "persistent prop is adb";
+        SetProperty("ctl.start", "adbd");
+    }
+
+    if (AServiceManager_isDeclared(service_name.c_str())) {
+        shared_ptr<aidl::android::hardware::usb::gadget::IUsbGadget> gadget_aidl =
+                aidl::android::hardware::usb::gadget::IUsbGadget::fromBinder(
+                        ndk::SpAIBinder(AServiceManager_waitForService(service_name.c_str())));
+        ScopedAStatus ret;
+        if (gadget_aidl != nullptr) {
+            LOG(INFO) << "Usb AIDL HAL found.";
+            if (function == "adb") {
+                ret = gadget_aidl->setCurrentUsbFunctions(
+                        static_cast<uint64_t>(GadgetFunction::ADB), nullptr, 0, operationId);
+            } else {
+                LOG(INFO) << "Signal MTP to enable default functions";
+                ret = gadget_aidl->setCurrentUsbFunctions(
+                        static_cast<uint64_t>(GadgetFunction::MTP), nullptr, 0, operationId);
+            }
+
+            if (!ret.isOk()) LOG(ERROR) << "Error while invoking usb hal";
         } else {
-            LOG(INFO) << "Signal MTP to enable default functions";
-            ret = gadget->setCurrentUsbFunctions(static_cast<uint64_t>(GadgetFunction::MTP),
-                                                 nullptr, 0);
+            LOG(INFO) << "Usb AIDL HAL not found";
         }
-
-        if (!ret.isOk()) LOG(ERROR) << "Error while invoking usb hal";
     } else {
-        LOG(INFO) << "Usb HAL not found";
+        android::sp<android::hardware::usb::gadget::V1_0::IUsbGadget> gadget =
+                android::hardware::usb::gadget::V1_0::IUsbGadget::getService();
+        Return<void> ret;
+        if (gadget != nullptr) {
+            LOG(INFO) << "Usb HAL found.";
+            if (function == "adb") {
+                ret = gadget->setCurrentUsbFunctions(static_cast<uint64_t>(GadgetFunction::ADB),
+                                                     nullptr, 0);
+            } else {
+                LOG(INFO) << "Signal MTP to enable default functions";
+                ret = gadget->setCurrentUsbFunctions(static_cast<uint64_t>(GadgetFunction::MTP),
+                                                     nullptr, 0);
+            }
+
+            if (!ret.isOk()) LOG(ERROR) << "Error while invoking usb hal";
+        } else {
+            LOG(INFO) << "Usb HAL not found";
+        }
     }
     exit(0);
 }