diff --git a/mkbootfs/mkbootfs.c b/mkbootfs/mkbootfs.c
index 64e5a8c..d3922bf 100644
--- a/mkbootfs/mkbootfs.c
+++ b/mkbootfs/mkbootfs.c
@@ -1,20 +1,18 @@
 
 #include <ctype.h>
+#include <dirent.h>
 #include <err.h>
 #include <errno.h>
+#include <fcntl.h>
 #include <getopt.h>
+#include <stdarg.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <unistd.h>
-
-#include <sys/types.h>
 #include <sys/stat.h>
 #include <sys/sysmacros.h>
-#include <dirent.h>
-
-#include <stdarg.h>
-#include <fcntl.h>
+#include <sys/types.h>
+#include <unistd.h>
 
 #include <linux/kdev_t.h>
 
@@ -23,23 +21,12 @@
 
 /* NOTES
 **
-** - see buffer-format.txt from the linux kernel docs for
-**   an explanation of this file format
+** - see https://www.kernel.org/doc/Documentation/early-userspace/buffer-format.txt
+**   for an explanation of this file format
 ** - dotfiles are ignored
 ** - directories named 'root' are ignored
 */
 
-static void die(const char* why, ...) {
-    va_list ap;
-
-    va_start(ap, why);
-    fprintf(stderr,"error: ");
-    vfprintf(stderr, why, ap);
-    fprintf(stderr,"\n");
-    va_end(ap);
-    exit(1);
-}
-
 struct fs_config_entry {
     char* name;
     int uid, gid, mode;
@@ -48,17 +35,8 @@
 static struct fs_config_entry* canned_config = NULL;
 static const char* target_out_path = NULL;
 
-/* Each line in the canned file should be a path plus three ints (uid,
- * gid, mode). */
-#ifdef PATH_MAX
-#define CANNED_LINE_LENGTH  (PATH_MAX+100)
-#else
-#define CANNED_LINE_LENGTH  (1024)
-#endif
-
 #define TRAILER "TRAILER!!!"
 
-static int verbose = 0;
 static int total_size = 0;
 
 static void fix_stat(const char *path, struct stat *s)
@@ -134,7 +112,7 @@
 
     total_size += 6 + 8*13 + olen + 1;
 
-    if(strlen(out) != (unsigned int)olen) die("ACK!");
+    if(strlen(out) != (unsigned int)olen) errx(1, "ACK!");
 
     while(total_size & 3) {
         total_size++;
@@ -168,23 +146,16 @@
 static void _archive_dir(char *in, char *out, int ilen, int olen)
 {
     int i, t;
-    DIR *d;
     struct dirent *de;
 
-    if(verbose) {
-        fprintf(stderr,"_archive_dir('%s','%s',%d,%d)\n",
-                in, out, ilen, olen);
-    }
-
-    d = opendir(in);
-    if(d == 0) die("cannot open directory '%s'", in);
+    DIR* d = opendir(in);
+    if (d == NULL) err(1, "cannot open directory '%s'", in);
 
     int size = 32;
     int entries = 0;
     char** names = malloc(size * sizeof(char*));
     if (names == NULL) {
-      fprintf(stderr, "failed to allocate dir names array (size %d)\n", size);
-      exit(1);
+      errx(1, "failed to allocate dir names array (size %d)", size);
     }
 
     while((de = readdir(d)) != 0){
@@ -198,16 +169,12 @@
           size *= 2;
           names = realloc(names, size * sizeof(char*));
           if (names == NULL) {
-            fprintf(stderr, "failed to reallocate dir names array (size %d)\n",
-                    size);
-            exit(1);
+            errx(1, "failed to reallocate dir names array (size %d)", size);
           }
         }
         names[entries] = strdup(de->d_name);
         if (names[entries] == NULL) {
-          fprintf(stderr, "failed to strdup name \"%s\"\n",
-                  de->d_name);
-          exit(1);
+          errx(1, "failed to strdup name \"%s\"", de->d_name);
         }
         ++entries;
     }
@@ -241,26 +208,17 @@
 static void _archive(char *in, char *out, int ilen, int olen)
 {
     struct stat s;
-
-    if(verbose) {
-        fprintf(stderr,"_archive('%s','%s',%d,%d)\n",
-                in, out, ilen, olen);
-    }
-
-    if(lstat(in, &s)) die("could not stat '%s'\n", in);
+    if(lstat(in, &s)) err(1, "could not stat '%s'", in);
 
     if(S_ISREG(s.st_mode)){
-        char *tmp;
-        int fd;
+        int fd = open(in, O_RDONLY);
+        if(fd < 0) err(1, "cannot open '%s' for read", in);
 
-        fd = open(in, O_RDONLY);
-        if(fd < 0) die("cannot open '%s' for read", in);
-
-        tmp = (char*) malloc(s.st_size);
-        if(tmp == 0) die("cannot allocate %d bytes", s.st_size);
+        char* tmp = (char*) malloc(s.st_size);
+        if(tmp == 0) errx(1, "cannot allocate %zd bytes", s.st_size);
 
         if(read(fd, tmp, s.st_size) != s.st_size) {
-            die("cannot read %d bytes", s.st_size);
+            err(1, "cannot read %zd bytes", s.st_size);
         }
 
         _eject(&s, out, olen, tmp, s.st_size);
@@ -274,13 +232,13 @@
         char buf[1024];
         int size;
         size = readlink(in, buf, 1024);
-        if(size < 0) die("cannot read symlink '%s'", in);
+        if(size < 0) err(1, "cannot read symlink '%s'", in);
         _eject(&s, out, olen, buf, size);
     } else if(S_ISBLK(s.st_mode) || S_ISCHR(s.st_mode) ||
               S_ISFIFO(s.st_mode) || S_ISSOCK(s.st_mode)) {
         _eject(&s, out, olen, NULL, 0);
     } else {
-        die("Unknown '%s' (mode %d)?\n", in, s.st_mode);
+        errx(1, "Unknown '%s' (mode %d)?", in, s.st_mode);
     }
 }
 
@@ -302,17 +260,18 @@
     canned_config =
         (struct fs_config_entry*)malloc(allocated * sizeof(struct fs_config_entry));
 
-    char line[CANNED_LINE_LENGTH];
-    FILE* f = fopen(filename, "r");
-    if (f == NULL) die("failed to open canned file '%s'", filename);
+    FILE* fp = fopen(filename, "r");
+    if (fp == NULL) err(1, "failed to open canned file '%s'", filename);
 
-    while (fgets(line, CANNED_LINE_LENGTH, f) != NULL) {
+    char* line = NULL;
+    size_t allocated_len;
+    while (getline(&line, &allocated_len, fp) != -1) {
         if (!line[0]) break;
         if (used >= allocated) {
             allocated *= 2;
             canned_config = (struct fs_config_entry*)realloc(
                 canned_config, allocated * sizeof(struct fs_config_entry));
-            if (canned_config == NULL) die("failed to reallocate memory");
+            if (canned_config == NULL) errx(1, "failed to reallocate memory");
         }
 
         struct fs_config_entry* cc = canned_config + used;
@@ -332,17 +291,18 @@
         ++allocated;
         canned_config = (struct fs_config_entry*)realloc(
             canned_config, allocated * sizeof(struct fs_config_entry));
-        if (canned_config == NULL) die("failed to reallocate memory");
+        if (canned_config == NULL) errx(1, "failed to reallocate memory");
     }
     canned_config[used].name = NULL;
 
-    fclose(f);
+    free(line);
+    fclose(fp);
 }
 
 static void devnodes_desc_error(const char* filename, unsigned long line_num,
                               const char* msg)
 {
-    errx(EXIT_FAILURE, "failed to read nodes desc file '%s' line %lu: %s", filename, line_num, msg);
+    errx(1, "failed to read nodes desc file '%s' line %lu: %s", filename, line_num, msg);
 }
 
 static int append_devnodes_desc_dir(char* path, char* args)
@@ -386,15 +346,15 @@
 
 static void append_devnodes_desc(const char* filename)
 {
-    FILE* f = fopen(filename, "re");
-    if (!f) err(EXIT_FAILURE, "failed to open nodes description file '%s'", filename);
+    FILE* fp = fopen(filename, "re");
+    if (!fp) err(1, "failed to open nodes description file '%s'", filename);
 
-    char *line, *args, *type, *path;
     unsigned long line_num = 0;
-    size_t allocated_len;
 
-    while (getline(&line, &allocated_len, f) != -1) {
-        char* type;
+    char* line = NULL;
+    size_t allocated_len;
+    while (getline(&line, &allocated_len, fp) != -1) {
+        char *type, *path, *args;
 
         line_num++;
 
@@ -428,7 +388,7 @@
     }
 
     free(line);
-    fclose(f);
+    fclose(fp);
 }
 
 static const struct option long_options[] = {
@@ -448,7 +408,8 @@
             "\t-f, --file=FILE: Canned configuration file\n"
             "\t-h, --help: Print this help\n"
             "\t-n, --nodes=FILE: Dev nodes description file\n"
-            "\nDev nodes description:\n"
+            "\n"
+            "Dev nodes description:\n"
             "\t[dir|nod] [perms] [uid] [gid] [c|b] [minor] [major]\n"
             "\tExample:\n"
             "\t\t# My device nodes\n"
@@ -477,7 +438,7 @@
             break;
         default:
             usage();
-            die("Unknown option %s", argv[optind - 1]);
+            errx(1, "Unknown option %s", argv[optind - 1]);
         }
     }
 
@@ -486,7 +447,7 @@
 
     if (num_dirs <= 0) {
         usage();
-        die("no directories to process?!");
+        errx(1, "no directories to process?!");
     }
 
     while(num_dirs-- > 0){
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);
 }