diff --git a/.gitignore b/.gitignore
index e524d79..05f43e6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,3 @@
 .vscode/
+.push.settings.jsonc
 build/
diff --git a/BootControl.cpp b/BootControl.cpp
deleted file mode 100644
index 76c911b..0000000
--- a/BootControl.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-/*
- * Copyright (C) 2016 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 <string>
-
-#include "boot_control.h"
-#include "BootControl.h"
-
-BootControl::BootControl(boot_control_module *module) : mModule(module) {
-}
-
-// Methods from ::android::hardware::boot::V1_0::IBootControl follow.
-unsigned int BootControl::getNumberSlots()  {
-    return mModule->getNumberSlots(mModule);
-}
-
-unsigned int BootControl::getCurrentSlot()  {
-    return mModule->getCurrentSlot(mModule);
-}
-
-int BootControl::markBootSuccessful()  {
-    int ret = mModule->markBootSuccessful(mModule);
-    return ret;
-}
-
-int BootControl::setActiveBootSlot(unsigned int slot)  {
-    int ret = mModule->setActiveBootSlot(mModule, slot);
-    return ret;
-}
-
-int BootControl::setSlotAsUnbootable(unsigned int slot)  {
-    int ret = mModule->setSlotAsUnbootable(mModule, slot);
-    return ret;
-}
-
-BoolResult BootControl::isSlotBootable(unsigned int slot)  {
-    int32_t ret = mModule->isSlotBootable(mModule, slot);
-    if (ret < 0) {
-        return BoolResult::INVALID_SLOT;
-    }
-    return ret ? BoolResult::TRUE : BoolResult::FALSE;
-}
-
-BoolResult BootControl::isSlotMarkedSuccessful(unsigned int slot)  {
-    int32_t ret = mModule->isSlotMarkedSuccessful(mModule, slot);
-    if (ret < 0) {
-        return BoolResult::INVALID_SLOT;
-    }
-    return ret ? BoolResult::TRUE : BoolResult::FALSE;
-}
-
-std::string BootControl::getSuffix(unsigned int slot)  {
-    std::string ans;
-    const char *suffix = mModule->getSuffix(mModule, slot);
-    if (suffix) {
-        ans = std::string(suffix);
-    }
-    return ans;
-}
-
-BootControl* BootControlInit() {
-    boot_control_module* module;
-
-    // For devices that don't build a standalone libhardware bootctrl impl for recovery,
-    // we simulate the hw_get_module() by accessing it from the current process directly.
-    const boot_control_module* hw_module = &HAL_MODULE_INFO_SYM;
-    module = reinterpret_cast<boot_control_module*>(const_cast<boot_control_module*>(hw_module));
-    module->init(module);
-    return new BootControl(module);
-}
-
-int main (int argc, char * argv []) {
-    BootControl *bootctl = BootControlInit();
-    printf("======= Current slot: %d\n", bootctl->getCurrentSlot());
-    printf("======= isslotbootable: a = %d, b = %d\n", bootctl->isSlotBootable(0),
-    bootctl->isSlotBootable(1));
-    printf("======= markBootSuccessful\n", bootctl->markBootSuccessful());
-    printf("======= isSlotMarkedSuccessful: a = %d, b = %d\n", bootctl->isSlotMarkedSuccessful(0),
-    bootctl->isSlotMarkedSuccessful(1));
-    // printf("\n\n\n trying to switch to slot b: %d\n",
-    //     bootctl->setActiveBootSlot(1));
-}
diff --git a/BootControl.h b/BootControl.h
deleted file mode 100644
index bc77e3c..0000000
--- a/BootControl.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * Copyright (C) 2016 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_BOOT_V1_0_BOOTCONTROL_H
-#define ANDROID_HARDWARE_BOOT_V1_0_BOOTCONTROL_H
-
-/**
- * A result encapsulating whether a function returned true, false or
- * failed due to an invalid slot number
- */
-enum class BoolResult : int {
-    FALSE = 0,
-    TRUE = 1,
-    INVALID_SLOT = -1 /* -1 */,
-};
-
-struct BootControl {
-    BootControl(boot_control_module* module);
-    // Methods from ::android::hardware::boot::V1_0::IBootControl follow.
-    unsigned int getNumberSlots();
-    unsigned int getCurrentSlot();
-    int markBootSuccessful();
-    int setActiveBootSlot(unsigned int slot);
-    int setSlotAsUnbootable(unsigned int slot);
-    BoolResult isSlotBootable(unsigned int slot);
-    BoolResult isSlotMarkedSuccessful(unsigned int slot);
-    std::string getSuffix(unsigned int slot);
-private:
-    boot_control_module* mModule;
-};
-
-
-#endif  // ANDROID_HARDWARE_BOOT_V1_0_BOOTCONTROL_H
diff --git a/CMakeLists.txt b/CMakeLists.txt
deleted file mode 100644
index 0fc516d..0000000
--- a/CMakeLists.txt
+++ /dev/null
@@ -1,15 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-
-# set the project name
-project(qbootctl)
-
-set(BootCtrl_Files BootControl.cpp
-        boot_control_impl_qcom.cpp
-        gpt-utils.cpp)
-
-include_directories(include)
-
-# add the executable
-add_executable(qbootctl ${BootCtrl_Files})
-
-target_link_libraries(qbootctl libz.so)
\ No newline at end of file
diff --git a/README.md b/README.md
index d594938..a3a7546 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,29 @@
-Qualcomm bootctl HAL for Linux
+# Qualcomm bootctl HAL for Linux
 
 This HAL was pulled from AOSP source code and bastardised to build and run on a musl/glibc system. This may or may not render any hardware you run it on unusable, you have been warned.
 
-# Dependencies
+## Dependencies
 
-* zlib-dev
\ No newline at end of file
+* zlib-dev
+
+## Usage
+
+```
+qbootctl: qcom bootctrl HAL port for Linux
+-------------------------------------------
+qbootctl [-c|-m|-s|-u|-b|-n|-x] [SLOT]
+
+    <no args>        dump slot info (default)
+    -h               this help text
+    -c               get the current slot
+    -b SLOT          check if SLOT is marked as bootable
+    -n SLOT          check if SLOT is marked as successful
+    -x [SLOT]        get the slot suffix for SLOT (default: current)
+    -s SLOT          set to active slot to SLOT
+    -m [SLOT]        mark a boot as successful (default: current)
+    -u [SLOT]        mark SLOT as unbootable (default: current)
+```
+
+## Debugging
+
+Set `DEBUG` to 1 in `utils.h` to enable debug logging.
diff --git a/boot_control.h b/boot_control.h
deleted file mode 100644
index b506505..0000000
--- a/boot_control.h
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * Copyright (C) 2015 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_INCLUDE_HARDWARE_BOOT_CONTROL_H
-#define ANDROID_INCLUDE_HARDWARE_BOOT_CONTROL_H
-
-#define BOOT_CONTROL_MODULE_API_VERSION_0_1  HARDWARE_MODULE_API_VERSION(0, 1)
-
-/**
- * The id of this module
- */
-#define BOOT_CONTROL_HARDWARE_MODULE_ID "bootctrl"
-
-/*
- * The Boot Control HAL is designed to allow for managing sets of redundant
- * partitions, called slots, that can be booted from independantly. Slots
- * are sets of partitions whose names differ only by a given suffix.
- * They are identified here by a 0 indexed number, and associated with their
- * suffix, which can be appended to the base name for any particular partition
- * to find the one associated with that slot. The bootloader must pass the suffix
- * of the currently active slot either through a kernel command line property at
- * androidboot.slot_suffix, or the device tree at /firmware/android/slot_suffix.
- * The primary use of this set up is to allow for background updates while the
- * device is running, and to provide a fallback in the event that the update fails.
- */
-
-
-/**
- * Every hardware module must have a data structure named HAL_MODULE_INFO_SYM
- * and the fields of this data structure must begin with hw_module_t
- * followed by module specific information.
- */
-struct boot_control_module {
-
-    /*
-     * (*init)() perform any initialization tasks needed for the HAL.
-     * This is called only once.
-     */
-    void (*init)(struct boot_control_module *module);
-
-    /*
-     * (*getNumberSlots)() returns the number of available slots.
-     * For instance, a system with a single set of partitions would return
-     * 1, a system with A/B would return 2, A/B/C -> 3...
-     */
-    unsigned (*getNumberSlots)(struct boot_control_module *module);
-
-    /*
-     * (*getCurrentSlot)() returns the value letting the system know
-     * whether the current slot is A or B. The meaning of A and B is
-     * left up to the implementer. It is assumed that if the current slot
-     * is A, then the block devices underlying B can be accessed directly
-     * without any risk of corruption.
-     * The returned value is always guaranteed to be strictly less than the
-     * value returned by getNumberSlots. Slots start at 0 and
-     * finish at getNumberSlots() - 1
-     */
-    unsigned (*getCurrentSlot)(struct boot_control_module *module);
-
-    /*
-     * (*markBootSuccessful)() marks the current slot
-     * as having booted successfully
-     *
-     * Returns 0 on success, -errno on error.
-     */
-    int (*markBootSuccessful)(struct boot_control_module *module);
-
-    /*
-     * (*setActiveBootSlot)() marks the slot passed in parameter as
-     * the active boot slot (see getCurrentSlot for an explanation
-     * of the "slot" parameter). This overrides any previous call to
-     * setSlotAsUnbootable.
-     * Returns 0 on success, -errno on error.
-     */
-    int (*setActiveBootSlot)(struct boot_control_module *module, unsigned slot);
-
-    /*
-     * (*setSlotAsUnbootable)() marks the slot passed in parameter as
-     * an unbootable. This can be used while updating the contents of the slot's
-     * partitions, so that the system will not attempt to boot a known bad set up.
-     * Returns 0 on success, -errno on error.
-     */
-    int (*setSlotAsUnbootable)(struct boot_control_module *module, unsigned slot);
-
-    /*
-     * (*isSlotBootable)() returns if the slot passed in parameter is
-     * bootable. Note that slots can be made unbootable by both the
-     * bootloader and by the OS using setSlotAsUnbootable.
-     * Returns 1 if the slot is bootable, 0 if it's not, and -errno on
-     * error.
-     */
-    int (*isSlotBootable)(struct boot_control_module *module, unsigned slot);
-
-    /*
-     * (*getSuffix)() returns the string suffix used by partitions that
-     * correspond to the slot number passed in parameter. The returned string
-     * is expected to be statically allocated and not need to be freed.
-     * Returns NULL if slot does not match an existing slot.
-     */
-    const char* (*getSuffix)(struct boot_control_module *module, unsigned slot);
-
-    /*
-     * (*isSlotMarkedSucessful)() returns if the slot passed in parameter has
-     * been marked as successful using markBootSuccessful.
-     * Returns 1 if the slot has been marked as successful, 0 if it's
-     * not the case, and -errno on error.
-     */
-    int (*isSlotMarkedSuccessful)(struct boot_control_module *module, unsigned slot);
-
-    /**
-     * Returns the active slot to boot into on the next boot. If
-     * setActiveBootSlot() has been called, the getter function should return
-     * the same slot as the one provided in the last setActiveBootSlot() call.
-     */
-    unsigned (*getActiveBootSlot)(struct boot_control_module *module);
-
-    void* reserved[30];
-};
-
-extern const struct boot_control_module HAL_MODULE_INFO_SYM;
-
-
-#endif  // ANDROID_INCLUDE_HARDWARE_BOOT_CONTROL_H
diff --git a/gpt-utils.cpp b/gpt-utils.cpp
index 4020ad3..12c6159 100644
--- a/gpt-utils.cpp
+++ b/gpt-utils.cpp
@@ -38,8 +38,6 @@
 #include <errno.h>
 #include <sys/stat.h>
 #include <sys/ioctl.h>
-#include <scsi/ufs/ioctl.h>
-#include <scsi/ufs/ufs.h>
 #include <unistd.h>
 #include <linux/fs.h>
 #include <limits.h>
@@ -53,6 +51,7 @@
 #include <endian.h>
 #include <zlib.h>
 
+#include "utils.h"
 #include "gpt-utils.h"
 
 
@@ -373,7 +372,7 @@ static int gpt2_set_boot_chain(int fd, enum boot_chain boot)
     if (r)
         goto EXIT;
 
-    crc = 0; // crc32(0, pentries, pentries_array_size);
+    crc = crc32(0, pentries, pentries_array_size);
     if (GET_4_BYTES(gpt_header + PARTITION_CRC_OFFSET) != crc) {
         fprintf(stderr, "Primary GPT partition entries array CRC invalid\n");
         r = -1;
@@ -396,12 +395,12 @@ static int gpt2_set_boot_chain(int fd, enum boot_chain boot)
             goto EXIT;
     }
 
-    crc = 0; // crc32(0, pentries, pentries_array_size);
+    crc = crc32(0, pentries, pentries_array_size);
     PUT_4_BYTES(gpt_header + PARTITION_CRC_OFFSET, crc);
 
     /* header CRC is calculated with this field cleared */
     PUT_4_BYTES(gpt_header + HEADER_CRC_OFFSET, 0);
-    crc = 0; // crc32(0, gpt_header, gpt_header_size);
+    crc = crc32(0, gpt_header, gpt_header_size);
     PUT_4_BYTES(gpt_header + HEADER_CRC_OFFSET, crc);
 
     /* Write the modified GPT header back to block dev */
@@ -542,7 +541,7 @@ static int gpt_set_state(int fd, enum gpt_instance gpt, enum gpt_state state)
 
     /* header CRC is calculated with this field cleared */
     PUT_4_BYTES(gpt_header + HEADER_CRC_OFFSET, 0);
-    crc = 0; // crc32(0, gpt_header, gpt_header_size);
+    crc = crc32(0, gpt_header, gpt_header_size);
     PUT_4_BYTES(gpt_header + HEADER_CRC_OFFSET, crc);
 
     if (blk_rw(fd, 1, gpt_header_offset, gpt_header, blk_size)) {
@@ -556,127 +555,9 @@ error:
     return -1;
 }
 
-int get_scsi_node_from_bootdevice(const char *bootdev_path,
-                char *sg_node_path,
-                size_t buf_size)
-{
-        char sg_dir_path[PATH_MAX] = {0};
-        char real_path[PATH_MAX] = {0};
-        DIR *scsi_dir = NULL;
-        struct dirent *de;
-        int node_found = 0;
-        if (!bootdev_path || !sg_node_path) {
-                fprintf(stderr, "%s : invalid argument\n",
-                                 __func__);
-                goto error;
-        }
-        if (realpath(bootdev_path, real_path) < 0) {
-                        fprintf(stderr, "failed to resolve link for %s(%s)\n",
-                                        bootdev_path,
-                                        strerror(errno));
-                        goto error;
-        }
-        if(strlen(real_path) < PATH_TRUNCATE_LOC + 1){
-            fprintf(stderr, "Unrecognized path :%s:\n",
-                           real_path);
-            goto error;
-        }
-        //For the safe side in case there are additional partitions on
-        //the XBL lun we truncate the name.
-        real_path[PATH_TRUNCATE_LOC] = '\0';
-        if(strlen(real_path) < LUN_NAME_START_LOC + 1){
-            fprintf(stderr, "Unrecognized truncated path :%s:\n",
-                           real_path);
-            goto error;
-        }
-        // snprintf(sg_node_path, PATH_MAX, "%s", real_path);
-        // return 0;
-        //This will give us /dev/disk/sdb/device/scsi_generic
-        //which contains a file sgY whose name gives us the path
-        //to /dev/sgY which we return
-        snprintf(sg_dir_path, sizeof(sg_dir_path) - 1,
-                        "/sys/block/%s/device/scsi_generic",
-                        &real_path[LUN_NAME_START_LOC]);
-        scsi_dir = opendir(sg_dir_path);
-        if (!scsi_dir) {
-                fprintf(stderr, "%s : Failed to open %s(%s)\n",
-                                __func__,
-                                sg_dir_path,
-                                strerror(errno));
-                goto error;
-        }
-        while((de = readdir(scsi_dir))) {
-                if (de->d_name[0] == '.')
-                        continue;
-                else if (!strncmp(de->d_name, "sg\n", 2)) {
-                          snprintf(sg_node_path,
-                                        buf_size -1,
-                                        "/dev/%s",
-                                        de->d_name);
-                          fprintf(stderr, "%s:scsi generic node is :%s:\n",
-                                          __func__,
-                                          sg_node_path);
-                          node_found = 1;
-                          break;
-                }
-        }
-        if(!node_found) {
-                fprintf(stderr,"%s: Unable to locate scsi generic node\n",
-                               __func__);
-                goto error;
-        }
-        closedir(scsi_dir);
-        return 0;
-error:
-        if (scsi_dir)
-                closedir(scsi_dir);
-        return -1;
-}
 
-int set_boot_lun(char *sg_dev, uint8_t boot_lun_id)
-{
-        int fd = -1;
-        int rc;
-        struct ufs_ioctl_query_data *data = NULL;
-        size_t ioctl_data_size = sizeof(struct ufs_ioctl_query_data) + UFS_ATTR_DATA_SIZE;
-
-        data = (struct ufs_ioctl_query_data*)malloc(ioctl_data_size);
-        if (!data) {
-                fprintf(stderr, "%s: Failed to alloc query data struct\n",
-                                __func__);
-                goto error;
-        }
-        printf("%s(): sg_dev = %s\n", __func__, sg_dev);
-        memset(data, 0, ioctl_data_size);
-        data->opcode = UPIU_QUERY_OPCODE_WRITE_ATTR;
-        data->idn = QUERY_ATTR_IDN_BOOT_LU_EN;
-        data->buf_size = UFS_ATTR_DATA_SIZE;
-        data->buffer[0] = boot_lun_id;
-        fd = open(sg_dev, O_RDWR);
-        if (fd < 0) {
-                fprintf(stderr, "%s: Failed to open %s(%s)\n",
-                                __func__,
-                                sg_dev,
-                                strerror(errno));
-                goto error;
-        }
-        rc = ioctl(fd, UFS_IOCTL_QUERY, data);
-        if (rc) {
-                fprintf(stderr, "%s: UFS query ioctl failed(%s)\n",
-                                __func__,
-                                strerror(errno));
-                goto error;
-        }
-        close(fd);
-        free(data);
-        return 0;
-error:
-        if (fd >= 0)
-                close(fd);
-        if (data)
-                free(data);
-        return -1;
-}
+// Defined in ufs-bsg.cpp
+int32_t set_boot_lun(uint8_t lun_id);
 
 //Swtich betwieen using either the primary or the backup
 //boot LUN for boot. This is required since UFS boot partitions
@@ -698,11 +579,12 @@ error:
 int gpt_utils_set_xbl_boot_partition(enum boot_chain chain)
 {
         struct stat st;
-        ///sys/block/sdX/device/scsi_generic/
-        char sg_dev_node[PATH_MAX] = {0};
         uint8_t boot_lun_id = 0;
         const char *boot_dev = NULL;
 
+        (void)st;
+        (void)boot_dev;
+
         if (chain == BACKUP_BOOT) {
                 boot_lun_id = BOOT_LUN_B_ID;
                 if (!stat(XBL_BACKUP, &st))
@@ -740,17 +622,10 @@ int gpt_utils_set_xbl_boot_partition(enum boot_chain chain)
                                 strerror(errno));
                 goto error;
         }
-        fprintf(stderr, "%s: setting %s lun as boot lun\n",
-                        __func__,
+        LOGD("%s: setting %s lun as boot lun\n", __func__,
                         boot_dev);
-        if (get_scsi_node_from_bootdevice(boot_dev,
-                                sg_dev_node,
-                                sizeof(sg_dev_node))) {
-                fprintf(stderr, "%s: Failed to get scsi node path for xblbak\n",
-                                __func__);
-                goto error;
-        }
-        if (set_boot_lun(sg_dev_node, boot_lun_id)) {
+
+        if (set_boot_lun(boot_lun_id)) {
                 fprintf(stderr, "%s: Failed to set xblbak as boot partition\n",
                                 __func__);
                 goto error;
@@ -1050,7 +925,7 @@ int prepare_boot_update(enum boot_update_stage stage)
                         if (stat(buf, &ufs_dir_stat)) {
                                 continue;
                         }
-                        if (realpath(buf, real_path) < 0)
+                        if (!realpath(buf, real_path))
                         {
                                 fprintf(stderr, "%s: realpath error. Skipping %s\n",
                                                 __func__,
@@ -1097,11 +972,13 @@ static int get_dev_path_from_partition_name(const char *partname,
                 size_t buflen)
 {
         struct stat st;
-        int rc;
         char path[PATH_MAX] = {0};
+
+        (void)st;
+
         if (!partname || !buf || buflen < ((PATH_TRUNCATE_LOC) + 1)) {
                 fprintf(stderr, "%s: Invalid argument\n", __func__);
-                goto error;
+                return -1;
         }
         if (gpt_utils_is_ufs_device()) {
                 //Need to find the lun that holds partition partname
@@ -1110,25 +987,20 @@ static int get_dev_path_from_partition_name(const char *partname,
                                 BOOT_DEV_DIR,
                                 partname);
                 // if (rc = stat(path, &st)) {
-                //         printf("stat failed: errno=%d\n", errno);
+                //         LOGD("stat failed: errno=%d\n", errno);
                 //         goto error;
                 // }
                 buf = realpath(path, buf);
                 if (!buf)
                 {
-                        printf("realpath failed\n");
-                        goto error;
+                        return -1;
                 } else {
                         buf[PATH_TRUNCATE_LOC] = '\0';
                 }
-                printf("PATH: %s, realpath: %s\n", path, buf);
         } else {
                 snprintf(buf, buflen, "/dev/mmcblk0");
         }
         return 0;
-
-error:
-        return -1;
 }
 
 int gpt_utils_get_partition_map(vector<string>& ptn_list,
@@ -1199,7 +1071,7 @@ static int gpt_set_header(uint8_t *gpt_header, int fd,
                 goto error;
         }
         block_size = gpt_get_block_size(fd);
-        printf("%s: Block size is : %d\n", __func__, block_size);
+        LOGD("%s: Block size is : %d\n", __func__, block_size);
         if (block_size == 0) {
                 fprintf(stderr, "%s: Failed to get block size\n", __func__);
                 goto error;
@@ -1212,7 +1084,7 @@ static int gpt_set_header(uint8_t *gpt_header, int fd,
                 fprintf(stderr, "%s: Failed to get gpt header offset\n",__func__);
                 goto error;
         }
-        printf("%s: Writing back header to offset %ld\n", __func__,
+        LOGD("%s: Writing back header to offset %ld\n", __func__,
                 gpt_header_offset);
         if (blk_rw(fd, 1, gpt_header_offset, gpt_header, block_size)) {
                 fprintf(stderr, "%s: Failed to write back GPT header\n", __func__);
@@ -1359,16 +1231,16 @@ static int gpt_set_pentry_arr(uint8_t *hdr, int fd, uint8_t* arr)
                                 __func__);
                 goto error;
         }
-        printf("%s : Block size is %d\n", __func__, block_size);
+        LOGD("%s : Block size is %d\n", __func__, block_size);
         pentries_start = GET_8_BYTES(hdr + PENTRIES_OFFSET) * block_size;
         pentry_size = GET_4_BYTES(hdr + PENTRY_SIZE_OFFSET);
         pentries_arr_size =
                 GET_4_BYTES(hdr + PARTITION_COUNT_OFFSET) * pentry_size;
-        printf("%s: Writing partition entry array of size %d to offset %" PRIu64 "\n",
+        LOGD("%s: Writing partition entry array of size %d to offset %" PRIu64 "\n",
                         __func__,
                         pentries_arr_size,
                         pentries_start);
-        printf("pentries_start: %lu\n", pentries_start);
+        LOGD("pentries_start: %lu\n", pentries_start);
         rc = blk_rw(fd, 1,
                         pentries_start,
                         arr,
@@ -1436,13 +1308,13 @@ int gpt_disk_get_disk_info(const char *dev, struct gpt_disk *dsk)
         }
         gpt_header_size = GET_4_BYTES(disk->hdr + HEADER_SIZE_OFFSET);
         // FIXME: pointer offsets crc bleh
-        disk->hdr_crc = 0; //crc32(0, disk->hdr, gpt_header_size);
+        disk->hdr_crc = crc32(0, disk->hdr, gpt_header_size);
         disk->hdr_bak = gpt_get_header(dev, PRIMARY_GPT);
         if (!disk->hdr_bak) {
                 fprintf(stderr, "%s: Failed to get backup header\n", __func__);
                 goto error;
         }
-        disk->hdr_bak_crc = 0; //crc32(0, disk->hdr_bak, gpt_header_size);
+        disk->hdr_bak_crc = crc32(0, disk->hdr_bak, gpt_header_size);
 
         //Descriptor for the block device. We will use this for further
         //modifications to the partition table
@@ -1522,13 +1394,13 @@ int gpt_disk_update_crc(struct gpt_disk *disk)
                 goto error;
         }
         //Recalculate the CRC of the primary partiton array
-        disk->pentry_arr_crc = 0; // crc32(0,
-                        //disk->pentry_arr,
-                        //disk->pentry_arr_size);
+        disk->pentry_arr_crc = crc32(0,
+                        disk->pentry_arr,
+                        disk->pentry_arr_size);
         //Recalculate the CRC of the backup partition array
-        disk->pentry_arr_bak_crc = 0; // crc32(0,
-                        //disk->pentry_arr_bak,
-                        //disk->pentry_arr_size);
+        disk->pentry_arr_bak_crc = crc32(0,
+                        disk->pentry_arr_bak,
+                        disk->pentry_arr_size);
         //Update the partition CRC value in the primary GPT header
         PUT_4_BYTES(disk->hdr + PARTITION_CRC_OFFSET, disk->pentry_arr_crc);
         //Update the partition CRC value in the backup GPT header
@@ -1539,8 +1411,8 @@ int gpt_disk_update_crc(struct gpt_disk *disk)
         //Header CRC is calculated with its own CRC field set to 0
         PUT_4_BYTES(disk->hdr + HEADER_CRC_OFFSET, 0);
         PUT_4_BYTES(disk->hdr_bak + HEADER_CRC_OFFSET, 0);
-        disk->hdr_crc = 0; // crc32(0, disk->hdr, gpt_header_size);
-        disk->hdr_bak_crc = 0; // crc32(0, disk->hdr_bak, gpt_header_size);
+        disk->hdr_crc = crc32(0, disk->hdr, gpt_header_size);
+        disk->hdr_bak_crc = crc32(0, disk->hdr_bak, gpt_header_size);
         PUT_4_BYTES(disk->hdr + HEADER_CRC_OFFSET, disk->hdr_crc);
         PUT_4_BYTES(disk->hdr_bak + HEADER_CRC_OFFSET, disk->hdr_bak_crc);
         return 0;
@@ -1564,14 +1436,14 @@ int gpt_disk_commit(struct gpt_disk *disk)
                                 strerror(errno));
                 goto error;
         }
-        printf("%s: Writing back primary GPT header\n", __func__);
+        LOGD("%s: Writing back primary GPT header\n", __func__);
         //Write the primary header
         if(gpt_set_header(disk->hdr, fd, PRIMARY_GPT) != 0) {
                 fprintf(stderr, "%s: Failed to update primary GPT header\n",
                                 __func__);
                 goto error;
         }
-        printf("%s: Writing back primary partition array\n", __func__);
+        LOGD("%s: Writing back primary partition array\n", __func__);
         //Write back the primary partition array
         if (gpt_set_pentry_arr(disk->hdr, fd, disk->pentry_arr)) {
                 fprintf(stderr, "%s: Failed to write primary GPT partition arr\n",
diff --git a/include/scsi/ufs/ioctl.h b/include/scsi/ufs/ioctl.h
deleted file mode 100644
index 524ca86..0000000
--- a/include/scsi/ufs/ioctl.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/****************************************************************************
- ****************************************************************************
- ***
- ***   This header was automatically generated from a Linux kernel header
- ***   of the same name, to make information necessary for userspace to
- ***   call into the kernel available to libc.  It contains only constants,
- ***   structures, and macros generated from the original header, and thus,
- ***   contains no copyrightable information.
- ***
- ***   To edit the content of this header, modify the corresponding
- ***   source file (e.g. under external/kernel-headers/original/) then
- ***   run bionic/libc/kernel/tools/update_all.py
- ***
- ***   Any manual change here will be lost the next time this script will
- ***   be run. You've been warned!
- ***
- ****************************************************************************
- ****************************************************************************/
-#ifndef UAPI_UFS_IOCTL_H_
-#define UAPI_UFS_IOCTL_H_
-#include <linux/types.h>
-#define UFS_IOCTL_QUERY 0x5388
-struct ufs_ioctl_query_data {
-  __u32 opcode;
-  __u8 idn;
-  __u16 buf_size;
-  __u8 buffer[0];
-};
-#endif
-
diff --git a/include/scsi/ufs/ufs.h b/include/scsi/ufs/ufs.h
deleted file mode 100644
index d15229d..0000000
--- a/include/scsi/ufs/ufs.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/****************************************************************************
- ****************************************************************************
- ***
- ***   This header was automatically generated from a Linux kernel header
- ***   of the same name, to make information necessary for userspace to
- ***   call into the kernel available to libc.  It contains only constants,
- ***   structures, and macros generated from the original header, and thus,
- ***   contains no copyrightable information.
- ***
- ***   To edit the content of this header, modify the corresponding
- ***   source file (e.g. under external/kernel-headers/original/) then
- ***   run bionic/libc/kernel/tools/update_all.py
- ***
- ***   Any manual change here will be lost the next time this script will
- ***   be run. You've been warned!
- ***
- ****************************************************************************
- ****************************************************************************/
-#ifndef UAPI_UFS_H_
-#define UAPI_UFS_H_
-#define MAX_QUERY_IDN 0x18
-enum flag_idn {
-  QUERY_FLAG_IDN_FDEVICEINIT = 0x01,
-  QUERY_FLAG_IDN_PERMANENT_WPE = 0x02,
-  QUERY_FLAG_IDN_PWR_ON_WPE = 0x03,
-  QUERY_FLAG_IDN_BKOPS_EN = 0x04,
-  QUERY_FLAG_IDN_RESERVED1 = 0x05,
-  QUERY_FLAG_IDN_PURGE_ENABLE = 0x06,
-  QUERY_FLAG_IDN_RESERVED2 = 0x07,
-  QUERY_FLAG_IDN_FPHYRESOURCEREMOVAL = 0x08,
-  QUERY_FLAG_IDN_BUSY_RTC = 0x09,
-  QUERY_FLAG_IDN_MANUAL_GC_CONT = 0x0E,
-};
-enum attr_idn {
-  QUERY_ATTR_IDN_BOOT_LU_EN = 0x00,
-  QUERY_ATTR_IDN_RESERVED = 0x01,
-  QUERY_ATTR_IDN_POWER_MODE = 0x02,
-  QUERY_ATTR_IDN_ACTIVE_ICC_LVL = 0x03,
-  QUERY_ATTR_IDN_OOO_DATA_EN = 0x04,
-  QUERY_ATTR_IDN_BKOPS_STATUS = 0x05,
-  QUERY_ATTR_IDN_PURGE_STATUS = 0x06,
-  QUERY_ATTR_IDN_MAX_DATA_IN = 0x07,
-  QUERY_ATTR_IDN_MAX_DATA_OUT = 0x08,
-  QUERY_ATTR_IDN_DYN_CAP_NEEDED = 0x09,
-  QUERY_ATTR_IDN_REF_CLK_FREQ = 0x0A,
-  QUERY_ATTR_IDN_CONF_DESC_LOCK = 0x0B,
-  QUERY_ATTR_IDN_MAX_NUM_OF_RTT = 0x0C,
-  QUERY_ATTR_IDN_EE_CONTROL = 0x0D,
-  QUERY_ATTR_IDN_EE_STATUS = 0x0E,
-  QUERY_ATTR_IDN_SECONDS_PASSED = 0x0F,
-  QUERY_ATTR_IDN_CNTX_CONF = 0x10,
-  QUERY_ATTR_IDN_CORR_PRG_BLK_NUM = 0x11,
-  QUERY_ATTR_IDN_MANUAL_GC_STATUS = 0x17,
-};
-#define QUERY_ATTR_IDN_BOOT_LU_EN_MAX 0x02
-enum desc_idn {
-  QUERY_DESC_IDN_DEVICE = 0x0,
-  QUERY_DESC_IDN_CONFIGURAION = 0x1,
-  QUERY_DESC_IDN_UNIT = 0x2,
-  QUERY_DESC_IDN_RFU_0 = 0x3,
-  QUERY_DESC_IDN_INTERCONNECT = 0x4,
-  QUERY_DESC_IDN_STRING = 0x5,
-  QUERY_DESC_IDN_RFU_1 = 0x6,
-  QUERY_DESC_IDN_GEOMETRY = 0x7,
-  QUERY_DESC_IDN_POWER = 0x8,
-  QUERY_DESC_IDN_HEALTH = 0x9,
-  QUERY_DESC_IDN_RFU_2 = 0xA,
-  QUERY_DESC_IDN_MAX,
-};
-enum query_opcode {
-  UPIU_QUERY_OPCODE_NOP = 0x0,
-  UPIU_QUERY_OPCODE_READ_DESC = 0x1,
-  UPIU_QUERY_OPCODE_WRITE_DESC = 0x2,
-  UPIU_QUERY_OPCODE_READ_ATTR = 0x3,
-  UPIU_QUERY_OPCODE_WRITE_ATTR = 0x4,
-  UPIU_QUERY_OPCODE_READ_FLAG = 0x5,
-  UPIU_QUERY_OPCODE_SET_FLAG = 0x6,
-  UPIU_QUERY_OPCODE_CLEAR_FLAG = 0x7,
-  UPIU_QUERY_OPCODE_TOGGLE_FLAG = 0x8,
-  UPIU_QUERY_OPCODE_MAX,
-};
-#define UPIU_QUERY_OPCODE_HIGH_HPB 0x5500
-#define UPIU_QUERY_OPCODE_HIGH(opcode) ((opcode) >> 16)
-#define UPIU_QUERY_OPCODE_LOW(opcode) ((opcode) & 0xffff)
-#endif
-
diff --git a/meson.build b/meson.build
new file mode 100644
index 0000000..a7db403
--- /dev/null
+++ b/meson.build
@@ -0,0 +1,22 @@
+project('qbootctl', 'cpp')
+
+deps = [
+        dependency('zlib'),
+]
+
+src = [
+        'qbootctl.cpp',
+        'gpt-utils.cpp',
+        'ufs-bsg.cpp',
+]
+
+inc = [
+        include_directories('.'),
+]
+
+executable('qbootctl', src,
+        include_directories: inc,
+        dependencies: deps,
+        install: true,
+        c_args: [],
+)
diff --git a/boot_control_impl_qcom.cpp b/qbootctl.cpp
similarity index 66%
rename from boot_control_impl_qcom.cpp
rename to qbootctl.cpp
index bac5957..12f0895 100644
--- a/boot_control_impl_qcom.cpp
+++ b/qbootctl.cpp
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016, The Linux Foundation. All rights reserved.
+ * Copyright (C) 2021-2022 Caleb Connolly <caleb@connolly.tech>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -41,7 +42,7 @@
 #include <fcntl.h>
 #include <limits.h>
 
-#include "boot_control.h"
+#include "utils.h"
 #include "gpt-utils.h"
 
 #define BOOTDEV_DIR "/dev/disk/by-partlabel"
@@ -72,27 +73,41 @@ enum part_attr_type {
 	ATTR_SLOT_ACTIVE = 0,
 	ATTR_BOOT_SUCCESSFUL,
 	ATTR_UNBOOTABLE,
+	ATTR_BOOTABLE,
 };
 
-void get_kernel_cmdline_arg(const char * arg, const char* buf, const char* def)
+void get_kernel_cmdline_arg(const char * arg, char* buf, const char* def)
 {
-	// int fd = open("/proc/cmdline\n", O_RDONLY);
-	// char buf[MAX_CMDLINE_SIZE];
-	// int rc = read(fd, &buf, MAX_CMDLINE_SIZE);
-	// close(fd);
-	// std::string cmdline(buf);
-	// std::regex rgx(std::string(arg).append("=.+"))
-	printf("%s\n", __func__);
-	buf = def;
+	int fd;
+	char pcmd[MAX_CMDLINE_SIZE];
+	char *val, *found, *ptr = buf;
+	fd = open("/proc/cmdline", O_RDONLY);
+	int rc = read(fd, pcmd, MAX_CMDLINE_SIZE);
+	if (rc < 0) {
+		fprintf(stderr, "Couldn't open /proc/cmdline: %d (%s)\n", rc, strerror(errno));
+		goto error;
+	}
+	close(fd);
+	found = strstr(pcmd, arg);
+	if (!found || !(val = strstr(found, "="))) {
+		fprintf(stderr, "Couldn't find cmdline arg: '%s'\n", arg);
+		goto error;
+	}
+
+	val++;
+	// no this doesn't handle quotes lol
+	while (*val != ' ') {
+		*ptr++ = *val++;
+	}
+
+	return;
+
+error:
+	strcpy(buf, def);
 }
 
-void boot_control_init(struct boot_control_module *module)
+void boot_control_init()
 {
-	printf("%s\n", __func__);
-	if (!module) {
-		fprintf(stderr, "Invalid argument passed to %s\n", __func__);
-		return;
-	}
 	return;
 }
 
@@ -100,7 +115,6 @@ void boot_control_init(struct boot_control_module *module)
 static int get_partition_attribute(char *partname,
 		enum part_attr_type part_attr)
 {
-	printf("%s\n", __func__);
 	struct gpt_disk *disk = NULL;
 	uint8_t *pentry = NULL;
 	int retval = -1;
@@ -112,7 +126,6 @@ static int get_partition_attribute(char *partname,
 		fprintf(stderr, "%s: Failed to alloc disk struct\n", __func__);
 		goto error;
 	}
-	printf("gpt_disk_get_disk_info\n");
 	if (gpt_disk_get_disk_info(partname, disk)) {
 		fprintf(stderr, "%s: Failed to get disk info\n", __func__);
 		goto error;
@@ -125,16 +138,16 @@ static int get_partition_attribute(char *partname,
 		goto error;
 	}
 	attr = pentry + AB_FLAG_OFFSET;
-	printf("get_partition_attribute() partname = %s, attr = 0x%x\n", partname, *attr);
+	LOGD("get_partition_attribute() partname = %s, attr = 0x%x\n", partname, *attr);
 	if (part_attr == ATTR_SLOT_ACTIVE) {
 		retval = !!(*attr & AB_PARTITION_ATTR_SLOT_ACTIVE);
-		printf("ATTR_SLOT_ACTIVE, retval = %d\n", retval);
+		LOGD("ATTR_SLOT_ACTIVE, retval = %d\n", retval);
 	} else if (part_attr == ATTR_BOOT_SUCCESSFUL) {
 		retval = !!(*attr & AB_PARTITION_ATTR_BOOT_SUCCESSFUL);
-		printf("AB_PARTITION_ATTR_BOOT_SUCCESSFUL, retval = %d\n", retval);
+		LOGD("AB_PARTITION_ATTR_BOOT_SUCCESSFUL, retval = %d\n", retval);
 	} else if (part_attr == ATTR_UNBOOTABLE) {
 		retval = !!(*attr & AB_PARTITION_ATTR_UNBOOTABLE);
-		printf("AB_PARTITION_ATTR_UNBOOTABLE, retval = %d\n", retval);
+		LOGD("AB_PARTITION_ATTR_UNBOOTABLE, retval = %d\n", retval);
 	} else {
 		retval = -1;
 	}
@@ -161,9 +174,8 @@ static int update_slot_attribute(const char *slot,
 	uint8_t *attr = NULL;
 	uint8_t *attr_bak = NULL;
 	char partName[MAX_GPT_NAME_SIZE + 1] = {0};
-	const char ptn_list[][MAX_GPT_NAME_SIZE] = { AB_PTN_LIST };
+	const char ptn_list[][MAX_GPT_NAME_SIZE - 1] = { AB_PTN_LIST };
 	int slot_name_valid = 0;
-	printf("%s\n", __func__);
 	if (!slot) {
 		fprintf(stderr, "%s: Invalid argument\n", __func__);
 		goto error;
@@ -182,30 +194,30 @@ static int update_slot_attribute(const char *slot,
 		memset(buf, '\0', sizeof(buf));
 		//Check if A/B versions of this ptn exist
 		snprintf(buf, sizeof(buf) - 1,
-                                        "%s/%s%s\n",
+                                        "%s/%s%s",
                                         BOOT_DEV_DIR,
                                         ptn_list[i],
 					AB_SLOT_A_SUFFIX
 					);
-		if (stat(buf, &st)) {
+		if (stat(buf, &st) < 0) {
 			//partition does not have _a version
 			continue;
 		}
 		memset(buf, '\0', sizeof(buf));
 		snprintf(buf, sizeof(buf) - 1,
-                                        "%s/%s%s\n",
+                                        "%s/%s%s",
                                         BOOT_DEV_DIR,
                                         ptn_list[i],
 					AB_SLOT_B_SUFFIX
 					);
-		if (stat(buf, &st)) {
-			//partition does not have _a version
+		if (stat(buf, &st) < 0) {
+			//partition does not have _b version
 			continue;
 		}
 		memset(partName, '\0', sizeof(partName));
 		snprintf(partName,
 				sizeof(partName) - 1,
-				"%s%s\n",
+				"%s%s",
 				ptn_list[i],
 				slot);
 		disk = gpt_disk_alloc();
@@ -230,18 +242,27 @@ static int update_slot_attribute(const char *slot,
 			goto error;
 		}
 		attr = pentry + AB_FLAG_OFFSET;
+		LOGD("%s: got pentry for part '%s': 0x%lx (at flags: 0x%x)\n", __func__, partName, *(uint64_t*)pentry, *attr);
 		attr_bak = pentry_bak + AB_FLAG_OFFSET;
-		if (ab_attr == ATTR_BOOT_SUCCESSFUL) {
+		switch(ab_attr) {
+		case ATTR_BOOT_SUCCESSFUL:
 			*attr = (*attr) | AB_PARTITION_ATTR_BOOT_SUCCESSFUL;
 			*attr_bak = (*attr_bak) |
 				AB_PARTITION_ATTR_BOOT_SUCCESSFUL;
-		} else if (ab_attr == ATTR_UNBOOTABLE) {
+			break;
+		case ATTR_UNBOOTABLE:
 			*attr = (*attr) | AB_PARTITION_ATTR_UNBOOTABLE;
 			*attr_bak = (*attr_bak) | AB_PARTITION_ATTR_UNBOOTABLE;
-		} else if (ab_attr == ATTR_SLOT_ACTIVE) {
+			break;
+		case ATTR_BOOTABLE:
+			*attr = (*attr) ^ AB_PARTITION_ATTR_UNBOOTABLE;
+			*attr_bak = (*attr_bak) ^ AB_PARTITION_ATTR_UNBOOTABLE;
+			break;
+		case ATTR_SLOT_ACTIVE:
 			*attr = (*attr) | AB_PARTITION_ATTR_SLOT_ACTIVE;
 			*attr_bak = (*attr) | AB_PARTITION_ATTR_SLOT_ACTIVE;
-		} else {
+			break;
+		default:
 			fprintf(stderr, "%s: Unrecognized attr\n", __func__);
 			goto error;
 		}
@@ -267,15 +288,12 @@ error:
 	return -1;
 }
 
-unsigned get_number_slots(struct boot_control_module *module)
+unsigned get_number_slots()
 {
 	struct dirent *de = NULL;
 	DIR *dir_bootdev = NULL;
 	unsigned slot_count = 0;
-	if (!module) {
-		fprintf(stderr, "%s: Invalid argument\n", __func__);
-		goto error;
-	}
+
 	dir_bootdev = opendir(BOOTDEV_DIR);
 	if (!dir_bootdev) {
 		fprintf(stderr, "%s: Failed to open bootdev dir (%s)\n",
@@ -301,16 +319,12 @@ error:
 	return 0;
 }
 
-static unsigned int get_current_slot(struct boot_control_module *module)
+static unsigned int get_current_slot()
 {
 	uint32_t num_slots = 0;
 	char bootSlotProp[MAX_CMDLINE_SIZE] = {'\0'};
 	unsigned i = 0;
-	if (!module) {
-		fprintf(stderr, "%s: Invalid argument\n", __func__);
-		goto error;
-	}
-	num_slots = get_number_slots(module);
+	num_slots = get_number_slots();
 	if (num_slots <= 1) {
 		//Slot 0 is the only slot around.
 		return 0;
@@ -327,7 +341,7 @@ static unsigned int get_current_slot(struct boot_control_module *module)
 		if (!strncmp(bootSlotProp,
 					slot_suffix_arr[i],
 					strlen(slot_suffix_arr[i]))) {
-			printf("%s current_slot = %d\n", __func__, i);
+			//printf("%s current_slot = %d\n", __func__, i);
 			return i;
 		}
 	}
@@ -338,13 +352,9 @@ error:
 	return 0;
 }
 
-static int boot_control_check_slot_sanity(struct boot_control_module *module,
-		unsigned slot)
+static int boot_control_check_slot_sanity(unsigned slot)
 {
-	printf("%s\n", __func__);
-	if (!module)
-		return -1;
-	uint32_t num_slots = get_number_slots(module);
+	uint32_t num_slots = get_number_slots();
 	if ((num_slots < 1) || (slot > num_slots - 1)) {
 		fprintf(stderr, "Invalid slot number");
 		return -1;
@@ -353,29 +363,60 @@ static int boot_control_check_slot_sanity(struct boot_control_module *module,
 
 }
 
-int mark_boot_successful(struct boot_control_module *module)
+int get_boot_attr(unsigned slot, enum part_attr_type attr)
 {
-	printf("%s\n", __func__);
-	unsigned cur_slot = 0;
-	if (!module) {
-		fprintf(stderr, "%s: Invalid argument\n", __func__);
-		goto error;
+	char bootPartition[MAX_GPT_NAME_SIZE + 1] = {0};
+
+	if (boot_control_check_slot_sanity(slot) != 0) {
+		fprintf(stderr, "%s: Argument check failed\n", __func__);
+		return -1;
+	}
+	snprintf(bootPartition,
+			sizeof(bootPartition) - 1, "boot%s",
+			slot_suffix_arr[slot]);
+	
+	return get_partition_attribute(bootPartition, attr);
+}
+
+int is_slot_bootable(unsigned slot)
+{
+	int attr = 0;
+	attr = get_boot_attr(slot, ATTR_UNBOOTABLE);
+	if (attr >= 0)
+		return !attr;
+
+	return -1;
+}
+
+int mark_boot_successful(unsigned slot)
+{
+	int successful = get_boot_attr(slot, ATTR_BOOT_SUCCESSFUL);
+
+	if (!is_slot_bootable(slot)) {
+		printf("SLOT %s: was marked unbootable, fixing this"
+		       " (I hope you know what you're doing...)\n", slot_suffix_arr[slot]);
+		update_slot_attribute(slot_suffix_arr[slot],
+				ATTR_BOOTABLE);
 	}
-	cur_slot = get_current_slot(module);
-	if (update_slot_attribute(slot_suffix_arr[cur_slot],
+
+	if (successful) {
+		fprintf(stderr, "SLOT %s: already marked successful\n", slot_suffix_arr[slot]);
+		return -1;
+	}
+
+	if (update_slot_attribute(slot_suffix_arr[slot],
 				ATTR_BOOT_SUCCESSFUL)) {
 		goto error;
 	}
 	return 0;
 error:
-	fprintf(stderr, "%s: Failed to mark boot successful\n", __func__);
+	fprintf(stderr, "SLOT %s: Failed to mark boot successful\n", slot_suffix_arr[slot]);
 	return -1;
 }
 
-const char *get_suffix(struct boot_control_module *module, unsigned slot)
+const char *get_suffix(unsigned slot)
 {
-	printf("%s\n", __func__);
-	if (boot_control_check_slot_sanity(module, slot) != 0)
+	if (boot_control_check_slot_sanity(slot) != 0)
 		return NULL;
 	else
 		return slot_suffix_arr[slot];
@@ -386,7 +427,6 @@ const char *get_suffix(struct boot_control_module *module, unsigned slot)
 //partition.
 static struct gpt_disk* boot_ctl_get_disk_info(char *partition)
 {
-	printf("%s\n", __func__);
 	struct gpt_disk *disk = NULL;
 	if (!partition)
 		return NULL;
@@ -413,13 +453,13 @@ error:
 static int boot_ctl_set_active_slot_for_partitions(vector<string> part_list,
 		unsigned slot)
 {
-	printf("%s\n", __func__);
 	char buf[PATH_MAX] = {0};
 	struct gpt_disk *disk = NULL;
 	char slotA[MAX_GPT_NAME_SIZE + 1] = {0};
 	char slotB[MAX_GPT_NAME_SIZE + 1] = {0};
 	char active_guid[TYPE_GUID_SIZE + 1] = {0};
 	char inactive_guid[TYPE_GUID_SIZE + 1] = {0};
+	int rc;
 	//Pointer to the partition entry of current 'A' partition
 	uint8_t *pentryA = NULL;
 	uint8_t *pentryA_bak = NULL;
@@ -429,34 +469,45 @@ static int boot_ctl_set_active_slot_for_partitions(vector<string> part_list,
 	struct stat st;
 	vector<string>::iterator partition_iterator;
 
+	LOGD("Marking slot %s as active:\n", slot_suffix_arr[slot]);
+
 	for (partition_iterator = part_list.begin();
 			partition_iterator != part_list.end();
 			partition_iterator++) {
 		//Chop off the slot suffix from the partition name to
 		//make the string easier to work with.
 		string prefix = *partition_iterator;
+		LOGD("Part: %s\n", prefix.c_str());
 		if (prefix.size() < (strlen(AB_SLOT_A_SUFFIX) + 1)) {
 			fprintf(stderr, "Invalid partition name: %s\n", prefix.c_str());
 			goto error;
 		}
 		prefix.resize(prefix.size() - strlen(AB_SLOT_A_SUFFIX));
 		//Check if A/B versions of this ptn exist
-		snprintf(buf, sizeof(buf) - 1, "%s/%s%s\n", BOOT_DEV_DIR,
+		snprintf(buf, sizeof(buf) - 1, "%s/%s%s", BOOT_DEV_DIR,
 				prefix.c_str(),
 				AB_SLOT_A_SUFFIX);
-		if (stat(buf, &st))
+		LOGD("\t_a Path: '%s'\n", buf);
+		rc = stat(buf, &st);
+		if (rc < 0) {
+			fprintf(stderr, "Failed to stat() path: %d: %s\n", rc, strerror(errno));
 			continue;
+		}
 		memset(buf, '\0', sizeof(buf));
-		snprintf(buf, sizeof(buf) - 1, "%s/%s%s\n", BOOT_DEV_DIR,
+		snprintf(buf, sizeof(buf) - 1, "%s/%s%s", BOOT_DEV_DIR,
 				prefix.c_str(),
 				AB_SLOT_B_SUFFIX);
-		if (stat(buf, &st))
+		LOGD("\t_b Path: '%s'\n", buf);
+		rc = stat(buf, &st);
+		if (rc < 0) {
+			fprintf(stderr, "Failed to stat() path: %d: %s\n", rc, strerror(errno));
 			continue;
+		}
 		memset(slotA, 0, sizeof(slotA));
 		memset(slotB, 0, sizeof(slotA));
-		snprintf(slotA, sizeof(slotA) - 1, "%s%s\n", prefix.c_str(),
+		snprintf(slotA, sizeof(slotA) - 1, "%s%s", prefix.c_str(),
 				AB_SLOT_A_SUFFIX);
-		snprintf(slotB, sizeof(slotB) - 1,"%s%s\n", prefix.c_str(),
+		snprintf(slotB, sizeof(slotB) - 1,"%s%s", prefix.c_str(),
 				AB_SLOT_B_SUFFIX);
 		//Get the disk containing the partitions that were passed in.
 		//All partitions passed in must lie on the same disk.
@@ -478,6 +529,12 @@ static int boot_ctl_set_active_slot_for_partitions(vector<string> part_list,
 					prefix.c_str());
 			goto error;
 		}
+		LOGD("\tAB attr (A): 0x%x (backup: 0x%x)\n",
+			*(uint16_t*)(pentryA + AB_FLAG_OFFSET),
+			*(uint16_t*)(pentryA_bak + AB_FLAG_OFFSET));
+		LOGD("\tAB attr (B): 0x%x (backup: 0x%x)\n",
+			*(uint16_t*)(pentryB + AB_FLAG_OFFSET),
+			*(uint16_t*)(pentryB_bak + AB_FLAG_OFFSET));
 		memset(active_guid, '\0', sizeof(active_guid));
 		memset(inactive_guid, '\0', sizeof(inactive_guid));
 		if (get_partition_attribute(slotA, ATTR_SLOT_ACTIVE) == 1) {
@@ -497,6 +554,8 @@ static int boot_ctl_set_active_slot_for_partitions(vector<string> part_list,
 			fprintf(stderr, "Both A & B are inactive..Aborting");
 			goto error;
 		}
+		// printf("\tActive GUID: %s\n", active_guid);
+		// printf("\tInactive GUID: %s\n", active_guid);
 		if (!strncmp(slot_suffix_arr[slot], AB_SLOT_A_SUFFIX,
 					strlen(AB_SLOT_A_SUFFIX))){
 			//Mark A as active in primary table
@@ -522,12 +581,10 @@ static int boot_ctl_set_active_slot_for_partitions(vector<string> part_list,
 			fprintf(stderr, "%s: Unknown slot suffix!\n", __func__);
 			goto error;
 		}
-		if (disk) {
-			if (gpt_disk_update_crc(disk) != 0) {
-				fprintf(stderr, "%s: Failed to update gpt_disk crc\n",
-						__func__);
-				goto error;
-			}
+		if (gpt_disk_update_crc(disk) != 0) {
+			fprintf(stderr, "%s: Failed to update gpt_disk crc\n",
+					__func__);
+			goto error;
 		}
 	}
 	//write updated content to disk
@@ -546,39 +603,26 @@ error:
 	return -1;
 }
 
-unsigned get_active_boot_slot(struct boot_control_module *module)
+unsigned get_active_boot_slot()
 {
-	printf("%s\n", __func__);
-	if (!module) {
-		fprintf(stderr, "%s: Invalid argument\n", __func__);
-		// The HAL spec requires that we return a number between
-		// 0 to num_slots - 1. Since something went wrong here we
-		// are just going to return the default slot.
-		return 0;
-	}
 
-	uint32_t num_slots = get_number_slots(module);
+	uint32_t num_slots = get_number_slots();
 	if (num_slots <= 1) {
 		//Slot 0 is the only slot around.
 		return 0;
 	}
 
 	for (uint32_t i = 0; i < num_slots; i++) {
-		char bootPartition[MAX_GPT_NAME_SIZE + 1] = {0};
-		snprintf(bootPartition, sizeof(bootPartition) - 1, "boot%s",
-		         slot_suffix_arr[i]);
-		if (get_partition_attribute(bootPartition, ATTR_SLOT_ACTIVE) == 1) {
+		if (get_boot_attr(i, ATTR_SLOT_ACTIVE))
 			return i;
-		}
 	}
 
 	fprintf(stderr, "%s: Failed to find the active boot slot\n", __func__);
 	return 0;
 }
 
-int set_active_boot_slot(struct boot_control_module *module, unsigned slot)
+int set_active_boot_slot(unsigned slot)
 {
-	printf("%s\n", __func__);
 	map<string, vector<string>> ptn_map;
 	vector<string> ptn_vec;
 	const char ptn_list[][MAX_GPT_NAME_SIZE] = { AB_PTN_LIST };
@@ -587,10 +631,11 @@ int set_active_boot_slot(struct boot_control_module *module, unsigned slot)
 	int is_ufs = gpt_utils_is_ufs_device();
 	map<string, vector<string>>::iterator map_iter;
 
-	if (boot_control_check_slot_sanity(module, slot)) {
+	if (boot_control_check_slot_sanity(slot)) {
 		fprintf(stderr, "%s: Bad arguments\n", __func__);
 		goto error;
 	}
+
 	//The partition list just contains prefixes(without the _a/_b) of the
 	//partitions that support A/B. In order to get the layout we need the
 	//actual names. To do this we append the slot suffix to every member
@@ -650,10 +695,9 @@ error:
 	return -1;
 }
 
-int set_slot_as_unbootable(struct boot_control_module *module, unsigned slot)
+int set_slot_as_unbootable(unsigned slot)
 {
-	printf("%s\n", __func__);
-	if (boot_control_check_slot_sanity(module, slot) != 0) {
+	if (boot_control_check_slot_sanity(slot) != 0) {
 		fprintf(stderr, "%s: Argument check failed\n", __func__);
 		goto error;
 	}
@@ -667,32 +711,12 @@ error:
 	return -1;
 }
 
-int is_slot_bootable(struct boot_control_module *module, unsigned slot)
+int is_slot_marked_successful(unsigned slot)
 {
-	printf("%s\n", __func__);
 	int attr = 0;
 	char bootPartition[MAX_GPT_NAME_SIZE + 1] = {0};
 
-	if (boot_control_check_slot_sanity(module, slot) != 0) {
-		fprintf(stderr, "%s: Argument check failed\n", __func__);
-		goto error;
-	}
-	snprintf(bootPartition,
-			sizeof(bootPartition) - 1, "boot%s",
-			slot_suffix_arr[slot]);
-	attr = get_partition_attribute(bootPartition, ATTR_UNBOOTABLE);
-	if (attr >= 0)
-		return !attr;
-error:
-	return -1;
-}
-
-int is_slot_marked_successful(struct boot_control_module *module, unsigned slot)
-{
-	int attr = 0;
-	char bootPartition[MAX_GPT_NAME_SIZE + 1] = {0};
-
-	if (boot_control_check_slot_sanity(module, slot) != 0) {
+	if (boot_control_check_slot_sanity(slot) != 0) {
 		fprintf(stderr, "%s: Argument check failed\n", __func__);
 		goto error;
 	}
@@ -700,13 +724,14 @@ int is_slot_marked_successful(struct boot_control_module *module, unsigned slot)
 			sizeof(bootPartition) - 1,
 			"boot%s", slot_suffix_arr[slot]);
 	attr = get_partition_attribute(bootPartition, ATTR_BOOT_SUCCESSFUL);
-	printf("%s(): slot = %d, attr = 0x%x\n", __func__, slot, attr);
+	LOGD("%s: slot = %d, attr = 0x%x\n", __func__, slot, attr);
 	if (attr >= 0)
 		return attr;
 error:
 	return -1;
 }
 
+/*
 const struct boot_control_module HAL_MODULE_INFO_SYM = {
 	.init = boot_control_init,
 	.getNumberSlots = get_number_slots,
@@ -719,3 +744,158 @@ const struct boot_control_module HAL_MODULE_INFO_SYM = {
 	.isSlotMarkedSuccessful = is_slot_marked_successful,
 	.getActiveBootSlot = get_active_boot_slot,
 };
+*/
+
+struct slot_info {
+	bool active;
+	bool successful;
+	bool bootable;
+};
+
+int usage()
+{
+   fprintf(stderr, "qbootctl: qcom bootctrl HAL port for Linux\n");
+   fprintf(stderr, "-------------------------------------------\n");
+   fprintf(stderr, "qbootctl [-c|-m|-s|-u|-b|-n|-x] [SLOT]\n\n");
+   fprintf(stderr, "    <no args>        dump slot info (default)\n");
+   fprintf(stderr, "    -h               this help text\n");
+   fprintf(stderr, "    -c               get the current slot\n");
+   fprintf(stderr, "    -a               get the active slot\n");
+   fprintf(stderr, "    -b SLOT          check if SLOT is marked as bootable\n");
+   fprintf(stderr, "    -n SLOT          check if SLOT is marked as successful\n");
+   fprintf(stderr, "    -x [SLOT]        get the slot suffix for SLOT (default: current)\n");
+   fprintf(stderr, "    -s SLOT          set to active slot to SLOT\n");
+   fprintf(stderr, "    -m [SLOT]        mark a boot as successful (default: current)\n");
+   fprintf(stderr, "    -u [SLOT]        mark SLOT as unbootable (default: current)\n");
+
+   return 1;
+}
+
+int get_slot_info(struct slot_info *slots) {
+	int rc;
+	uint32_t active_slot = get_active_boot_slot();
+
+	slots[active_slot].active = true;
+
+	for (size_t i = 0; i < 2; i++)
+	{
+		rc = is_slot_marked_successful(i);
+		if (rc < 0)
+			return rc;
+		slots[i].successful = rc;
+		rc = is_slot_bootable(i);
+		if (rc < 0)
+			return rc;
+		slots[i].bootable = rc;
+	}
+
+	return 0;
+}
+
+void dump_info() {
+	struct slot_info slots[2] = {{0}};
+	int current_slot = get_current_slot();
+	
+	get_slot_info(slots);
+
+	printf("Current slot: %s\n", current_slot >= 0 ? slot_suffix_arr[current_slot] : "N/A");
+	for (size_t i = 0; i < 2; i++)
+	{
+		printf("SLOT %s:\n", slot_suffix_arr[i]);
+		printf("\tActive      : %d\n", slots[i].active);
+		printf("\tSuccessful  : %d\n", slots[i].successful);
+		printf("\tBootable    : %d\n", slots[i].bootable);
+	}
+}
+
+int main (int argc, char **argv) {
+	int optflag;
+	int slot = -1;
+	int rc;
+	char *end;
+
+	switch(argc) {
+	case 1:
+		dump_info();
+		return 0;
+	case 2:
+		break;
+	case 3:
+		slot = (int)strtol(argv[2], &end, 10);
+		printf("Using slot %d\n", slot);
+		if (end == argv[2] || slot < 0 || slot > 1) {
+			fprintf(stderr, "Expected slot to be '0' or '1' not '%s'\n", argv[2]);
+			return 1;
+		}
+		break;
+	default:
+		return usage();
+	}
+
+	if (slot < 0)
+		slot = get_current_slot();
+
+	optflag = getopt(argc, argv, "hcmas:ub:n:x");
+
+	switch(optflag) {
+	case 'c':
+		slot = get_current_slot();
+		printf("Current slot: %s\n", slot_suffix_arr[slot]);
+		return 0;
+	case 'a':
+		slot = get_active_boot_slot();
+		printf("Active slot: %s\n", slot_suffix_arr[slot]);
+		return 0;
+	case 'b':
+		printf("SLOT %s: is %smarked bootable\n", slot_suffix_arr[slot],
+			is_slot_bootable(slot) == 1 ? "" : "not ");
+		return 0;
+	case 'n':
+		printf("SLOT %s: is %smarked successful\n", slot_suffix_arr[slot],
+			is_slot_marked_successful(slot) == 1 ? "" : "not ");
+		return 0;
+	case 'x':
+		printf("%s\n", slot_suffix_arr[slot]);
+		return 0;
+	case 's':
+		rc = set_active_boot_slot(slot);
+		if (rc < 0) {
+			fprintf(stderr, "SLOT %s: Failed to set active\n",
+					slot_suffix_arr[slot]);
+			return 1;
+		}
+		printf("SLOT %d: Set as active slot\n", slot);
+		return 0;
+	case 'm':
+		rc = mark_boot_successful(slot);
+		if (rc < 0)
+			return 1;
+		printf("SLOT %s: Marked boot successful\n", slot_suffix_arr[slot]);
+		return 0;
+	case 'u':
+		rc = set_slot_as_unbootable(slot);
+		if (rc < 0) {
+			fprintf(stderr, "SLOT %s: Failed to set as unbootable\n",
+					slot_suffix_arr[slot]);
+			return 1;
+		}
+		printf("SLOT %s: Set as unbootable\n", slot_suffix_arr[slot]);
+		return 0;
+	case 'h':
+	default:
+		usage();
+		return 0;
+	}
+//     printf("======= Current slot: %d\n", get_current_slot());
+//     printf("======= isslotbootable: a = %d, b = %d\n", is_slot_bootable(0),
+//     is_slot_bootable(1));
+//     printf("======= markBootSuccessful: %d\n", bootctl->markBootSuccessful());
+//     printf("======= isSlotMarkedSuccessful: a = %d, b = %d\n", bootctl->isSlotMarkedSuccessful(0),
+//     bootctl->isSlotMarkedSuccessful(1));
+//     printf("\n\n\n trying to switch to slot b: %d\n",
+//         bootctl->setActiveBootSlot(1));
+
+	return 0;
+}
+
+
diff --git a/ufs-bsg.cpp b/ufs-bsg.cpp
new file mode 100644
index 0000000..dd9de6a
--- /dev/null
+++ b/ufs-bsg.cpp
@@ -0,0 +1,206 @@
+/*
+ * Copyright (c) 2020 The Linux Foundation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above
+ *       copyright notice, this list of conditions and the following
+ *       disclaimer in the documentation and/or other materials provided
+ *       with the distribution.
+ *     * Neither the name of The Linux Foundation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <linux/bsg.h>
+#include <scsi/scsi_bsg_ufs.h>
+#include <endian.h>
+#include <dirent.h>
+#include <string.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include "utils.h"
+#include "ufs-bsg.h"
+
+// FIXME: replace this with something that actually works
+// static int get_ufs_bsg_dev(void)
+// {
+//     DIR *dir;
+//     struct dirent *ent;
+//     int ret = -ENODEV;
+
+//     if ((dir = opendir ("/dev")) != NULL) {
+//         /* read all the files and directories within directory */
+//         while ((ent = readdir(dir)) != NULL) {
+//             if (!strcmp(ent->d_name, "bsg") ||
+//                     !strcmp(ent->d_name, "ufs-bsg0")) {
+//                 snprintf(ufs_bsg_dev, FNAME_SZ, "/dev/%s", ent->d_name);
+//                 ret = 0;
+//                 break;
+//             }
+//         }
+//         if (ret)
+//             fprintf(stderr, "could not find the ufs-bsg dev\n");
+//         closedir (dir);
+//     } else {
+//         /* could not open directory */
+//         fprintf(stderr, "could not open /dev (error no: %d)\n", errno);
+//         ret = -EINVAL;
+//     }
+
+//     return ret;
+// }
+
+int ufs_bsg_dev_open(void)
+{
+    int ret;
+    if (!fd_ufs_bsg) {
+        fd_ufs_bsg = open(ufs_bsg_dev, O_RDWR);
+        ret = errno;
+        if (fd_ufs_bsg < 0) {
+            fprintf(stderr, "Unable to open %s (error no: %d)",
+                    ufs_bsg_dev, errno);
+            fd_ufs_bsg = 0;
+            return ret;
+        }
+    }
+    return 0;
+}
+
+void ufs_bsg_dev_close(void)
+{
+    if (fd_ufs_bsg) {
+        close(fd_ufs_bsg);
+        fd_ufs_bsg = 0;
+    }
+}
+
+static int ufs_bsg_ioctl(int fd, struct ufs_bsg_request *req,
+        struct ufs_bsg_reply *rsp, __u8 *buf, __u32 buf_len,
+        enum bsg_ioctl_dir dir)
+{
+    int ret;
+    struct sg_io_v4 sg_io{};
+
+    sg_io.guard = 'Q';
+    sg_io.protocol = BSG_PROTOCOL_SCSI;
+    sg_io.subprotocol = BSG_SUB_PROTOCOL_SCSI_TRANSPORT;
+    sg_io.request_len = sizeof(*req);
+    sg_io.request = (__u64)req;
+    sg_io.response = (__u64)rsp;
+    sg_io.max_response_len = sizeof(*rsp);
+    if (dir == BSG_IOCTL_DIR_FROM_DEV) {
+        sg_io.din_xfer_len = buf_len;
+        sg_io.din_xferp = (__u64)(buf);
+    } else {
+        sg_io.dout_xfer_len = buf_len;
+        sg_io.dout_xferp = (__u64)(buf);
+    }
+
+    ret = ioctl(fd, SG_IO, &sg_io);
+    if (ret)
+        fprintf(stderr, "%s: Error from sg_io ioctl (return value: %d, error no: %d, reply result from LLD: %d\n)",
+                __func__, ret, errno, rsp->result);
+
+    if (sg_io.info || rsp->result) {
+        fprintf(stderr, "%s: Error from sg_io info (check sg info: device_status: 0x%x, transport_status: 0x%x, driver_status: 0x%x, reply result from LLD: %d\n)",
+                __func__, sg_io.device_status, sg_io.transport_status,
+                sg_io.driver_status, rsp->result);
+        ret = -EAGAIN;
+    }
+
+    return ret;
+}
+
+static void compose_ufs_bsg_query_req(struct ufs_bsg_request *req, __u8 func,
+        __u8 opcode, __u8 idn, __u8 index, __u8 sel,
+        __u16 length)
+{
+    struct utp_upiu_header *hdr = &req->upiu_req.header;
+    struct utp_upiu_query *qr = &req->upiu_req.qr;
+
+    req->msgcode = UTP_UPIU_QUERY_REQ;
+    hdr->dword_0 = DWORD(UTP_UPIU_QUERY_REQ, 0, 0, 0);
+    hdr->dword_1 = DWORD(0, func, 0, 0);
+    hdr->dword_2 = DWORD(0, 0, length >> 8, (__u8)length);
+    qr->opcode = opcode;
+    qr->idn = idn;
+    qr->index = index;
+    qr->selector = sel;
+    qr->length = htobe16(length);
+}
+
+
+static int ufs_query_attr(int fd, __u32 value,
+        __u8 func, __u8 opcode, __u8 idn,
+        __u8 index, __u8 sel)
+{
+    struct ufs_bsg_request req{};
+    struct ufs_bsg_reply rsp{};
+    enum bsg_ioctl_dir dir = BSG_IOCTL_DIR_FROM_DEV;
+    int ret = 0;
+
+    if (opcode == QUERY_REQ_OP_WRITE_DESC || opcode == QUERY_REQ_OP_WRITE_ATTR)
+        dir = BSG_IOCTL_DIR_TO_DEV;
+
+    req.upiu_req.qr.value = htobe32(value);
+
+    compose_ufs_bsg_query_req(&req, func, opcode, idn, index, sel, 0);
+
+    ret = ufs_bsg_ioctl(fd, &req, &rsp, 0, 0, dir);
+    if (ret)
+        fprintf(stderr, "%s: Error from ufs_bsg_ioctl (return value: %d, error no: %d\n)",
+                __func__, ret, errno);
+
+    return ret;
+}
+
+int32_t set_boot_lun(__u8 lun_id)
+{
+    int32_t ret;
+    __u32 boot_lun_id  = lun_id;
+
+//     ret = get_ufs_bsg_dev();
+//     if (ret)
+//         return ret;
+    LOGD("Using UFS bsg device: %s\n", ufs_bsg_dev);
+
+    ret = ufs_bsg_dev_open();
+    if (ret)
+        return ret;
+    LOGD("Opened ufs bsg dev: %s\n", ufs_bsg_dev);
+
+    ret = ufs_query_attr(fd_ufs_bsg, boot_lun_id, QUERY_REQ_FUNC_STD_WRITE,
+            QUERY_REQ_OP_WRITE_ATTR, QUERY_ATTR_IDN_BOOT_LU_EN, 0, 0);
+    if (ret) {
+        fprintf(stderr, "Error requesting ufs attr idn %d via query ioctl (return value: %d, error no: %d)",
+                QUERY_ATTR_IDN_BOOT_LU_EN, ret, errno);
+        goto out;
+    }
+out:
+    ufs_bsg_dev_close();
+    return ret;
+}
diff --git a/ufs-bsg.h b/ufs-bsg.h
new file mode 100644
index 0000000..a581b31
--- /dev/null
+++ b/ufs-bsg.h
@@ -0,0 +1,95 @@
+#ifndef __RECOVERY_UFS_BSG_H__
+#define __RECOVERY_UFS_BSG_H__
+
+/*
+ * Copyright (c) 2020 The Linux Foundation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above
+ *       copyright notice, this list of conditions and the following
+ *       disclaimer in the documentation and/or other materials provided
+ *       with the distribution.
+ *     * Neither the name of The Linux Foundation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define FNAME_SZ 64
+
+#define SG_IO  0x2285
+
+#define DWORD(b3, b2, b1, b0) htobe32((b3 << 24) | (b2 << 16) |\
+        (b1 << 8) | b0)
+
+/* UFS BSG device node */
+char ufs_bsg_dev[FNAME_SZ] = "/dev/bsg/ufs-bsg0";
+
+int fd_ufs_bsg;
+
+/* UPIU Transaction Codes */
+enum {
+    UTP_UPIU_NOP_OUT    = 0x00,
+    UTP_UPIU_COMMAND    = 0x01,
+    UTP_UPIU_DATA_OUT   = 0x02,
+    UTP_UPIU_TASK_REQ   = 0x04,
+    UTP_UPIU_QUERY_REQ  = 0x16,
+};
+
+/* UPIU Query Function field */
+enum {
+    QUERY_REQ_FUNC_STD_READ    = 0x01,
+    QUERY_REQ_FUNC_STD_WRITE   = 0x81,
+};
+
+enum query_req_opcode {
+    QUERY_REQ_OP_READ_DESC    = 0x1,
+    QUERY_REQ_OP_WRITE_DESC   = 0x2,
+    QUERY_REQ_OP_READ_ATTR    = 0x3,
+    QUERY_REQ_OP_WRITE_ATTR   = 0x4,
+    QUERY_REQ_OP_READ_FLAG    = 0x5,
+    QUERY_REQ_OP_SET_FLAG     = 0x6,
+    QUERY_REQ_OP_CLEAR_FLAG   = 0x7,
+    QUERY_REQ_OP_TOGGLE_FLAG  = 0x8,
+};
+
+enum query_desc_idn {
+    QUERY_DESC_IDN_DEVICE    = 0x0,
+    QUERY_DESC_IDN_UNIT      = 0x2,
+    QUERY_DESC_IDN_GEOMETRY  = 0x7,
+};
+
+enum query_desc_size {
+    QUERY_DESC_SIZE_DEVICE    = 0x40,
+    QUERY_DESC_SIZE_GEOMETRY  = 0x48,
+    QUERY_DESC_SIZE_UNIT      = 0x23,
+};
+
+enum bsg_ioctl_dir {
+    BSG_IOCTL_DIR_TO_DEV,
+    BSG_IOCTL_DIR_FROM_DEV,
+};
+
+enum query_attr_idn {
+    QUERY_ATTR_IDN_BOOT_LU_EN        = 0x00,
+    QUERY_ATTR_IDN_RESERVED          = 0x01,
+    QUERY_ATTR_IDN_POWER_MODE        = 0x02,
+    QUERY_ATTR_IDN_ACTIVE_ICC_LVL    = 0x03,
+};
+
+#endif /* __RECOVERY_UFS_BSG_H__ */
diff --git a/utils.h b/utils.h
new file mode 100644
index 0000000..9ac103f
--- /dev/null
+++ b/utils.h
@@ -0,0 +1,13 @@
+#ifndef __UTILS_H__
+#define __UTILS_H__
+
+// Enable debug logging
+//#define DEBUG 1
+
+#ifdef DEBUG
+#define LOGD(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define LOGD(fmtn, ...)
+#endif
+
+#endif
\ No newline at end of file