main.cpp

/*
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#ifdef _WIN32
#define _CRT_SECURE_NO_WARNINGS
#endif

#include "cli.h"
#include "clipp/clipp.h"
#include <cinttypes>
#include <csignal>
#include <cstdio>
#include <regex>
#include <random>
#if !defined(__APPLE__) && !defined(__FreeBSD__)
#include <cuchar>
#endif
#include <cwchar>
#include <map>
#include <iostream>
#include <vector>
#include <set>
#include <array>
#include <cstring>
#include <cstdarg>
#include <algorithm>
#include <iomanip>
#include <numeric>
#include <memory>
#include <functional>

#include "boot/uf2.h"
#include "boot/picobin.h"
#include "get_enc_bootloader.h"
#if HAS_LIBUSB
    #include "picoboot_connection_cxx.h"
    #include "get_xip_ram_perms.h"
    #include "lfs.h"
    #include "ff.h"
    #include "diskio.h"
#else
    #include "picoboot_connection.h"
#endif
#include "bintool.h"
#include "elf2uf2.h"
#include "boot/bootrom_constants.h"
#include "pico/binary_info.h"
#include "pico/usb_reset_interface.h"
#include "elf.h"
#include "otp.h"
#include "model.h"
#include "errors.h"
#include "hardware/regs/otp_data.h"

#include "nlohmann/json.hpp"

#if defined(__unix__) || defined(__APPLE__)
#include <unistd.h>
#endif

// missing __builtins on windows
#if defined(_MSC_VER) && !defined(__clang__)
#  include <intrin.h>
#  define __builtin_popcount __popcnt
static __forceinline int __builtin_ctz(unsigned x) {
    unsigned long r;
    _BitScanForward(&r, x);
    return (int)r;
}
#endif

// tsk namespace is polluted on windows
#ifdef _WIN32
#undef min
#undef max

#define _CRT_SECURE_NO_WARNINGS
#endif

// preprocessor macros
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)


#define MAX_REBOOT_TRIES 5

#define OTP_PAGE_COUNT 64
#define OTP_PAGE_ROWS  64
#define OTP_ROW_COUNT (OTP_PAGE_COUNT * OTP_PAGE_ROWS)
#define OTP_SPECIAL_PAGES 3

// Support for SDK 2.1.0 & SDK 2.1.1 -----
#ifndef CYW43_FIRMWARE_FAMILY_ID
#define CYW43_FIRMWARE_FAMILY_ID    0xe48bff55u
#endif
#ifndef BOOTROM_FAMILY_ID_MIN
#define BOOTROM_FAMILY_ID_MIN       RP2040_FAMILY_ID
#endif
#ifndef BOOTROM_FAMILY_ID_MAX
#define BOOTROM_FAMILY_ID_MAX       RP2350_ARM_NS_FAMILY_ID
#endif
#ifndef BLOCK_DEVICE_DEFAULT_PARTITION_ID
#define BLOCK_DEVICE_DEFAULT_PARTITION_ID 0x626C6F636B646576
#endif
// ------

using std::string;
using std::vector;
using std::pair;
using std::map;
using std::tuple;
using std::ios;
using json = nlohmann::json;

#if HAS_LIBUSB
typedef map<enum picoboot_device_result,vector<tuple<chip_t, libusb_device *, libusb_device_handle *>>> device_map;
#else
typedef map<enum picoboot_device_result,vector<tuple<chip_t, void *, void *>>> device_map;
#endif

auto memory_names = map<enum memory_type, string>{
        {memory_type::sram, "RAM"},
        {memory_type::flash, "Flash"},
        {memory_type::xip_sram, "XIP RAM"},
        {memory_type::rom, "ROM"}
};

static const string tool_name = "picotool";

static const string data_family_name = "data";
static const string absolute_family_name = "absolute";
static const string rp2040_family_name = "rp2040";
static const string rp2350_arm_s_family_name = "rp2350-arm-s";
static const string rp2350_arm_ns_family_name = "rp2350-arm-ns";
static const string rp2350_riscv_family_name = "rp2350-riscv";
static const string cyw43_firmware_family_name = "cyw43-firmware";

static const std::vector<std::string> family_names = {
    data_family_name,
    absolute_family_name,
    rp2040_family_name,
    rp2350_arm_s_family_name,
    rp2350_arm_ns_family_name,
    rp2350_riscv_family_name,
    cyw43_firmware_family_name
};

static const std::map<uint32_t, std::string> family_id_to_name = {
    {DATA_FAMILY_ID, data_family_name},
    {ABSOLUTE_FAMILY_ID, absolute_family_name},
    {RP2040_FAMILY_ID, rp2040_family_name},
    {RP2350_ARM_S_FAMILY_ID, rp2350_arm_s_family_name},
    {RP2350_ARM_NS_FAMILY_ID, rp2350_arm_ns_family_name},
    {RP2350_RISCV_FAMILY_ID, rp2350_riscv_family_name},
    {CYW43_FIRMWARE_FAMILY_ID, cyw43_firmware_family_name}
};

static const std::map<std::string, uint32_t> family_name_to_id = {
    {data_family_name, DATA_FAMILY_ID},
    {absolute_family_name, ABSOLUTE_FAMILY_ID},
    {rp2040_family_name, RP2040_FAMILY_ID},
    {rp2350_arm_s_family_name, RP2350_ARM_S_FAMILY_ID},
    {rp2350_arm_ns_family_name, RP2350_ARM_NS_FAMILY_ID},
    {rp2350_riscv_family_name, RP2350_RISCV_FAMILY_ID},
    {cyw43_firmware_family_name, CYW43_FIRMWARE_FAMILY_ID}
};

#if !HAS_LIBUSB
static const string built_without_libusb_message = "\nThis version of picotool was compiled without USB support. Some commands are not available.\n";
#endif

static string hex_string(int64_t value, int width=8, bool prefix=true, bool uppercase=false) {
    std::stringstream ss;
    if (prefix) ss << "0x";
    ss << std::setfill('0') << std::setw(width);
    if (uppercase) ss << std::uppercase;
    ss << std::hex << value;
    return ss.str();
}

std::array<std::array<string, 1>, 12> pin_functions_unknown{{
    {""},
    {""},
    {""},
    {""},
    {""},
    {""},
    {""},
    {""},
    {""},
    {""},
    {""},
    {""},
}};

std::array<std::array<string, 30>, 10> pin_functions_rp2040{{
    {"", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", ""},
    {"SPI0 RX", "SPI0 CSn", "SPI0 SCK", "SPI0 TX", "SPI0 RX", "SPI0 CSn", "SPI0 SCK", "SPI0 TX", "SPI1 RX", "SPI1 CSn", "SPI1 SCK", "SPI1 TX", "SPI1 RX", "SPI1 CSn", "SPI1 SCK", "SPI1 TX", "SPI0 RX", "SPI0 CSn", "SPI0 SCK", "SPI0 TX", "SPI0 RX", "SPI0 CSn", "SPI0 SCK", "SPI0 TX", "SPI1 RX", "SPI1 CSn", "SPI1 SCK", "SPI1 TX", "SPI1 RX", "SPI1 CSn"},
    {"UART0 TX", "UART0 RX", "UART0 CTS", "UART0 RTS", "UART1 TX", "UART1 RX", "UART1 CTS", "UART1 RTS", "UART1 TX", "UART1 RX", "UART1 CTS", "UART1 RTS", "UART0 TX", "UART0 RX", "UART0 CTS", "UART0 RTS", "UART0 TX", "UART0 RX", "UART0 CTS", "UART0 RTS", "UART1 TX", "UART1 RX", "UART1 CTS", "UART1 RTS", "UART1 TX", "UART1 RX", "UART1 CTS", "UART1 RTS", "UART0 TX", "UART0 RX"},
    {"I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL"},
    {"PWM0 A", "PWM0 B", "PWM1 A", "PWM1 B", "PWM2 A", "PWM2 B", "PWM3 A", "PWM3 B", "PWM4 A", "PWM4 B", "PWM5 A", "PWM5 B", "PWM6 A", "PWM6 B", "PWM7 A", "PWM7 B", "PWM0 A", "PWM0 B", "PWM1 A", "PWM1 B", "PWM2 A", "PWM2 B", "PWM3 A", "PWM3 B", "PWM4 A", "PWM4 B", "PWM5 A", "PWM5 B", "PWM6 A", "PWM6 B"},
    {"SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO", "SIO"},
    {"PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0", "PIO0"},
    {"PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1", "PIO1"},
    {"","","","","","","","","","","","","","","","","","","","","CLOCK GPIN0","CLOCK GPOUT0","CLOCK GPIN1","CLOCK GPOUT1","CLOCK GPOUT2","CLOCK GPOUT3","","","",""},
    {"USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN"},
}};

std::array<std::array<string, 48>, 12> pin_functions_rp2350{{
    {"JTAG TCK","JTAG TMS", "JTAG TDI", "JTAG TDO", "",         "",         "",         "",         "",         "",         "",         "",         "HSTX0",    "HSTX1",    "HSTX2",    "HSTX3",    "HSTX4",    "HSTX5",    "HSTX6",    "HSTX7",},
    {"SPI0 RX", "SPI0 CSn", "SPI0 SCK", "SPI0 TX",  "SPI0 RX",  "SPI0 CSn", "SPI0 SCK", "SPI0 TX",  "SPI1 RX",  "SPI1 CSn", "SPI1 SCK", "SPI1 TX",  "SPI1 RX",  "SPI1 CSn", "SPI1 SCK", "SPI1 TX",  "SPI0 RX",  "SPI0 CSn", "SPI0 SCK", "SPI0 TX",  "SPI0 RX",  "SPI0 CSn", "SPI0 SCK", "SPI0 TX",  "SPI1 RX",  "SPI1 CSn", "SPI1 SCK", "SPI1 TX",  "SPI1 RX",  "SPI1 CSn", "SPI1 SCK", "SPI1 TX",  "SPI0 RX",  "SPI0 CSn", "SPI0 SCK", "SPI0 TX",  "SPI0 RX",  "SPI0 CSn", "SPI0 SCK", "SPI0 TX",  "SPI1 RX",  "SPI1 CSn", "SPI1 SCK", "SPI1 TX",  "SPI1 RX",  "SPI1 CSn", "SPI1 SCK", "SPI1 TX"},
    {"UART0 TX","UART0 RX", "UART0 CTS","UART0 RTS","UART1 TX", "UART1 RX", "UART1 CTS","UART1 RTS","UART1 TX", "UART1 RX", "UART1 CTS","UART1 RTS","UART0 TX", "UART0 RX", "UART0 CTS","UART0 RTS","UART0 TX", "UART0 RX", "UART0 CTS","UART0 RTS","UART1 TX", "UART1 RX", "UART1 CTS","UART1 RTS","UART1 TX", "UART1 RX", "UART1 CTS","UART1 RTS","UART0 TX", "UART0 RX", "UART0 CTS","UART0 RTS","UART0 TX", "UART0 RX", "UART0 CTS","UART0 RTS","UART1 TX", "UART1 RX", "UART1 CTS","UART1 RTS","UART1 TX", "UART1 RX", "UART1 CTS","UART1 RTS","UART0 TX", "UART0 RX", "UART0 CTS","UART0 RTS"},
    {"I2C0 SDA","I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL", "I2C0 SDA", "I2C0 SCL", "I2C1 SDA", "I2C1 SCL"},
    {"PWM0 A",  "PWM0 B",   "PWM1 A",   "PWM1 B",   "PWM2 A",   "PWM2 B",   "PWM3 A",   "PWM3 B",   "PWM4 A",   "PWM4 B",   "PWM5 A",   "PWM5 B",   "PWM6 A",   "PWM6 B",   "PWM7 A",   "PWM7 B",   "PWM0 A",   "PWM0 B",   "PWM1 A",   "PWM1 B",   "PWM2 A",   "PWM2 B",   "PWM3 A",   "PWM3 B",   "PWM4 A",   "PWM4 B",   "PWM5 A",   "PWM5 B",   "PWM6 A",   "PWM6 B",   "PWM7 A",   "PWM7 B",   "PWM8 A",   "PWM8 B",   "PWM9 A",   "PWM9 B",   "PWM10 A",  "PWM10 B",  "PWM11 A",  "PWM11 B",  "PWM8 A",   "PWM8 B",   "PWM9 A",   "PWM9 B",   "PWM10 A",  "PWM10 B",  "PWM11 A",  "PWM11 B"},
    {"SIO",     "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO",      "SIO"},
    {"PIO0",    "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0",     "PIO0"},
    {"PIO1",    "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1",     "PIO1"},
    {"PIO2",    "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2",     "PIO2"},
    {"XIP CS1", "CORESIGHT TRACECLK","CORESIGHT TRACEDATA0","CORESIGHT TDATA1","CORESIGHT TDATA2","CORESIGHT TDATA3","","","XIP CS1", "", "","",    "CLK GPIN", "CLK GPOUT","CLK GPIN", "CLK GPOUT","",         "",         "",         "XIP CS1",  "CLK GPIN", "CLK GPOUT","CLK GPIN", "CLK GPOUT","CLK GPOUT","CLK GPOUT","",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "",         "XIP CS1"},
    {"USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN", "USB OVCUR DET", "USB VBUS DET", "USB VBUS EN"},
    {"",        "",         "UART0 TX", "UART0 RX", "",         "",         "UART1 TX", "UART1 RX", "",         "",         "UART1 TX", "UART1 RX", "",         "",         "UART0 TX", "UART0 RX", "",         "",         "UART0 TX", "UART0 RX", "",         "",         "UART1 TX", "UART1 RX", "",         "",         "UART1 TX", "UART1 RX", "",         "",         "UART0 TX", "UART0 RX", "",         "",         "UART0 TX", "UART0 RX", "",         "",         "UART1 TX", "UART1 RX", "",         "",         "UART1 TX", "UART1 RX", "",         "",         "UART0 TX", "UART0 RX"}
}};

static_assert(pin_functions_unknown.size() >= std::max(pin_functions_rp2040.size(), pin_functions_rp2350.size()), "pin_functions_unknown size is too small");

std::map<uint32_t, otp_reg> otp_regs;

#if HAS_LIBUSB
auto bus_device_string = [](struct libusb_device *device, chip_t chip) {
    string bus_device;
    bus_device = chip_name(chip) + string(" device at bus ");
    return bus_device + std::to_string(libusb_get_bus_number(device)) + ", address " + std::to_string(libusb_get_device_address(device));
};
#endif

enum class filetype {bin, elf, uf2, pem, json};
const string getFiletypeName(enum filetype type) 
{
   switch (type) 
   {
      case filetype::elf: return "ELF";
      case filetype::bin: return "BIN";
      case filetype::uf2: return "UF2";
      case filetype::pem: return "PEM";
      case filetype::json: return "JSON";
      default: assert(false); return "ERROR_TYPE";
   }
}

struct cancelled_exception : std::exception { };

struct not_mapped_exception : std::exception {
    explicit not_mapped_exception(uint32_t addr) : addr(addr), std::exception() {}
    const char *what() const noexcept override {
        return "Hmm uncaught not mapped";
    }
    uint32_t addr;
};

// from -> to
struct range {
    range() : from(0), to(0) {}
    range(uint32_t from, uint32_t to) : from(from), to(to) {}
    uint32_t from;
    uint32_t to;

    uint32_t len() const {
        return to - from;
    }

    bool empty() const {
        return from >= to;
    }
    bool contains(uint32_t addr) const { return addr>=from && addr<to; }
    uint32_t clamp(uint32_t addr) const {
        if (addr < from) addr = from;
        if (addr > to) addr = to;
        return addr;
    }

    void intersect(const range& other) {
        from = other.clamp(from);
        to = other.clamp(to);
    }

    bool intersects(const range& other) const {
        return !(other.from >= to || other.to < from);
    }

};

// ranges should not overlap
template <typename T> struct range_map {
    range_map() = default;
    struct mapping {
        mapping(uint32_t offset, uint32_t max_offset) : offset(offset), max_offset(max_offset) {}
        const uint32_t offset;
        const uint32_t max_offset;
    };

    void insert(const range& r, T t) {
        if (r.to != r.from) {
            assert(r.to > r.from);
            // check we don't overlap any existing map entries

            auto f = m.upper_bound(r.from); // first element that starts after r.from
            if (f != m.begin()) f--; // back up, to catch element that starts on or before r.from
            for(; f != m.end() && f->first < r.to; f++) { // loop till we can't possibly overlap
                range r2(f->first, f->second.first);
                if (r2.intersects(r)) {
                    fail(ERROR_FORMAT, "Found overlapping memory ranges 0x%08x->0x%08x and 0x%08x->%08x\n",
                         r.from, r.to, r2.from, r2.to);
                }
            }
            m.insert(std::make_pair(r.from, std::make_pair(r.to, t)));
        }
    }

    void insert_overwrite(const range& r, T t) {
        if (r.to != r.from) {
            assert(r.to > r.from);
            // insert overlapping entry, and overwrite any it overlaps

            // avoid modifying m while iterating through it
            vector<uint32_t> to_erase;
            vector<pair<uint32_t, pair<uint32_t, T>>> to_add;

            auto f = m.upper_bound(r.from); // first element that starts after r.from
            if (f != m.begin()) f--; // back up, to catch element that starts on or before r.from
            for(; f != m.end() && f->first < r.to; f++) { // loop till we can't possibly overlap
                range r2(f->first, f->second.first);
                T r2off = f->second.second;
                if (r2.intersects(r)) {
                    // remove existing r2
                    to_erase.push_back(r2.from);
                    if (r2.from < r.from) {
                        // add r2 which ends at start of r
                        to_add.push_back(std::make_pair(r2.from, std::make_pair(r.from, r2off)));
                    }
                    if (r2.to > r.to) {
                        // add r2 which starts at end of r
                        to_add.push_back(std::make_pair(r.to, std::make_pair(r2.to, r2off + (r.to - r2.from))));
                    }
                }
            }
            for (auto k : to_erase) m.erase(k);
            for (auto v : to_add) m.insert(v);

            // finally, add the new entry
            m.insert(std::make_pair(r.from, std::make_pair(r.to, t)));
        }
    }

    pair<mapping, T> get(uint32_t p) {
        auto f = m.upper_bound(p);
        if (f == m.end()) {
            if (m.empty())
                throw not_mapped_exception(p);
        } else if (f == m.begin()) {
            throw not_mapped_exception(p);
        }
        f--;
        assert(p >= f->first);
        if (p >= f->second.first) {
            throw not_mapped_exception(p);
        }
        return std::make_pair(mapping(p - f->first, f->second.first - f->first), f->second.second);
    }

    uint32_t next(uint32_t p) {
        auto f = m.upper_bound(p);
        if (f == m.end()) {
            return std::numeric_limits<uint32_t>::max();
        }
        return f->first;
    }

    vector<range> ranges() {
        vector<range> r;
        r.reserve(m.size());
        for(const auto &e : m) {
            r.emplace_back(range(e.first, e.second.first));
        }
        return r;
    }

    size_t size() const { return m.size(); }

    range_map<T> offset_by(uint32_t offset) {
        range_map<T> rmap_offset;
        for(const auto &e : m) {
            rmap_offset.insert(range(e.first + offset, e.second.first + offset), e.second.second);
        }
        return rmap_offset;
    }
private:
    map<uint32_t, pair<uint32_t, T>> m;
};


// Calculate chunk size for load/save/verify
// Returns size/100 rounded up to FLASH_SECTOR_ERASE_SIZE
uint32_t calculate_chunk_size(uint32_t size) {
    return (((size + (100 - 1))/100 + FLASH_SECTOR_ERASE_SIZE - 1) & ~(FLASH_SECTOR_ERASE_SIZE - 1));
}


using cli::group;
using cli::option;
using cli::integer;
using cli::hex;
using cli::value;

// todo can we derive from hex?
struct family_id : public cli::value_base<family_id> {
    explicit family_id(string name) : value_base(std::move(name)) {}

    template<typename T>
    family_id &set(T &t) {
        string nm = "<" + name() + ">";
        // note we cannot capture "this"
        on_action([&t, nm](string value) {
            std::transform(value.begin(), value.end(), value.begin(),
                [](unsigned char c){ return std::tolower(c); });
            std::replace( value.begin(), value.end(), '_', '-');
            auto family_id = family_name_to_id.find(value);
            if (family_id != family_name_to_id.end()) {
                t = family_id->second;
            } else if (value.find("0x") == 0) {
                value = value.substr(2);
                size_t pos = 0;
                long lvalue = std::numeric_limits<long>::max();
                try {
                    lvalue = std::stoul(value, &pos, 16);
                    if (pos != value.length()) {
                        return "Garbage after hex value: " + value.substr(pos);
                    }
                } catch (std::invalid_argument &) {
                    return value + " is not a valid hex value";
                } catch (std::out_of_range &) {
                }
                if (lvalue != (unsigned int) lvalue) {
                    return value + " is not a valid 32 bit value";
                }
                t = (unsigned int) lvalue;
            } else {
                return value + " is not a valid family ID"
                + "\n\nValid family IDs are: " + cli::join(family_names, ", ") + ", or hex strings starting with 0x";
            }
            return string("");
        });
        return *this;
    }
};

string family_name(unsigned int family_id) {
    auto family_name = family_id_to_name.find(family_id);
    if (family_name != family_id_to_name.end()) {
        return "'" + family_name->second + "'";
    }
    if (!family_id) return "none";
    return hex_string(family_id);
}

struct platform_model : public cli::value_base<platform_model> {
    explicit platform_model(string name) : value_base(std::move(name)) {}

    template<typename T>
    platform_model &set(T &t) {
        string nm = "<" + name() + ">";
        // note we cannot capture "this"
        on_action([&t, nm](string value) {
            auto ovalue = value;
            if (value == "rp2040") {
                t = std::make_shared<model_rp2040>();
            } else if (value == "rp2350") {
                t = std::make_shared<model_rp2350>();
            } else {
                return value + " is not a valid platform";
            }
            return string("");
        });
        return *this;
    }
};

typedef enum {
    fs_detect,
    fs_littlefs,
    fs_fatfs
} bdev_fs_t;

struct bdev_fs : public cli::value_base<bdev_fs> {
    explicit bdev_fs(string name) : value_base(std::move(name)) {}

    template<typename T>
    bdev_fs &set(T &t) {
        string nm = "<" + name() + ">";
        // note we cannot capture "this"
        on_action([&t, nm](string value) {
            std::transform(value.begin(), value.end(), value.begin(),
                [](unsigned char c){ return std::tolower(c); });
            if (value == "littlefs") {
                t = fs_littlefs;
            } else if (value == "fatfs") {
                t = fs_fatfs;
            } else {
                return value + " is not a supported file system for block devices - only LittleFS and FatFS are supported";
            }
            return string("");
        });
        return *this;
    }
};

struct cmd {
    explicit cmd(string name) : _name(std::move(name)) {}
    virtual ~cmd() = default;
    enum device_support { none, one, zero_or_more };
    virtual group get_cli() = 0;
    virtual string get_doc() const = 0;
    virtual device_support get_device_support() { return one; }
    virtual bool force_requires_pre_reboot() { return true; }
    // return true if the command caused a reboot
    virtual bool execute(device_map& devices) = 0;
    virtual bool is_multi() const { return false; }
    virtual bool requires_rp2350() const { return false; }
    virtual std::vector<std::shared_ptr<cmd>> sub_commands() const { return std::vector<std::shared_ptr<cmd>>(); }
    const string& name() { return _name; }
private:
    string _name;
};

struct multi_cmd : public cmd {
    explicit multi_cmd(std::string name, std::vector<std::shared_ptr<cmd>> sub_commands) : cmd(name), _sub_commands(sub_commands) {}
    virtual group get_cli() override { assert(false); return group(); }
    virtual bool execute(device_map& devices) override { assert(false); return false; }
    virtual bool is_multi() const override { return true; }
    virtual std::vector<std::shared_ptr<cmd>> sub_commands() const override {
        return _sub_commands;
    }
private:
     std::vector<std::shared_ptr<cmd>> _sub_commands;
};

#ifndef DEFAULT_BOOTSEL_LED
#define DEFAULT_BOOTSEL_LED -1
#endif

struct _settings {
    std::array<std::string, 6> filenames;
    std::array<std::string, 6> file_types;
    uint32_t binary_start = FLASH_START;
    int bus=-1;
    int address=-1;
    int vid=-1;
    int pid=-1;
    string ser;
    int led=DEFAULT_BOOTSEL_LED;
    bool active_low=false;
    bool force_rp2040 = false;
    uint32_t offset = 0;
    uint32_t from = 0;
    uint32_t to = 0;
    uint32_t partition_size = 0;
    bool offset_set = false;
    bool range_set = false;
    bool reboot_usb = false;
    bool reboot_app_specified = false;
    int reboot_diagnostic_partition = BOOT_PARTITION_NONE;
    bool force = false;
    bool force_no_reboot = false;
    string switch_cpu;
    uint32_t family_id = 0;
    model_t model = nullptr;
    bool quiet = false;
    bool verbose = false;
    bool use_flash_cache = false;

    struct {
        int redundancy = -1;
        bool raw = false;
        bool ecc = false;
        bool ignore_set = false;
        bool fuzzy = false;
        uint32_t value = 0;
        uint8_t lock0 = 0;
        uint8_t lock1 = 0;
        int8_t led_pin = -1;
        std::vector<uint32_t> pages;
        bool list_pages = false;
        bool list_no_descriptions = false;
        bool list_field_descriptions = false;
        std::vector<std::string> selectors;
        uint32_t row = 0;
        std::vector<std::string> extra_files;
        bool dump_pages = false;
    } otp;

    struct {
        bool show_basic = false;
        bool all = false;
        bool show_metadata = false;
        bool show_pins = false;
        bool show_device = false;
        bool show_debug = false;
        bool show_build = false;
    } info;

    struct {
        string group;
        string key;
        string value;
    } config;

    struct {
        bool verify = false;
        bool execute = false;
        bool no_overwrite = false;
        bool no_overwrite_force = false;
        bool update = false;
        bool ignore_pt = false;
        int partition = -1;
    } load;

    struct {
        bool hash = false;
        bool sign = false;
        bool clear_sram = false;
        bool pin_xip_sram = false;
        bool set_tbyb = false;
        bool no_squash = false;
        uint16_t major_version = 0;
        uint16_t minor_version = 0;
        uint16_t rollback_version = 0;
        std::vector<uint16_t> rollback_rows;
    } seal;

    struct {
        bool embed = false;
        bool otp_key_page_set = false;
        bool fast_rosc = false;
        bool use_mbedtls = false;
        bool no_clear_sram = false;
        uint16_t otp_key_page = 29;
    } encrypt;

    struct {
        uint32_t align = 0x1000;
    } link;

    struct {
        bool all = false;
        bool verify = false;
    } save;

    struct {
        bool semantic = false;
        string version;
    } version;

    struct {
        #if HAS_MBEDTLS
        bool hash = true;
        #else
        bool hash = false;
        #endif
        bool sign = false;
        bool singleton = false;
        bool no_btstack_flash_bank = false;
    } partition;

    struct {
        bool abs_block = false;
        #if SUPPORT_RP2350_A2
        uint32_t abs_block_loc = 0x11000000 - UF2_PAGE_SIZE;
        #else
        uint32_t abs_block_loc = 0;
        #endif
        uint32_t offset = 0;
        bool offset_set = false;
        int partition = -1;
        bool partition_set = false;
    } uf2;

    struct {
        bdev_fs_t fs = fs_detect;
        bool recursive = false;
        int partition_number = -1;
        uint64_t partition_id = BLOCK_DEVICE_DEFAULT_PARTITION_ID;
        string partition_name = "";
        bool format = false;
        bool always_format = false;
        bool force_formattable = false;
        bool force_writeable = false;
    } bdev;
};
_settings settings;
std::shared_ptr<cmd> selected_cmd;
chip_t selected_chip = unknown;

auto device_selection =
    (
        (option("--bus") & integer("bus").min_value(0).max_value(255).set(settings.bus)
            .if_missing([] { return "missing bus number"; })) % "Filter devices by USB bus number" +
        (option("--address") & integer("addr").min_value(1).max_value(127).set(settings.address)
            .if_missing([] { return "missing address"; })) % "Filter devices by USB device address" +
        (option("--vid") & integer("vid").set(settings.vid).if_missing([] { return "missing vid"; })) % "Filter by vendor id" +
        (option("--pid") & integer("pid").set(settings.pid)) % "Filter by product id" +
        (option("--ser") & value("ser").set(settings.ser)) % "Filter by serial number"
        + option("--rp2040").set(settings.force_rp2040) % "Assume the device is an RP2040 - this is only required when using a custom vid/pid with an RP2040 on Windows, and is ignored on other operating systems"
        + option('f', "--force").set(settings.force) % "Force a device not in BOOTSEL mode but running compatible code to reset so the command can be executed. After executing the command (unless the command itself is a 'reboot') the device will be rebooted back to application mode" +
                option('F', "--force-no-reboot").set(settings.force_no_reboot) % "Force a device not in BOOTSEL mode but running compatible code to reset so the command can be executed. After executing the command (unless the command itself is a 'reboot') the device will be left connected and accessible to picotool, but without the USB drive mounted"
        + (option("--bootsel-led") & integer("gpio").set(settings.led)) % 
        "Specify the GPIO for the BOOTSEL activity LED to flash (default "
    #if DEFAULT_BOOTSEL_LED < 0
        "none"
    #else
        STR(DEFAULT_BOOTSEL_LED)
    #endif
        ", ignored by RP2350A-A2 in Arm mode) - only applicable if this command reboots the device to BOOTSEL mode"
        + option("--bootsel-led-active-low").set(settings.active_low) % "The BOOTSEL activity LED is active low (ignored by RP2040 and RP2350-A4)"
    ).min(0).doc_non_optional(true).collapse_synopsys("device-selection");

#define file_types_x(i)\
(option ('t', "--type") & value("type").set(settings.file_types[i]))\
    % "Specify file type (uf2 | elf | bin) explicitly, ignoring file extension"

#define named_file_types_x(types, i)\
(option ('t', "--type") & value("type").set(settings.file_types[i]))\
    % "Specify file type (" types ") explicitly, ignoring file extension"


#define file_selection_x(i)\
(\
    value("filename").with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]) % "The file name" +\
    file_types_x(i)\
)

#define named_file_selection_x(name, i)\
(\
    value(name).with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]) % "The file name" +\
    file_types_x(i)\
)

#define named_typed_file_selection_x(name, i, types)\
(\
    value(name).with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]) % "The file name" +\
    named_file_types_x(types, i)\
)

#define named_untyped_path_selection_x(name, desc, i)\
(\
    value(name).with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]) % desc\
)

#define named_untyped_file_selection_x(name, i) named_untyped_path_selection_x(name, "The file name", i)

#define optional_file_selection_x(name, i)\
(\
    value(name).with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]).min(0) % "The file name" +\
    file_types_x(i)\
).min(0).doc_non_optional(true)

#define optional_typed_file_selection_x(name, i, types)\
(\
    value(name).with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]).min(0) % "The file name" +\
    named_file_types_x(types, i)\
).min(0).doc_non_optional(true)

#define optional_untyped_path_selection_x(name, desc, i)\
(\
    value(name).with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]).min(0) % desc\
).min(0)

#define optional_untyped_file_selection_x(name, i) optional_untyped_path_selection_x(name, "The file name", i)

#define option_file_selection_x(option, i)\
(\
    option & value("filename").with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]) % "The file name" +\
    file_types_x(i)\
)

#define option_typed_file_selection_x(option, i, types)\
(\
    option & value("filename").with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]) % "The file name" +\
    named_file_types_x(types, i)\
)

#define option_untyped_file_selection_x(option, i)\
(\
    option & value("filename").with_exclusion_filter([](const string &value) {\
            return value.find_first_of('-') == 0;\
        }).set(settings.filenames[i]) % "The file name"\
)

auto file_types = (option ('t', "--type") & value("type").set(settings.file_types[0]))
            % "Specify file type (uf2 | elf | bin) explicitly, ignoring file extension";

auto file_selection =
        (
            value("filename").with_exclusion_filter([](const string &value) {
                    return value.find_first_of('-') == 0;
                }).set(settings.filenames[0]) % "The file name" +
            file_types
        );

struct info_command : public cmd {
    info_command() : cmd("info") {}
    bool execute(device_map& devices) override;
    device_support get_device_support() override {
        if (settings.filenames[0].empty())
            return zero_or_more;
        else
            return none;
    }

    group get_cli() override {
        return (
            (
                option('b', "--basic").set(settings.info.show_basic) % "Include basic information. This is the default" +
                option('m', "--metadata").set(settings.info.show_metadata) % "Include all metadata blocks" +
                option('p', "--pins").set(settings.info.show_pins) % "Include pin information" +
                option('d', "--device").set(settings.info.show_device) % "Include device information" +
                option("--debug").set(settings.info.show_debug) % "Include device debug information" +
                option('l', "--build").set(settings.info.show_build) % "Include build attributes" +
                option('a', "--all").set(settings.info.all) % "Include all information"
            ).min(0).doc_non_optional(true) % "Information to display" +
            (
            #if HAS_LIBUSB
                device_selection % "To target one or more connected RP-series device(s) in BOOTSEL mode (the default)" |
            #endif
                file_selection % "To target a file"
            ).major_group("TARGET SELECTION").min(0).doc_non_optional(true)
         );
    }

    string get_doc() const override {
        #if HAS_LIBUSB
        return "Display information from the target device(s) or file.\nWithout any arguments, this will display basic information for all connected RP-series devices in BOOTSEL mode";
        #else
        return "Display information from the target file.";
        #endif
    }
};

struct config_command : public cmd {
    config_command() : cmd("config") {}
    bool execute(device_map& devices) override;
    device_support get_device_support() override {
        if (settings.filenames[0].empty())
            return zero_or_more;
        else
            return none;
    }

    group get_cli() override {
        return (
            (option('s', "--set") & (
                value("key").set(settings.config.key) % "Variable name" +
                value("value").set(settings.config.value) % "New value")
            ).force_expand_help(true) +
            (option('g', "--group") & value("group").set(settings.config.group)) % "Filter by feature group" + 
            (
            #if HAS_LIBUSB
                device_selection % "To target one or more connected RP-series device(s) in BOOTSEL mode (the default)" |
            #endif
                file_selection % "To target a file"
            ).major_group("TARGET SELECTION").min(0).doc_non_optional(true)
         );
    }

    string get_doc() const override {
        #if HAS_LIBUSB
        return "Display or change program configuration settings from the target device(s) or file.";
        #else
        return "Display or change program configuration settings from the target file.";
        #endif
    }
};

#if HAS_LIBUSB
auto bdev_base_options = (
    (option('p', "--partition-number") % "Partition number to use as block device" &
            integer("partition number").set(settings.bdev.partition_number) % "partition number").force_expand_help(true) +
    (option("--partition-name") % "Partition name to use as block device" &
            value("partition name").set(settings.bdev.partition_name) % "partition name").force_expand_help(true) +
    (option("--partition-id") % "Partition ID to use as block device" &
            integer("partition id").set(settings.bdev.partition_id) % "partition id").force_expand_help(true) +
    (option("--filesystem") % "Specify filesystem to use" &
            bdev_fs("fs").set(settings.bdev.fs) % "littlefs|fatfs").force_expand_help(true) +
    (option("--force-formattable").set(settings.bdev.force_formattable) % "Allow formatting, even if the block device is not marked as fomattable") +
    (option("--force-writeable").set(settings.bdev.force_writeable) % "Allow writing, even if the block device is not marked as writeable")
).min(0).doc_non_optional(true) % "Block device options";
auto bdev_format_option = (option("--format").set(settings.bdev.format) % "Format the drive if necessary (may result in data loss)");
auto bdev_options_no_format = bdev_base_options;
auto bdev_options = bdev_base_options + bdev_format_option;

struct bdev_ls_command : public cmd {
    bdev_ls_command() : cmd("ls") {}
    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
            optional_untyped_path_selection_x("dirname", "The directory name to list (optional)", 0) +
            option('r', "--recursive").set(settings.bdev.recursive) % "List files in directories recursively" +
            bdev_options +
            device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "List contents of the block device";
    }
};

struct bdev_mkdir_command : public cmd {
    bdev_mkdir_command() : cmd("mkdir") {}
    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
            named_untyped_path_selection_x("dirname", "The directory name", 0) +
            bdev_options +
            device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Create directory on the block device";
    }
};

struct bdev_cp_command : public cmd {
    bdev_cp_command() : cmd("cp") {}
    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
            named_untyped_file_selection_x("src", 0) +
            named_untyped_file_selection_x("dest", 1) +
            bdev_options +
            device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Copy file to/from the block device - use :filename to indicate files on the device (eg `cp main.py :main.py` to upload to the device)";
    }
};

struct bdev_rm_command : public cmd {
    bdev_rm_command() : cmd("rm") {}
    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
            named_untyped_file_selection_x("filename", 0) +
            bdev_options +
            device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Delete a file or an empty directory on the block device";
    }
};

struct bdev_cat_command : public cmd {
    bdev_cat_command() : cmd("cat") {}
    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
            named_untyped_file_selection_x("filename", 0) +
            bdev_options +
            device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Print contents of file on the block device";
    }
};

struct bdev_format_command : public cmd {
    bdev_format_command() : cmd("format") {}
    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
            bdev_options_no_format +
            device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Format the block device";
    }
};

vector<std::shared_ptr<cmd>> bdev_sub_commands {
    std::shared_ptr<cmd>(new bdev_ls_command()),
    std::shared_ptr<cmd>(new bdev_mkdir_command()),
    std::shared_ptr<cmd>(new bdev_cp_command()),
    std::shared_ptr<cmd>(new bdev_rm_command()),
    std::shared_ptr<cmd>(new bdev_cat_command()),
    std::shared_ptr<cmd>(new bdev_format_command()),
};
struct bdev_command : public multi_cmd {
    bdev_command() : multi_cmd("bdev", bdev_sub_commands) {}
    string get_doc() const override {
        return "Commands related to embedded block devices";
    }
};

struct verify_command : public cmd {
    verify_command() : cmd("verify") {}
    bool execute(device_map &devices) override;

    group get_cli() override {
        return (
            file_selection % "The file to compare against" +
            (
                (option('r', "--range").set(settings.range_set) % "Compare a sub range of memory only" &
                    hex("from").set(settings.from) % "The lower address bound in hex" &
                    hex("to").set(settings.to) % "The upper address bound in hex").force_expand_help(true) +
                (option('o', "--offset").set(settings.offset_set) % "Specify the load address when comparing with a BIN file" &
                    hex("offset").set(settings.offset) % "Load offset (memory address; default 0x10000000)").force_expand_help(true)
            ).min(0).doc_non_optional(true) % "Address options" +
            device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Check that the device contents match those in the file.";
    }
};

struct save_command : public cmd {
    save_command() : cmd("save") {}
    bool execute(device_map &devices) override;

    group get_cli() override {
        return (
            (
                option('p', "--program") % "Save the installed program only. This is the default" |
                option('a', "--all").doc_non_optional(true).set(settings.save.all) % "Save all of flash memory" |
                (
                    option('r', "--range").set(settings.range_set) % "Save a range of memory. Note that UF2s always store complete 256 byte-aligned blocks of 256 bytes, and the range is expanded accordingly" &
                        hex("from").set(settings.from) % "The lower address bound in hex" &
                        hex("to").set(settings.to) % "The upper address bound in hex"
                ).min(0).doc_non_optional(true)
            ).min(0).doc_non_optional(true).no_match_beats_error(false) % "Selection of data to save" +
            option('v', "--verify").set(settings.save.verify) % "Verify the data was saved correctly" +
            (option("--family") % "Specify the family ID to save the file as" &
                family_id("family_id").set(settings.family_id) % "family ID to save file as").force_expand_help(true) +
            ( // note this parenthesis seems to help with error messages for say save --foo
                file_selection % "File to save to" +
                device_selection % "Source device selection"
            )
        );
    }
    string get_doc() const override {
        return "Save the program / memory stored in flash on the device to a file.";
    }
};

struct load_command : public cmd {
    load_command() : cmd("load") {}
    bool execute(device_map &devices) override;

    group get_cli() override {
        return (
            (
                option("--ignore-partitions").set(settings.load.ignore_pt) % "When writing flash data, ignore the partition table and write to absolute space" +
                (option("--family") % "Specify the family ID of the file to load" &
                        family_id("family_id").set(settings.family_id) % "family ID to use for load").force_expand_help(true) +
                (option('p', "--partition") % "Specify the partition to load into" &
                        integer("partition").set(settings.load.partition) % "partition to load into").force_expand_help(true) +
                option('n', "--no-overwrite").set(settings.load.no_overwrite) % "When writing flash data, do not overwrite an existing program in flash. If picotool cannot determine the size/presence of the program in flash, the command fails" +
                option('N', "--no-overwrite-unsafe").set(settings.load.no_overwrite_force) % "When writing flash data, do not overwrite an existing program in flash. If picotool cannot determine the size/presence of the program in flash, the load continues anyway" +
                option('u', "--update").set(settings.load.update) % "Skip writing flash sectors that already contain identical data" +
                option('v', "--verify").set(settings.load.verify) % "Verify the data was written correctly" +
                option('x', "--execute").set(settings.load.execute) % "Perform a bootrom reboot to execute the downloaded file as a program after the load - either a flash update boot for binaries in flash, or a RAM image boot for other binaries "
            ).min(0).doc_non_optional(true) % "Post load actions" +
            file_selection % "File to load from" +
            (
                option('o', "--offset").set(settings.offset_set) % "Specify the load address for a BIN file" &
                     hex("offset").set(settings.offset) % "Load offset (memory address; default 0x10000000)"
            ).force_expand_help(true) % "BIN file options" +
            device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Load the program / memory range stored in a file onto the device.";
    }
};

struct erase_command : public cmd {
    erase_command() : cmd("erase") {}
    bool execute(device_map &devices) override;

    group get_cli() override {
        return (
            (
                option('a', "--all") % "Erase all of flash memory. This is the default" |
                (
                    option('p', "--partition") % "Erase a partition" &
                        integer("partition").set(settings.load.partition) % "Partition number to erase"
                ).min(0).doc_non_optional(true) |
                (
                    option('r', "--range").set(settings.range_set) % "Erase a range of memory. Note that erases must be 4096 byte-aligned, so the range is expanded accordingly" &
                        hex("from").set(settings.from) % "The lower address bound in hex" &
                        hex("to").set(settings.to) % "The upper address bound in hex"
                ).min(0).doc_non_optional(true)
            ).min(0).doc_non_optional(true).no_match_beats_error(false) % "Selection of data to erase" +
            ( // note this parenthesis seems to help with error messages for say erase --foo
                device_selection % "Source device selection"
            )
        );
    }
    string get_doc() const override {
        return "Erase the program / memory stored in flash on the device.";
    }
};
#endif

#if HAS_MBEDTLS
struct encrypt_command : public cmd {
    encrypt_command() : cmd("encrypt") {}
    bool execute(device_map &devices) override;
    virtual device_support get_device_support() override { return none; }

    group get_cli() override {
        return (
            option("--quiet").set(settings.quiet) % "Don't print any output" +
            option("--verbose").set(settings.verbose) % "Print verbose output" +
            option("--embed").set(settings.encrypt.embed) % "Embed bootloader in output file" +
            option("--fast-rosc").set(settings.encrypt.fast_rosc) % "Use ~180MHz ROSC configuration for embedded bootloader" +
            option("--use-mbedtls").set(settings.encrypt.use_mbedtls) % "Use MbedTLS implementation of embedded bootloader (faster but less secure)" +
            (
                option("--otp-key-page").set(settings.encrypt.otp_key_page_set) % "Specify the OTP page storing the AES key (IV salt is stored on the next page)" &
                    integer("page").set(settings.encrypt.otp_key_page) % "OTP page (default 29)"
            ).force_expand_help(true) +
            (
                option("--hash").set(settings.seal.hash) % "Hash the encrypted file" +
                option("--sign").set(settings.seal.sign) % "Sign the encrypted file" +
                option("--no-clear").set(settings.encrypt.no_clear_sram) % "Don't clear all of main SRAM on load" +
                option("--pin-xip-sram").set(settings.seal.pin_xip_sram) % "Pin XIP SRAM on load"
            ).min(0).doc_non_optional(true) % "Signing Configuration" +
            named_file_selection_x("infile", 0) % "File to load from" +
            (
                option('o', "--offset").set(settings.offset_set) % "Specify the load address for a BIN file" &
                     hex("offset").set(settings.offset) % "Load offset (memory address; default 0x10000000)"
            ).force_expand_help(true) % "BIN file options" +
            named_file_selection_x("outfile", 1) % "File to save to" +
            named_untyped_file_selection_x("aes_key", 2) % "AES Key Share or AES Key" +
            named_untyped_file_selection_x("iv_salt", 3) % "IV Salt" +
            optional_untyped_file_selection_x("signing_key", 4) % "Signing Key file (.pem)" +
            optional_untyped_file_selection_x("otp", 5) % "JSON file to save OTP to (will edit existing file if it exists)"
        );
    }

    string get_doc() const override {
        return "Encrypt the program.";
    }
};

struct seal_command : public cmd {
    seal_command() : cmd("seal") {}
    bool execute(device_map &devices) override;
    virtual device_support get_device_support() override { return none; }

    group get_cli() override {
        return (
            option("--quiet").set(settings.quiet) % "Don't print any output" +
            option("--verbose").set(settings.verbose) % "Print verbose output" +
            (
                option("--hash").set(settings.seal.hash) % "Hash the file" +
                option("--sign").set(settings.seal.sign) % "Sign the file" +
                option("--clear").set(settings.seal.clear_sram) % "Clear all of main SRAM on load" +
                option("--pin-xip-sram").set(settings.seal.pin_xip_sram) % "Pin XIP SRAM on load" +
                option("--no-squash").set(settings.seal.no_squash) % "Don't squash segments in the ELF file"
            ).min(0).doc_non_optional(true) % "Configuration" +
            named_file_selection_x("infile", 0) % "File to load from" +
            (
                option('o', "--offset").set(settings.offset_set) % "Specify the load address for a BIN file" &
                     hex("offset").set(settings.offset) % "Load offset (memory address; default 0x10000000)"
            ).force_expand_help(true) % "BIN file options" +
            named_file_selection_x("outfile", 1) % "File to save to" +
            optional_untyped_file_selection_x("key", 2) % "Key file (.pem)" +
            optional_untyped_file_selection_x("otp", 3) % "JSON file to save OTP to (will edit existing file if it exists)" +
            (
                option("--major") &
                    integer("major").set(settings.seal.major_version)
            ).min(0) % "Add Major Version" +
            (
                option("--minor") &
                    integer("minor").set(settings.seal.minor_version)
            ).min(0) % "Add Minor Version" +
            (
                option("--rollback") &
                    integer("rollback").set(settings.seal.rollback_version) +
                    hex("rows").add_to(settings.seal.rollback_rows).min(0).repeatable()
            ).min(0) % "Add Rollback Version"
        );
    }

    string get_doc() const override {
        return "Add final metadata to a binary, optionally including a hash and/or signature.";
    }
};
#endif

struct link_command : public cmd {
    link_command() : cmd("link") {}
    bool execute(device_map &devices) override;
    virtual device_support get_device_support() override { return none; }

    group get_cli() override {
        return (
            option("--quiet").set(settings.quiet) % "Don't print any output" +
            option("--verbose").set(settings.verbose) % "Print verbose output" +
            named_typed_file_selection_x("outfile", 0, "uf2 | bin") % "File to write to" +
            named_file_selection_x("infile1", 1) % "Files to link" +
            named_file_selection_x("infile2", 2) % "Files to link" +
            optional_file_selection_x("infile3", 3) % "Files to link" +
            (option('p', "--pad") & hex("pad").set(settings.link.align)) % "Specify alignment to pad to, defaults to 0x1000"
        );
    }

    string get_doc() const override {
        return "Link multiple binaries into one block loop.";
    }
};

#if HAS_LIBUSB
struct partition_info_command : public cmd {
    partition_info_command() : cmd("info") {}
    bool execute(device_map &devices) override;

    group get_cli() override {
        return (
                (option('m', "--family") & family_id("family_id").set(settings.family_id)) % "family ID (will show target partition for said family)" +
                device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Print the device's partition table.";
    }
};
#endif

struct partition_create_command : public cmd {
    partition_create_command() : cmd("create") {}
    bool execute(device_map &devices) override;
    virtual device_support get_device_support() override { return none; }

    group get_cli() override {
        return (
                option("--quiet").set(settings.quiet) % "Don't print any output" +
                option("--verbose").set(settings.verbose) % "Print verbose output" +
                named_untyped_file_selection_x("infile", 0) % "partition table JSON" +
                (named_file_selection_x("outfile", 1) % "output file" +
                (
                    (option('o', "--offset").set(settings.offset_set) % "Specify the load address for UF2 file output" &
                        hex("offset").set(settings.offset) % "Load offset (memory address; default 0x10000000)").force_expand_help(true) +
                    (option("--family") % "Specify the family if for UF2 file output" &
                        family_id("family_id").set(settings.family_id) % "family ID for UF2 (default absolute)").force_expand_help(true)
                ).min(0).force_expand_help(true) % "UF2 output options") +
                optional_typed_file_selection_x("bootloader", 2, "elf") % "embed partition table into bootloader ELF" + 
                (
                #if HAS_MBEDTLS
                    // todo why doesn't this set settings.partition.sign?
                    ((option("--sign").set(settings.partition.sign) & value("keyfile").with_exclusion_filter([](const string &value) {
                            return value.find_first_of('-') == 0;
                        }).set(settings.filenames[3])) % "The file name" +
                    named_file_types_x("pem", 3)) % "Sign the partition table" + 
                    (option("--no-hash").clear(settings.partition.hash) % "Don't hash the partition table") + 
                #endif
                    (option("--singleton").set(settings.partition.singleton) % "Singleton partition table") +
                    (option("--no-btstack-flash-bank").set(settings.partition.no_btstack_flash_bank) % "Don't check for compatibility with BTStack flash bank")
                ).min(0).force_expand_help(true) % "Partition Table Options"
            #if SUPPORT_RP2350_A2
                + (
                    option("--abs-block").set(settings.uf2.abs_block) % "Enforce support for an absolute block" +
                        hex("abs_block_loc").set(settings.uf2.abs_block_loc).min(0) % "absolute block location (default to 0x10ffff00)"
                ).force_expand_help(true).min(0) % "Errata RP2350-E10 Fix"
            #endif
        );
    }

    string get_doc() const override {
        return "Create a partition table from json";
    }
};


vector<std::shared_ptr<cmd>> partition_sub_commands {
    #if HAS_LIBUSB
        std::shared_ptr<cmd>(new partition_info_command()),
    #endif
        std::shared_ptr<cmd>(new partition_create_command()),
};

struct partition_command : public multi_cmd {
    partition_command() : multi_cmd("partition", partition_sub_commands) {}
    string get_doc() const override {
        return "Commands related to RP2350 Partition Tables";
    }
};


struct otp_list_command : public cmd {
    otp_list_command() : cmd("list") {}
    bool execute(device_map& devices) override;
    virtual device_support get_device_support() override { return none; }

    group get_cli() override {
        return (
                (
                        option('p', "--pages").set(settings.otp.list_pages) % "Show page number/page row number" +
                        option('n', "--no-descriptions").set(settings.otp.list_no_descriptions) % "Don't show descriptions" +
                        option('f', "--field-descriptions").set(settings.otp.list_field_descriptions) % "Show all field descriptions" +
                        (option('i', "--include") & value("filename").add_to(settings.otp.extra_files)).min(0).max(1) % "Include extra otp definition" + // todo more than 1
                        (value("selector").add_to(settings.otp.selectors) %
                         "The row/field selector, each of which can select a whole row:\n\n" \
                        "ROW_NAME to select a whole row by name.\n" \
                        "ROW_NUMBER to select a whole row by number.\n" \
                        "PAGE:PAGE_ROW_NUMBER to select a whole row by page and number within page.\n\n" \
                        "... or can select a single field/subset of a row (where ROW_SEL is one of the above row selectors):\n\n"
                         "ROW_SEL.FIELD_NAME to select a field within a row by name.\n" \
                        "ROW_SEL.n-m to select a range of bits within a row.\n" \
                        "ROW_SEL.n to select a single bit within a row.\n" \
                        ".FIELD_NAME to select any row's field by name.\n\n" \
                        ".. or can select multiple rows by using blank or '*' for PAGE or PAGE_ROW_NUMBER").repeatable().min(0)
                ) % "Row/Field Selection"
        );
    }

    string get_doc() const override {
        return "List matching known registers/fields";
    }
};
#if HAS_LIBUSB
struct otp_get_command : public cmd {
    otp_get_command() : cmd("get") {}
    bool execute(device_map& devices) override;
    virtual bool requires_rp2350() const override { return true; }

    group get_cli() override {
        return (
                (
                        (option('c', "--copies") & integer("copies").min(1).set(settings.otp.redundancy)) % "Read multiple redundant values" +
                        option('r', "--raw").set(settings.otp.raw) % "Get raw 24-bit values" +
                        option('e', "--ecc").set(settings.otp.ecc) % "Use error correction" +
                        option('n', "--no-descriptions").set(settings.otp.list_no_descriptions) % "Don't show descriptions" +
                        (option('i', "--include") & value("filename").add_to(settings.otp.extra_files)).min(0).max(1) % "Include extra otp definition" // todo more than 1
                ).min(0).doc_non_optional(true) % "Row/field options" +
                (
                        device_selection % "Target device selection"
                ).major_group("TARGET SELECTION").min(0).doc_non_optional(true) +
                (
                        option('z', "--fuzzy").set(settings.otp.fuzzy) % "Allow fuzzy name searches in selector vs exact match" +
                        (value("selector").add_to(settings.otp.selectors) %
                         "The row/field selector, each of which can select a whole row:\n\n" \
                        "ROW_NAME to select a whole row by name.\n" \
                        "ROW_NUMBER to select a whole row by number.\n" \
                        "PAGE:PAGE_ROW_NUMBER to select a whole row by page and number within page.\n\n" \
                        "... or can select a single field/subset of a row (where ROW_SEL is one of the above row selectors):\n\n"
                         "ROW_SEL.FIELD_NAME to select a field within a row by name.\n" \
                        "ROW_SEL.n-m to select a range of bits within a row.\n" \
                        "ROW_SEL.n to select a single bit within a row.\n" \
                        ".FIELD_NAME to select any row's field by name.\n\n" \
                        ".. or can select multiple rows by using blank or '*' for PAGE or PAGE_ROW_NUMBER").repeatable().min(0)
                ) % "Row/Field Selection"
        );
    }

    string get_doc() const override {
        return "Get the value of one or more OTP registers/fields";
    }
};

// possible temporary
struct otp_dump_command : public cmd {
    otp_dump_command() : cmd("dump") {}
    bool execute(device_map& devices) override;
    virtual bool requires_rp2350() const override { return true; }
    device_support get_device_support() override {
        if (settings.filenames[0].empty())
            return one;
        else
            return none;
    }

    group get_cli() override {
        return (
                (
                        option('r', "--raw").set(settings.otp.raw) % "Get raw 24-bit values. This is the default" +
                        option('e', "--ecc").set(settings.otp.ecc) % "Use error correction" +
                        option('p', "--pages").set(settings.otp.dump_pages) % "Index by page number & row number" +
                        option_untyped_file_selection_x(option("--output"), 1) % "Output BIN file to dump to (optional)"
                ).min(0).doc_non_optional(true) % "Row/field options" +
                (
                        device_selection % "To dump the contents of a target device" |
                        named_typed_file_selection_x("input", 0, "json") % "To dump the contents of an OTP JSON file"
                ).major_group("TARGET SELECTION").min(0).doc_non_optional(true)
        );
    }

    string get_doc() const override {
        return "Dump entire OTP";
    }
};

struct otp_load_command : public cmd {
    otp_load_command() : cmd("load") {}
    bool execute(device_map &devices) override;
    virtual bool requires_rp2350() const override { return true; }

    group get_cli() override {
        return (
                (
                        option('r', "--raw").set(settings.otp.raw) % "Set raw 24-bit values. This is the default for BIN files" +
                        option('e', "--ecc").set(settings.otp.ecc) % "Use error correction" +
                        (option('s', "--start_row") & integer("row").set(settings.otp.row)) % "Start row to load at (note use 0x for hex)" +
                        (option('i', "--include") & value("filename").add_to(settings.otp.extra_files)).min(0).max(1) % "Include extra otp definition" // todo more than 1
                ).min(0).doc_non_optional(true) % "Row options" +
                named_typed_file_selection_x("filename", 0, "json | bin") % "File to load row(s) from" +
                device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Load the row range stored in a file into OTP and verify. Data is 2 bytes/row for ECC, 4 bytes/row for raw (MSB is ignored).";
    }
};

struct otp_set_command : public cmd {
    otp_set_command() : cmd("set") {}
    virtual bool requires_rp2350() const override { return true; }

    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
                (
                        (option('c', "--copies") & integer("copies").min(1).set(settings.otp.redundancy)) % "Write multiple redundant values" +
                        option('r', "--raw").set(settings.otp.raw) % "Set raw 24-bit values" +
                        option('e', "--ecc").set(settings.otp.ecc) % "Use error correction" +
                        option('s', "--set-bits").set(settings.otp.ignore_set) % "Set bits only" +
                        (option('i', "--include") & value("filename").add_to(settings.otp.extra_files)).min(0).max(1) % "Include extra otp definition" // todo more than 1
                ).min(0).doc_non_optional(true) % "Redundancy/Error Correction Overrides" +
                (
                        option('z', "--fuzzy").set(settings.otp.fuzzy) % "Allow fuzzy name searches in selector vs exact match" +
                        (value("selector").add_to(settings.otp.selectors) %
                        "The row/field selector, which can select a whole row:\n\n" \
                        "ROW_NAME to select a whole row by name.\n" \
                        "ROW_NUMBER to select a whole row by number.\n" \
                        "PAGE:PAGE_ROW_NUMBER to select a whole row by page and number within page.\n\n" \
                        "... or can select a single field/subset of a row (where ROW_SEL is one of the above row selectors):\n\n"
                         "ROW_SEL.FIELD_NAME to select a field within a row by name.\n" \
                        "ROW_SEL.n-m to select a range of bits within a row.\n" \
                        "ROW_SEL.n to select a single bit within a row.\n" \
                        ".FIELD_NAME to select any row's field by name.")
                ) % "Row/Field Selection" +
                integer("value").set(settings.otp.value) % "The value to set" +
                (
                        device_selection % "Target device selection"
                ).major_group("TARGET SELECTION").min(0).doc_non_optional(true)
        );
    }

    string get_doc() const override {
        return "Set the value of an OTP row/field";
    }
};

struct otp_permissions_command : public cmd {
    otp_permissions_command() : cmd("permissions") {}
    virtual bool requires_rp2350() const override { return true; }

    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
                named_untyped_file_selection_x("filename", 0) % "JSON file to load permissions from" +
                (option("--led") & integer("pin").set(settings.otp.led_pin)) % "LED Pin to flash; default 25" +
                (
                    option("--hash").set(settings.seal.hash) % "Hash the executable" +
                    option("--sign").set(settings.seal.sign) % "Sign the executable" +
                    optional_untyped_file_selection_x("key", 2) % "Key file (.pem)"
                ).min(0).doc_non_optional(true) % "Signing Configuration" +
                device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Set the OTP access permissions";
    }
};

struct otp_white_label_command : public cmd {
    otp_white_label_command() : cmd("white-label") {}
    virtual bool requires_rp2350() const override { return true; }

    bool execute(device_map& devices) override;

    group get_cli() override {
        return (
                (
                        (option('s', "--start_row") & integer("row").set(settings.otp.row)) % "Start row for white label struct (default 0x100) (note use 0x for hex)"
                ).min(0).doc_non_optional(true) % "Row options" +
                named_untyped_file_selection_x("filename", 0) % "JSON file with white labelling values" +
                device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Set the white labelling values in OTP";
    }
};
#endif


vector<std::shared_ptr<cmd>> otp_sub_commands {
    #if HAS_LIBUSB
        std::shared_ptr<cmd>(new otp_get_command()),
        std::shared_ptr<cmd>(new otp_set_command()),
        std::shared_ptr<cmd>(new otp_load_command()),
        std::shared_ptr<cmd>(new otp_white_label_command()),
        std::shared_ptr<cmd>(new otp_permissions_command()),
        std::shared_ptr<cmd>(new otp_dump_command()),
    #endif
        std::shared_ptr<cmd>(new otp_list_command()),
};

struct otp_command : public multi_cmd {
    otp_command() : multi_cmd("otp", otp_sub_commands) {}
    string get_doc() const override {
        return "Commands related to the RP2350 OTP (One-Time-Programmable) Memory";
    }
};

#if HAS_LIBUSB
struct uf2_info_command : public cmd {
    uf2_info_command() : cmd("info") {}
    bool execute(device_map &devices) override;

    group get_cli() override {
        return (
              device_selection % "Target device selection"
        );
    }

    string get_doc() const override {
        return "Print info about UF2 download.";
    }
};
#endif

struct uf2_convert_command : public cmd {
    uf2_convert_command() : cmd("convert") {}
    bool execute(device_map &devices) override;
    virtual device_support get_device_support() override { return none; }

    group get_cli() override {
        return (
                option("--quiet").set(settings.quiet) % "Don't print any output" +
                option("--verbose").set(settings.verbose) % "Print verbose output" +
                named_file_selection_x("infile", 0) % "File to load from" +
                named_typed_file_selection_x("outfile", 1, "uf2") % "File to save UF2 to" +
                (
                    option('o', "--offset").set(settings.offset_set) % "Specify the load address" &
                        hex("offset").set(settings.offset) % "Load offset (memory address; default 0x10000000 for BIN file)"
                ).force_expand_help(true) % "Packaging Options" + 
                (
                    option("--family") % "Specify the family ID" &
                        family_id("family_id").set(settings.family_id) % "family ID for UF2"
                ).force_expand_help(true) % "UF2 Family options" +
                (
                    option("--platform") % "Optional platform for memory-address validation" &
                        platform_model("platform").set(settings.model) % "platform to use (eg rp2040, rp2350)"
                ).force_expand_help(true) % "Platform options"
            #if SUPPORT_RP2350_A2
                + (
                    option("--abs-block").set(settings.uf2.abs_block) % "Add an absolute block" +
                        hex("abs_block_loc").set(settings.uf2.abs_block_loc).min(0) % "absolute block location (default to 0x10ffff00)"
                ).force_expand_help(true).min(0) % "Errata RP2350-E10 Fix"
            #endif
        );
    }

    string get_doc() const override {
        return "Convert ELF/BIN to UF2.";
    }
};

struct uf2_combine_command : public cmd {
    uf2_combine_command() : cmd("combine") {}
    bool execute(device_map &devices) override;
    virtual device_support get_device_support() override { return none; }

    group get_cli() override {
        return (
                option("--quiet").set(settings.quiet) % "Don't print any output" +
                option("--verbose").set(settings.verbose) % "Print verbose output" +
                named_typed_file_selection_x("infile1", 1, "uf2") % "First file to combine" +
                named_typed_file_selection_x("infile2", 2, "uf2") % "Second file to combine" +
                named_typed_file_selection_x("outfile", 0, "uf2") % "File to save output to" +
                (
                    option("--family") % "Specify the family ID" &
                        family_id("family_id").set(settings.family_id) % "family ID for combined UF2 (defaults to first one)"
                ).force_expand_help(true) % "UF2 Family options" +
                (
                    option("--offset").set(settings.uf2.offset_set) % "Offset second UF2 by amount" &
                        hex("offset").set(settings.uf2.offset) % "offset amount (default to 0)"
                ).force_expand_help(true) % "Offset options" +
                (
                    option("--partition").set(settings.uf2.partition_set) % "Place second UF2 in partition (first UF2 must contain a partition table)" &
                        integer("partition").min_value(0).max_value(PARTITION_TABLE_MAX_PARTITIONS-1).set(settings.uf2.partition) % "partition number (default to 0)"
                ).force_expand_help(true) % "Partition options"
            #if SUPPORT_RP2350_A2
                + (
                    option("--abs-block").set(settings.uf2.abs_block) % "Add an absolute block" +
                        hex("abs_block_loc").set(settings.uf2.abs_block_loc).min(0) % "absolute block location (default to 0x10ffff00)"
                ).force_expand_help(true).min(0) % "Errata RP2350-E10 Fix"
            #endif
        );
    }

    string get_doc() const override {
        return "Combine multiple UF2 files.";
    }
};

vector<std::shared_ptr<cmd>> uf2_sub_commands {
        std::shared_ptr<cmd>(new uf2_convert_command()),
        std::shared_ptr<cmd>(new uf2_combine_command()),
    #if HAS_LIBUSB
        std::shared_ptr<cmd>(new uf2_info_command()),
    #endif
};

struct uf2_command : public multi_cmd {
    uf2_command() : multi_cmd("uf2", uf2_sub_commands) {}
    string get_doc() const override {
        return "Commands related to UF2 creation and status";
    }
};

struct coprodis_command : public cmd {
    coprodis_command() : cmd("coprodis") {}
    bool execute(device_map &devices) override;
    virtual device_support get_device_support() override { return none; }

    group get_cli() override {
        return (
                option("--quiet").set(settings.quiet) % "Don't print any output" +
                option("--verbose").set(settings.verbose) % "Print verbose output" +
                named_untyped_file_selection_x("infile", 0) % "Input DIS" +
                named_untyped_file_selection_x("outfile", 1) % "Output DIS"
        );
    }

    string get_doc() const override {
        return "Post-process coprocessor instructions in disassembly files.";
    }

    bool decode_line(uint32_t val, char *buf, size_t buf_len);
};

struct help_command : public cmd {
    help_command() : cmd("help") {}
    bool execute(device_map &devices) override;

    device_support get_device_support() override {
        return device_support::none;
    }

    group get_cli() override {
        return group(
            value("cmd").min(0) % "The command to get help for"
        );
    }

    string get_doc() const override {
        return "Show general help or help for a specific command";
    }
};

struct version_command : public cmd {
    version_command() : cmd("version") {}
    bool execute(device_map &devices) override {
        if (settings.version.semantic)
            std::cout << PICOTOOL_VERSION << "\n";
        else {
            std::cout << "picotool v" << PICOTOOL_VERSION << " (" << SYSTEM_VERSION << ", " << COMPILER_INFO << ")\n";
            #if !HAS_LIBUSB
            std::cout << built_without_libusb_message;
            #endif
        }
        if (!settings.version.version.empty()) {
            string picotool_v = string(PICOTOOL_VERSION);
            picotool_v = picotool_v.substr(0, picotool_v.find("-"));

            int check_v[3], cur_v[3];
            sscanf(settings.version.version.c_str(), "%d.%d.%d", &check_v[0], &check_v[1], &check_v[2]);
            sscanf(picotool_v.c_str(), "%d.%d.%d", &cur_v[0], &cur_v[1], &cur_v[2]);

            if (check_v[0] != cur_v[0])
                fail(ERROR_INCOMPATIBLE, "Version %s not compatible with this software\n", settings.version.version.c_str());
            for (int i = 1; i < 3; i++) {
                if (check_v[i] > cur_v[i])
                    fail(ERROR_INCOMPATIBLE, "Version %s not compatible with this software\n", settings.version.version.c_str());
                else if (check_v[i] < cur_v[i])
                    break;
            }
        }
        return false;
    }

    device_support get_device_support() override {
        return device_support::none;
    }

    group get_cli() override {
        return group(
                option('s', "--semantic").set(settings.version.semantic) % "Output semantic version number only" +
                value("version").set(settings.version.version).min(0) % "Check compatibility with version"
        );
    }

    string get_doc() const override {
        return "Display picotool version";
    }
};

#if HAS_LIBUSB
struct reboot_command : public cmd {
    bool quiet;
    reboot_command() : cmd("reboot") {}
    bool execute(device_map &devices) override;

    group get_cli() override {
        return
        (
            option('a', "--application").set(settings.reboot_app_specified) % "Reboot back into the application (this is the default)" +
            option('u', "--usb").set(settings.reboot_usb) % "Reboot back into BOOTSEL mode" +
            (option('g', "--diagnostic") & integer("partition").min_value(-3).max_value(15).set(settings.reboot_diagnostic_partition)).min(0) % "Select diagnostic partition" +
            (option('c', "--cpu") & value("cpu").set(settings.switch_cpu)) % "Select arm | riscv CPU (if possible)"
        ).min(0).doc_non_optional(true) % "Reboot type" +
        device_selection % "Selecting the device to reboot";
    }

    bool force_requires_pre_reboot() override {
        // no point in rebooting twice
        return false;
    }

    string get_doc() const override {
        return "Reboot the device";
    }
};
auto reboot_cmd = std::shared_ptr<reboot_command>(new reboot_command());
#endif
auto help_cmd = std::shared_ptr<help_command>(new help_command());

vector<std::shared_ptr<cmd>> commands {
        help_cmd,
        std::shared_ptr<cmd>(new version_command()),
        std::shared_ptr<cmd>(new info_command()),
        std::shared_ptr<cmd>(new config_command()),
    #if HAS_LIBUSB
        std::shared_ptr<cmd>(new load_command()),
        std::shared_ptr<cmd>(new save_command()),
        std::shared_ptr<cmd>(new verify_command()),
        std::shared_ptr<cmd>(new erase_command()),
        reboot_cmd,
    #endif
    #if HAS_MBEDTLS
        std::shared_ptr<cmd>(new seal_command()),
        std::shared_ptr<cmd>(new encrypt_command()),
    #endif
        std::shared_ptr<cmd>(new partition_command()),
        std::shared_ptr<cmd>(new uf2_command()),
        std::shared_ptr<cmd>(new otp_command()),
        std::shared_ptr<cmd>(new coprodis_command()),
        std::shared_ptr<cmd>(new link_command()),
    #if HAS_LIBUSB
        std::shared_ptr<cmd>(new bdev_command()),
    #endif
};

template <typename T>
std::basic_string<T> lowercase(const std::basic_string<T>& s)
{
    std::basic_string<T> s2 = s;
    std::transform(s2.begin(), s2.end(), s2.begin(), tolower);
    return s2;
}

template <typename T>
std::basic_string<T> uppercase(const std::basic_string<T>& s)
{
    std::basic_string<T> s2 = s;
    std::transform(s2.begin(), s2.end(), s2.begin(), toupper);
    return s2;
}


std::ostream null_buffer(nullptr);
clipp::formatting_ostream<std::ostream> fos_base(std::cout);
clipp::formatting_ostream<std::ostream> fos_null(null_buffer);

auto fos_base_ptr = std::make_shared<clipp::formatting_ostream<std::ostream>>(fos_base);
auto fos_null_ptr = std::make_shared<clipp::formatting_ostream<std::ostream>>(fos_null);
auto fos_ptr = fos_base_ptr;
#define fos (*fos_ptr)
#define fos_verbose if(settings.verbose) fos

using cli::option;
using cli::integer;
int parse(const int argc, char **argv) {
    bool help_mode = false;
    bool no_global_header = false;
    bool no_synopsis = false;
    std::string help_mode_prefix;

    int tab = 4;
    bool first = true;
    auto section_header=[&](const string &name) {
        fos.first_column(0);
        fos.hanging_indent(0);
        if (!first) fos.wrap_hard();
        first = false;
        fos << (uppercase(name) + ":\n");
    };
    auto usage=[&]() {
        if (help_mode && selected_cmd) {
            section_header(help_mode_prefix + selected_cmd->name());
            fos.first_column(tab);
            fos << selected_cmd->get_doc() << "\n";
        } else if (!selected_cmd && !no_global_header) {
            section_header(tool_name);
            fos.first_column(tab);
        #if HAS_LIBUSB
            fos << "Tool for interacting with RP-series device(s) in BOOTSEL mode, or with an RP-series binary" << "\n";
        #else
            fos << "Tool for interacting with an RP-series binary" << "\n";
        #endif
        }
        vector<string> synopsis;
        auto maybe_add_synopsys = [&](const std::string& name, std::shared_ptr<cmd>& c, bool force = false) {
            if (!force && selected_cmd && c != selected_cmd) return;
            vector<string> cmd_synopsis;
            if (c->is_multi()) {
                string s;
                for(auto& subc : c->sub_commands()) { s += subc->name() + "|"; }
                if (s.size() > 0) s.pop_back();
                cmd_synopsis.push_back(s);
            } else {
                cmd_synopsis = c->get_cli().synopsys();
            }
            for(auto &s : cmd_synopsis) {
                synopsis.emplace_back(name + " " + s);
            }
        };
        for(auto &c : commands) {
            if (c->is_multi()) {
                if (c == selected_cmd) {
                    // Selected multi-command, so print sub commands
                    for(auto& subc : c->sub_commands()) {
                        maybe_add_synopsys( c->name() + " " + subc->name(), subc, true);
                    }
                } else if (selected_cmd) {
                    for(auto& subc : c->sub_commands()) {
                        // Selected sub-command, so print that
                        if (selected_cmd && subc == selected_cmd) maybe_add_synopsys( c->name() + " " + subc->name(), subc, true);
                    }
                } else {
                    // No command selected, so print multi-command
                    maybe_add_synopsys(c->name(), c);
                }
            } else {
                maybe_add_synopsys(c->name(), c);
            }
        }
        if (!no_synopsis) {
            section_header("SYNOPSIS");
            for (auto &s : synopsis) {
                fos.first_column(tab);
                fos.hanging_indent((int)tool_name.length() + tab);
                fos << (tool_name + " ").append(s).append("\n");
            }
        }
        auto write_command = [&](size_t max, const std::string& name, std::shared_ptr<cmd> c) {
            fos.first_column(tab);
            fos << name;
            fos.first_column((int) (max + tab + 3));
            std::stringstream s;
            s << c->get_doc();
            if (c->requires_rp2350()) {
                s << " (RP2350 only)";
            }
            s << "\n";
            fos << s.str();
        };
        if (!selected_cmd) {
            size_t max = 0;
            section_header("COMMANDS");
            for (auto &cmd: commands) {
                max = std::max(cmd->name().size(), max);
            }
            for (auto &cmd: commands) {
                write_command(max, cmd->name(), cmd);
            }
        } else if (selected_cmd->is_multi()) {
            section_header("SUB COMMANDS");
            size_t max = 0;
            auto sub_commands = selected_cmd->sub_commands();
            for (auto &cmd: sub_commands) {
                max = std::max(cmd->name().size(), max);
            }
            for (auto &cmd: sub_commands) {
                write_command(max, cmd->name(), cmd);
            }
        } else if (!help_mode) {
            fos.first_column(0);
            fos.hanging_indent(0);
            fos.wrap_hard();
            // Check if sub command
            std::shared_ptr<cmd> super_command = nullptr;
            for(auto &c : commands) {
                if (c->is_multi()) {
                    if (selected_cmd) {
                        for(auto& subc : c->sub_commands()) {
                            // Selected sub-command, so print that
                            if (selected_cmd && subc == selected_cmd) super_command = c;
                        }
                    }
                }
            }
            if (super_command != nullptr) {
                fos << string("Use \"picotool help ").append(super_command->name() +" "+ selected_cmd->name()).append("\" for more info\n");
            } else {
                fos << string("Use \"picotool help ").append(selected_cmd->name()).append("\" for more info\n");
            }
            #if !HAS_LIBUSB
            fos << built_without_libusb_message;
            #endif
        } else {
            cli::option_map options;
            selected_cmd->get_cli().get_option_help("", "", options);
            for (const auto &major : options.contents.ordered_keys()) {
                section_header(major.empty() ? "OPTIONS" : major);
                bool first = true;
                for (const auto &minor : options.contents[major].ordered_keys()) {
                    fos.first_column(tab);
                    fos.hanging_indent(tab*2);
                    if (!minor.empty()) {
                        fos << minor << "\n";
                    } else if (!first) {
                        fos << "Other\n";
                    }
                    first = false;
                    for (const auto &opts : options.contents[major][minor]) {
                        fos.first_column(tab*2);
                        fos.hanging_indent(0);
                        fos << opts.first << "\n";
                        fos.first_column(tab*3);
                        fos.hanging_indent(0);
                        fos << opts.second << "\n";
                    }
                }
            }
        }
        if (!selected_cmd) {
            fos.first_column(0);
            fos.hanging_indent(0);
            fos.wrap_hard();
            fos << "Use \"picotool help <cmd>\" for more info\n";
            #if !HAS_LIBUSB
            if (!help_mode) {
                fos << built_without_libusb_message;
            }
            #endif
        }
        fos.flush();
    };

    auto args = cli::make_args(argc, argv);

    // Check if any -h or --help argument was requested, if so insert the help subcommand before any
    // arguments, this will ensure the correct help gets printed automatically.
    bool help_requested = std::find(args.begin(), args.end(), "--help") != args.end()
                          || std::find(args.begin(), args.end(), "-h") != args.end();
    if (help_requested) {
      args.insert(args.begin(), "help");
    }
    if (args.empty()) {
        usage();
        return 0;
    }

    auto find_command = [&](const string& name) {
        if (name[0]=='-') {
            no_global_header = true;
            throw cli::parse_error("Expected command name before any options");
        }
        auto cmd = std::find_if(commands.begin(), commands.end(), [&](auto &x) { return x->name() == name; });
        if (cmd == commands.end()) {
            selected_cmd = nullptr; // we want to list all commands
            no_synopsis = true;
            no_global_header = true;
            throw cli::parse_error("Unknown command: " + name);
        }
        return *cmd;
    };

    auto find_sub_command = [&](std::shared_ptr<cmd>& parent_cmd, const string& name) {
        if (name[0]=='-') {
            no_global_header = true;
            throw cli::parse_error("Expected "+parent_cmd->name()+" sub command name before any options");
        }
        auto commands = parent_cmd->sub_commands();
        auto cmd = std::find_if(commands.begin(), commands.end(), [&](auto &x) { return x->name() == name; });
        if (cmd == commands.end()) {
            selected_cmd = parent_cmd; // we want to list all commands
            no_synopsis = true;
            no_global_header = true;
            throw cli::parse_error("Unknown "+parent_cmd->name()+" sub command: " + name);
        }
        return *cmd;
    };

    try {
        selected_cmd = find_command(args[0]);
        args.erase(args.begin()); // remove the cmd itself
        if (selected_cmd->is_multi()) {
            if (args.empty()) {
                no_synopsis = true;
                no_global_header = true;
                throw cli::parse_error("Expected "+selected_cmd->name()+" sub-command");
            } else {
                selected_cmd = find_sub_command(selected_cmd, args[0]);
                args.erase(args.begin());
            }
        }
        if (selected_cmd->name() == "help") {
            help_mode = true;
            if (args.empty()) {
                selected_cmd = nullptr;
                usage();
                return 0;
            } else {
                selected_cmd = find_command(args[0]);
                if (selected_cmd->is_multi() && args.size() > 1) {
                    help_mode_prefix = selected_cmd->name() + " ";
                    selected_cmd = find_sub_command(selected_cmd, args[1]);
                }
                usage();
                selected_cmd = nullptr;
                return 0;
            }
        } else if (!selected_cmd) {
            no_synopsis = true;
            no_global_header = true;
            throw cli::parse_error("unknown command '" + args[0] + "'");
        }
        cli::match(settings, selected_cmd->get_cli(), args);
    } catch (std::exception &e) {
        fos.wrap_hard();
        fos << "ERROR: " << e.what() << "\n\n";
        usage();
        return ERROR_ARGS;
    }
    return 0;
}

template <typename T> struct raw_type_mapping {
};

#define SAFE_MAPPING(type) template<> struct raw_type_mapping<type> { typedef type access_type; }

//template<> struct raw_type_mapping<uint32_t> {
//    typedef uint32_t access_type;
//};

// these types may be filled directly from byte representation
SAFE_MAPPING(uint8_t);
SAFE_MAPPING(char);
SAFE_MAPPING(uint16_t);
SAFE_MAPPING(uint32_t);
SAFE_MAPPING(binary_info_core_t);
SAFE_MAPPING(binary_info_id_and_int_t);
SAFE_MAPPING(binary_info_id_and_string_t);
SAFE_MAPPING(binary_info_ptr_int32_with_name_t);
SAFE_MAPPING(binary_info_ptr_string_with_name_t);
SAFE_MAPPING(binary_info_block_device_t);
SAFE_MAPPING(binary_info_pins_with_func_t);
SAFE_MAPPING(binary_info_pins_with_name_t);
SAFE_MAPPING(binary_info_pins64_with_func_t);
SAFE_MAPPING(binary_info_pins64_with_name_t);
SAFE_MAPPING(binary_info_named_group_t);

#define BOOTROM_MAGIC_RP2040 0x01754d
#define BOOTROM_MAGIC_RP2350 0x02754d
#define BOOTROM_MAGIC_UNKNOWN 0x000000
#define BOOTROM_MAGIC_ADDR 0x00000010

static inline uint32_t rom_table_code(char c1, char c2) {
    return (c2 << 8u) | c1;
}

struct memory_access {
    virtual void read(uint32_t p, uint8_t *buffer, unsigned int size) {
        read(p, buffer, size, false);
    }

    virtual void read(uint32_t, uint8_t *buffer, unsigned int size, bool zero_fill) = 0;

    virtual void write(uint32_t, uint8_t *buffer, unsigned int size) = 0;

    virtual bool is_device() { return false; }

    virtual uint32_t get_binary_start() = 0;

    uint32_t read_int(uint32_t addr, bool zero_fill = false) {
        assert(!(addr & 3u));
        uint32_t rc;
        read(addr, (uint8_t *)&rc, 4, zero_fill);
        return rc;
    }

    uint32_t read_short(uint32_t addr, bool zero_fill = false) {
        assert(!(addr & 1u));
        uint16_t rc;
        read(addr, (uint8_t *)&rc, 2, zero_fill);
        return rc;
    }

    // read a vector of types that have a raw_type_mapping
    template <typename T> void read_raw(uint32_t addr, T &v, bool zero_fill = false) {
        typename raw_type_mapping<T>::access_type& check = v; // ugly check that we aren't trying to read into something we shouldn't
        read(addr, (uint8_t *)&v, sizeof(typename raw_type_mapping<T>::access_type), zero_fill);
    }

    // read a vector of types that have a raw_type_mapping
    template <typename T> vector<T> read_vector(uint32_t addr, unsigned int count, bool zero_fill = false) {
        assert(count);
        vector<typename raw_type_mapping<T>::access_type> buffer(count);
        read(addr, (uint8_t *)buffer.data(), count * sizeof(typename raw_type_mapping<T>::access_type), zero_fill);
        vector<T> v;
        v.reserve(count);
        for(const auto &e : buffer) {
            v.push_back(e);
        }
        return v;
    }

    // write a vector of types that have a raw_type_mapping
    template <typename T> void write_vector(uint32_t addr, vector<T> &v) {
        assert(v.size());
        write(addr, (uint8_t *)v.data(), v.size() * sizeof(typename raw_type_mapping<T>::access_type));
    }

    template <typename T> void read_into_vector(uint32_t addr, unsigned int count, vector<T> &v, bool zero_fill = false) {
        v.clear();
        v.resize(count);
        if (count) {
            read(addr, (uint8_t *)v.data(), count * sizeof(typename raw_type_mapping<T>::access_type), zero_fill);
        }
    }

    model_t get_model() {
        assert(model);
        return model;
    }

protected:
    model_t model = models::unknown; // something we can read from the start of ROM with
};

template<typename T>
bool get_int(const std::string& s, T& out) {
    return integer::parse_string(s, out).empty();
}

template<typename T>
bool get_json_int(json value, T& out) {
    if (value.is_string()) {
        string str = value;
        if (str.back() == 'k' || str.back() == 'K') {
            str.pop_back();
            int tmp;
            if (get_int(str, tmp)) {
                out = tmp * 1024;
                return true;
            } else {
                return false;
            }
        }
        return get_int(str, out);
    } else if (value.is_number_integer()) {
        out = value;
        return true;
    } else {
        return false;
    }
}

bool get_json_bool(json value, bool& out) {
    if (value.is_boolean()) {
        out = value;
        return true;
    } else {
        return false;
    }
}

uint32_t bootrom_func_lookup_rp2040(memory_access& access, uint16_t tag) {
    model_t model = access.get_model();
    // we are only used on RP2040
    if (model->chip() != rp2040) {
        fail(ERROR_INCOMPATIBLE, "RP2040 BOOT ROM not found");
    }

    // dereference the table pointer
    uint32_t table_entry = access.read_short(BOOTROM_MAGIC_ADDR + 4);
    uint16_t entry_tag;
    do {
        entry_tag = access.read_short(table_entry);
        if (entry_tag == tag) {
            // 16 bit symbol is next
            return access.read_short(table_entry+2);
        }
        table_entry += 4;
    } while (entry_tag);
    fail(ERROR_INCOMPATIBLE, "Function not found in BOOT ROM");
    return 0;
}

uint32_t bootrom_table_lookup_v2(memory_access& access, uint16_t tag, uint16_t flags) {
    model_t model = access.get_model();
    // we are only used on RP2350
    if (model->rom_table_version() != 2) {
        fail(ERROR_INCOMPATIBLE, "BOOT ROM TABLE (v2) not found");
    }

    // dereference the table pointer
    uint32_t table_entry = access.read_short(BOOTROM_MAGIC_ADDR + 4);
    uint16_t entry_tag, entry_flags;
    do {
        entry_tag = access.read_short(table_entry);
        entry_flags = access.read_short(table_entry + 2);
        uint16_t matching_flags = flags & entry_flags;
        table_entry += 4;
        if (tag == entry_tag && matching_flags != 0) {
            /* This is our entry, seek to the correct data item and return it. */
            bool is_riscv_func = matching_flags & RT_FLAG_FUNC_RISCV;
            while (!(matching_flags & 1)) {
                if (entry_flags & 1) {
                    table_entry += 2;
                }
                matching_flags >>= 1;
                entry_flags >>= 1;
            }
            if (is_riscv_func) {
                /* For RISC-V, the table entry itself is the entry point -- trick
                    to make shared function implementations smaller */
                return table_entry;
            } else {
                return access.read_short(table_entry);
            }
        } else {
            /* Skip past this entry */
            while (entry_flags) {
                if (entry_flags & 1) {
                    table_entry += 2;
                }
                entry_flags >>= 1;
            }
        }
    } while (entry_tag);
    fail(ERROR_INCOMPATIBLE, "Entry not found in BOOT ROM");
    return 0;
}

static uint32_t get_rom_git_revision_v2(memory_access &raw_access) {
    unsigned int addr = bootrom_table_lookup_v2(raw_access, rom_table_code('G','R'), RT_FLAG_DATA);
    return raw_access.read_int(addr);
}

static model_t determine_model(memory_access &raw_access) {
    auto raw = raw_access.read_int(BOOTROM_MAGIC_ADDR);
    auto magic = raw & 0xf0ffffff; // ignoring bootrom version
    if (magic == BOOTROM_MAGIC_RP2040) {
        return std::make_shared<model_rp2040>();
    } else if (magic == BOOTROM_MAGIC_RP2350) {
        uint32_t table_entry = raw_access.read_short(BOOTROM_MAGIC_ADDR + 4);
        static_assert(ROM_END_RP2350 == 0x8000, "");
        if (table_entry < 0x8000) {
            return std::make_shared<model_rp2350>();
        } else {
            return std::make_shared<model_rp2350>(0x10000);
        }
    }
    return models::unknown;
}

static inline bool is_transfer_aligned(uint32_t addr, const model_t& model) {
    enum memory_type t = get_memory_type(addr, model);
    return t != invalid && !(t == flash && addr & (PAGE_SIZE-1));
}

// this must be called after the right model is set on raw_access which is why it isn't
// part of init_model
static chip_revision_t determine_chip_revision(memory_access &raw_access) {
    chip_revision_t chip_revision = unknown_revision;
    model_t model = raw_access.get_model();
    uint8_t rom_version;
    raw_access.read_raw(0x13, rom_version);
    if (model->chip() == rp2040) {
        switch (rom_version) {
            case 1:
                chip_revision = rp2040_b0;
                break;
            case 2:
                chip_revision = rp2040_b1;
                break;
            case 3:
                chip_revision = rp2040_b2;
                break;
            default:
                break;
        };
    } else if (model->chip() == rp2350) {
        // switch (get_rom_git_revision_v2(raw_access)) {
        //     default:
        //         break;
        // }
        switch (rom_version) {
            case 2:
                chip_revision = rp2350_a2;
                break;
            case 3:
                chip_revision = rp2350_a3;
                break;
            case 4:
                chip_revision = rp2350_a4;
                break;
            default:
                break;
        };
    }
    return chip_revision;
}
#if HAS_LIBUSB
struct picoboot_memory_access : public memory_access {
    explicit picoboot_memory_access(picoboot::connection &connection) : connection(connection) {
        model = determine_model(*this);
        if (model->chip() != unknown)
            model->set_chip_revision(determine_chip_revision(*this));
    }

    bool is_device() override {
        return true;
    }

    uint32_t get_binary_start() override {
        return FLASH_START;
    }

    void read(uint32_t address, uint8_t *buffer, unsigned int size, __unused bool zero_fill) override {
        if (settings.use_flash_cache && flash == get_memory_type(address, model)) {
            read_cached(address, buffer, size);
        } else {
            read_raw(address, buffer, size);
        }
    }

    void clear_cache() {
        flash_cache.clear();
    }

    void read_cached(uint32_t address, uint8_t *buffer, unsigned int size) {
        for (auto range: flash_cache) {
            uint32_t cached_start = std::get<0>(range);
            uint32_t cached_size = std::get<1>(range);
            auto cached_data = std::get<2>(range);
            if (address >= cached_start) {
                if (address >= cached_start + cached_size) {
                    // No data already cached
                    continue;
                } else if (address + size <= cached_start + cached_size) {
                    // All data already cached
                    DEBUG_LOG("Flash Cache Hit %08x+%08x\n", address, size);
                    std::copy(cached_data.cbegin() + (address - cached_start), cached_data.cbegin() + (address + size - cached_start), buffer);
                    return;
                } else {
                    // Start of data already cached, but end needs reading
                    uint32_t cached_used_size = cached_size - (address - cached_start);
                    DEBUG_LOG("Flash Cache Hit Start %08x+%08x\n", address, cached_used_size);
                    std::copy(cached_data.cbegin() + (address - cached_start), cached_data.cbegin() + cached_size, buffer);
                    size -= cached_used_size;
                    address += cached_used_size;
                    buffer += cached_used_size;
                }
            } else {
                if (address + size <= cached_start) {
                    // No data already cached
                    continue;
                } else if (address + size <= cached_start + cached_size) {
                    DEBUG_LOG("Flash Cache Hit End %08x+%08x\n", cached_start, (address + size - cached_start));
                    // End of data already cached, but start needs reading
                    std::copy(cached_data.cbegin(), cached_data.cbegin() + (address + size - cached_start), buffer + (cached_start - address));
                    size = cached_start - address;
                } else {
                    // Middle of data already cached, start and end needs reading
                    uint32_t split_size = cached_start - address; // split into address->cached_start and cached_start->(address + size)
                    DEBUG_LOG("Flash Cache Hit Middle %08x+%08x\n", cached_start, cached_size);
                    // Read data before
                    read_cached(address, buffer, split_size);
                    // Read rest
                    read_cached(address + split_size, buffer + split_size, size - split_size);
                    return;
                }
            }
        }

        DEBUG_LOG("Flash Caching %08x+%08x\n", address, size);
        read_raw(address, buffer, size);
        std::vector<uint8_t> cached_data(buffer, buffer + size);
        flash_cache.push_back(std::make_tuple(address, size, cached_data));
    }

    void read_raw(uint32_t address, uint8_t *buffer, unsigned int size) {
        if (flash == get_memory_type(address, model)) {
            connection.exit_xip();
        }
        if (model->chip() == rp2040 && rom == get_memory_type(address, model) && (address+size) >= 0x2000) {
            // read by memcpy instead
            unsigned int program_base = SRAM_START + 0x4000;
            // program is "return memcpy(SRAM_BASE, 0, 0x4000);"
            std::vector<uint32_t> program = {
                    0x07482101, // movs r1, #1;       lsls r0, r1, #29
                    0x2100038a, // lsls r2, r1, #14;  movs r1, #0
                    0x47184b00, // ldr  r3, [pc, #0]; bx r3
                    bootrom_func_lookup_rp2040(*this, rom_table_code('M','C'))
            };
            write_vector(program_base, program);
            connection.exec(program_base);
            // 4k is copied into the start of RAM
            connection.read(SRAM_START + address, (uint8_t *) buffer, size);
        } else if (contains_unreadable_rom(address, size, model)) {
            // Cannot read end section of rom from device, so use the saved header for the chip
            const unsigned char *unreadable_data = model->unreadable_rom_data();
            if (unreadable_data == nullptr) {
                fail(ERROR_INCOMPATIBLE, "Cannot read unreadable ROM data for %s revision %s", model->name().c_str(), model->revision_name().c_str());
            }
            uint16_t unreadable_start = MAX(address, model->unreadable_rom_start());
            uint16_t unreadable_end = MIN(address+size, model->unreadable_rom_end());
            uint16_t idx = 0;
            if (address < unreadable_start) {
                connection.read(address, (uint8_t *) buffer, unreadable_start - address);
                idx += unreadable_start - address;
            }
            memcpy(buffer+idx, unreadable_data+(unreadable_start-(model->unreadable_rom_start())), unreadable_end - unreadable_start);
            idx += unreadable_end - unreadable_start;
            if (address + size > unreadable_end) {
                connection.read(unreadable_end, (uint8_t *) buffer+idx, size - idx);
            }
        } else if (is_transfer_aligned(address, model) && is_transfer_aligned(address + size, model)) {
            connection.read(address, (uint8_t *) buffer, size);
        } else {
            if (flash == get_memory_type(address, model)) {
                uint32_t aligned_start = address & ~(PAGE_SIZE - 1);
                uint32_t aligned_end = (address + size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1);
                vector<uint8_t> tmp_buffer(aligned_end - aligned_start);
                connection.read(aligned_start, tmp_buffer.data(), aligned_end - aligned_start);
                    std::copy(tmp_buffer.cbegin() + (address - aligned_start), tmp_buffer.cbegin() + (address + size - aligned_start), buffer);
            } else {
                std::stringstream sstream;
                sstream << "Address range " << hex_string(address) << " + " << hex_string(size);
                throw std::invalid_argument(sstream.str());
            }
        }
    }

    // note this does not automatically erase flash unless erase is set
    void write(uint32_t address, uint8_t *buffer, unsigned int size) override {
        vector<uint8_t> write_data; // used when erasing flash
        if (flash == get_memory_type(address, model)) {
            connection.exit_xip();
            // Flash Translation Layer - auto-erase, and only write changed data
            if (enable_ftl) {
                // Check what's there already
                write_data.resize(size);
                read(address, write_data.data(), size, false);
                // Check if we even need to write
                if (std::equal(write_data.cbegin(), write_data.cend(), buffer)) {
                    return;
                }
                // Check if we need to erase (ie check for bits that need to be set)
                bool do_erase = false;
                for (int i = 0; i < write_data.size(); i++) {
                    if (buffer[i] & ~write_data[i]) {
                        do_erase = true;
                        break;
                    }
                }
                if (do_erase) {
                    // Automatically erase flash, and make it aligned
                    // we have to erase in whole pages
                    range aligned_range(address & ~(FLASH_SECTOR_ERASE_SIZE - 1),
                                        ((address + size) & ~(FLASH_SECTOR_ERASE_SIZE - 1)) + FLASH_SECTOR_ERASE_SIZE);
                    assert(aligned_range.contains(address));
                    assert(aligned_range.contains(address + size));

                    uint32_t pre_len = address - aligned_range.from;
                    uint32_t post_len = aligned_range.to - (address + size);
                    assert(pre_len + size + post_len == aligned_range.len());

                    // save data before the changing data
                    write_data.resize(pre_len);
                    if (pre_len) read(aligned_range.from, write_data.data(), write_data.size(), false);
                    // now add the data that is changing
                    write_data.insert(write_data.end(), buffer, buffer + size);
                    // save data after the changing data
                    write_data.resize(aligned_range.len());
                    if (post_len) read(address + size, write_data.data() + pre_len + size, post_len, false);

                    // Do the erase
                    connection.flash_erase(aligned_range.from, aligned_range.len());

                    // Update what will now be written
                    address = aligned_range.from;
                    buffer = write_data.data();
                    size = aligned_range.len();
                }
            }
        }
        if (is_transfer_aligned(address, model) && is_transfer_aligned(address + size, model)) {
            connection.write(address, (uint8_t *) buffer, size);
            clear_cache(); // clear entire flash cache after any write, to be safe
        } else {
            // for write, we must be correctly sized/aligned in 256 byte chunks
            std::stringstream sstream;
            sstream << "Address range " << hex_string(address) << " + " << hex_string(size);
            throw std::invalid_argument(sstream.str());
        }
    }

    template <typename T> void write_vector(uint32_t addr, vector<T> v) {
        assert(!v.empty());
        write(addr, (uint8_t *)v.data(), v.size() * sizeof(typename raw_type_mapping<T>::access_type));
    }

    // Enable Flash Translation Layer, which performs automatic erase, and only writes changed data
    bool enable_ftl = false;
private:
    picoboot::connection& connection;
    vector<std::tuple<uint32_t,uint32_t,vector<uint8_t>>> flash_cache;
};
#endif


struct iostream_memory_access : public memory_access {
    iostream_memory_access(std::shared_ptr<std::iostream> file, range_map<size_t>& rmap, uint32_t binary_start) : file(file), rmap(rmap), binary_start(binary_start) {

    }

    uint32_t get_binary_start() override {
        return binary_start;
    }

    void set_model(model_t m) {
        model = m;
    }

    void read(uint32_t address, uint8_t *buffer, uint32_t size, bool zero_fill) override {
        if (address == BOOTROM_MAGIC_ADDR && size == 4) {
            // return the memory model
            if (model->chip()== rp2040) {
                *(uint32_t*)buffer = BOOTROM_MAGIC_RP2040;
                return;
            } else if (model->chip() == rp2350) {
                *(uint32_t*)buffer = BOOTROM_MAGIC_RP2350;
                return;
            } else {
                *(uint32_t*)buffer = BOOTROM_MAGIC_UNKNOWN;
                return;
            }
        }
        while (size) {
            unsigned int this_size;
            try {
                auto result = rmap.get(address);
                this_size = std::min(size, result.first.max_offset - result.first.offset);
                assert(this_size);
                file->seekg(result.second + result.first.offset, ios::beg);
                file->read((char*)buffer, this_size);
            } catch (not_mapped_exception &) {
                if (zero_fill) {
                    // address is not in a range, so fill up to next range with zeros
                    this_size = rmap.next(address) - address;
                    this_size = std::min(this_size, size);
                    memset(buffer, 0, this_size);
                } else {
                    throw;
                }
            }
            buffer += this_size;
            address += this_size;
            size -= this_size;
        }
    }

    void write(uint32_t address, uint8_t *buffer, uint32_t size) override {
        while (size) {
            unsigned int this_size;
            auto result = rmap.get(address);
            this_size = std::min(size, result.first.max_offset - result.first.offset);
            assert(this_size);
            file->seekp(result.second + result.first.offset, ios::beg);
            file->write((char*)buffer, this_size);
            if (file->fail()) {
                fail(ERROR_WRITE_FAILED, "Write to file failed");
            }
            buffer += this_size;
            address += this_size;
            size -= this_size;
        }
    }

    const range_map<size_t> &get_rmap() {
        return rmap;
    }
private:
    std::shared_ptr<std::iostream>file;
    range_map<size_t> rmap;
    uint32_t binary_start;
};


struct file_memory_access : public iostream_memory_access {
    file_memory_access(std::shared_ptr<std::fstream> file, range_map<size_t>& rmap, uint32_t binary_start) : iostream_memory_access(file, rmap, binary_start), file(file) {
        
    }

    ~file_memory_access() {
        file->close();
    }
private:
    std::shared_ptr<std::fstream>file;
};

struct remapped_memory_access : public memory_access {
    remapped_memory_access(memory_access &wrap, range_map<uint32_t> rmap) : wrap(wrap), rmap(rmap) {
        model = wrap.get_model();
    }

    void read(uint32_t address, uint8_t *buffer, unsigned int size, bool zero_fill) override {
        while (size) {
            auto result = get_remapped(address);
            unsigned int this_size = std::min(size, result.first.max_offset - result.first.offset);
            assert( this_size);
            wrap.read(result.second + result.first.offset, buffer, this_size, zero_fill);
            buffer += this_size;
            address += this_size;
            size -= this_size;
        }
    }

    void write(uint32_t address, uint8_t *buffer, unsigned int size) override {
        while (size) {
            auto result = get_remapped(address);
            unsigned int this_size = std::min(size, result.first.max_offset - result.first.offset);
            assert( this_size);
            wrap.write(result.second + result.first.offset, buffer, this_size);
            buffer += this_size;
            address += this_size;
            size -= this_size;
        }
    }

    bool is_device() override {
        return wrap.is_device();
    }

    uint32_t get_binary_start() override {
        return wrap.get_binary_start(); // this is an absolute address
    }

    pair<range_map<uint32_t>::mapping, uint32_t> get_remapped(uint32_t address) {
        try {
            return rmap.get(address);
        } catch (not_mapped_exception&) {
            return std::make_pair(range_map<uint32_t>::mapping(0, rmap.next(address) - address), address);
        }
    }

private:
    memory_access& wrap;
    range_map<uint32_t> rmap;
};

struct partition_memory_access : public memory_access {
    partition_memory_access(memory_access &wrap, uint32_t partition_start) : wrap(wrap), partition_start(partition_start) {
        model = wrap.get_model();
    }

    void read(uint32_t address, uint8_t *buffer, unsigned int size, bool zero_fill) override {
        if (get_memory_type(address, model) == flash) {
            // Only change address when reading from flash
            wrap.read(address + partition_start, buffer, size, zero_fill);
        } else {
            wrap.read(address, buffer, size, zero_fill);
        }
    }

    void write(uint32_t address, uint8_t *buffer, unsigned int size) override {
        wrap.write(address + partition_start, buffer, size);
    }

    bool is_device() override {
        return wrap.is_device();
    }

    uint32_t get_binary_start() override {
        return wrap.get_binary_start(); // this is an absolute address
    }

private:
    memory_access& wrap;
    uint32_t partition_start;
};

static void read_and_check_elf32_header(std::shared_ptr<std::iostream>in, elf32_header& eh_out) {
    in->read((char*)&eh_out, sizeof(eh_out));
    if (in->fail()) {
        fail(ERROR_FORMAT, "'" + settings.filenames[0] +"' is not an ELF file");
    }
    try {
        rp_check_elf_header(eh_out);
    } catch (failure_error &e) {
        fail(e.code(), "'" + settings.filenames[0] +"' failed validation - " + e.what());
    }
}

static void fail_read_error() {
    fail(ERROR_READ_FAILED, "Failed to read input file");
}

static void fail_write_error() {
    fail(ERROR_WRITE_FAILED, "Failed to write output file");
}

struct binary_info_header {
    vector<uint32_t> bi_addr;
    range_map<uint32_t> reverse_copy_mapping;
};

bool find_binary_info(memory_access& access, binary_info_header &hdr) {
    uint32_t base = access.get_binary_start();
    model_t model = access.get_model();
    if (!base) {
        fail(ERROR_FORMAT, "UF2 file does not contain a valid RP2 executable image");
    }
    uint32_t max_dist = 256;
    if (model->chip() == rp2040) {
        max_dist = 64;
        if (base == FLASH_START) base += 0x100; // skip the boot2
    }
    vector<uint32_t> buffer = access.read_vector<uint32_t>(base, max_dist, true);
    for(unsigned int i=0;i<buffer.size();i++) {
        if (buffer[i] == BINARY_INFO_MARKER_START) {
            if (i + 4 < max_dist && buffer[i+4] == BINARY_INFO_MARKER_END) {
                uint32_t from = buffer[i+1];
                uint32_t to = buffer[i+2];
                enum memory_type from_type = get_memory_type(from, model);
                enum memory_type to_type = get_memory_type(to, model);
                if (to > from &&
                        from_type == to_type &&
                        is_size_aligned(from, 4) &&
                        is_size_aligned(to, 4)) {
                    access.read_into_vector(from, (to - from) / 4, hdr.bi_addr);
                    uint32_t cpy_table = buffer[i+3];
                    vector<uint32_t> mapping;
                    do {
                        mapping = access.read_vector<uint32_t>(cpy_table, 3);
                        if (!mapping[0]) break;
                        // from, to_start, to_end
                        hdr.reverse_copy_mapping.insert(range(mapping[1], mapping[2]), mapping[0]);
                        cpy_table += 12;
                    } while (hdr.reverse_copy_mapping.size() < 10); // arbitrary max
                    return true;
                }
            }
        }
    }
    return false;
}

string read_string(memory_access &access, uint32_t addr) {
    const unsigned int max_length = 512;
    auto v = access.read_vector<char>(addr, max_length, true); // zero fill
    unsigned int length;
    for (length = 0; length < max_length; length++) {
        if (!v[length]) {
            break;
        }
    }
    return string(v.data(), length);
}

struct bi_visitor_base {
    template <typename T> void do_pins_func(T value) {
        uint8_t bpp = 5u; // bits per pin
        uint8_t pm = 0x1fu; // pin mask
        uint8_t fp = 7u; // first pin
        uint8_t max_pins = 5;
        if (std::is_same<T, binary_info_pins64_with_func_t>::value) {
            bpp = 8u;
            pm = 0xffu;
            fp = 8u;
            max_pins = 7;
        }
        unsigned int type = value.pin_encoding & 7u;
        unsigned int func = (value.pin_encoding >> 3u) & 0xfu;
        if (type == BI_PINS_ENCODING_RANGE) {
            uint64_t mask = 0;
            uint8_t plo = (value.pin_encoding >> fp) & pm;
            uint8_t phi = (value.pin_encoding >> (fp + bpp)) & pm;
            for(int i=plo;i<=phi;i++) { // range includes end
                mask |= 1ull << i;
            }
            pins(mask, func, "");
        } else if (type == BI_PINS_ENCODING_MULTI) {
            uint64_t mask = 0;
            int last = -1;
            uint64_t work = value.pin_encoding >> fp;
            for(int i=0;i<max_pins;i++) {
                int cur = (int) (work & pm);
                mask |= 1ull << cur;
                if (cur == last) break;
                last = cur;
                work >>= bpp;
            }
            pins(mask, func, "");
        }
    }

    void visit(memory_access& access, const binary_info_header& hdr) {
        chip = access.get_model()->chip();
        for (const auto &a : hdr.bi_addr) {
            visit(access, a);
        }
    }

    void visit(memory_access& access, uint32_t addr) {
        binary_info_core_t bi;
        access.read_raw(addr, bi);
        switch (bi.type) {
            case BINARY_INFO_TYPE_RAW_DATA:
                break;
            case BINARY_INFO_TYPE_SIZED_DATA:
                break;
            case BINARY_INFO_TYPE_BINARY_INFO_LIST_ZERO_TERMINATED:
                zero_terminated_bi_list(access, bi, addr);
                break;
            case BINARY_INFO_TYPE_BSON:
                break;
            case BINARY_INFO_TYPE_ID_AND_INT: {
                binary_info_id_and_int_t value;
                access.read_raw(addr, value);
                id_and_value(bi.tag, value.id, value.value);
                break;
            }
            case BINARY_INFO_TYPE_ID_AND_STRING: {
                binary_info_id_and_string_t value;
                access.read_raw(addr, value);
                string s = read_string(access, value.value);
                id_and_string(bi.tag, value.id, s);
                break;
            }
            case BINARY_INFO_TYPE_PTR_INT32_WITH_NAME: {
                binary_info_ptr_int32_with_name_t value;
                access.read_raw(addr, value);
                string s = read_string(access, value.label);
                int i;
                access.read(value.value, (uint8_t*)&i, sizeof(i));
                ptr_int32_t_with_name(access, bi.tag, value.id, s, i, value.value);
                break;
            }
            case BINARY_INFO_TYPE_PTR_STRING_WITH_NAME: {
                binary_info_ptr_string_with_name_t value;
                access.read_raw(addr, value);
                string s = read_string(access, value.label);
                string v = read_string(access, value.value);
                ptr_string_t_with_name(access, bi.tag, value.id, s, v, value.value, value.len);
                break;
            }
            case BINARY_INFO_TYPE_BLOCK_DEVICE: {
                binary_info_block_device_t value;
                access.read_raw(addr, value);
                block_device(access, value);
                break;
            }
            case BINARY_INFO_TYPE_PINS_WITH_FUNC: {
                binary_info_pins_with_func_t value;
                access.read_raw(addr, value);
                do_pins_func(value);
                break;
            }
            case BINARY_INFO_TYPE_PINS64_WITH_FUNC: {
                binary_info_pins64_with_func_t value;
                access.read_raw(addr, value);
                do_pins_func(value);
                break;
            }
            case BINARY_INFO_TYPE_PINS_WITH_NAME: {
                binary_info_pins_with_name_t value;
                access.read_raw(addr, value);
                pins(value.pin_mask, -1, read_string(access, value.label));
                break;
            }
            case BINARY_INFO_TYPE_PINS64_WITH_NAME: {
                binary_info_pins64_with_name_t value;
                access.read_raw(addr, value);
                pins(value.pin_mask, -1, read_string(access, value.label));
                break;
            }
            case BINARY_INFO_TYPE_NAMED_GROUP: {
                binary_info_named_group_t value;
                access.read_raw(addr, value);
                named_group(value.core.tag, value.parent_id, value.group_tag, value.group_id, read_string(access, value.label), value.flags);
                break;
            }
            default:
                unknown(access, bi, addr);
        }
    }

    virtual void unknown(memory_access& access, const binary_info_core_t &bi_core, uint32_t addr) {}
    virtual void id_and_value(int tag, uint32_t id, uint32_t value) {

    }
    virtual void id_and_string(int tag, uint32_t id, const string& value) {

    }
    virtual void ptr_int32_t_with_name(memory_access& access, int tag, uint32_t id, const string& label, int32_t value, uint32_t addr) {

    }
    virtual void ptr_string_t_with_name(memory_access& access, int tag, uint32_t id, const string& label, const string& value, uint32_t addr, uint32_t max_len) {

    }
    virtual void block_device(memory_access& access, binary_info_block_device_t &bi_bdev) {
    }

    virtual void pins(uint64_t pin_mask, int func, string name) {
        switch (chip) {
            case rp2040:
                pins(pin_mask, func, name, pin_functions_rp2040);
                break;
            case rp2350:
                pins(pin_mask, func, name, pin_functions_rp2350);
                break;
            default:
                pins(pin_mask, func, name, pin_functions_unknown);
                break;
        }
    }

    template <class pin_functions_t> void pins(uint64_t pin_mask, int func, string name, pin_functions_t pin_functions) {
        if (func != -1) {
            if (func >= (int)pin_functions.size())
                return;
        }
        for(unsigned int i=0; i<64; i++) {
            if (pin_mask & (1ull << i)) {
                if (func != -1) {
                    if (pin_functions[func][i].empty()) {
                        std::stringstream sstream;
                        sstream << "Unknown pin function " << func;
                        pin(i, sstream.str());
                    } else {
                        pin(i, pin_functions[func][i]);
                    }
                } else {
                    auto sep = name.find_first_of('|');
                    auto cur = name.substr(0, sep);
                    if (cur.empty()) continue;
                    pin(i, cur.c_str());
                    if (sep != string::npos) {
                        name = name.substr(sep + 1);
                    }
                }
            }
        }
    }

    virtual void pin(unsigned int i, const string& name) {

    }

    virtual void zero_terminated_bi_list(memory_access& access, const binary_info_core_t &bi_core, uint32_t addr) {
        uint32_t bi_addr;
        access.read_raw<uint32_t>(addr,bi_addr);
        while (bi_addr) {
            visit(access, addr);
            access.read_raw<uint32_t>(addr,bi_addr);
        }
    }

    virtual void named_group(int parent_tag, uint32_t parent_id, int group_tag, uint32_t group_id, const string& label, unsigned int flags) {

    }

    chip_t chip = chip_t::unknown;
};

struct bi_visitor : public bi_visitor_base {
    typedef std::function<void(int tag, uint32_t id, uint32_t value)> id_and_int_fn;
    typedef std::function<void(int tag, uint32_t id, const string &value)> id_and_string_fn;
    typedef std::function<void(int tag, uint32_t id, const string &label, int32_t value)> ptr_int32_t_with_name_fn;
    typedef std::function<void(int tag, uint32_t id, const string &label, const string &value)> ptr_string_t_with_name_fn;
    typedef std::function<void(unsigned int num, const string &label)> pin_fn;
    typedef std::function<void(int parent_tag, uint32_t parent_id, int group_tag, uint32_t group_id,
                               const string &label, unsigned int flags)> named_group_fn;
    typedef std::function<void(memory_access &access, binary_info_block_device_t &bi_bdev)> block_device_fn;

    id_and_int_fn _id_and_int;
    id_and_string_fn _id_and_string;
    ptr_int32_t_with_name_fn _ptr_int32_t_with_name;
    ptr_string_t_with_name_fn _ptr_string_t_with_name;
    pin_fn _pin;
    named_group_fn _named_group;
    block_device_fn _block_device;

    bi_visitor &id_and_int(id_and_int_fn fn) {
        _id_and_int = std::move(fn);
        return *this;
    }

    bi_visitor &id_and_string(id_and_string_fn fn) {
        _id_and_string = std::move(fn);
        return *this;
    }

    bi_visitor &ptr_int32_t_with_name(ptr_int32_t_with_name_fn fn) {
        _ptr_int32_t_with_name = std::move(fn);
        return *this;
    }

    bi_visitor &ptr_string_t_with_name(ptr_string_t_with_name_fn fn) {
        _ptr_string_t_with_name = std::move(fn);
        return *this;
    }

    bi_visitor &pin(pin_fn fn) {
        _pin = std::move(fn);
        return *this;
    }

    bi_visitor &named_group(named_group_fn fn) {
        _named_group = std::move(fn);
        return *this;
    }

    bi_visitor &block_device(block_device_fn fn) {
        _block_device = std::move(fn);
        return *this;
    }

protected:
    void id_and_value(int tag, uint32_t id, uint32_t value) override {
        if (_id_and_int) _id_and_int(tag, id, value);
    }

    void id_and_string(int tag, uint32_t id, const string &value) override {
        if (_id_and_string) _id_and_string(tag, id, value);
    }

    void ptr_int32_t_with_name(memory_access& access, int tag, uint32_t id, const string &label, int32_t value, uint32_t addr) override {
        if (_ptr_int32_t_with_name) _ptr_int32_t_with_name(tag, id, label, value);
    }

    void ptr_string_t_with_name(memory_access& access, int tag, uint32_t id, const string &label, const string &value, uint32_t addr, uint32_t max_len) override {
        if (_ptr_string_t_with_name) _ptr_string_t_with_name(tag, id, label, value);
    }

    void pin(unsigned int i, const string &name) override {
        if (_pin) _pin(i, name);
    }

    void named_group(int parent_tag, uint32_t parent_id, int group_tag, uint32_t group_id, const string &label,
                     unsigned int flags) override {
        if (_named_group) _named_group(parent_tag, parent_id, group_tag, group_id, label, flags);
    }

    void block_device(memory_access &access, binary_info_block_device_t &bi_bdev) override {
        if (_block_device) _block_device(access, bi_bdev);
    }
};

struct bi_modifier : public bi_visitor_base {
    typedef std::function<bool(int tag, uint32_t id, const string &label, int32_t value, int32_t& new_value)> ptr_int32_t_with_name_fn;
    typedef std::function<bool(int tag, uint32_t id, const string &label, const string &value, string& new_value)> ptr_string_t_with_name_fn;

    ptr_int32_t_with_name_fn _ptr_int32_t_with_name;
    ptr_string_t_with_name_fn _ptr_string_t_with_name;

    bi_modifier &ptr_int32_t_with_name(ptr_int32_t_with_name_fn fn) {
        _ptr_int32_t_with_name = std::move(fn);
        return *this;
    }

    bi_modifier &ptr_string_t_with_name(ptr_string_t_with_name_fn fn) {
        _ptr_string_t_with_name = std::move(fn);
        return *this;
    }

protected:
    void ptr_int32_t_with_name(memory_access& access, int tag, uint32_t id, const string &label, int32_t value, uint32_t addr) override {
        if (_ptr_int32_t_with_name) {
            int32_t ret;
            if (_ptr_int32_t_with_name(tag, id, label, value, ret)) {
                DEBUG_LOG("Setting %x to %d\n", addr, ret);
                access.write(addr, (uint8_t*)&ret, sizeof(ret));
            }
        }
    }

    void ptr_string_t_with_name(memory_access& access, int tag, uint32_t id, const string &label, const string &value, uint32_t addr, uint32_t max_len) override {
        if (_ptr_string_t_with_name) {
            string ret;
            if (_ptr_string_t_with_name(tag, id, label, value, ret)) {
                if (ret.size() < max_len) {
                    DEBUG_LOG("Setting %x to %s\n", addr, ret.c_str());
                    access.write(addr, (unsigned char*)ret.c_str(), ret.size() + 1);
                } else {
                    fail(ERROR_INCOMPATIBLE, "String \"%s\" does not fit in %s - max length is %d (including null termination)", ret.c_str(), label.c_str(), max_len);
                }
            }
        }
    }
};

uint32_t guess_flash_size(memory_access &access) {
    assert(access.is_device());
    // Check that flash is not erased (TODO should check for second stage)
    auto first_two_pages = access.read_vector<uint8_t>(FLASH_START, 2 * PAGE_SIZE);
    bool all_match = std::equal(first_two_pages.begin(),
                                first_two_pages.begin() + PAGE_SIZE,
                                first_two_pages.begin() + PAGE_SIZE);
    if (all_match) {
        return 0;
    }

    // Read at decreasing power-of-two addresses until we don't see the boot pages again
    const int min_size = 16 * PAGE_SIZE;
    const int max_size = 8 * 1024 * 1024;
    int size;
    for (size = max_size; size >= min_size; size >>= 1) {
        auto new_pages = access.read_vector<uint8_t>(FLASH_START + size, 2 * PAGE_SIZE);
        if (!std::equal(first_two_pages.begin(), first_two_pages.end(), new_pages.begin())) break;
    }
    return size * 2;
}

// returns true if string is a hex string, and fills array with the values
bool string_to_hex_array(const string& str, uint8_t *array, size_t size, const string& error_msg) {

    if (!str.empty() && str.find("0x") == 0) {
        // Hex string instead of file
        if (str.size() != size*2 + 2) {
            fail(ERROR_ARGS, "%s hex string must be %d characters long (the supplied string is %d characters)", error_msg.c_str(), size*2, str.size() - 2);
        }
        for (size_t i=0; i < size; i++) {
            auto value = "0x" + str.substr(2 + i*2, 2);
            auto ret = integer::parse_string(value, array[i]);
            if (!ret.empty()) {
                fail(ERROR_ARGS, "Invalid hex string: %s %s", value.c_str(), ret.c_str());
            }
        }
        return true;
    }

    return false;
}

std::shared_ptr<std::fstream> get_file_idx(ios::openmode mode, uint8_t idx) {
    auto filename = settings.filenames[idx];
    auto file = std::make_shared<std::fstream>(filename, mode);
    if (file->fail()) fail(ERROR_READ_FAILED, "Could not open '%s'", filename.c_str());
    return file;
}

std::shared_ptr<std::fstream> get_file(ios::openmode mode) {
    return get_file_idx(mode, 0);
}

enum filetype get_file_type_idx(uint8_t idx) {
    auto filename = settings.filenames[idx];
    auto file_type = settings.file_types[idx];
    auto low = lowercase(filename);
    if (file_type.empty() && low.size() >= 4) {
        if (low.rfind(".uf2") == low.size() - 4) {
            return filetype::uf2;
        } else if (low.rfind(".elf") == low.size() - 4) {
            return filetype::elf;
        } else if (low.rfind(".bin") == low.size() - 4) {
            return filetype::bin;
        } else if (low.rfind(".pem") == low.size() - 4) {
            return filetype::pem;
        } else if (low.rfind(".json") == low.size() - 5) {
            return filetype::json;
        }
    } else if (!file_type.empty()) {
        low = lowercase(file_type);
        if (low == "uf2") {
            return filetype::uf2;
        }
        if (low == "bin") {
            return filetype::bin;
        }
        if (low == "elf") {
            return filetype::elf;
        }
        if (low == "pem") {
            return filetype::pem;
        }
        if (low == "json") {
            return filetype::json;
        }
        throw cli::parse_error("unsupported file type '" + low + "'");
    }
    throw cli::parse_error("filename '" + filename+ "' does not have a recognized file type (extension)");
}

enum filetype get_file_type() {
    return get_file_type_idx(0);
}

void build_rmap_elf(std::shared_ptr<std::iostream>file, range_map<size_t>& rmap) {
    elf32_header eh;
    read_and_check_elf32_header(file, eh);
    if (eh.ph_entry_size != sizeof(elf32_ph_entry)) {
        fail(ERROR_FORMAT, "Invalid ELF32 program header");
    }
    if (eh.ph_num) {
        vector<elf32_ph_entry> entries(eh.ph_num);
        file->seekg(eh.ph_offset, ios::beg);
        if (file->fail()) {
            return fail_read_error();
        }
        file->read((char*)&entries[0], sizeof(struct elf32_ph_entry) * eh.ph_num);
        if (file->fail()) {
            fail_read_error();
        }
        for (unsigned int i = 0; i < eh.ph_num; i++) {
             elf32_ph_entry &entry = entries[i];
             if (entry.type == PT_LOAD && entry.memsz) {
                unsigned int mapped_size = std::min(entry.filez, entry.memsz);
                if (mapped_size) {
                    auto r = range(entry.paddr, entry.paddr + mapped_size);
                    rmap.insert(r, entry.offset);
                    if (entry.memsz > entry.filez) {
                        // ignore uninitialized for now
                    }
                }
            }
        }
    }
}

uint32_t build_rmap_uf2(std::shared_ptr<std::iostream>file, range_map<size_t>& rmap, uint32_t family_id=0) {
    file->seekg(0, ios::beg);
    uf2_block block;
    unsigned int pos = 0;
    uint32_t next_family_id = 0;
    do {
        file->read((char*)&block, sizeof(uf2_block));
        if (file->fail()) {
            if (file->eof()) { file->clear(); break; }
            fail(ERROR_READ_FAILED, "unexpected end of input file");
        }
        if (block.magic_start0 == UF2_MAGIC_START0 && block.magic_start1 == UF2_MAGIC_START1 &&
            block.magic_end == UF2_MAGIC_END) {
            if (block.flags & UF2_FLAG_FAMILY_ID_PRESENT &&
                !(block.flags & UF2_FLAG_NOT_MAIN_FLASH) && block.payload_size == PAGE_SIZE &&
                (!family_id || block.file_size == family_id)) {
                #if SUPPORT_RP2350_A2
                // ignore the absolute block, but save the address
                if (check_abs_block(block)) {
                    DEBUG_LOG("Ignoring RP2350-E10 absolute block\n");
                    settings.uf2.abs_block_loc = block.target_addr;
                } else {
                    rmap.insert(range(block.target_addr, block.target_addr + PAGE_SIZE), pos + offsetof(uf2_block, data[0]));
                    family_id = block.file_size;
                    next_family_id = 0;
                }
                #else
                rmap.insert(range(block.target_addr, block.target_addr + PAGE_SIZE), pos + offsetof(uf2_block, data[0]));
                family_id = block.file_size;
                next_family_id = 0;
                #endif
            } else if (block.file_size != family_id && family_id && !next_family_id) {
                #if SUPPORT_RP2350_A2
                if (!check_abs_block(block)) {
                #endif
                    next_family_id = block.file_size;
                #if SUPPORT_RP2350_A2
                }
                #endif
            }
        }
        pos += sizeof(uf2_block);
    } while (true);

    return next_family_id;
}

void build_rmap_load_map(std::shared_ptr<load_map_item>load_map, range_map<uint32_t>& rmap) {
    for (unsigned int i=0; i < load_map->entries.size(); i++) {
        auto e = load_map->entries[i];
        if (e.storage_address != 0) {
            try {
                rmap.insert(range(e.runtime_address, e.runtime_address + e.size), e.storage_address);
            } catch (failure_error&) {
                // Overlapping memory ranges are permitted in a load_map, so overwrite overlapping range
                rmap.insert_overwrite(range(e.runtime_address, e.runtime_address + e.size), e.storage_address);
            }
        }
    }
}

uint32_t find_binary_start(range_map<size_t>& rmap) {
    range flash(FLASH_START, FLASH_END_RP2350); // pick biggest (rp2350) here for now
    range sram(SRAM_START, SRAM_END_RP2350); // pick biggest (rp2350) here for now
    range xip_sram(XIP_SRAM_START_RP2350, XIP_SRAM_END_RP2040); // pick biggest (rp2350-rp2040) here for now
    uint32_t binary_start = std::numeric_limits<uint32_t>::max();
    for (const auto &r : rmap.ranges()) {
        if (r.contains(FLASH_START)) {
            return FLASH_START;
        }
        if (sram.contains(r.from) || (xip_sram.contains(r.from) && !sram.contains(binary_start))) {
            if (r.from < binary_start || (xip_sram.contains(binary_start) && sram.contains(r.from))) {
                binary_start = r.from;
            }
        }
    }
    if (get_memory_type(binary_start, models::largest) == invalid) { // pick biggest (rp2350) here for now
        return 0;
    }
    return binary_start;
}

template <typename ACCESS, typename STREAM> ACCESS get_iostream_memory_access(std::shared_ptr<STREAM> file, filetype type, bool writeable = false, uint32_t *next_family_id=nullptr) {
    range_map<size_t> rmap;
    uint32_t binary_start = 0;
    uint32_t tmp = 0;
    if (next_family_id != nullptr) tmp = *next_family_id;
    switch (type) {
        case filetype::bin:
            file->seekg(0, std::ios::end);
            binary_start = settings.offset_set ? settings.offset : FLASH_START;
            rmap.insert(range(binary_start, binary_start + file->tellg()), 0);
            return ACCESS(file, rmap, binary_start);
        case filetype::elf:
            build_rmap_elf(file, rmap);
            binary_start = find_binary_start(rmap);
            break;
        case filetype::uf2:
            tmp = build_rmap_uf2(file, rmap, tmp);
            if (next_family_id != nullptr) {
                *next_family_id = tmp;
            } else if (tmp) {
                fos << "WARNING: Multiple family IDs in a single UF2 file - only using first one\n";
            }
            binary_start = find_binary_start(rmap);
            break;
        default:
            fail(ERROR_INCOMPATIBLE, "Cannot create memory access with filetype %s", getFiletypeName(type).c_str());
    }
    if (settings.offset_set) {
        unsigned int rel_offset = settings.offset - binary_start;
        rmap = rmap.offset_by(rel_offset);
        binary_start = settings.offset;
        DEBUG_LOG("BINARY START now %08x, rmaps offset by %08x\n", binary_start, rel_offset);
    }
    return ACCESS(file, rmap, binary_start);
}

file_memory_access get_file_memory_access(uint8_t idx, bool writeable = false, uint32_t *next_family_id=nullptr) {
    ios::openmode mode = (writeable ? ios::out|ios::in : ios::in)|ios::binary;
    auto file = get_file_idx(mode, idx);
    try {
        return get_iostream_memory_access<file_memory_access>(file, get_file_type_idx(idx), writeable, next_family_id);
    } catch (std::exception&) {
        file->close();
        throw;
    }
}

const char *cpu_name(unsigned int cpu) {
    if (cpu == PICOBIN_IMAGE_TYPE_EXE_CPU_ARM) return "ARM";
    if (cpu == PICOBIN_IMAGE_TYPE_EXE_CPU_RISCV) return "RISC-V";
    return "unknown";
}

std::string boot_type_string(uint8_t type) {
    std::string s;
    switch (type & 0x7f) {
        case BOOT_TYPE_BOOTSEL: s = "bootsel"; break;
        case 1: // old PC_SP; fall thru
        case BOOT_TYPE_PC_SP: s = "pc/sp"; break;
        case BOOT_TYPE_FLASH_UPDATE: s = "flash update"; break;
        case BOOT_TYPE_RAM_IMAGE: s = "ram image"; break;
        case BOOT_TYPE_NORMAL: s = "normal"; break;
        default: s = "<unknown>"; break;
    }
    if (type & BOOT_TYPE_CHAINED_FLAG) s += " into chained image";
    return s;
}

std::string boot_partition_string(int8_t type) {
    if (type == BOOT_PARTITION_NONE) return "none";
    if (type == BOOT_PARTITION_SLOT0) return "slot 0";
    if (type == BOOT_PARTITION_SLOT1) return "slot 1";
    if (type == BOOT_PARTITION_WINDOW) return "window";
    if (type >= 0 && type < PARTITION_TABLE_MAX_PARTITIONS) return "partition "+std::to_string(type);
    return "<invalid>";
}

#define OTP_CRITICAL_RISCV_DISABLE_BITS   _u(0x00020000)
#define OTP_CRITICAL_ARM_DISABLE_BITS   _u(0x00010000)
#define OTP_CRITICAL_GLITCH_DETECTOR_SENS_BITS   _u(0x00000060)
#define OTP_CRITICAL_GLITCH_DETECTOR_ENABLE_BITS   _u(0x00000010)
#define OTP_CRITICAL_DEFAULT_ARCHSEL_BITS   _u(0x00000008)
#define OTP_CRITICAL_DEBUG_DISABLE_BITS   _u(0x00000004)
#define OTP_CRITICAL_SECURE_DEBUG_DISABLE_BITS   _u(0x00000002)
#define OTP_CRITICAL_SECURE_BOOT_ENABLE_BITS   _u(0x00000001)

std::unique_ptr<block> find_best_block(memory_access &raw_access, vector<uint8_t> &bin, bool riscv = false) {
    // todo read the right amount
    uint32_t read_size = 0x1000;
    DEBUG_LOG("Reading from %x size %x\n", raw_access.get_binary_start(), read_size);
    bin = raw_access.read_vector<uint8_t>(raw_access.get_binary_start(), read_size, true);

    std::unique_ptr<block> best_block = find_first_block(bin, raw_access.get_binary_start());
    if (best_block) {
        // verify stuff
        get_more_bin_cb more_cb = [&raw_access](std::vector<uint8_t> &bin, uint32_t offset, uint32_t size) {
            DEBUG_LOG("Now reading from %x size %x\n", offset, size);
            bin = raw_access.read_vector<uint8_t>(offset, size, true);
        };
        auto all_blocks = get_all_blocks(bin, raw_access.get_binary_start(), best_block, more_cb);

        bool has_arch = false;
        // for (int i=0; i < all_blocks.size(); i++) {
        //     auto &block = all_blocks[i];
        for (auto &block : all_blocks) {
            DEBUG_LOG("Checking block at %x, num items %d\n", block->physical_addr, block->items.size());
            // Image Def
            auto image_def = block->get_item<image_type_item>();
            if (image_def != nullptr) {
                if (image_def->image_type() == type_exe) {
                    switch (image_def->chip()) {
                        case chip_rp2040:
                            break;
                        case chip_rp2350:
                            switch (image_def->cpu()) {
                                case cpu_riscv:
                                    if (riscv || !has_arch) {
                                        best_block.swap(block);
                                        has_arch = riscv;
                                    }
                                    break;
                                case cpu_varmulet:
                                    if (!has_arch) {
                                        best_block.swap(block);
                                    }
                                    break;
                                case cpu_arm:
                                    if (image_def->security() == sec_s) {
                                        if (!riscv || !has_arch) {
                                            best_block.swap(block);
                                            has_arch = !riscv;
                                        }
                                    }
                            }
                            break;
                        default:
                            break;
                    }
                }
            }

            // Image Def
            image_def = best_block->get_item<image_type_item>();
            DEBUG_LOG("Current best is at %x, num items %d\n", best_block->physical_addr, best_block->items.size());
        }
    }

    return best_block;
}

std::unique_ptr<block> find_last_block(memory_access &raw_access, vector<uint8_t> &bin) {
    // todo read the right amount
    uint32_t read_size = 0x1000;
    DEBUG_LOG("Reading from %x size %x\n", raw_access.get_binary_start(), read_size);
    bin = raw_access.read_vector<uint8_t>(raw_access.get_binary_start(), read_size, true);

    std::unique_ptr<block> first_block = find_first_block(bin, raw_access.get_binary_start());
    if (first_block) {
        // verify stuff
        get_more_bin_cb more_cb = [&raw_access](std::vector<uint8_t> &bin, uint32_t offset, uint32_t size) {
            DEBUG_LOG("Now reading from %x size %x\n", offset, size);
            bin = raw_access.read_vector<uint8_t>(offset, size, true);
        };
        auto last_block = get_last_block(bin, raw_access.get_binary_start(), first_block, more_cb);
        return last_block;
    }

    return nullptr;
}

std::vector<std::unique_ptr<block>> find_all_blocks(memory_access &raw_access, vector<uint8_t> &bin) {
    // todo read the right amount
    uint32_t read_size = 0x1000;
    DEBUG_LOG("Reading from %x size %x\n", raw_access.get_binary_start(), read_size);
    bin = raw_access.read_vector<uint8_t>(raw_access.get_binary_start(), read_size, true);

    std::unique_ptr<block> first_block = find_first_block(bin, raw_access.get_binary_start());
    if (first_block) {
        // verify stuff
        get_more_bin_cb more_cb = [&raw_access](std::vector<uint8_t> &bin, uint32_t offset, uint32_t size) {
            DEBUG_LOG("Now reading from %x size %x\n", offset, size);
            bin = raw_access.read_vector<uint8_t>(offset, size, true);
        };
        return get_all_blocks(bin, raw_access.get_binary_start(), first_block, more_cb);
    }

    return std::vector<std::unique_ptr<block>>();
}

std::shared_ptr<memory_access> get_bi_access(memory_access &raw_access) {
    vector<uint8_t> bin;
    std::unique_ptr<block> best_block = find_best_block(raw_access, bin);
    range_map<uint32_t> rmap;
    if (best_block) {
        auto load_map = best_block->get_item<load_map_item>();
        if (load_map != nullptr) {
            // Remap for find_binary_info
            build_rmap_load_map(load_map, rmap);
        }
    }

    return std::make_shared<remapped_memory_access>(raw_access, rmap);
}

string str_permissions(unsigned int p) {
    static_assert(PICOBIN_PARTITION_PERMISSION_S_R_BITS == (1u << 26), "");
    static_assert(PICOBIN_PARTITION_PERMISSION_S_W_BITS == (1u << 27), "");
    static_assert(PICOBIN_PARTITION_PERMISSION_NS_W_BITS == (1u << 29), "");
    static_assert(PICOBIN_PARTITION_PERMISSION_NSBOOT_W_BITS == (1u << 31), "");

    std::stringstream ss;
    ss << " S(";
    unsigned int r = (p >> 26) & 3;
    if (r & 1) ss << "r";
    if (r & 2) ss << "w"; else if (!r) ss << "-";
    ss << ") NSBOOT(";
    r = (p >> 30) & 3;
    if (r & 1) ss << "r";
    if (r & 2) ss << "w"; else if (!r) ss << "-";
    ss << ") NS(";
    r = (p >> 28) & 3;
    if (r & 1) ss << "r";
    if (r & 2) ss << "w"; else if (!r) ss << "-";
    ss << ")";

    return ss.str();
}

void insert_default_families(uint32_t flags_and_permissions, vector<std::string> &family_names) {
    if (flags_and_permissions & PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_ABSOLUTE_BITS) family_names.emplace_back(family_name(ABSOLUTE_FAMILY_ID));
    if (flags_and_permissions & PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_RP2040_BITS) family_names.emplace_back(family_name(RP2040_FAMILY_ID));
    if (flags_and_permissions & PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_RP2350_ARM_S_BITS) family_names.emplace_back(family_name(RP2350_ARM_S_FAMILY_ID));
    if (flags_and_permissions & PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_RP2350_ARM_NS_BITS) family_names.emplace_back(family_name(RP2350_ARM_NS_FAMILY_ID));
    if (flags_and_permissions & PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_RP2350_RISCV_BITS) family_names.emplace_back(family_name(RP2350_RISCV_FAMILY_ID));
    if (flags_and_permissions & PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_DATA_BITS) family_names.emplace_back(family_name(DATA_FAMILY_ID));
}

static chip_t image_type_exe_chip_to_chip(uint image_type_exe_chip) {
    static_assert((int)chip_rp2040 == (int)rp2040, "");
    static_assert((int)chip_rp2350 == (int)rp2350, "");
    return (chip_t)image_type_exe_chip;
}

#if HAS_LIBUSB
void info_guts(memory_access &raw_access, picoboot::connection *con, bool no_pt_loaded=false) {
    model_t model = raw_access.get_model();
#else
void info_guts(memory_access &raw_access, void *con, bool no_pt_loaded=false) {
#endif
    // Use flash caching
    settings.use_flash_cache = true;
    // Track if a valid partition table was found in the block loop
    bool has_pt = false;
    bool pt_is_valid = true;
    // Callback to pass to bintool, to get more bin data
    get_more_bin_cb more_cb = [&raw_access](std::vector<uint8_t> &bin, uint32_t offset, uint32_t size) {
        DEBUG_LOG("Now reading from %x size %x\n", offset, size);
        bin = raw_access.read_vector<uint8_t>(offset, size, true);
    };
    try {
        struct group {
            explicit group(string name, bool enabled = true, int min_tab = 0) : name(std::move(name)), enabled(enabled), min_tab(min_tab) {}
            string name;
            bool enabled;
            int min_tab;
        };
        vector<group> groups;
        string current_group;
        map<string, vector<pair<string,string>>> infos;
        // Set enable to true to enable the selected group
        auto select_group = [&](const group &g1, bool enable = false) {
            if (std::find_if(groups.begin(), groups.end(), [&](const group &g2) {
                return g1.name == g2.name;
            }) == groups.end()) {
                groups.push_back(g1);
            }
            auto enable_changed = std::find_if(groups.begin(), groups.end(), [&](const group &g2) {
                return g1.name == g2.name && (enable && !g2.enabled);
            });
            if (enable_changed != groups.end()) {
                enable_changed->enabled = true;
            }
            current_group = g1.name;
        };
        auto info_pair = [&](const string &name, const string &value) {
            if (!value.empty()) {
                assert(!current_group.empty());
                infos[current_group].emplace_back(std::make_pair(name, value));
            }
        };
        auto info_metadata = [&](block *current_block, bool verbose_metadata = false) {
            bool is_pt = false;
            verified_t hash_verified = none;
            verified_t sig_verified = none;
        #if HAS_MBEDTLS
            // Pass empty bin, which will be populated by more_cb if there is a signature/hash_value
            verify_block({}, raw_access.get_binary_start(), raw_access.get_binary_start(), current_block, raw_access.get_model(), hash_verified, sig_verified, more_cb);
        #endif

            // Addresses
            if (verbose_metadata) {
                info_pair("address", hex_string(current_block->physical_addr));
                info_pair("next block address", hex_string(current_block->next_block_rel + current_block->physical_addr));
                if (current_block->get_item<ignored_item>() != nullptr) info_pair("block type", "ignored");
            }

            // Image Def
            auto image_def = current_block->get_item<image_type_item>();
            if (image_def != nullptr) {
                if (verbose_metadata) info_pair("block type", "image def");
                if (image_def->image_type() == type_exe) {
                    info_pair("target chip", chip_name(image_type_exe_chip_to_chip(image_def->chip())));
                    if (image_def->chip() != chip_rp2040) {
                        switch (image_def->cpu()) {
                            case cpu_riscv:
                                info_pair("image type", "RISC-V");
                                break;
                            case cpu_varmulet:
                                info_pair("image type", "Varmulet");
                                break;
                            case cpu_arm:
                                if (image_def->security() == sec_s) {
                                    info_pair("image type", "ARM Secure");
                                } else if (image_def->security() == sec_ns) {
                                    info_pair("image type", "ARM Non-Secure");
                                } else if (image_def->security() == sec_unspecified) {
                                    info_pair("image type", "ARM");
                                }
                                break;
                        }
                    }
                } else if (image_def->image_type() == type_data) {
                    info_pair("image type", "data");
                }
                
                if (image_def->tbyb()) {
                    info_pair("tbyb", "not bought");
                }

                if (verbose_metadata) {
                    info_pair("extra security", image_def->extra_security() ? "enabled" : "not enabled");
                }
            }

            // Partition Table
            auto partition_table = current_block->get_item<partition_table_item>();
            if (partition_table != nullptr) {
                is_pt = true; // this block is a partition table
                has_pt = true; // assume it is valid - hash and sig checks will invalidate it if not
                if (verbose_metadata) info_pair("block type", "partition table");
                info_pair("partition table", partition_table->singleton ? "singleton" : "non-singleton");
                std::stringstream unpartitioned;
                unpartitioned << str_permissions(partition_table->unpartitioned_flags);
                std::vector<std::string> family_names;
                insert_default_families(partition_table->unpartitioned_flags, family_names);
                unpartitioned << ", uf2 { " << cli::join(family_names, ", ") << " }";
                info_pair("un-partitioned space", unpartitioned.str());

                for (size_t i=0; i < partition_table->partitions.size(); i++) {
                    std::stringstream pstring;
                    std::stringstream pname;
                    auto partition = partition_table->partitions[i];
                    uint32_t flags = partition.flags;
                    uint64_t id = partition.id;
                    pname << "partition " << i;
                    if ((flags & PICOBIN_PARTITION_FLAGS_LINK_TYPE_BITS) ==
                        PICOBIN_PARTITION_FLAGS_LINK_TYPE_AS_BITS(A_PARTITION)) {
                        pname << " (B w/ " << ((flags & PICOBIN_PARTITION_FLAGS_LINK_VALUE_BITS)
                                            >> PICOBIN_PARTITION_FLAGS_LINK_VALUE_LSB)
                           << ")";
                    } else if ((flags & PICOBIN_PARTITION_FLAGS_LINK_TYPE_BITS) ==
                            PICOBIN_PARTITION_FLAGS_LINK_TYPE_AS_BITS(OWNER_PARTITION)) {
                            pname << " (A ob/ " << ((flags & PICOBIN_PARTITION_FLAGS_LINK_VALUE_BITS)
                                                 >> PICOBIN_PARTITION_FLAGS_LINK_VALUE_LSB)
                               << ")";
                    } else {
                        pname << " (A)";
                    }
                    pstring << hex_string(partition.first_sector * 4096, 8, false) << "->" << hex_string((partition.last_sector + 1) * 4096, 8, false);
                    unsigned int p = partition.permissions;
                    pstring << str_permissions(p << PICOBIN_PARTITION_PERMISSIONS_LSB);
                    if (flags & PICOBIN_PARTITION_FLAGS_HAS_ID_BITS) {
                        pstring << ", id=" << hex_string(id, 16, false);
                    }
                    uint32_t num_extra_families = partition.extra_families.size();
                    family_names.clear();
                    insert_default_families(flags, family_names);
                    for (auto family : partition.extra_families) {
                        family_names.emplace_back(family_name(family));
                    }
                    if (flags & PICOBIN_PARTITION_FLAGS_HAS_NAME_BITS) {
                        pstring << ", \"";
                        pstring << partition.name;
                        pstring << '"';
                    }
                    pstring << ", uf2 { " << cli::join(family_names, ", ") << " }";
                    pstring << ", arm_boot " << !(flags & PICOBIN_PARTITION_FLAGS_IGNORED_DURING_ARM_BOOT_BITS);
                    pstring << ", riscv_boot " << !(flags & PICOBIN_PARTITION_FLAGS_IGNORED_DURING_RISCV_BOOT_BITS);
                    info_pair(pname.str(), pstring.str());
                }
            }

            // Version
            auto version = current_block->get_item<version_item>();
            if (version != nullptr) {
                info_pair("version", std::to_string(version->major) + "." + std::to_string(version->minor));
                if (version->otp_rows.size() > 0) {
                    info_pair("rollback version", std::to_string(version->rollback));
                    std::stringstream rows;
                    for (const auto row : version->otp_rows) { rows << hex_string(row, 3) << " "; }
                    info_pair("rollback rows", rows.str());
                }
            }

            if (verbose_metadata) {
                // Load Map
                // todo what should this really report
                auto load_map = current_block->get_item<load_map_item>();
                if (load_map != nullptr) {
                    for (unsigned int i=0; i < load_map->entries.size(); i++) {
                        std::stringstream ss;
                        auto e = load_map->entries[i];
                        if (e.storage_address == 0) {
                            ss << "Clear 0x" << std::hex << e.runtime_address;
                            ss << "->0x" << std::hex << e.runtime_address + e.size;
                        } else if (e.storage_address != e.runtime_address) {
                            address_ranges ranges_flash = address_ranges_flash(raw_access.get_model());
                            if (is_address_initialized(ranges_flash, e.runtime_address)) {
                                ss << "ERROR: COPY TO FLASH NOT PERMITTED ";
                            }
                            ss << "Copy 0x" << std::hex << e.storage_address;
                            ss << "->0x" << std::hex << e.storage_address + e.size;
                            ss << " to 0x" << std::hex << e.runtime_address;
                            ss << "->0x" << std::hex << e.runtime_address + e.size;
                        } else {
                            ss << "Load 0x" << std::hex << e.storage_address;
                            ss << "->0x" << std::hex << e.storage_address + e.size;
                        }
                        info_pair("load map entry " + std::to_string(i), ss.str());
                    }
                }

                // Rolling Window Delta
                auto rwd = current_block->get_item<rolling_window_delta_item>();
                if (rwd != nullptr) {
                    info_pair("rolling window delta", hex_string(rwd->addr));
                }

                // Vector Table
                auto vtor = current_block->get_item<vector_table_item>();
                if (vtor != nullptr) {
                    info_pair("vector table", hex_string(vtor->addr));
                }

                // Entry Point
                auto entry_point = current_block->get_item<entry_point_item>();
                if (entry_point != nullptr) {
                    std::stringstream ss;
                    ss << "EP " << hex_string(entry_point->ep);
                    ss << ", SP " << hex_string(entry_point->sp);
                    if (entry_point->splim_set) ss << ", SPLIM " << hex_string(entry_point->splim);
                    info_pair("entry point", ss.str());
                }
            }

            // Hash and Sig
            if (hash_verified != none) {
                info_pair("hash", hash_verified == passed ? "verified" : "incorrect");
                if (verbose_metadata) {
                    std::shared_ptr<hash_value_item> hash_value = current_block->get_item<hash_value_item>();
                    assert(hash_value != nullptr); // verify_block would return none if it's not present
                    std::stringstream val;
                    for(uint8_t i : hash_value->hash_bytes) {
                        val << hex_string(i, 2, false, true);
                    }
                    info_pair("hash value", val.str());
                }
                if (is_pt && hash_verified != passed) {
                    pt_is_valid = false; // partition table has bad hash
                }
            }
            if (sig_verified != none) {
                info_pair("signature", sig_verified == passed ? "verified" : "incorrect");
                if (verbose_metadata) {
                    std::shared_ptr<signature_item> signature = current_block->get_item<signature_item>();
                    assert(signature != nullptr); // verify_block would return none if it's not present
                    std::stringstream sig;
                    for(uint8_t i : signature->signature_bytes) {
                        sig << hex_string(i, 2, false, true);
                    }
                    info_pair("signature value", sig.str());
                    std::stringstream pkey;
                    for(uint8_t i : signature->public_key_bytes) {
                        pkey << hex_string(i, 2, false, true);
                    }
                    info_pair("public key", pkey.str());
                }
                if (is_pt && sig_verified != passed) {
                    pt_is_valid = false; // partition table has bad signature
                }
            }
        };

        // establish core groups and their order
        if (!settings.info.show_basic && !settings.info.all && !settings.info.show_metadata && !settings.info.show_pins && !settings.info.show_device && !settings.info.show_debug && !settings.info.show_build) {
            settings.info.show_basic = true;
        }
        if (settings.info.show_debug && !settings.info.show_device) {
            settings.info.show_device = true;
        }
        auto program_info = group("Program Information", settings.info.show_basic || settings.info.all);
        auto no_metadata_info = group("Metadata Blocks", false);
        vector<group> metadata_info;
        #define MAX_METADATA_BLOCKS 10
        for (int i=1; i <= MAX_METADATA_BLOCKS; i++) {
            // These groups are enabled later, depending on how many metadata blocks the binary has
            metadata_info.push_back(group("Metadata Block " + std::to_string(i), false));
        }
        auto pin_info = group("Fixed Pin Information", settings.info.show_pins || settings.info.all);
        auto build_info = group("Build Information", settings.info.show_build || settings.info.all);
        auto device_info = group("Device Information", (settings.info.show_device || settings.info.all) & raw_access.is_device());
        // select them up front to impose order
        select_group(program_info);
        select_group(pin_info);
        select_group(build_info);
        for (auto mb : metadata_info) {
            select_group(mb);
        }
        select_group(device_info);
        binary_info_header hdr;
        try {
            auto bi_access = get_bi_access(raw_access);
            bool has_binary_info = find_binary_info(*bi_access, hdr);
            if (has_binary_info) {
                auto access = remapped_memory_access(*bi_access, hdr.reverse_copy_mapping);
                auto visitor = bi_visitor{};
                map<string, string> output;
                map<unsigned int, vector<string>> pins;

                map<pair<int, uint32_t>, pair<string, unsigned int>> named_feature_groups;
                map<string, vector<string>> named_feature_group_values;

                string program_name, program_build_date, program_version, program_url, program_description;
                string pico_board, sdk_version, boot2_name;
                vector<string> program_features, build_attributes;

                uint32_t binary_end = 0;

                // do a pass first to find named groups
                visitor.named_group(
                        [&](int parent_tag, uint32_t parent_id, int group_tag, uint32_t group_id, const string &label,
                            unsigned int flags) {
                            if (parent_tag != BINARY_INFO_TAG_RASPBERRY_PI)
                                return;
                            if (parent_id != BINARY_INFO_ID_RP_PROGRAM_FEATURE)
                                return;
                            named_feature_groups[std::make_pair(group_tag, group_id)] = std::make_pair(label, flags);
                        });

                visitor.visit(access, hdr);

                visitor = bi_visitor{};
                visitor.id_and_int([&](int tag, uint32_t id, uint32_t value) {
                    if (tag != BINARY_INFO_TAG_RASPBERRY_PI)
                        return;
                    if (id == BINARY_INFO_ID_RP_BINARY_END) binary_end = value;
                });
                visitor.id_and_string([&](int tag, uint32_t id, const string &value) {
                    const auto &nfg = named_feature_groups.find(std::make_pair(tag, id));
                    if (nfg != named_feature_groups.end()) {
                        named_feature_group_values[nfg->second.first].push_back(value);
                        return;
                    }
                    if (tag != BINARY_INFO_TAG_RASPBERRY_PI)
                        return;
                    if (id == BINARY_INFO_ID_RP_PROGRAM_NAME) program_name = value;
                    else if (id == BINARY_INFO_ID_RP_PROGRAM_VERSION_STRING) program_version = value;
                    else if (id == BINARY_INFO_ID_RP_PROGRAM_BUILD_DATE_STRING) program_build_date = value;
                    else if (id == BINARY_INFO_ID_RP_PROGRAM_URL) program_url = value;
                    else if (id == BINARY_INFO_ID_RP_PROGRAM_DESCRIPTION) program_description = value;
                    else if (id == BINARY_INFO_ID_RP_PROGRAM_FEATURE) program_features.push_back(value);
                    else if (id == BINARY_INFO_ID_RP_PROGRAM_BUILD_ATTRIBUTE) build_attributes.push_back(value);
                    else if (id == BINARY_INFO_ID_RP_PICO_BOARD) pico_board = value;
                    else if (id == BINARY_INFO_ID_RP_SDK_VERSION) sdk_version = value;
                    else if (id == BINARY_INFO_ID_RP_BOOT2_NAME) boot2_name = value;
                });
                visitor.ptr_int32_t_with_name([&](int tag, uint32_t id, const string &label, int32_t value) {
                    const auto &nfg = named_feature_groups.find(std::make_pair(tag, id));
                    if (nfg != named_feature_groups.end()) {
                        std::stringstream ss;
                        ss << label << " = " << value;
                        named_feature_group_values[nfg->second.first].push_back(ss.str());
                        return;
                    }
                });
                visitor.ptr_string_t_with_name([&](int tag, uint32_t id, const string &label, const string &value) {
                    const auto &nfg = named_feature_groups.find(std::make_pair(tag, id));
                    if (nfg != named_feature_groups.end()) {
                        std::stringstream ss;
                        ss << label << " = \"" << value << "\"";
                        named_feature_group_values[nfg->second.first].push_back(ss.str());
                        return;
                    }
                });
                visitor.pin([&](unsigned int pin, const string &name) {
                    pins[pin].push_back(name);
                });

                // some simple info in --all for now.. split out into separate section later
                vector<std::function<void()>> deferred;
                visitor.block_device([&](memory_access &access, binary_info_block_device_t &bi_bdev) {
                    if (settings.info.all) {
                        std::stringstream ss;
                        ss << hex_string(bi_bdev.address) << "-" << hex_string(bi_bdev.address + bi_bdev.size) <<
                           " (" << ((bi_bdev.size + 1023) / 1024) << "K): " << read_string(access, bi_bdev.name);
                        if (bi_bdev.flags) {
                            ss << " flags " << hex_string(bi_bdev.flags, 4) << " ";
                            if (bi_bdev.flags & BINARY_INFO_BLOCK_DEV_FLAG_READ) ss << "r";
                            if (bi_bdev.flags & BINARY_INFO_BLOCK_DEV_FLAG_WRITE) ss << "w";
                            if (bi_bdev.flags & BINARY_INFO_BLOCK_DEV_FLAG_REFORMAT) ss << "f";
                        }
                        string s = ss.str();
                        deferred.emplace_back([&select_group, &program_info, &info_pair, s]() {
                            select_group(program_info);
                            info_pair("embedded drive", s);
                        });
                    }
                });

                visitor.visit(access, hdr);

                if (settings.info.show_basic || settings.info.all) {
                    select_group(program_info);
                    info_pair("name", program_name);
                    info_pair("version", program_version);
                    info_pair("web site", program_url);
                    info_pair("description", program_description);
                    info_pair("features", cli::join(program_features, "\n"));
                    for (const auto &e: named_feature_groups) {
                        const auto &name = e.second.first;
                        const auto &flags = e.second.second;
                        const auto &values = named_feature_group_values[name];
                        if (!values.empty() || (flags & BI_NAMED_GROUP_SHOW_IF_EMPTY)) {
                            info_pair(name, cli::join(values, (flags & BI_NAMED_GROUP_SEPARATE_COMMAS) ? ", " : "\n"));
                        }
                    }
                    if (access.get_binary_start())
                        info_pair("binary start", hex_string(access.get_binary_start()));
                    if (binary_end)
                        info_pair("binary end", hex_string(binary_end));

                    for (auto &f: deferred) {
                        f();
                    }
                }
                if (settings.info.show_pins || settings.info.all) {
                    select_group(pin_info);
                    int first_pin = -1;
                    string last_label;
                    for (auto i = pins.begin(); i != pins.end(); i++) {
                        auto j = i;
                        j++;
                        if (j == pins.end() || j->first != i->first + 1 || j->second != i->second) {
                            std::sort(i->second.begin(), i->second.end());
                            auto label = cli::join(i->second, ", ");
                            if (first_pin < 0) {
                                info_pair(std::to_string(i->first), label);
                            } else {
                                info_pair(std::to_string(first_pin) + "-" + std::to_string(i->first), label);
                                first_pin = -1;
                            }
                        } else if (first_pin < 0) {
                            first_pin = (int) i->first;
                        }
                    }
                }
                if (settings.info.show_build || settings.info.all) {
                    select_group(build_info);
                    info_pair("sdk version", sdk_version);
                    info_pair("pico_board", pico_board);
                    info_pair("boot2_name", boot2_name);
                    info_pair("build date", program_build_date);
                    info_pair("build attributes", cli::join(build_attributes, "\n"));
                }
            }
            vector<uint8_t> bin;
            if (settings.info.show_metadata || settings.info.all) {
                uint32_t read_size = 0x1000;
                DEBUG_LOG("Reading from %x size %x\n", raw_access.get_binary_start(), read_size);
                bin = raw_access.read_vector<uint8_t>(raw_access.get_binary_start(), read_size, true);
                std::unique_ptr<block> first_block = find_first_block(bin, raw_access.get_binary_start());
                if (first_block) {
                    int block_i = 0;
                    select_group(metadata_info[block_i++], true);
                    info_metadata(first_block.get(), true);
                    if (first_block->next_block_rel != 0) { // if there is more than one block
                        auto all_blocks = get_all_blocks(bin, raw_access.get_binary_start(), first_block, more_cb);
                        for (auto &block : all_blocks) {
                            select_group(metadata_info[block_i++], true);
                            info_metadata(block.get(), true);
                        }
                    }
                } else {
                    // This displays that there are no metadata blocks
                    select_group(no_metadata_info, true);
                }
            }
            std::unique_ptr<block> best_block = find_best_block(raw_access, bin);
            if (best_block && (settings.info.show_basic || settings.info.all)) {
                select_group(program_info);
                info_metadata(best_block.get());
            } else if (!best_block && has_binary_info && raw_access.get_model()->requires_block_loop()) {
                fos << "WARNING: Binary on " << raw_access.get_model()->name() << " device does not contain a block loop - this binary will not boot\n";
            }
        } catch (std::invalid_argument &e) {
            fos << "Error reading binary info\n";
    #if HAS_LIBUSB
        } catch (picoboot::command_failure &e) {
            if (e.get_code() != PICOBOOT_NOT_PERMITTED) throw;
            info_pair("flash size", "not determined due to access permissions");
    #endif
        }
    #if HAS_LIBUSB
        std::vector<std::pair<string,string>> device_state_pairs;
        if ((settings.info.show_device || settings.info.all) && raw_access.is_device()) {
            select_group(device_info);
            uint8_t rom_version;
            raw_access.read_raw(0x13, rom_version);
            info_pair("type", model->name());
            if (settings.info.show_debug || settings.info.all) {
                info_pair("revision", model->revision_name());
            }
            if (model->supports_picoboot_cmd(PC_GET_INFO)) {
                assert(con);
                struct picoboot_get_info_cmd info_cmd;
                info_cmd.bType = PICOBOOT_GET_INFO_SYS,
                info_cmd.dParams[0] = (uint32_t) (settings.info.show_debug || settings.info.all ?
                                                          SYS_INFO_CHIP_INFO | SYS_INFO_CRITICAL |
                                                          SYS_INFO_BOOT_RANDOM |
                                                          SYS_INFO_CPU_INFO | SYS_INFO_FLASH_DEV_INFO | SYS_INFO_BOOT_INFO :
                                                          SYS_INFO_CHIP_INFO | SYS_INFO_CRITICAL |
                                                          SYS_INFO_CPU_INFO | SYS_INFO_FLASH_DEV_INFO);
                uint32_t word_buf[64];
                con->get_info(&info_cmd, (uint8_t *) word_buf, sizeof(word_buf));
                uint32_t *data = word_buf;
                unsigned int word_count = *data++;
                unsigned int included = *data++;
                if (included & SYS_INFO_CHIP_INFO) {
                    // package_id, device_id, wafer_id
                    if (model->chip_revision() == rp2350_a2) {
                        // On A2, package_id is incorrect, so we use num_gpios instead
                        struct picoboot_otp_cmd otp_cmd;
                        uint16_t num_gpios = 0;
                        otp_cmd.wRow = OTP_DATA_NUM_GPIOS_ROW;
                        otp_cmd.wRowCount = 1;
                        otp_cmd.bEcc = 1;
                        con->otp_read(&otp_cmd, (uint8_t *)&num_gpios, sizeof(num_gpios));
                        if (num_gpios == 30) {
                            info_pair("package", "QFN60");
                        } else if (num_gpios == 48) {
                            info_pair("package", "QFN80");
                        } else {
                            info_pair("package", "unknown");
                        }
                    } else {
                        info_pair("package", data[0] ? "QFN60" : "QFN80");
                    }
                    info_pair("chipid", hex_string(data[1] | (uint64_t)data[2] << 32, 16));
                    data += 3;
                }
                unsigned int critical = 0;
                if (included & SYS_INFO_CRITICAL) {
                    critical = *data++;
                }
                unsigned int cpu_info = 0;
                if (included & SYS_INFO_CPU_INFO) {
                    cpu_info = *data++;
                }
                if (included & SYS_INFO_FLASH_DEV_INFO) {
                    unsigned int flash_dev_info = *data++;
                    info_pair("flash devinfo", hex_string(flash_dev_info, 4));
                }
                if (included & SYS_INFO_CPU_INFO) {
                    info_pair("current cpu", cpu_name((uint8_t) cpu_info));
                }
                if (included & SYS_INFO_CRITICAL) {
                    std::vector<string> supported_cpus;
                    if (!(critical & OTP_CRITICAL_ARM_DISABLE_BITS)) supported_cpus.emplace_back("ARM");
                    if (!(critical &
                            (OTP_CRITICAL_RISCV_DISABLE_BITS | OTP_CRITICAL_SECURE_BOOT_ENABLE_BITS)))
                        supported_cpus.emplace_back("RISC-V");
                    info_pair("available cpus", cli::join(supported_cpus, ", "));
                    if (supported_cpus.size() > 1) {
                        info_pair("default cpu",
                                    (critical & OTP_CRITICAL_DEFAULT_ARCHSEL_BITS) ? "RISC-V" : "ARM");
                    }
                    info_pair("secure boot",
                                std::to_string((bool) (critical & OTP_CRITICAL_SECURE_BOOT_ENABLE_BITS)));
                    info_pair("debug enable",
                                std::to_string((bool) !(critical & OTP_CRITICAL_DEBUG_DISABLE_BITS)));
                    info_pair("secure debug enable",
                                std::to_string((bool) !(critical & OTP_CRITICAL_SECURE_DEBUG_DISABLE_BITS)));
                }
                if (included & SYS_INFO_BOOT_RANDOM) {
                    char buf[40];
                    snprintf(buf, sizeof(buf), "%08x:%08x:%08x:%08x", data[0], data[1], data[2], data[3]);
                    data += 4;
                    info_pair("boot_random", buf);
                }
//                    if (included & SYS_INFO_NONCE) {
//                        info_pair("nonce", hex_string(*(int64_t *) data, 16));
//                        data += 2;
//                    }
                if (included & SYS_INFO_BOOT_INFO) {
                    uint32_t boot_word = *data++;
                    unsigned int boot_type = (uint8_t)(boot_word>>8);
                    info_pair("boot type", boot_type_string(boot_type));
                    int8_t boot_partition = (int8_t)(boot_word >> 16);
                    uint8_t tbyb_and_update = (uint8_t)(boot_word >> 24);
                    info_pair("last booted partition", boot_partition_string(boot_partition));
                    if (!(tbyb_and_update & 0x80)) {
                        if (tbyb_and_update & BOOT_TBYB_AND_UPDATE_FLAG_BUY_PENDING) info_pair("explicit buy pending", "true");
                        if (tbyb_and_update & BOOT_TBYB_AND_UPDATE_FLAG_OTHER_ERASED) info_pair("other slot/partition erased", "true");
                        if (tbyb_and_update & BOOT_TBYB_AND_UPDATE_FLAG_OTP_VERSION_APPLIED) info_pair("OTP version applied", "true");
                    }
                    uint32_t diagnostics = *data++;
                    if ((int8_t)boot_word != BOOT_PARTITION_NONE) {
                        info_pair("diagnostic source", boot_partition_string((int8_t)boot_word));
                        info_pair("last boot diagnostics", hex_string(diagnostics, 8));
                    }
                    uint32_t p0 = *data++;
                    uint32_t p1 = *data++;
                    // todo remove these; not particularly useful except for debugging
                    if (boot_type & ~BOOT_TYPE_CHAINED_FLAG) {
                        info_pair("reboot param 0", hex_string(p0));
                        info_pair("reboot param 1", hex_string(p1));
                    }
                }
                if (settings.info.show_debug || settings.info.all) {
                    info_pair("rom gitrev", hex_string(get_rom_git_revision_v2(raw_access)));
                }
                select_group(device_info);
            }

            try {
                int32_t size_guess = guess_flash_size(raw_access);
                if (size_guess > 0) {
                    info_pair("flash size", std::to_string(size_guess/1024) + "K");
                    if (model->chip() == rp2040) {
                        uint64_t flash_id = 0;
                        con->flash_id(flash_id);
                        info_pair("flash id", hex_string(flash_id, 16, true, true));
                    }
                }
            } catch (picoboot::command_failure &e) {
                if (e.get_code() == PICOBOOT_NOT_PERMITTED) {
                    info_pair("flash size", "not determined due to access permissions");
                } else {
                    throw;
                }
            }

            // not sure how interesting this is given the chip revision which is correlated
            // info_pair("ROM version", std::to_string(rom_version));
        }
    #endif
        bool first = true;
        int fr_col = fos.first_column();
        // Standardise indent for whole info printout
        int tab = 0;
        for(const auto& group : groups) {
            if (group.enabled) {
                const auto& info = infos[group.name];
                if (!info.empty()) {
                    tab = std::max(tab, group.min_tab);
                    for(const auto& item : info) {
                        tab = std::max(tab, 3 + (int)item.first.length()); // +3 for ":  "
                    }
                }
            }
        }
        for(const auto& group : groups) {
            if (group.enabled) {
                const auto& info = infos[group.name];
                fos.first_column(fr_col);
                fos.hanging_indent(0);
                if (!first) {
                    fos.wrap_hard();
                } else {
                    first = false;
                }
                fos << group.name << "\n";
                fos.first_column(fr_col + 1);
                if (info.empty()) {
                   fos << "none\n";
                } else {
                    for(const auto& item : info) {
                        fos.first_column(fr_col + 1);
                        fos << (item.first + ":");
                        fos.first_column(fr_col + 1 + tab);
                        fos << (item.second + "\n");
                    }
                }
            }
        }
        fos.flush();
    } catch (not_mapped_exception&e) {
        std::cout << "\nfailed to read memory at " << hex_string(e.addr) << "\n";
    }

#if HAS_LIBUSB
    if (has_pt && no_pt_loaded && model->chip() != rp2040) {
        if (pt_is_valid) {
            fos.first_column(0); fos.hanging_indent(0);
            fos << "\nWARNING: Device has no partition table loaded, but has a partition table at the start of flash.\n";
            fos.first_column(sizeof("WARNING: ") - 1);
            fos << "This may cause picotool to behave unpredictably.\n";
            fos << "This could be due to using a custom board which is missing the resistor in series with the BOOTSEL button, ";
            fos << "or if BOOT_FLAGS0.HASHED_PARTITION_TABLE or BOOT_FLAGS0.SECURE_PARTITION_TABLE is set in OTP and the partition table is not hashed/signed\n";
        } else {
            fos.first_column(0); fos.hanging_indent(0);
            fos << "\nWARNING: Device has an incorrectly hashed/signed partition table at the start of flash.\n";
            fos.first_column(sizeof("WARNING: ") - 1);
            fos << "This may cause picotool to behave unpredictably.\n";
        }
    }
#endif
}

void config_guts(memory_access &raw_access) {
    binary_info_header hdr;
    int int_value;
    bool not_int = false;
    string string_value;

    if (!settings.config.value.empty()) {
        string_value = settings.config.value;
        if (!get_int(settings.config.value, int_value)) {
            not_int = true;
        }
    }

    auto bi_access = get_bi_access(raw_access);
    if (find_binary_info(*bi_access, hdr)) {
        auto access = remapped_memory_access(*bi_access, hdr.reverse_copy_mapping);
        auto visitor = bi_visitor{};

        map<pair<int, uint32_t>, pair<string, unsigned int>> named_feature_groups;
        map<string, vector<pair<string, int>>> named_feature_group_ints;
        map<string, vector<pair<string, string>>> named_feature_group_strings;

        // do a pass first to find named groups
        visitor.named_group(
            [&](int parent_tag, uint32_t parent_id, int group_tag, uint32_t group_id, const string &label,
                unsigned int flags) {
                if (parent_tag != BINARY_INFO_TAG_RASPBERRY_PI)
                    return;
                if (parent_id != BINARY_INFO_ID_RP_PROGRAM_FEATURE)
                    return;
                if (!settings.config.group.empty() && label != settings.config.group)
                    return;
                named_feature_groups[std::make_pair(group_tag, group_id)] = std::make_pair(label, flags);
            });

        visitor.visit(access, hdr);

        int fr_col = fos.first_column();
        if (settings.config.value.empty()) {
            visitor = bi_visitor{};
            visitor.ptr_int32_t_with_name([&](int tag, uint32_t id, const string &label, int32_t value) {
                const auto &nfg = named_feature_groups.find(std::make_pair(tag, id));
                if (nfg != named_feature_groups.end()) {
                    named_feature_group_ints[nfg->second.first].push_back({label, value});
                    return;
                } else if (settings.config.group.empty()) {
                    named_feature_group_ints[""].push_back({label, value});
                }
            });

            visitor.ptr_string_t_with_name([&](int tag, uint32_t id, const string &label, const string &value) {
                const auto &nfg = named_feature_groups.find(std::make_pair(tag, id));
                if (nfg != named_feature_groups.end()) {
                    named_feature_group_strings[nfg->second.first].push_back({label, value});
                    return;
                } else if (settings.config.group.empty()) {
                    named_feature_group_strings[""].push_back({label, value});
                }
            });

            visitor.visit(access, hdr);

            std::set<string> group_names;
            for (auto kv : named_feature_group_ints)
                group_names.insert(kv.first);
            for (auto kv : named_feature_group_strings)
                group_names.insert(kv.first);

            for (auto n : group_names) {
                auto ints = named_feature_group_ints[n];
                auto strings = named_feature_group_strings[n];
                fos.first_column(fr_col);
                if (!n.empty()) {
                    fos << n << ":\n";
                    fos.first_column(fr_col + 1);
                }
                for (auto val : ints) {
                    fos << val.first << " = " << val.second << "\n";
                }
                for (auto val : strings) {
                    fos << val.first << " = \"" << val.second << "\"\n";
                }
            }
        } else {
            auto modifier = bi_modifier{};

            if (!not_int) {
                modifier.ptr_int32_t_with_name([&](int tag, uint32_t id, const string &label, int32_t value, int32_t& new_value) -> bool {
                    const auto &nfg = named_feature_groups.find(std::make_pair(tag, id));
                    if (nfg == named_feature_groups.end() && !settings.config.group.empty()) {
                        // Group specified, and this isn't in that group
                        return false;
                    }
                    if (settings.config.key != label)
                        return false;
                    fos << label << " = " << value << "\n";
                    new_value = int_value;
                    fos << "setting " << label << " -> " << new_value << "\n";
                    return true;
                });
            }

            modifier.ptr_string_t_with_name([&](int tag, uint32_t id, const string &label, const string &value, string& new_value) -> bool {
                const auto &nfg = named_feature_groups.find(std::make_pair(tag, id));
                if (nfg == named_feature_groups.end() && !settings.config.group.empty()) {
                    // Group specified, and this isn't in that group
                    return false;
                }
                if (settings.config.key != label)
                    return false;
                fos << label << " = \"" << value << "\"\n";
                new_value = string_value;
                fos << "setting " << label << " -> \"" << new_value << "\"\n";
                return true;
            });

            modifier.visit(access, hdr);
        }
    }
}

string missing_device_string(bool wasRetry, bool requires_rp2350 = false) {
    char b[256];
    const char* device_name = requires_rp2350 ? "RP2350" : "RP-series";
    if (wasRetry) {
        strcpy(b, "Despite the reboot attempt, no ");
    } else {
        strcpy(b, "No ");
    }
    char *buf = b + strlen(b);
    int buf_len = b + sizeof(b) - buf;
    if (settings.address != -1) {
        if (settings.bus != -1) {
            snprintf(buf, buf_len, "accessible %s device in BOOTSEL mode was found at bus %d, address %d.", device_name, settings.bus, settings.address);
        } else {
            snprintf(buf, buf_len, "accessible %s devices in BOOTSEL mode were found with address %d.", device_name, settings.address);
        }
    } else if (settings.bus != -1) {
        snprintf(buf, buf_len, "accessible %s devices in BOOTSEL mode were found on bus %d.", device_name, settings.bus);
    } else if (!settings.ser.empty()) {
        snprintf(buf, buf_len, "accessible %s devices in BOOTSEL mode were found with serial number %s.", device_name, settings.ser.c_str());
    } else {
        snprintf(buf, buf_len, "accessible %s devices in BOOTSEL mode were found.", device_name);
    }
    return b;
}

bool help_command::execute(device_map &devices) {
    assert(false);
    return false;
}

model_t get_access_model(memory_access &file_access) {
    vector<uint8_t> bin;
    std::unique_ptr<block> best_block = find_best_block(file_access, bin);
    if (best_block == NULL) {
        // No block, so RP2040 or absolute
        if (file_access.get_binary_start() == FLASH_START) {
            vector<uint8_t> checksum_data = {};
            file_access.read_into_vector(FLASH_START, 252, checksum_data, true);
            uint32_t checksum = file_access.read_int(FLASH_START + 252, true);
            if (checksum == calc_checksum(checksum_data)) {
                // Checksum is correct, so RP2040
                DEBUG_LOG("Detected family ID %s due to boot2 checksum\n", family_name(RP2040_FAMILY_ID).c_str());
                return std::make_shared<model_rp2040>();
            } else {
                // Checksum incorrect, so absolute
                DEBUG_LOG("Assumed family ID %s\n", family_name(ABSOLUTE_FAMILY_ID).c_str());
                model_t ret = std::make_shared<model_rp_generic>();
                ret->set_family_id(ABSOLUTE_FAMILY_ID);
                return ret;
            }
        } else {
            // no_flash RP2040 binaries have no checksum
            DEBUG_LOG("Assumed family ID %s\n", family_name(RP2040_FAMILY_ID).c_str());
            return std::make_shared<model_rp2040>();
        }
    }
    auto first_item = best_block->items[0].get();
    if (first_item->type() != PICOBIN_BLOCK_ITEM_1BS_IMAGE_TYPE) {
        // This will apply for partition tables
        DEBUG_LOG("Assumed family ID %s due to block with no IMAGE_DEF\n", family_name(ABSOLUTE_FAMILY_ID).c_str());
        model_t ret = std::make_shared<model_rp_generic>();
        ret->set_family_id(ABSOLUTE_FAMILY_ID);
        return ret;
    }
    auto image_def = dynamic_cast<image_type_item*>(first_item);
    if (image_def->image_type() == type_exe) {
        if (image_def->chip() == chip_rp2040) {
            return std::make_shared<model_rp2040>();
        } else if (image_def->chip() == chip_rp2350) {
            if (image_def->cpu() == cpu_riscv) {
                return std::make_shared<model_rp2350_riscv>();
            } else if (image_def->cpu() == cpu_arm) {
                if (image_def->security() == sec_s) {
                    return std::make_shared<model_rp2350_arm_s>();
                } else if (image_def->security() == sec_ns) {
                    return std::make_shared<model_rp2350_arm_ns>();
                } else {
                    fail(ERROR_INCOMPATIBLE, "Cannot autodetect UF2 family: Unsupported security level %x\n", image_def->security());
                }
            } else {
                fail(ERROR_INCOMPATIBLE, "Cannot autodetect UF2 family: Unsupported cpu %x\n", image_def->cpu());
            }
        } else {
            fail(ERROR_INCOMPATIBLE, "Cannot autodetect UF2 family: Unsupported chip %x\n", image_def->chip());
        }
    } else if (image_def->image_type() == type_data) {
        model_t ret = std::make_shared<model_rp_generic>();
        ret->set_family_id(DATA_FAMILY_ID);
        return ret;
    } else {
        fail(ERROR_INCOMPATIBLE, "Cannot autodetect UF2 family: Unsupported image type %x\n", image_def->image_type());
    }

    return models::unknown;
}

uint32_t get_family_id(uint8_t file_idx) {
    uint32_t family_id = 0;
    if (settings.family_id) {
        family_id = settings.family_id;
    } else if (get_file_type_idx(file_idx) == filetype::elf || get_file_type_idx(file_idx) == filetype::bin) {
        auto file_access = get_file_memory_access(file_idx);
        family_id = get_access_model(file_access)->family_id();
    } else if (get_file_type_idx(file_idx) == filetype::uf2) {
        auto file = get_file_idx(ios::in|ios::binary, file_idx);
        uf2_block block;
        file->read((char*)&block, sizeof(block));
        #if SUPPORT_RP2350_A2
        // ignore the absolute block
        if (check_abs_block(block)) {
            DEBUG_LOG("Ignoring RP2350-E10 absolute block\n");
            file->read((char*)&block, sizeof(block));
        }
        #endif
        family_id = block.file_size;
    } else {
        // todo this can be done - need to add block search for bin files
        fail(ERROR_FORMAT, "Cannot autodetect UF2 family - must specify the family\n");
    }
    DEBUG_LOG("Detected family ID %s\n", family_name(family_id).c_str());;
    return family_id;
}

model_t get_model(uint8_t file_idx) {
    model_t model = nullptr;
    if (settings.model) {
        model = settings.model;
    } else if (settings.family_id) {
        auto model_guess = model_from_family(settings.family_id);
        if (model_guess->chip() != unknown) {
            model = model_guess;
        }
    }
    if (model == nullptr) {
        auto file_access = get_file_memory_access(file_idx);
        model = get_access_model(file_access);
    }
    // Clear the family ID, as get_family_id should be used for that, to allow command line override
    model->set_family_id(0);
    return model;
}

#if HAS_LIBUSB

struct partition_details {
    uint32_t start = 0;
    uint32_t end = 0;
    uint32_t flags_and_permissions = 0;
    bool has_id = false;
    uint64_t id = 0;
    bool has_name = false;
    string name = "";
    vector<uint32_t> extra_families = {};
};

std::shared_ptr<tuple<resident_partition_t, std::shared_ptr<vector<partition_details>>>> get_partition_info(picoboot::connection &con) {
    picoboot_memory_access raw_access(con);
    auto model = raw_access.get_model();
    if (!model->supports_partition_table()) {
        // no partition table
        return nullptr;
    }

    if (model->chip_revision() == rp2350_a2) con.exit_xip();

    uint8_t loc_flags_id_buf[256];
    uint8_t family_id_name_buf[256];
    uint32_t *loc_flags_id_buf_32 = (uint32_t *)loc_flags_id_buf;
    uint32_t *family_id_name_buf_32 = (uint32_t *)family_id_name_buf;
    picoboot_get_info_cmd cmd;
    cmd.bType = PICOBOOT_GET_INFO_PARTTION_TABLE;
    cmd.dParams[0] = PT_INFO_PT_INFO | PT_INFO_PARTITION_LOCATION_AND_FLAGS | PT_INFO_PARTITION_ID;
    con.get_info(&cmd, loc_flags_id_buf, sizeof(loc_flags_id_buf));
    unsigned int lfi_pos = 0;
    unsigned int words = loc_flags_id_buf_32[lfi_pos++];
    unsigned int included_fields = loc_flags_id_buf_32[lfi_pos++];
    assert(included_fields == cmd.dParams[0]);
    unsigned int partition_count = loc_flags_id_buf[lfi_pos * 4];
    unsigned int has_pt = loc_flags_id_buf[lfi_pos * 4 + 1];
    lfi_pos++;
    resident_partition_t unpartitioned = *(resident_partition_t *) &loc_flags_id_buf_32[lfi_pos];
    lfi_pos += 2;

    if (!has_pt) {
        // there is no partition table
        return std::make_shared<tuple<resident_partition_t, std::shared_ptr<vector<partition_details>>>>(std::make_tuple(unpartitioned, nullptr));
    }

    vector<partition_details> ret;

    for (unsigned int i = 0; i < partition_count; i++) {
        uint32_t location_and_permissions = loc_flags_id_buf_32[lfi_pos++];
        uint32_t flags_and_permissions = loc_flags_id_buf_32[lfi_pos++];
        uint64_t id = 0;
        string name = "";
        vector<uint32_t> extra_families = {};
        if (flags_and_permissions & PICOBIN_PARTITION_FLAGS_HAS_ID_BITS) {
            id = loc_flags_id_buf_32[lfi_pos] | ((uint64_t) loc_flags_id_buf_32[lfi_pos + 1] << 32u);
            lfi_pos += 2;
        }
        uint32_t num_extra_families =
                (flags_and_permissions & PICOBIN_PARTITION_FLAGS_ACCEPTS_NUM_EXTRA_FAMILIES_BITS)
                        >> PICOBIN_PARTITION_FLAGS_ACCEPTS_NUM_EXTRA_FAMILIES_LSB;
        if (num_extra_families | (flags_and_permissions & PICOBIN_PARTITION_FLAGS_HAS_NAME_BITS)) {
            cmd.dParams[0] = PT_INFO_SINGLE_PARTITION | PT_INFO_PARTITION_FAMILY_IDS | PT_INFO_PARTITION_NAME |
                            (i << 24);
            con.get_info(&cmd, family_id_name_buf, sizeof(family_id_name_buf));
            unsigned int got = family_id_name_buf_32[0];
            assert(family_id_name_buf_32[1] == (cmd.dParams[0] & 0xffffffu));
            assert((flags_and_permissions & PICOBIN_PARTITION_FLAGS_HAS_NAME_BITS) ||
                    got == num_extra_families + 1);
            for (unsigned int j = 1; j < num_extra_families + 1; j++) {
                extra_families.push_back(family_id_name_buf_32[j + 1]);
            }
            if (flags_and_permissions & PICOBIN_PARTITION_FLAGS_HAS_NAME_BITS) {
                uint8_t *bytes = &family_id_name_buf[(num_extra_families + 2) * 4];
                for (int l = *(bytes++) & 0x7f; l > 0; l--) {
                    name += (char)*bytes++;
                }
            }
        }
        ret.push_back({
            .start = ((location_and_permissions >> PICOBIN_PARTITION_LOCATION_FIRST_SECTOR_LSB) & 0x1fffu) * 4096,
            .end = (((location_and_permissions >> PICOBIN_PARTITION_LOCATION_LAST_SECTOR_LSB) & 0x1fffu) + 1) * 4096,
            .flags_and_permissions = flags_and_permissions,
            .has_id = (bool)(flags_and_permissions & PICOBIN_PARTITION_FLAGS_HAS_ID_BITS),
            .id = id,
            .has_name = (bool)(flags_and_permissions & PICOBIN_PARTITION_FLAGS_HAS_NAME_BITS),
            .name = name,
            .extra_families = extra_families,
        });
        if ((location_and_permissions ^ flags_and_permissions) &
            PICOBIN_PARTITION_PERMISSIONS_BITS) {
            printf("PARTITION TABLE PERMISSION MISMATCH!\n");
            return nullptr;
        }
    }

    return std::make_shared<tuple<resident_partition_t, std::shared_ptr<vector<partition_details>>>>(std::make_tuple(unpartitioned, std::make_shared<vector<partition_details>>(ret)));
}

std::shared_ptr<vector<partition_details>> get_partitions(picoboot::connection &con) {
    auto pi = get_partition_info(con);
    return pi ? std::get<1>(*pi) : nullptr;
}
#endif

bool config_command::execute(device_map &devices) {
    fos.first_column(0); fos.hanging_indent(0);

    if (!settings.filenames[0].empty()) {
        auto raw_access = get_file_memory_access(0, true);
        fos << "File " << settings.filenames[0] << ":\n\n";
        config_guts(raw_access);
        return false;
    }
#if HAS_LIBUSB
    int size = devices[dr_vidpid_bootrom_ok].size();
    if (size) {
        if (size > 1) {
            fos << "Multiple RP-series devices in BOOTSEL mode found:\n";
        }
        for (auto handles : devices[dr_vidpid_bootrom_ok]) {
            selected_chip = std::get<0>(handles);
            fos.first_column(0); fos.hanging_indent(0);
            if (size > 1) {
                auto s = bus_device_string(std::get<1>(handles), std::get<0>(handles));
                string dashes;
                std::generate_n(std::back_inserter(dashes), s.length() + 1, [] { return '-'; });
                fos << "\n" << s << ":\n" << dashes << "\n";
            }
            picoboot::connection connection(std::get<2>(handles), std::get<0>(handles));
            picoboot_memory_access access(connection);
            // Enable auto-erase
            access.enable_ftl = true;
            auto partitions = get_partitions(connection);
            vector<uint32_t> starts;
            if (partitions) {
                for (auto partition : *partitions) {
                    starts.push_back(partition.start);
                }
                for (unsigned int i=0; i < starts.size(); i++) {
                    uint32_t start = starts[i];
                    fos.first_column(0); fos.hanging_indent(0);
                    fos << "\nPartition " << i << "\n";
                    fos.first_column(1);
                    partition_memory_access part_access(access, start);
                    config_guts(part_access);
                }
            } else {
                config_guts(access);
            }
        }
    } else {
        fail(ERROR_NO_DEVICE, missing_device_string(false));
    }
#endif
    return false;
}

void set_model_from_metadata(file_memory_access& access) {
    auto model = get_access_model(access);
    access.set_model(model);
}

bool info_command::execute(device_map &devices) {
    fos.first_column(0); fos.hanging_indent(0);
    if (!settings.filenames[0].empty()) {
        uint32_t next_id = 0;
        auto access = get_file_memory_access(0, false, &next_id);
        set_model_from_metadata(access);
        uint32_t id = get_family_id(0);
        if (next_id) {
            next_id = id;
            while (next_id) {
                fos.first_column(0); fos.hanging_indent(0);
                std::stringstream s;
                s << "File " << settings.filenames[0] << " family ID " << family_name(next_id) << ":";
                if (next_id != id) {
                    string dashes;
                    std::generate_n(std::back_inserter(dashes), s.str().length() + 1, [] { return '-'; });
                    fos << "\n" << dashes << "\n";
                }
                fos << s.str() << "\n\n";
                auto tmp_access = get_file_memory_access(0, false, &next_id);
                set_model_from_metadata(tmp_access);
                info_guts(tmp_access, nullptr);
            }
        } else {
            if (get_file_type() == filetype::uf2) {
                fos << "File " << settings.filenames[0] << " family ID " << family_name(id) << ":\n\n";
            } else {
                fos << "File " << settings.filenames[0] << ":\n\n";
            }
            info_guts(access, nullptr);
        }
        return false;
    }
#if HAS_LIBUSB
    int size = devices[dr_vidpid_bootrom_ok].size();
    if (size) {
        if (size > 1) {
            fos << "Multiple RP-series devices in BOOTSEL mode found:\n";
        }
        for (auto handles : devices[dr_vidpid_bootrom_ok]) {
            selected_chip = std::get<0>(handles);
            fos.first_column(0); fos.hanging_indent(0);
            if (size > 1) {
                auto s = bus_device_string(std::get<1>(handles), std::get<0>(handles));
                string dashes;
                std::generate_n(std::back_inserter(dashes), s.length() + 1, [] { return '-'; });
                fos << "\n" << s << ":\n" << dashes << "\n";
            }
            picoboot::connection connection(std::get<2>(handles), std::get<0>(handles));
            picoboot_memory_access access(connection);
            auto partitions = get_partitions(connection);
            vector<uint32_t> starts;
            if (partitions) {
                // Check if bootloader is present, based on presence of binary info
                binary_info_header hdr;
                auto bi_access = get_bi_access(access);
                bool has_bootloader = find_binary_info(*bi_access, hdr);

                // Don't show device, until all partitions done
                bool device = settings.info.show_device || settings.info.all;
                bool debug = settings.info.show_debug || settings.info.all;
                if (settings.info.all) {
                    settings.info.show_basic = true;
                    settings.info.show_pins = true;
                    settings.info.show_build = true;
                    settings.info.show_metadata = true;
                    settings.info.all = false;
                }
                if ((settings.info.show_basic || settings.info.show_pins || settings.info.show_build || settings.info.show_metadata) || !(settings.info.show_device || settings.info.show_debug)) {
                    settings.info.show_device = false;
                    settings.info.show_debug = false;
                    for (auto partition : *partitions) {
                        starts.push_back(partition.start);
                    }
                    if (has_bootloader && std::none_of(starts.cbegin(), starts.cend(), [](int i) { return i == 0; })) {
                        // Print bootloader info, only if bootloader is present and not in a partition
                        fos.first_column(0); fos.hanging_indent(0);
                        fos << "\nBootloader\n";
                        fos.first_column(1);
                        partition_memory_access part_access(access, 0);
                        info_guts(part_access, &connection);
                    }
                    for (unsigned int i=0; i < starts.size(); i++) {
                        uint32_t start = starts[i];
                        fos.first_column(0); fos.hanging_indent(0);
                        fos << "\nPartition " << i << "\n";
                        fos.first_column(1);
                        partition_memory_access part_access(access, start);
                        info_guts(part_access, &connection);
                    }
                }
                if (device || debug) {
                    fos.first_column(0); fos.hanging_indent(0);
                    fos << "\n";
                    settings.info.show_basic = false;
                    settings.info.show_pins = false;
                    settings.info.show_build = false;
                    settings.info.show_metadata = false;
                    settings.info.show_device = device;
                    settings.info.show_debug = debug;
                    info_guts(access, &connection);
                }
            } else {
                info_guts(access, &connection, true);
            }
        }
    } else {
        fail(ERROR_NO_DEVICE, missing_device_string(false));
    }
#endif
    return false;
}

#if HAS_LIBUSB
static picoboot::connection get_single_bootsel_device_connection(device_map& devices, bool exclusive = true) {
    assert(devices[dr_vidpid_bootrom_ok].size() == 1);
    auto device = devices[dr_vidpid_bootrom_ok][0];
    selected_chip = std::get<0>(device);
    libusb_device_handle *rc = std::get<2>(device);
    if (!rc) fail(ERROR_USB, "Unable to connect to device");
    return picoboot::connection(rc, exclusive);
}

static picoboot::connection get_single_picoboot_cmd_compatible_device_connection(const std::string& cmd_name, device_map& devices, std::set<picoboot_cmd_id> picoboot_cmds, bool exclusive = true) {
    auto con = get_single_bootsel_device_connection(devices, exclusive);
    // todo amy we may have a different VID PID?
    picoboot_memory_access raw_access(con);
    std::string failed_device_name;
    if (!raw_access.get_model()->supports_picoboot_cmds(picoboot_cmds, failed_device_name)) {
        fail(ERROR_INCOMPATIBLE, "'%s' command cannot be used with a %s device", cmd_name.c_str(), failed_device_name.c_str());
    }
    return con;
}
#endif

struct progress_bar {
    explicit progress_bar(string new_prefix, int width = 30) : width(width) {
        // Align all bars with the longest possible prefix string
        auto longest_mem = std::max_element(
            std::begin(memory_names), std::end(memory_names),
            [] (const auto & p1, const auto & p2) {
                return p1.second.length() < p2.second.length();
            }
        );
        string extra_space(string("Loading into " + longest_mem->second + ": ").length() - new_prefix.length(), ' ');
        prefix = new_prefix + extra_space;
        progress(0);
    }

    void progress(int _percent) {
        if (_percent != percent) {
            percent = _percent;
            unsigned int len = (width * percent) / 100;
            std::cout << prefix << "[" << string(len, '=') << string(width-len, ' ') << "]  " << std::to_string(percent) << "%\r" << std::flush;
        }
    }

    void progress(long dividend, long divisor) {
        progress(divisor ? (int)((100 * dividend) / divisor) : 100);
    }

    ~progress_bar() {
        std::cout << "\n";
    }

    std::string prefix;
    int percent = -1;
    int width;
};

#if HAS_LIBUSB
vector<range> get_coalesced_ranges(iostream_memory_access &file_access, model_t model) {
    auto rmap = file_access.get_rmap();
    auto ranges = rmap.ranges();
    std::sort(ranges.begin(), ranges.end(), [](const range& a, const range &b) {
        return a.from < b.from;
    });
    // coalesce all the contiguous ranges
    for(auto i = ranges.begin(); i < ranges.end(); ) {
        if (i != ranges.end() - 1) {
            uint32_t erase_size;
            // we want to coalesce flash sectors together (this ends up creating ranges that may have holes)
            if( get_memory_type(i->from, model) == flash ) {
                erase_size = FLASH_SECTOR_ERASE_SIZE;
            } else {
                erase_size = 1;
            }
            if (i->to / erase_size == (i+1)->from / erase_size) {
                i->to = (i+1)->to;
                i = ranges.erase(i+1) - 1;
                continue;
            }
        }
        i++;
    }
    return ranges;
}

bool save_command::execute(device_map &devices) {
    auto con = get_single_bootsel_device_connection(devices);
    picoboot_memory_access raw_access(con);

    uint32_t end = 0;
    uint32_t binary_end = 0;
    binary_info_header hdr;
    uint32_t start = FLASH_START;
    if (!settings.save.all) {
        if (settings.range_set) {
            if (get_file_type() == filetype::uf2) {
                start = settings.from & ~(PAGE_SIZE - 1);
                end = (settings.to + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1);
            } else {
                start = settings.from;
                end = settings.to;
                // Set offset for verifying
                settings.offset = start;
                settings.offset_set = true;
            }
            if (end <= start) {
                fail(ERROR_ARGS, "Save range is invalid/empty");
            }
        } else {
            if (find_binary_info(raw_access, hdr)) {
                auto access = remapped_memory_access(raw_access, hdr.reverse_copy_mapping);
                auto visitor = bi_visitor{};
                visitor.id_and_int([&](int tag, uint32_t id, uint32_t value) {
                    if (tag != BINARY_INFO_TAG_RASPBERRY_PI)
                        return;
                    if (id == BINARY_INFO_ID_RP_BINARY_END) binary_end = value;
                });
                visitor.visit(access, hdr);
            }
            end = binary_end;
            vector<uint8_t> bin;
            std::unique_ptr<block> last_block = find_last_block(raw_access, bin);
            if (last_block != nullptr) {
                uint32_t new_end = last_block->physical_addr + (last_block->to_words().size())*4;
                DEBUG_LOG("Adjusting end to max of %x %x\n", end, new_end);
                end = MAX(end, new_end);
            }
            if (end == 0) {
                fail(ERROR_NOT_POSSIBLE,
                     "Cannot determine the binary size, so cannot save the program only, try --all.");
            }
        }
    } else {
        end = FLASH_START + guess_flash_size(raw_access);
        if (end <= FLASH_START) {
            fail(ERROR_NOT_POSSIBLE, "Cannot determine the flash size, so cannot save the entirety of flash, try --range.");
        }
    }

    model_t model = raw_access.get_model();
    enum memory_type t1 = get_memory_type(start , model);
    enum memory_type t2 = get_memory_type(end, model);
    if (t1 != t2 || t1 == invalid || t1 == sram_unstriped) {
        fail(ERROR_NOT_POSSIBLE, "Save range crosses unmapped memory");
    }
    uint32_t size = end - start;
    uint32_t chunk_size = calculate_chunk_size(size);

    std::function<void(FILE *out, const uint8_t *buffer, unsigned int size, unsigned int offset)> writer256 = [](FILE *out, const uint8_t *buffer, unsigned int size, unsigned int offset) { assert(false); };
    uf2_block block;
    memset(&block, 0, sizeof(block));
    switch (get_file_type()) {
        case filetype::bin:
//            if (start != FLASH_START) {
//                fail(ERROR_ARGS, "range must start at 0x%08x for saving as a BIN file", FLASH_START);
//            }
            writer256 = [](FILE *out, const uint8_t *buffer, unsigned int actual_size, unsigned int offset) {
                fseek(out, offset, SEEK_SET);
                if (1 != fwrite(buffer, actual_size, 1, out)) {
                    fail_write_error();
                }
            };
            break;
        case filetype::elf:
            fail(ERROR_ARGS, "Save to ELF file is not supported");
            break;
        case filetype::uf2:
            block.magic_start0 = UF2_MAGIC_START0;
            block.magic_start1 = UF2_MAGIC_START1;
            block.flags = UF2_FLAG_FAMILY_ID_PRESENT;
            block.payload_size = PAGE_SIZE;
            block.num_blocks = (size + PAGE_SIZE - 1)/PAGE_SIZE;
            block.file_size = settings.family_id ? settings.family_id : get_access_model(raw_access)->family_id();
            block.magic_end = UF2_MAGIC_END;
            writer256 = [&](FILE *out, const uint8_t *buffer, unsigned int size, unsigned int offset) {
                static_assert(512 == sizeof(block), "");
                block.target_addr = start + offset;
                block.block_no = offset / PAGE_SIZE;
                assert(size <= PAGE_SIZE);
                memcpy(block.data, buffer, size);
                if (size < PAGE_SIZE) memset(block.data + size, 0, PAGE_SIZE - size);
                if (1 != fwrite(&block, sizeof(block), 1, out)) {
                    fail_write_error();
                }
            };
            break;
        default:
            throw failure_error(-1, "Unsupported output file type");
    }
    FILE *out = fopen(settings.filenames[0].c_str(), "wb");
    if (out) {
        try {
            vector<uint8_t> buf;
            {
                progress_bar bar("Saving file: ");
                for (uint32_t addr = start; addr < end; addr += chunk_size) {
                    bar.progress(addr-start, end-start);
                    uint32_t this_chunk_size = std::min(chunk_size, end - addr);
                    raw_access.read_into_vector(addr, this_chunk_size, buf);
                    uint32_t remaining_size = this_chunk_size;
                    while (remaining_size) {
                        uint32_t this_size = std::min(PAGE_SIZE, remaining_size);
                        writer256(out, buf.data() + (this_chunk_size - remaining_size), this_size, addr - start + (this_chunk_size - remaining_size));
                        remaining_size -= this_size;
                    }
                }
                bar.progress(100);
            }
            fseek(out, 0, SEEK_END);
            std::cout << "Wrote " << ftell(out) << " bytes to " << settings.filenames[0].c_str() << "\n";
            fclose(out);
        } catch (std::exception &) {
            fclose(out);
            throw;
        }
    }

    if (settings.save.verify) {
        raw_access.clear_cache();
        auto file_access = get_file_memory_access(0);
        model_t model = raw_access.get_model();
        auto ranges = get_coalesced_ranges(file_access, model);
        for (auto mem_range : ranges) {
            enum memory_type type = get_memory_type(mem_range.from, model);
            bool ok = true;
            {
                progress_bar bar("Verifying " + memory_names[type] + ": ");
                vector<uint8_t> file_buf;
                vector<uint8_t> device_buf;
                uint32_t pos = mem_range.from;
                for (uint32_t base = mem_range.from; base < mem_range.to && ok; base += chunk_size) {
                    uint32_t this_batch = std::min(std::min(mem_range.to, end) - base, chunk_size);
                    // note we pass zero_fill = true in case the file has holes, but this does
                    // mean that the verification will fail if those holes are not filled with zeros
                    // on the device
                    file_access.read_into_vector(base, this_batch, file_buf, true);
                    raw_access.read_into_vector(base, this_batch, device_buf);
                    assert(file_buf.size() == device_buf.size());
                    for (unsigned int i = 0; i < this_batch; i++) {
                        if (file_buf[i] != device_buf[i]) {
                            pos = base + i;
                            printf("Unmatch file %x, device %x, pos %x\n", file_buf[i], device_buf[i], pos);
                            ok = false;
                            break;
                        }
                    }
                    if (ok) {
                        pos = base + this_batch;
                    }
                    bar.progress(pos - mem_range.from, mem_range.to - mem_range.from);
                }
            }
            if (ok) {
                std::cout << "  OK\n";
            } else {
                std::cout << "  FAILED\n";
                fail(ERROR_VERIFICATION_FAILED, "The device contents did not match the saved file");
            }
        }
    }
    return false;
}

bool erase_command::execute(device_map &devices) {
    auto con = get_single_bootsel_device_connection(devices);
    picoboot_memory_access raw_access(con);

    // Errata RP2350-E10 fix
    auto model = raw_access.get_model();
    if (model->chip_revision() == rp2350_a2) con.exit_xip();

    uint32_t end = 0;
    uint32_t binary_end = 0;
    binary_info_header hdr;
    uint32_t start = FLASH_START;
    if (settings.load.partition >= 0) {
        auto partitions = get_partitions(con);
        if (!partitions) {
            fail(ERROR_NOT_POSSIBLE, "There is no partition table on the device");
        }
        if (settings.load.partition >= partitions->size()) {
            fail(ERROR_NOT_POSSIBLE, "There are only %d partitions on the device", partitions->size());
        }
        size_t tmp;
        tmp = (*partitions)[settings.load.partition].start;
        if (tmp > UINT32_MAX) {
            fail(ERROR_NOT_POSSIBLE, "Partition start address is too large");
        }
        start = tmp;
        tmp = (*partitions)[settings.load.partition].end;
        if (tmp > UINT32_MAX) {
            fail(ERROR_NOT_POSSIBLE, "Partition end address is too large");
        }
        end = tmp;

        printf("Erasing partition %d:\n", settings.load.partition);
        printf("  %08x->%08x\n", start, end);
        start += FLASH_START;
        end += FLASH_START;
        if (end <= start) {
            fail(ERROR_ARGS, "Erase range is invalid/empty");
        }
    } else if (settings.range_set) {
        start = settings.from & ~(FLASH_SECTOR_ERASE_SIZE - 1);
        end = (settings.to + (FLASH_SECTOR_ERASE_SIZE - 1)) & ~(FLASH_SECTOR_ERASE_SIZE - 1);
        if (end <= start) {
            fail(ERROR_ARGS, "Erase range is invalid/empty");
        }
    } else {
        end = FLASH_START + guess_flash_size(raw_access);
        if (end <= FLASH_START) {
            fail(ERROR_NOT_POSSIBLE, "Cannot determine the flash size, so cannot erase the entirety of flash, try --range.");
        }
    }

    enum memory_type t1 = get_memory_type(start , model);
    enum memory_type t2 = get_memory_type(end, model);
    if (t1 != flash || t1 != t2) {
        fail(ERROR_NOT_POSSIBLE, "Erase range not all in flash");
    }
    uint32_t size = end - start;

    {
        progress_bar bar("Erasing: ");
        con.exit_xip();
        for (uint32_t addr = start; addr < end; addr += FLASH_SECTOR_ERASE_SIZE) {
            bar.progress(addr-start, end-start);
            con.flash_erase(addr, FLASH_SECTOR_ERASE_SIZE);
        }
        bar.progress(100);
    }
    std::cout << "Erased " << size << " bytes\n";
    return false;
}
#endif

#if HAS_LIBUSB
bool get_target_partition(picoboot::connection &con, uint32_t* start = nullptr, uint32_t* end = nullptr) {
    picoboot_memory_access raw_access(con);
    auto model = raw_access.get_model();
    if (model->chip_revision() == rp2350_a2) con.exit_xip();

    uint8_t loc_flags_id_buf[256];
    uint32_t *loc_flags_id_buf_32 = (uint32_t *)loc_flags_id_buf;
    picoboot_get_info_cmd cmd;
    cmd.bType = PICOBOOT_GET_INFO_UF2_TARGET_PARTITION;
    cmd.dParams[0] = settings.family_id;
    con.get_info(&cmd, loc_flags_id_buf, sizeof(loc_flags_id_buf));
    assert(loc_flags_id_buf_32[0] == 3);
    if ((int)loc_flags_id_buf_32[1] < 0) {
        printf("Family ID %s cannot be downloaded anywhere\n", family_name(settings.family_id).c_str());
        return false;
    } else {
        if (loc_flags_id_buf_32[1] == PARTITION_TABLE_NO_PARTITION_INDEX) {
            printf("Family ID %s can be downloaded in absolute space:\n", family_name(settings.family_id).c_str());
        } else {
            printf("Family ID %s can be downloaded in partition %d:\n", family_name(settings.family_id).c_str(), loc_flags_id_buf_32[1]);
        }
        uint32_t location_and_permissions = loc_flags_id_buf_32[2];
        uint32_t saddr = ((location_and_permissions >> PICOBIN_PARTITION_LOCATION_FIRST_SECTOR_LSB) & 0x1fffu) * 4096;
        uint32_t eaddr = (((location_and_permissions >> PICOBIN_PARTITION_LOCATION_LAST_SECTOR_LSB) & 0x1fffu) + 1) * 4096;
        printf("  %08x->%08x\n", saddr, eaddr);
        if (start) *start = saddr;
        if (end) *end = eaddr;
        return true;
    }
}

bool load_guts(picoboot::connection con, iostream_memory_access &file_access) {
    picoboot_memory_access raw_access(con);
    range flash_binary_range(FLASH_START, FLASH_END_RP2350); // pick biggest (rp2350) here for now
    bool flash_binary_end_unknown = true;
    if (settings.load.no_overwrite_force) settings.load.no_overwrite = true;
    if (settings.load.no_overwrite) {
        binary_info_header hdr;
        if (find_binary_info(raw_access, hdr)) {
            auto access = remapped_memory_access(raw_access, hdr.reverse_copy_mapping);
            auto visitor = bi_visitor{};
            visitor.id_and_int([&](int tag, uint32_t id, uint32_t value) {
                if (tag != BINARY_INFO_TAG_RASPBERRY_PI)
                    return;
                if (id == BINARY_INFO_ID_RP_BINARY_END) {
                    flash_binary_range.to = value;
                    flash_binary_end_unknown = false;
                }
            });
            visitor.visit(access, hdr);
        }
    }
    model_t model = raw_access.get_model();
    auto ranges = get_coalesced_ranges(file_access, model);
    bool uses_flash = false;
    uint32_t flash_min = std::numeric_limits<uint32_t>::max();
    uint32_t flash_max = std::numeric_limits<uint32_t>::min();
    for (auto mem_range : ranges) {
        enum memory_type t1 = get_memory_type(mem_range.from, model);
        enum memory_type t2 = get_memory_type(mem_range.to, model);
        if (t1 != t2 || t1 == invalid || t1 == rom || t1 == sram_unstriped) {
            fail(ERROR_FORMAT, "File to load contained an invalid memory range 0x%08x-0x%08x", mem_range.from,
                 mem_range.to);
        }
        if (t1 == flash) {
            uses_flash = true;
            flash_min = std::min(flash_min, mem_range.from);
            flash_max = std::max(flash_max, mem_range.to);
        }
        if (settings.load.no_overwrite && mem_range.intersects(flash_binary_range)) {
            if (flash_binary_end_unknown) {
                if (!settings.load.no_overwrite_force) {
                    fail(ERROR_NOT_POSSIBLE, "-n option specified, but the size/presence of an existing flash binary could not be detected; aborting. Consider using the -N option");
                }
            } else {
                fail(ERROR_NOT_POSSIBLE, "-n option specified, and the loaded data range clashes with the existing flash binary range %08x->%08x",
                     flash_binary_range.from, flash_binary_range.to);
            }
        }
    }
    if (uses_flash) {
        assert(flash_max > flash_min);
        uint32_t flash_data_size = flash_max - flash_min;
        assert(flash_min >= FLASH_START);
        uint32_t flash_start_offset = flash_min - FLASH_START;
        uint32_t size_guess = guess_flash_size(raw_access);
        if (size_guess > 0) {
            // Skip check when targeting PSRAM, which is anything above 0x11000000
            if (flash_start_offset < FLASH_END_RP2040 && (flash_start_offset + flash_data_size) > size_guess) {
                if (flash_start_offset) {
                    fail(ERROR_NOT_POSSIBLE, "File size 0x%x starting at 0x%x is too big to fit in flash size 0x%x", flash_data_size, flash_start_offset, size_guess);
                } else {
                    fail(ERROR_NOT_POSSIBLE, "File size 0x%x is too big to fit in flash size 0x%x", flash_data_size, size_guess);
                }
            }
        }
        if (settings.partition_size > 0) {
            if (flash_data_size > settings.partition_size) {
                fail(ERROR_NOT_POSSIBLE, "File size 0x%x is too big to fit in partition size 0x%x", flash_data_size, settings.partition_size);
            }
        }
    }
    for (auto mem_range : ranges) {
        enum memory_type type = get_memory_type(mem_range.from, model);
        // new scope for progress bar
        {
            progress_bar bar("Loading into " + memory_names[type] + ": ");
            // Use batches of size/100 rounded up to FLASH_SECTOR_ERASE_SIZE
            uint32_t batch_size = calculate_chunk_size(mem_range.len());
            bool ok = true;
            vector<uint8_t> file_buf;
            vector<uint8_t> device_buf;
            for (uint32_t base = mem_range.from; base < mem_range.to && ok;) {
                uint32_t this_batch = std::min(mem_range.to - base, batch_size);
                if (type == flash) {
                    // we have to erase an entire page, so then fill with zeros
                    range aligned_range(base & ~(FLASH_SECTOR_ERASE_SIZE - 1),
                                        (base + this_batch + FLASH_SECTOR_ERASE_SIZE - 1) & ~(FLASH_SECTOR_ERASE_SIZE - 1));
                    range read_range(base, base + this_batch);
                    read_range.intersect(aligned_range);
                    file_access.read_into_vector(read_range.from, read_range.to - read_range.from, file_buf, true); // zero fill to cope with holes
                    // zero padding up to batch_size
                    file_buf.insert(file_buf.begin(), read_range.from - aligned_range.from, 0);
                    file_buf.insert(file_buf.end(), aligned_range.to - read_range.to, 0);
                    assert(file_buf.size() == aligned_range.len());

                    bool skip = false;
                    if (settings.load.update) {
                        vector<uint8_t> read_device_buf;
                        raw_access.read_into_vector(aligned_range.from, file_buf.size(), read_device_buf);
                        skip = file_buf == read_device_buf;
                    }
                    if (!skip) {
                        con.exit_xip();
                        con.flash_erase(aligned_range.from, file_buf.size());
                        raw_access.write_vector(aligned_range.from, file_buf);
                    }
                    base = read_range.to; // about to add batch_size
                } else {
                    file_access.read_into_vector(base, this_batch, file_buf);
                    raw_access.write_vector(base, file_buf);
                    base += this_batch;
                }
                bar.progress(base - mem_range.from, mem_range.to - mem_range.from);
            }
        }
    }
    for (auto mem_range : ranges) {
        enum memory_type type = get_memory_type(mem_range.from, model);
        if (settings.load.verify) {
            bool ok = true;
            {
                progress_bar bar("Verifying " + memory_names[type] + ": ");
                uint32_t batch_size = calculate_chunk_size(mem_range.len());
                vector<uint8_t> file_buf;
                vector<uint8_t> device_buf;
                uint32_t pos = mem_range.from;
                for (uint32_t base = mem_range.from; base < mem_range.to && ok; base += batch_size) {
                    uint32_t this_batch = std::min(mem_range.to - base, batch_size);
                    // note we pass zero_fill = true in case the file has holes, but this does
                    // mean that the verification will fail if those holes are not filled with zeros
                    // on the device
                    file_access.read_into_vector(base, this_batch, file_buf, true);
                    raw_access.read_into_vector(base, this_batch, device_buf);
                    assert(file_buf.size() == device_buf.size());
                    for (unsigned int i = 0; i < this_batch; i++) {
                        if (file_buf[i] != device_buf[i]) {
                            pos = base + i;
                            ok = false;
                            break;
                        }
                    }
                    if (ok) {
                        pos = base + this_batch;
                    }
                    bar.progress(pos - mem_range.from, mem_range.to - mem_range.from);
                }
            }
            if (ok) {
                std::cout << "  OK\n";
            } else {
                std::cout << "  FAILED\n";
                fail(ERROR_VERIFICATION_FAILED, "The device contents did not match the file");
            }
        }
    }
    if (settings.load.execute) {
        uint32_t start = file_access.get_binary_start();
        if (!start) {
            fail(ERROR_FORMAT, "Cannot execute as file does not contain a valid RP2 executable image");
        }
        if (raw_access.get_model()->supports_picoboot_cmd(PC_REBOOT2)) {
            struct picoboot_reboot2_cmd cmd;
            auto mt = get_memory_type(start, model);
            if (mt == flash) {
                cmd.dParam0 = settings.offset;
                cmd.dFlags = REBOOT2_FLAG_REBOOT_TYPE_FLASH_UPDATE;
                DEBUG_LOG(">>> using flash update boot of %08x\n", cmd.dParam0);
            } else {
                cmd.dParam0 = start;
                unsigned int end;
                switch (mt) {
                    case sram:
                        end = model->sram_end();
                        break;
                    case xip_sram:
                        end = model->xip_sram_end();
                        break;
                    default:
                        end = model->sram_end();
                }
                cmd.dParam1 = end - start;
                cmd.dFlags = REBOOT2_FLAG_REBOOT_TYPE_RAM_IMAGE;
                DEBUG_LOG(">>> using flash update boot of %08x\n", cmd.dParam0);
            }
            cmd.dDelayMS = 500,
            con.reboot2(&cmd);
        } else {
            con.reboot(flash == get_memory_type(start, model) ? 0 : start,
                       model->sram_end(), 500);
        }
        std::cout << "\nThe device was rebooted to start the application.\n";
        return true;
    }
    return false;
}

bool load_command::execute(device_map &devices) {
    auto con = get_single_bootsel_device_connection(devices);
    picoboot_memory_access raw_access(con);
    auto tmp_file_access = get_file_memory_access(0);
    if (settings.load.partition >= 0) {
        auto partitions = get_partitions(con);
        if (!partitions) {
            fail(ERROR_NOT_POSSIBLE, "There is no partition table on the device");
        }
        if (settings.load.partition >= partitions->size()) {
            fail(ERROR_NOT_POSSIBLE, "There are only %d partitions on the device", partitions->size());
        }
        uint32_t start = (*partitions)[settings.load.partition].start;
        uint32_t end = (*partitions)[settings.load.partition].end;
        printf("Downloading into partition %d:\n", settings.load.partition);
        printf("  %08x->%08x\n", start, end);
        settings.offset = start + FLASH_START;
        settings.offset_set = true;
        settings.partition_size = end - start;
    } else if (!settings.load.ignore_pt && !settings.offset_set && tmp_file_access.get_binary_start() == FLASH_START) {
        uint32_t family_id = get_family_id(0);
        settings.family_id = family_id;
        uint32_t start;
        uint32_t end;
        if (raw_access.get_model()->supports_partition_table()) {
            if (get_target_partition(con, &start, &end)) {
                settings.offset = start + FLASH_START;
                settings.offset_set = true;
                settings.partition_size = end - start;
            } else {
                // Check if partition table is present, for correct error message
                auto partitions = get_partitions(con);
                if (!partitions) {
                    fail(ERROR_NOT_POSSIBLE, "This file cannot be loaded onto a device with no partition table");
                } else {
                    fail(ERROR_NOT_POSSIBLE, "This file cannot be loaded into the partition table on the device");
                }
            }
        }
    }
    auto file_access = get_file_memory_access(0);
    if (settings.offset_set && get_file_type() != filetype::bin && raw_access.get_model()->chip() == rp2040) {
        fail(ERROR_ARGS, "Offset only valid for BIN files");
    }
    bool ret = load_guts(con, file_access);
    return ret;
}
#endif


static uint32_t even_parity(uint32_t input) {
    return __builtin_popcount(input) & 1;
}

// In: 16-bit unsigned integer. Out: 22-bit unsigned integer.
uint32_t __noinline otp_calculate_ecc(uint16_t x) {
    // Source: db_shf40_ap_ab.pdf, page 25, "TABLE 9: PARITY BIT GENERATION MAP
    // FOR 16 BIT USER DATA (X24 SHF MACROCELL)"
    // https://drive.google.com/drive/u/1/folders/1jgU3tZt2BDeGkWUFhi6KZAlaYUpGrFaG
    uint32_t p0 = even_parity(x & 0b1010110101011011);
    uint32_t p1 = even_parity(x & 0b0011011001101101);
    uint32_t p2 = even_parity(x & 0b1100011110001110);
    uint32_t p3 = even_parity(x & 0b0000011111110000);
    uint32_t p4 = even_parity(x & 0b1111100000000000);
    uint32_t p5 = even_parity(x) ^ p0 ^ p1 ^ p2 ^ p3 ^ p4;
    uint32_t p = p0 | (p1 << 1) | (p2 << 2) | (p3 << 3) | (p4 << 4) | (p5 << 5);
    return x | (p << 16);
}


#if HAS_MBEDTLS
void sign_guts_elf(elf_file* elf, private_t private_key, public_t public_key, model_t model) {
    std::unique_ptr<block> first_block = find_first_block(elf);
    if (!first_block) {
        // Throw a clearer error for RP2040 binaries with no block loop
        auto family_id = get_family_id(0);
        if (family_id == RP2040_FAMILY_ID) {
            fail(ERROR_FORMAT, "No metadata block found when sealing RP2040 binary - either use RP2350, or set PICO_CRT0_INCLUDE_PICOBIN_BLOCK=1");
        } else {
            fail(ERROR_FORMAT, "No metadata block found");
        }
    }

    // Workaround RP2350-E13, which means when using rollback versions, all other blocks must be set as ignored
    block new_block = place_new_block(elf, first_block, model, settings.seal.rollback_version);

    if (settings.seal.set_tbyb) {
        // Set the TBYB bit on the image_type_item
        std::shared_ptr<image_type_item> image_type = new_block.get_item<image_type_item>();
        image_type->flags |= PICOBIN_IMAGE_TYPE_EXE_TBYB_BITS;
    }

    if (settings.seal.major_version || settings.seal.minor_version || settings.seal.rollback_version) {
        std::shared_ptr<version_item> version = new_block.get_item<version_item>();
        if (version != nullptr) {
            // Use existing major and minor versions, if not being overridden
            if (settings.seal.major_version == 0) settings.seal.major_version = version->major;
            if (settings.seal.minor_version == 0) settings.seal.minor_version = version->minor;
            new_block.items.erase(std::find(new_block.items.begin(), new_block.items.end(), version));
        }
        if (settings.seal.rollback_version) {
            if (!settings.seal.sign) {
                fail(ERROR_INCOMPATIBLE, "You must sign the binary if adding a rollback version");
            }
            version = std::make_shared<version_item>(settings.seal.major_version, settings.seal.minor_version, settings.seal.rollback_version, settings.seal.rollback_rows);
        } else {
            version = std::make_shared<version_item>(settings.seal.major_version, settings.seal.minor_version);
        }
        new_block.items.push_back(version);
    }

    // Add entry point and vector table when signing Arm images, and set PICOBIN_IMAGE_TYPE_EXE_EXTRA_SECURITY_BITS
    std::shared_ptr<image_type_item> image_type = new_block.get_item<image_type_item>();
    if (settings.seal.sign && image_type != nullptr && image_type->image_type() == type_exe && image_type->cpu() == cpu_arm) {
        // Set PICOBIN_IMAGE_TYPE_EXE_EXTRA_SECURITY_BITS
        image_type->flags |= PICOBIN_IMAGE_TYPE_EXE_EXTRA_SECURITY_BITS;
        std::shared_ptr<entry_point_item> entry_point = new_block.get_item<entry_point_item>();
        if (entry_point == nullptr) {
            std::shared_ptr<vector_table_item> vtor = new_block.get_item<vector_table_item>();
            uint32_t vtor_loc = 0x10000000;
            if (vtor != nullptr) {
                vtor_loc = vtor->addr;
            } else {
                if (elf->header().entry >= SRAM_START) {
                    vtor_loc = 0x20000000;
                } else if (elf->header().entry >= XIP_SRAM_START_RP2350) {
                    vtor_loc = 0x13ffc000;
                } else {
                    vtor_loc = 0x10000000;
                    std::shared_ptr<rolling_window_delta_item> rwd = new_block.get_item<rolling_window_delta_item>();
                    if (rwd != nullptr) {
                        vtor_loc += rwd->addr;
                    }
                }

                vtor = std::make_shared<vector_table_item>(vtor_loc);
                new_block.items.push_back(vtor);
            }
            auto segment = elf->segment_from_virtual_address(vtor_loc);
            if (segment == nullptr) {
                fail(ERROR_NOT_POSSIBLE, "The ELF file does not contain the vector table location %x", vtor_loc);
            }
            auto content = elf->content(*segment);
            auto offset = vtor_loc - segment->virtual_address();
            uint32_t ep;
            memcpy(&ep, content.data() + offset + 4, sizeof(ep));
            uint32_t sp;
            memcpy(&sp, content.data() + offset, sizeof(sp));
            DEBUG_LOG("Adding entry_point_item: ep %08x, sp %08x\n", ep, sp);
            entry_point = std::make_shared<entry_point_item>(ep, sp);
            new_block.items.push_back(entry_point);
        }
    }

    hash_andor_sign(
        elf, &new_block, public_key, private_key,
        model,
        settings.seal.hash, settings.seal.sign,
        settings.seal.clear_sram, settings.seal.pin_xip_sram
    );
}

vector<uint8_t> sign_guts_bin(iostream_memory_access in, private_t private_key, public_t public_key, uint32_t bin_start, uint32_t bin_size, model_t model) {
    vector<uint8_t> bin = in.read_vector<uint8_t>(bin_start, bin_size, false);

    std::unique_ptr<block> first_block = find_first_block(bin, bin_start);
    if (!first_block) {
        // Throw a clearer error for RP2040 binaries with no block loop
        auto family_id = get_family_id(0);
        if (family_id == RP2040_FAMILY_ID) {
            fail(ERROR_FORMAT, "No metadata block found when sealing RP2040 binary - either use RP2350, or set PICO_CRT0_INCLUDE_PICOBIN_BLOCK");
        } else {
            fail(ERROR_FORMAT, "No metadata block found");
        }
    }

    // Workaround RP2350-E13, which means when using rollback versions, all other blocks must be set as ignored
    block new_block = place_new_block(bin, bin_start, first_block, model, settings.seal.rollback_version);

    if (settings.seal.major_version || settings.seal.minor_version || settings.seal.rollback_version) {
        std::shared_ptr<version_item> version = new_block.get_item<version_item>();
        if (version != nullptr) {
            // Use existing major and minor versions, if not being overridden
            if (settings.seal.major_version == 0) settings.seal.major_version = version->major;
            if (settings.seal.minor_version == 0) settings.seal.minor_version = version->minor;
            new_block.items.erase(std::find(new_block.items.begin(), new_block.items.end(), version));
        }
        if (settings.seal.rollback_version) {
            if (!settings.seal.sign) {
                fail(ERROR_INCOMPATIBLE, "You must sign the binary if adding a rollback version");
            }
            version = std::make_shared<version_item>(settings.seal.major_version, settings.seal.minor_version, settings.seal.rollback_version, settings.seal.rollback_rows);
        } else {
            version = std::make_shared<version_item>(settings.seal.major_version, settings.seal.minor_version);
        }
        new_block.items.push_back(version);
    }

    // Add entry point and vector table when signing Arm images, and set PICOBIN_IMAGE_TYPE_EXE_EXTRA_SECURITY_BITS
    std::shared_ptr<image_type_item> image_type = new_block.get_item<image_type_item>();
    if (settings.seal.sign && image_type != nullptr && image_type->image_type() == type_exe && image_type->cpu() == cpu_arm) {
        // Set PICOBIN_IMAGE_TYPE_EXE_EXTRA_SECURITY_BITS
        image_type->flags |= PICOBIN_IMAGE_TYPE_EXE_EXTRA_SECURITY_BITS;
        std::shared_ptr<entry_point_item> entry_point = new_block.get_item<entry_point_item>();
        if (entry_point == nullptr) {
            std::shared_ptr<vector_table_item> vtor = new_block.get_item<vector_table_item>();
            uint32_t vtor_loc = bin_start;
            if (vtor != nullptr) {
                vtor_loc = vtor->addr;
            } else {
                vtor = std::make_shared<vector_table_item>(vtor_loc);
                new_block.items.push_back(vtor);
            }
            auto offset = vtor_loc - bin_start;
            uint32_t ep;
            memcpy(&ep, bin.data() + offset + 4, sizeof(ep));
            uint32_t sp;
            memcpy(&sp, bin.data() + offset, sizeof(sp));
            DEBUG_LOG("Adding entry_point_item: ep %08x, sp %08x\n", ep, sp);
            entry_point = std::make_shared<entry_point_item>(ep, sp);
            new_block.items.push_back(entry_point);
        }
    }

    auto sig_data = hash_andor_sign(
        bin, bin_start, bin_start,
        &new_block, public_key, private_key,
        in.get_model(),
        settings.seal.hash, settings.seal.sign,
        settings.seal.clear_sram, settings.seal.pin_xip_sram
    );

    return sig_data;
}

bool encrypt_command::execute(device_map &devices) {
    bool isElf = false;
    bool isBin = false;

    bool keyFromFile = true;
    bool keyIsShare = false;
    bool ivFromFile = true;

    // Set settings.seal.clear_sram to opposite of settings.encrypt.no_clear_sram
    settings.seal.clear_sram = !settings.encrypt.no_clear_sram;

    aes_key_t aes_key;
    aes_key_share_t aes_key_share;
    std::vector<uint8_t> iv_salt;
    iv_salt.resize(16);

    if (get_file_type() == filetype::elf) {
        isElf = true;
    } else if (get_file_type() == filetype::bin) {
        if (settings.encrypt.embed) {
            fail(ERROR_ARGS, "Can only embed decrypting bootloader into ELFs");
        }
        isBin = true;
    } else {
        fail(ERROR_ARGS, "Can only sign ELFs or BINs");
    }

    if (get_file_type_idx(1) != get_file_type()) {
        fail(ERROR_ARGS, "Can only sign to same file type");
    }

    if (string_to_hex_array(settings.filenames[2], aes_key.bytes, sizeof(aes_key.bytes), "AES key")) {
        keyFromFile = false;
    } else if (get_file_type_idx(2) != filetype::bin) {
        fail(ERROR_ARGS, "Can only read AES key or AES key share from BIN file");
    }

    if (string_to_hex_array(settings.filenames[3], iv_salt.data(), iv_salt.size(), "IV OTP salt")) {
        ivFromFile = false;
    } else if (get_file_type_idx(3) != filetype::bin) {
        if (get_file_type_idx(3) == filetype::pem) {
            // picotool encrypt <=2.1.1 would take PEM key file in the location of the IV OTP salt
            fail(ERROR_ARGS, "This picotool version (%s) is not compatible with SDK versions <=2.1.1 - you must manually build & install picotool version 2.1.1 to use those SDK versions with encryption", PICOTOOL_VERSION);
        }
        fail(ERROR_ARGS, "Can only read IV OTP salt from BIN file");
    }

    if (settings.seal.sign && settings.filenames[4].empty()) {
        fail(ERROR_ARGS, "missing key file for signing after encryption");
    }

    if (!settings.filenames[4].empty() && get_file_type_idx(4) != filetype::pem) {
        fail(ERROR_ARGS, "Can only read pem keys");
    }

    if (keyFromFile) {
        auto aes_file = get_file_idx(ios::in|ios::binary, 2);
        aes_file->exceptions(std::iostream::failbit | std::iostream::badbit);
        aes_file->seekg(0, std::ios::end);
        auto aes_key_file_size = aes_file->tellg();
        if (aes_key_file_size == 32) {
            keyIsShare = false;
            aes_file->seekg(0, std::ios::beg);
            aes_file->read((char*)aes_key.bytes, sizeof(aes_key.bytes));
        } else if (aes_key_file_size == 128) {
            keyIsShare = true;
            aes_file->seekg(0, std::ios::beg);
            aes_file->read((char*)aes_key_share.bytes, sizeof(aes_key_share.bytes));
        } else {
            fail(ERROR_INCOMPATIBLE, "The AES key file must be a 128 byte key share, or a 32 byte key (the supplied file is %d bytes)", aes_key_file_size);
        }
    }

    int min_weight = 6;
    int max_weight = 10;
    int min_combined_weight = 28;
    int max_combined_weight = 36;

    if (!keyIsShare) {
        std::vector<int> num_attempts;
        // Generate a random key share from 256-bit key
        std::random_device rand{};
        assert(rand.max() - rand.min() >= 256);
        for(int i=0; i < 8; i++) {
            // Regenerate the share word until the hamming weights are close to each other
            bool pass = false;
            for (int attempt=0; attempt < 100000; attempt++) {
                for (int j=0; j < 12; j++) {
                    aes_key_share.bytes[i*16 + j] = rand();
                }
                aes_key_share.words[i*4 + 3] = aes_key.words[i]
                                            ^ aes_key_share.words[i*4]
                                            ^ aes_key_share.words[i*4 + 1]
                                            ^ aes_key_share.words[i*4 + 2];

                pass = true;
                for (int half=0; half < 2; half++) {
                    uint8_t combined_weight = 0;
                    for (int j=0; j < 4; j++) {
                        uint16_t half_word = aes_key_share.words[i*4 + j] >> half*16;
                        uint8_t weight = __builtin_popcount(half_word);
                        if (weight < min_weight || weight > max_weight) {
                            DEBUG_LOG("Generated share %d word %d half %d has hamming weights out of range %d -> %d - regenerating attempt %d\n", j, i, half, min_weight, max_weight, attempt);
                            pass = false;
                            break;
                        }
                        combined_weight += weight;
                    }
                    if (!pass) break;
                    if (combined_weight < min_combined_weight || combined_weight > max_combined_weight) {
                        DEBUG_LOG("Generated share word %d half %d has hamming weights out of range %d -> %d - regenerating attempt %d\n", i, half, min_combined_weight, max_combined_weight, attempt);
                        pass = false;
                        break;
                    }
                }
                if (pass) {
                    num_attempts.push_back(attempt);
                    break;
                }
            }
            if (!pass) {
                fail(ERROR_INCOMPATIBLE, "Failed to generate a share word with hamming weights within %d -> %d", min_weight, max_weight);
            }
        }

        DEBUG_LOG("Average number of attempts: %d\n", std::accumulate(num_attempts.begin(), num_attempts.end(), 0) / num_attempts.size());
        DEBUG_LOG("Max number of attempts: %d\n", *std::max_element(num_attempts.begin(), num_attempts.end()));
        DEBUG_LOG("Min number of attempts: %d\n", *std::min_element(num_attempts.begin(), num_attempts.end()));
    } else {
        // Check the share word hamming weights are close to each other
        for(int i=0; i < 8; i++) {
            for (int half=0; half < 2; half++) {
                uint8_t combined_weight = 0;
                for (int j=0; j < 4; j++) {
                    uint16_t half_word = aes_key_share.words[i*4 + j] >> half*16;
                    uint8_t weight = __builtin_popcount(half_word);
                    if (weight < min_weight || weight > max_weight) {
                        std::cout << "WARNING: Key Share " << j << " Word " << i << " half " << half << " has hamming weights out of range " << min_weight << " -> " << max_weight << " - this may leak information about the key\n";
                    }
                    combined_weight += weight;
                }
                if (combined_weight < min_combined_weight || combined_weight > max_combined_weight) {
                    std::cout << "WARNING: Key Share Word " << i << " half " << half << " has hamming weights out of range " << min_combined_weight << " -> " << max_combined_weight << " - this may leak information about the key\n";
                }
            }
        }
    }

    // Key is stored as a 4-way share of each word, ie X[0] = A[0] ^ B[0] ^ C[0] ^ D[0], stored as A[0], B[0], C[0], D[0]
    for (int i=0; i < count_of(aes_key.words); i++) {
        aes_key.words[i] = aes_key_share.words[i*4]
                         ^ aes_key_share.words[i*4 + 1]
                         ^ aes_key_share.words[i*4 + 2]
                         ^ aes_key_share.words[i*4 + 3];
    }

    private_t private_key = {};
    public_t public_key = {};

    if (settings.seal.sign) read_keys(settings.filenames[4], &public_key, &private_key);

    // Read IV Salt
    if (ivFromFile) {
        auto iv_salt_file = get_file_idx(ios::in|ios::binary, 3);
        iv_salt_file->exceptions(std::iostream::failbit | std::iostream::badbit);
        iv_salt_file->seekg(0, std::ios::end);
        if (iv_salt_file->tellg() != 16) {
            fail(ERROR_INCOMPATIBLE, "The IV OTP salt must be a 16 byte file (the supplied file is %d bytes)", iv_salt_file->tellg());
        }
        iv_salt_file->seekg(0, std::ios::beg);
        iv_salt_file->read((char*)iv_salt.data(), iv_salt.size());
    }

    model_t model = get_model(0);

    if (isElf) {
        elf_file source_file(settings.verbose);
        elf_file *elf = &source_file;
        elf->read_file(get_file(ios::in|ios::binary));
        // Remove any holes in the ELF file, as these cause issues when encrypting
        elf->remove_ph_holes();
        elf->remove_sh_holes();

        std::unique_ptr<block> first_block = find_first_block(elf);
        if (!first_block) {
            fail(ERROR_FORMAT, "No first block found");
        }
        elf->editable = false;
        block new_block = place_new_block(elf, first_block, model);
        elf->editable = true;

        // Delete any non-generic load_map entries, as they will be invalid after encryption
        remove_non_generic_load_map_entries(&new_block, model);

        if (settings.encrypt.embed) {
            // Detect generic load maps from the encrypted binary, and populate options to propogate them
            std::shared_ptr<load_map_item> load_map = new_block.get_item<load_map_item>();
            if (load_map != nullptr) {
                detect_generic_load_map(load_map, model, settings.seal.pin_xip_sram);
            }

            std::vector<uint8_t> iv_data;
            std::vector<uint8_t> enc_data;
            uint32_t data_start_address = SRAM_START;
            encrypt_guts(elf, &new_block, aes_key, model, iv_data, enc_data);

            // Salt IV
            assert(iv_data.size() == iv_salt.size());
            for (int i=0; i < iv_data.size(); i++) {
                iv_data[i] ^= iv_salt[i];
            }
            auto tmp = std::make_shared<std::stringstream>();
            auto file = get_enc_bootloader(settings.encrypt.use_mbedtls);
            *tmp << file->rdbuf();

            auto program = get_iostream_memory_access<iostream_memory_access>(tmp, filetype::elf, true);
            // todo should be determined from image_def
            program.set_model(model);

            // data_start_addr
            settings.config.key = "data_start_addr";
            settings.config.value = hex_string(data_start_address);
            config_guts(program);
            // data_size
            settings.config.key = "data_size";
            settings.config.value = hex_string(enc_data.size());
            config_guts(program);
            // iv
            {
                string s((char*)iv_data.data(), iv_data.size());
                settings.config.key = "iv";
                settings.config.value = s;
                config_guts(program);
            }
            // otp_key_page
            if (settings.encrypt.otp_key_page_set) {
                settings.config.key = "otp_key_page";
                settings.config.value = hex_string(settings.encrypt.otp_key_page);
                config_guts(program);
            }

            // fast rosc
            if (settings.encrypt.fast_rosc) {
                settings.config.key = "rosc_div";
                settings.config.value = "0x1";
                config_guts(program);
                settings.config.key = "rosc_drive";
                settings.config.value = "0x0000";
                config_guts(program);
            }

            elf_file source_file(settings.verbose);
            elf_file *enc_elf = &source_file;
            enc_elf->read_file(tmp);

            // Bootloader size
            auto bootloader_start = enc_elf->get_symbol("__enc_bootloader_start");
            auto bootloader_end = enc_elf->get_symbol("__enc_bootloader_end");
            uint32_t bootloader_size = bootloader_end - bootloader_start;

            // Move bootloader down in physical space to start of SRAM (which will be start of flash once packaged)
            enc_elf->move_all(data_start_address - bootloader_start);

            // Add encrypted blob
            enc_elf->append_segment(data_start_address, data_start_address + bootloader_size, enc_data.size(), ".enc_data");
            auto data_section = enc_elf->get_section(".enc_data");
            assert(data_section);
            assert(data_section->virtual_address() == data_start_address);

            if (data_section->size < enc_data.size()) {
                fail(ERROR_UNKNOWN, "Block is too big for elf section\n");
            }

            DEBUG_LOG("Adding enc_data len %d\n", (int)enc_data.size());
            for (auto x : enc_data) DEBUG_LOG("%02x", x);
            DEBUG_LOG("\n");

            enc_elf->content(*data_section, enc_data);

            // Get the version from the encrypted binary
            std::shared_ptr<version_item> version = new_block.get_item<version_item>();
            if (version != nullptr) {
                settings.seal.major_version = version->major;
                settings.seal.minor_version = version->minor;
                settings.seal.rollback_version = version->rollback;
                for (auto row : version->otp_rows) {
                    settings.seal.rollback_rows.push_back(row);
                }
            }

            // Get the TBYB from the encrypted binary
            std::shared_ptr<image_type_item> image_type = new_block.get_item<image_type_item>();
            if (image_type->tbyb()) {
                settings.seal.set_tbyb = true;
            }

            // Sign the final thing
            sign_guts_elf(enc_elf, private_key, public_key, model);

            auto out = get_file_idx(ios::out|ios::binary, 1);
            enc_elf->write(out);
            out->close();
        } else {
            encrypt(elf, &new_block, aes_key, public_key, private_key, model, iv_salt, settings.seal.hash, settings.seal.sign);
            auto out = get_file_idx(ios::out|ios::binary, 1);
            elf->write(out);
            out->close();
        }
    } else if (isBin) {
        auto binfile = get_file_memory_access(0);
        auto rmap = binfile.get_rmap();
        auto ranges = rmap.ranges();
        assert(ranges.size() == 1);
        auto bin_start = ranges[0].from;
        auto bin_size = ranges[0].len();

        vector<uint8_t> bin = binfile.read_vector<uint8_t>(bin_start, bin_size, false);

        std::unique_ptr<block> first_block = find_first_block(bin, bin_start);
        if (!first_block) {
            fail(ERROR_FORMAT, "No first block found");
        }
        auto bin_cp = bin;
        block new_block = place_new_block(bin_cp, bin_start, first_block, model);

        // Delete any non-generic load_map entries, as they will be invalid after encryption
        remove_non_generic_load_map_entries(&new_block, model);

        auto enc_data = encrypt(bin, bin_start, bin_start, &new_block, aes_key, public_key, private_key, binfile.get_model(), iv_salt, settings.seal.hash, settings.seal.sign);

        auto out = get_file_idx(ios::out|ios::binary, 1);
        out->write((const char *)enc_data.data(), enc_data.size());
        out->close();
    } else {
        fail(ERROR_ARGS, "Must be ELF or BIN");
    }

    if (!settings.filenames[5].empty()) {
        if (get_file_type_idx(5) != filetype::json) {
            fail(ERROR_ARGS, "Can only output OTP json");
        }
        auto check_json_file = std::ifstream(settings.filenames[5]);
        json otp_json;
        if (check_json_file.good()) {
            otp_json = json::parse(check_json_file);
            DEBUG_LOG("Appending to existing otp json\n");
            check_json_file.close();
        }
        auto json_out = get_file_idx(ios::out, 5);

    #define FIB_WORKAROUND 1
    #if FIB_WORKAROUND
        // Make inverse pages to work around OTP FIB attack
        vector<uint8_t> page0_data;
        page0_data.resize(64);
        vector<uint8_t> page1_data;
        page1_data.resize(64);
        vector<uint8_t> page2_data;
        page2_data.resize(iv_salt.size());

        // Inverse pages need to be raw, to invert the ECC bits too
        vector<uint8_t> page0_inverse;
        page0_inverse.resize(page0_data.size()*2);
        vector<uint8_t> page1_inverse;
        page1_inverse.resize(page1_data.size()*2);
        vector<uint8_t> page2_inverse;
        page2_inverse.resize(page2_data.size()*2);

        memcpy(page0_data.data(), aes_key_share.bytes, 64);
        memcpy(page1_data.data(), aes_key_share.bytes + 64, 64);
        memcpy(page2_data.data(), iv_salt.data(), iv_salt.size());

        // The bits in rows 32-63 must be the inverse of the bits in rows 0-31
        for (int i = 0; i < page0_data.size(); i += 2) {
            page0_inverse[i*2] = ~page0_data[i];
            page0_inverse[i*2+1] = ~page0_data[i+1];
            page0_inverse[i*2+2] = ~otp_calculate_ecc(*(uint16_t*)&page0_data[i]) >> 16;
        }
        for (int i = 0; i < page1_data.size(); i += 2) {
            page1_inverse[i*2] = ~page1_data[i];
            page1_inverse[i*2+1] = ~page1_data[i+1];
            page1_inverse[i*2+2] = ~otp_calculate_ecc(*(uint16_t*)&page1_data[i]) >> 16;
        }
        for (int i = 0; i < page2_data.size(); i += 2) {
            page2_inverse[i*2] = ~page2_data[i];
            page2_inverse[i*2+1] = ~page2_data[i+1];
            page2_inverse[i*2+2] = ~otp_calculate_ecc(*(uint16_t*)&page2_data[i]) >> 16;
        }

        // Add otp AES key pages
        for (int i = 0; i < page0_data.size(); i++) {
            std::stringstream ss;
            ss << settings.encrypt.otp_key_page << ":0";
            otp_json[ss.str()]["ecc"] = true;
            otp_json[ss.str()]["value"][i] = page0_data[i];
        }
        for (int i = 0; i < page1_data.size(); i++) {
            std::stringstream ss;
            ss << settings.encrypt.otp_key_page + 1 << ":0";
            otp_json[ss.str()]["ecc"] = true;
            otp_json[ss.str()]["value"][i] = page1_data[i];
        }

        // Add otp IV salt page
        for (int i = 0; i < page2_data.size(); i++) {
            std::stringstream ss;
            ss << settings.encrypt.otp_key_page + 2 << ":0";
            otp_json[ss.str()]["ecc"] = true;
            otp_json[ss.str()]["value"][i] = page2_data[i];
        }

        // Add inverse pages
        for (int i = 0; i < page0_inverse.size(); i++) {
            std::stringstream ss;
            ss << settings.encrypt.otp_key_page << ":32";
            otp_json[ss.str()]["ecc"] = false;
            otp_json[ss.str()]["value"][i] = page0_inverse[i];
        }
        for (int i = 0; i < page1_inverse.size(); i++) {
            std::stringstream ss;
            ss << settings.encrypt.otp_key_page + 1 << ":32";
            otp_json[ss.str()]["ecc"] = false;
            otp_json[ss.str()]["value"][i] = page1_inverse[i];
        }
        for (int i = 0; i < page2_inverse.size(); i++) {
            std::stringstream ss;
            ss << settings.encrypt.otp_key_page + 2 << ":32";
            otp_json[ss.str()]["ecc"] = false;
            otp_json[ss.str()]["value"][i] = page2_inverse[i];
        }
    #else
        // Add otp AES key page
        for (int i = 0; i < 128; ++i) {
            std::stringstream ss;
            ss << settings.encrypt.otp_key_page << ":0";
            otp_json[ss.str()]["ecc"] = true;
            otp_json[ss.str()]["value"][i] = aes_key_share.bytes[i];
        }

        // Add otp IV salt page
        for (int i = 0; i < iv_salt.size(); ++i) {
            std::stringstream ss;
            ss << settings.encrypt.otp_key_page + 1 << ":0";
            otp_json[ss.str()]["ecc"] = true;
            otp_json[ss.str()]["value"][i] = iv_salt[i];
        }
    #endif

        // Add page locks to prevent BL and NS access, and only allow S reads
        {
            std::stringstream ss;
            ss << "PAGE" << settings.encrypt.otp_key_page << "_LOCK1";
            otp_json[ss.str()] = "0x3d3d3d";
            ss.str(string());
            ss << "PAGE" << settings.encrypt.otp_key_page + 1 << "_LOCK1";
            otp_json[ss.str()] = "0x3d3d3d";
            ss.str(string());
            ss << "PAGE" << settings.encrypt.otp_key_page + 2 << "_LOCK1";
            otp_json[ss.str()] = "0x3d3d3d";
        }

        *json_out << std::setw(4) << otp_json << std::endl;
        json_out->close();
    }

    return false;
}

bool seal_command::execute(device_map &devices) {
    bool isElf = false;
    bool isBin = false;
    bool isUf2 = false;
    if (get_file_type() == filetype::elf) {
        isElf = true;
    } else if (get_file_type() == filetype::bin) {
        isBin = true;
    } else if (get_file_type() == filetype::uf2) {
        isUf2 = true;
    } else {
        fail(ERROR_ARGS, "Can only sign ELFs, BINs or UF2s");
    }

    if (get_file_type_idx(1) != get_file_type()) {
        fail(ERROR_ARGS, "Can only sign to same file type");
    }

    if (settings.seal.sign && settings.filenames[2].empty()) {
        fail(ERROR_ARGS, "missing key file for signing");
    }

    if (!settings.filenames[2].empty() && get_file_type_idx(2) != filetype::pem) {
        fail(ERROR_ARGS, "Can only read pem keys");
    }

    if (settings.seal.rollback_version) {
        bool defaulted = false;
        if (!settings.seal.rollback_rows.size()) {
            settings.seal.rollback_rows.push_back(OTP_DATA_DEFAULT_BOOT_VERSION0_ROW);
            settings.seal.rollback_rows.push_back(OTP_DATA_DEFAULT_BOOT_VERSION1_ROW);
            defaulted = true;
        }
        int num_rows = settings.seal.rollback_rows.size();
        if (num_rows < (settings.seal.rollback_version / 24) + 1) {
            fail(
                ERROR_ARGS, "Rollback version %d requires %d rows - only %d %s",
                settings.seal.rollback_version, (settings.seal.rollback_version / 24) + 1,
                num_rows, defaulted ? "set by default" : "specified"
            );
        }
        std::sort(settings.seal.rollback_rows.begin(), settings.seal.rollback_rows.end());
        for (int i=0; i < num_rows - 1; i++) {
            if (settings.seal.rollback_rows[i+1] < settings.seal.rollback_rows[i] + 3) {
                fail(
                    ERROR_ARGS, "Rollback rows are RBIT3, so must be three rows apart - %x and %x are too close",
                    settings.seal.rollback_rows[i], settings.seal.rollback_rows[i+1]
                );
            }
        }
    }


    private_t private_key = {};
    public_t public_key = {};

    if (settings.seal.sign) read_keys(settings.filenames[2], &public_key, &private_key);

    model_t model = get_model(0);

    if (isElf) {
        elf_file source_file(settings.verbose);
        elf_file *elf = &source_file;
        elf->read_file(get_file(ios::in|ios::binary));
        if (!settings.seal.no_squash) {
            // Segments containing a block that is already part of a metadata block loop must not be
            // moved, as other blocks in the loop link to them by absolute physical address
            std::vector<uint32_t> pinned_addresses;
            std::unique_ptr<block> first_block = find_first_block(elf);
            if (first_block) {
                pinned_addresses.push_back(first_block->physical_addr);
                if (first_block->next_block_rel) {
                    for (auto &blk : get_all_blocks(elf, first_block)) {
                        pinned_addresses.push_back(blk->physical_addr);
                    }
                }
            }
            // Squash the segments together
            elf->store_squashed(model, pinned_addresses);
        }
        // Remove any holes in the ELF file, as these cause issues when signing/hashing
        elf->remove_sh_holes();
        sign_guts_elf(elf, private_key, public_key, model);

        auto out = get_file_idx(ios::out|ios::binary, 1);
        elf->write(out);
        out->close();
    } else if (isBin) {
        auto access = get_file_memory_access(0);
        auto rmap = access.get_rmap();
        auto ranges = rmap.ranges();
        assert(ranges.size() == 1);
        auto bin_start = ranges[0].from;
        auto bin_size = ranges[0].len();

        auto sig_data = sign_guts_bin(access, private_key, public_key, bin_start, bin_size, model);
        auto out = get_file_idx(ios::out|ios::binary, 1);
        out->write((const char *)sig_data.data(), sig_data.size());
        out->close();
    } else if (isUf2) {
        auto access = get_file_memory_access(0);
        auto rmap = access.get_rmap();
        auto ranges = rmap.ranges();
        auto bin_start = ranges.front().from;
        auto bin_size = ranges.back().to - bin_start;
        auto family_id = get_family_id(0);

        auto sig_data = sign_guts_bin(access, private_key, public_key, bin_start, bin_size, model);
        auto tmp = std::make_shared<std::stringstream>();
        tmp->write(reinterpret_cast<const char*>(sig_data.data()), sig_data.size());
        auto out = get_file_idx(ios::out|ios::binary, 1);
        bin2uf2(tmp, out, bin_start, family_id, access.get_model(), settings.uf2.abs_block_loc);
        out->close();
    } else {
        fail(ERROR_ARGS, "Must be ELF or BIN");
    }

    if (settings.seal.sign) {
        message_digest_t pub_sha256;
        sha256_buffer(public_key.bytes, sizeof(public_key.bytes), &pub_sha256);
        DEBUG_LOG("PUBLIC KEY SHA256 ");
        for(uint8_t i : pub_sha256.bytes) {
            DEBUG_LOG("%02x", i);
        }
        DEBUG_LOG("\n");

        if (!settings.filenames[3].empty()) {
            if (get_file_type_idx(3) != filetype::json) {
                fail(ERROR_ARGS, "Can only output OTP json");
            }
            auto check_json_file = std::ifstream(settings.filenames[3]);
            json otp_json;
            if (check_json_file.good()) {
                otp_json = json::parse(check_json_file);
                DEBUG_LOG("Appending to existing otp json\n");
                check_json_file.close();
            }
            auto json_out = get_file_idx(ios::out, 3);

            // Add otp bootkey rows
            for (int i = 0; i < 32; ++i) {
                otp_json["bootkey0"][i] = pub_sha256.bytes[i];
            }

            // Add otp fields to enable secure boot
            otp_json["crit1"]["secure_boot_enable"] = 1;
            otp_json["boot_flags1"]["key_valid"] = 1;

            *json_out << std::setw(4) << otp_json << std::endl;
            json_out->close();
        }
    }

    if (!settings.quiet) {
        auto access = get_file_memory_access(1);
        set_model_from_metadata(access);
        fos << "Output File " << settings.filenames[1] << ":\n\n";
        settings.info.show_basic = true;
        info_guts(access, nullptr);
    }

    return false;
}
#endif

bool link_command::execute(device_map &devices) {
    if (get_file_type() != filetype::bin) {
        fail(ERROR_ARGS, "Can only link to BINs");
    }

    if (__builtin_popcount(settings.link.align) != 1) {
        fail(ERROR_ARGS, "Can only pad to powers of 2");
    }

    vector<uint8_t> output;
    vector<std::unique_ptr<block>> first_blocks;
    for (size_t i=1; i < settings.filenames.size(); i++) {
        if (settings.filenames[i].empty()) break;
        if (get_file_type_idx(i) != filetype::bin) {
            fail(ERROR_ARGS, "Can only link BINs");
        }

        auto access = get_file_memory_access(i);
        auto rmap = access.get_rmap();
        auto ranges = rmap.ranges();
        assert(ranges.size() == 1);
        auto bin_start = ranges[0].from;
        auto bin_size = ranges[0].len();

#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfree-nonheap-object" // false GCC 15 warning
#endif
        vector<uint8_t> bin = access.read_vector<uint8_t>(bin_start, bin_size, false);
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif

        std::unique_ptr<block> first_block = find_first_block(bin, bin_start);
        if (!first_block) {
            fail(ERROR_FORMAT, "No first block found");
        }

        first_blocks.push_back(std::move(first_block));
    }

    for (size_t i=0; i < first_blocks.size(); i++) {
        auto access = get_file_memory_access(i+1);
        auto rmap = access.get_rmap();
        auto ranges = rmap.ranges();
        assert(ranges.size() == 1);
        auto bin_start = ranges[0].from;
        auto bin_size = ranges[0].len();

        vector<uint8_t> bin = access.read_vector<uint8_t>(bin_start, bin_size, false);

        std::unique_ptr<block> first_block = find_first_block(bin, bin_start);
        if (!first_block) {
            fail(ERROR_FORMAT, "No first block found");
        }

        auto last_block = get_last_block(bin, bin_start, first_block);
        if (last_block == nullptr || last_block->get_item<image_type_item>() == nullptr) {
            // Use first block instead of last block, as last block doesn't have an image_def
            first_block.swap(last_block);
            fos_verbose << "Using first block, as last block has no image_def\n";
        }

        // Use last block items in new block
        block new_block = place_new_block(bin, bin_start, first_block, models::largest);
        new_block.items.clear();
        std::copy(last_block->items.begin(),
            last_block->items.end(),
            std::back_inserter(new_block.items));

        if (output.size() > 0) {
            // Add rwd to block, if required
            fos_verbose << "Adding rwd, as output is size " << hex_string(output.size()) << "\n";
            std::shared_ptr<rolling_window_delta_item> rwd = std::make_shared<rolling_window_delta_item>(output.size());
            new_block.items.push_back(rwd);

            if (last_block->get_item<image_type_item>()->cpu() == cpu_arm && last_block->get_item<vector_table_item>() == nullptr) {
                // Add vtor too
                fos_verbose << "Adding vtor too\n";
                std::shared_ptr<vector_table_item> vtor = std::make_shared<vector_table_item>(bin_start);
                new_block.items.push_back(vtor);
            }
        }

        // Link new block to next first block
        if (i+1 != first_blocks.size()) {
            auto next_first_block_rel = (first_blocks[i+1]->physical_addr - bin_start) + (settings.link.align - bin.size() % settings.link.align);
            new_block.next_block_rel = next_first_block_rel;
        } else {
            auto next_first_block_rel = (first_blocks[0]->physical_addr - bin_start) - (output.size() + bin.size());
            new_block.next_block_rel = next_first_block_rel;
        }

        // Add new block to bin
        fos_verbose << "Size before block: " << hex_string(bin.size()) << "\n";
        auto tmp = new_block.to_words();
        std::vector<uint8_t> data = words_to_lsb_bytes(tmp.begin(), tmp.end());
        bin.insert(bin.end(), data.begin(), data.end());
        fos_verbose << "Size after block: " << hex_string(bin.size()) << "\n";

        // Pad 0s in between binaries
        fos_verbose << "Size before padding: " << hex_string(bin.size()) << "\n";
        bin.resize((bin.size() + settings.link.align -1) & ~(settings.link.align -1), 0);
        fos_verbose << "Size after padding: " << hex_string(bin.size()) << "\n";

        // Copy into output
        std::copy(bin.begin(),
              bin.end(),
              std::back_inserter(output));
    }

    auto out = get_file_idx(ios::out|ios::binary, 0);
    out->write((const char *)output.data(), output.size());
    out->close();

    return false;
}

#if HAS_LIBUSB
struct _bdevfs_setup {
    picoboot::connection *connection;
    std::shared_ptr<picoboot_memory_access> access;
    uint32_t base_addr;
    uint32_t size;
    bool writeable = true;
    bool formattable = true;
};
_bdevfs_setup bdevfs_setup;

#define setup_bdevfs(con) bdevfs_setup.access = std::make_shared<picoboot_memory_access>(con); bdevfs_setup.connection = &con; setup_bdevfs_internal()

void setup_bdevfs_internal() {
    auto raw_access = *bdevfs_setup.access;

    auto partitions = get_partitions(*bdevfs_setup.connection);

    bool block_device_found = false;

    if (partitions) {
        auto chosen_partition = (*partitions)[0];
        if (settings.bdev.partition_number >= 0) { // use specified partition number
            if (!partitions) {
                fail(ERROR_NOT_POSSIBLE, "There is no partition table on the device");
            }
            if (settings.bdev.partition_number >= partitions->size()) {
                fail(ERROR_NOT_POSSIBLE, "There are only %d partitions on the device", partitions->size());
            }
            chosen_partition = (*partitions)[settings.bdev.partition_number];
            block_device_found = true;
        } else { // name and ID search
            for (auto &partition : *partitions) {
                if (settings.bdev.partition_name.empty()) { // no partition name specified
                    if (!partition.has_id || partition.id != settings.bdev.partition_id) { // match partition ID, which defaults to BLOCK_DEVICE_DEFAULT_PARTITION_ID
                        continue;
                    }
                } else { // partition name specified
                    if (!partition.has_name || partition.name != settings.bdev.partition_name) { // match partition name
                        continue;
                    }
                }
                chosen_partition = partition;
                block_device_found = true;
                break;
            }
        }

        if (block_device_found) {
            bdevfs_setup.base_addr = chosen_partition.start + FLASH_START;
            bdevfs_setup.size = chosen_partition.end - chosen_partition.start;
            bdevfs_setup.writeable = chosen_partition.flags_and_permissions & PICOBIN_PARTITION_PERMISSION_NSBOOT_W_BITS;
        } else if (!settings.bdev.partition_name.empty()) {
            fail(ERROR_NOT_POSSIBLE, "There is no partition with name %s on the device", settings.bdev.partition_name.c_str());
        } else if (settings.bdev.partition_id != BLOCK_DEVICE_DEFAULT_PARTITION_ID) {
            fail(ERROR_NOT_POSSIBLE, "There is no partition with ID %016" PRIx64 " on the device", settings.bdev.partition_id);
        }
    } else if (
        settings.bdev.partition_number >= 0 ||
        !settings.bdev.partition_name.empty() ||
        settings.bdev.partition_id != BLOCK_DEVICE_DEFAULT_PARTITION_ID
    ) {
        // Partition table arguments were specified, but none was present
        fail(ERROR_NOT_POSSIBLE, "There is no partition table on the device");
    }
    
    if (!block_device_found) { // use binary info
        binary_info_header hdr;
        auto bi_access = get_bi_access(raw_access);
        bool has_binary_info = find_binary_info(*bi_access, hdr);
        if (has_binary_info) {
            auto access = remapped_memory_access(*bi_access, hdr.reverse_copy_mapping);
            auto visitor = bi_visitor{};
            bool block_device_found = false;
            visitor.block_device([&](memory_access &access, binary_info_block_device_t &bi_bdev) {
                block_device_found = true;
                std::stringstream ss;
                ss << hex_string(bi_bdev.address) << "-" << hex_string(bi_bdev.address + bi_bdev.size) <<
                    " (" << ((bi_bdev.size + 1023) / 1024) << "K): " << read_string(access, bi_bdev.name);
                if (bi_bdev.flags) {
                    ss << " flags " << hex_string(bi_bdev.flags, 4) << " ";
                    if (bi_bdev.flags & BINARY_INFO_BLOCK_DEV_FLAG_READ) ss << "r";
                    if (bi_bdev.flags & BINARY_INFO_BLOCK_DEV_FLAG_WRITE) ss << "w";
                    if (bi_bdev.flags & BINARY_INFO_BLOCK_DEV_FLAG_REFORMAT) ss << "f";
                    bdevfs_setup.writeable = bi_bdev.flags & BINARY_INFO_BLOCK_DEV_FLAG_WRITE;
                    bdevfs_setup.formattable = bi_bdev.flags & BINARY_INFO_BLOCK_DEV_FLAG_REFORMAT;
                }
                string s = ss.str();
                fos << "embedded drive: " << s << "\n";

                if (bi_bdev.address < FLASH_START) {
                    // Some devices have the block device address relative to the start of flash
                    bdevfs_setup.base_addr = bi_bdev.address + FLASH_START;
                } else {
                    bdevfs_setup.base_addr = bi_bdev.address;
                }
                bdevfs_setup.size = bi_bdev.size;
            });
            visitor.visit(access, hdr);
            if (!block_device_found) {
                fail(ERROR_NOT_POSSIBLE, "No block device found on device");
            }
        } else {
            fail(ERROR_NOT_POSSIBLE, "No binary info found on device");
        }
    }

    if (settings.bdev.force_formattable) {
        bdevfs_setup.formattable = true;
    }

    if (settings.bdev.force_writeable) {
        bdevfs_setup.writeable = true;
    }

    if (settings.bdev.fs == fs_detect) {
        // Auto-detect filesystem

        // FatFS
        FATFS fatfs;
        int res = f_mount(&fatfs);
        if (res == FR_OK) {
            // FatFS Found
            fos << "FatFS Filesystem Detected\n";
            settings.bdev.fs = fs_fatfs;
            return;
        }

        // LittleFS
        char littlefs_str[] = "littlefs";
        memset(littlefs_str, 0, sizeof(littlefs_str));
        raw_access.read(bdevfs_setup.base_addr + 8, (uint8_t*)littlefs_str, sizeof(littlefs_str)-1, false);
        res = strcmp(littlefs_str, "littlefs");
        if (res == 0) {
            // LittleFS Found
            fos << "LittleFS Filesystem Detected\n";
            settings.bdev.fs = fs_littlefs;
            return;
        }

        // No FS Found
        if (settings.bdev.format) {
            fail(ERROR_CONNECTION, "No file system detected - must pass `--filesystem <littlefs|fatfs>` to specify which filesystem to format");
        } else {
            fail(ERROR_CONNECTION, "No file system detected - to format the drive use `--format --filesystem <littlefs|fatfs>`");
        }
        
    }
}

// FatFS Functions
string fatfs_err_str(int err) {
    switch (err) {
        case FR_OK: return "Succeeded";
        case FR_DISK_ERR: return "A hard error occurred in the low level disk I/O layer";
        case FR_INT_ERR: return "Assertion failed";
        case FR_NOT_READY: return "The physical drive cannot work";
        case FR_NO_FILE: return "Could not find the file";
        case FR_NO_PATH: return "Could not find the path";
        case FR_INVALID_NAME: return "The path name format is invalid";
        case FR_DENIED: return "Access denied due to prohibited access or directory full";
        case FR_EXIST: return "Access denied due to prohibited access";
        case FR_INVALID_OBJECT: return "The file/directory object is invalid";
        case FR_WRITE_PROTECTED: return "The physical drive is write protected";
        case FR_INVALID_DRIVE: return "The logical drive number is invalid";
        case FR_NOT_ENABLED: return "The volume has no work area";
        case FR_NO_FILESYSTEM: return "There is no valid FAT volume";
        case FR_MKFS_ABORTED: return "The f_mkfs() aborted due to any problem";
        case FR_TIMEOUT: return "Could not get a grant to access the volume within defined period";
        case FR_LOCKED: return "The operation is rejected according to the file sharing policy";
        case FR_NOT_ENOUGH_CORE: return "LFN working buffer could not be allocated";
        case FR_TOO_MANY_OPEN_FILES: return "Number of open files > FF_FS_LOCK";
        case FR_INVALID_PARAMETER: return "Given parameter is invalid";
        default: return "Unknown error";
    }
}

DWORD get_fattime (void) {
    std::time_t t = std::time(0);
    std::tm* now = std::localtime(&t);
    // now gives year since 1900 and 0-11 month
    // fattime wants year since 1980 and 1-12 month
    DWORD fattime = ((now->tm_year - 80) << 25)
                    | ((now->tm_mon + 1) << 21)
                    | ((now->tm_mday) << 16)
                    | ((now->tm_hour) << 11)
                    | ((now->tm_min) << 5)
                    | (now->tm_sec);
    return fattime;
}

static_assert(FF_MAX_SS == FF_MIN_SS, "FF_MAX_SS must be equal to FF_MIN_SS");
#define SECTOR_SIZE FF_MAX_SS

DRESULT disk_read (void *drv, BYTE* buff, DWORD sector, UINT count) {
    if (sector >= bdevfs_setup.size / SECTOR_SIZE) {
        fail(ERROR_NOT_POSSIBLE, "Sector %d is out of range", sector);
        return RES_PARERR;
    }
    bdevfs_setup.access->read(bdevfs_setup.base_addr + (sector * SECTOR_SIZE), (uint8_t*)buff, count * SECTOR_SIZE, false);
    return RES_OK;
}

DRESULT disk_write (void *drv, const BYTE* buff, DWORD sector, UINT count) {
    if (sector >= bdevfs_setup.size / SECTOR_SIZE) {
        fail(ERROR_NOT_POSSIBLE, "Sector %d is out of range", sector);
        return RES_PARERR;
    }
    if (bdevfs_setup.writeable) {
        bdevfs_setup.access->write(bdevfs_setup.base_addr + (sector * SECTOR_SIZE), (uint8_t*)buff, count * SECTOR_SIZE);
        return RES_OK;
    } else {
        fail(ERROR_NOT_POSSIBLE, "This block device is not writeable");
        return RES_WRPRT;
    }
}

DRESULT disk_ioctl (void *drv, BYTE cmd, void* buff) {
    switch (cmd) {
        case CTRL_SYNC:
            return RES_OK;

        case GET_SECTOR_COUNT:
            *(DWORD*)buff = bdevfs_setup.size / SECTOR_SIZE;
            return RES_OK;

        case GET_SECTOR_SIZE:
            *(DWORD*)buff = SECTOR_SIZE;
            return RES_OK;

        case GET_BLOCK_SIZE:
            *(DWORD*)buff = FLASH_SECTOR_ERASE_SIZE / SECTOR_SIZE;
            return RES_OK;

        case IOCTL_INIT:
        case IOCTL_STATUS: {
            DSTATUS stat = 0;
            *((DSTATUS *)buff) = stat;
            return RES_OK;
        }

        case CTRL_TRIM: {
            if (bdevfs_setup.writeable) {
                DWORD* p = (DWORD*)buff;
                uint32_t start = (*p * SECTOR_SIZE) + bdevfs_setup.base_addr;
                uint32_t end = (*(p + 1) * SECTOR_SIZE) + bdevfs_setup.base_addr;
                // Only trim complete flash sectors
                if (start % FLASH_SECTOR_ERASE_SIZE) start += FLASH_SECTOR_ERASE_SIZE - (start % FLASH_SECTOR_ERASE_SIZE);
                end -= end % FLASH_SECTOR_ERASE_SIZE;
                // Check if start and end are in range
                if (start < bdevfs_setup.base_addr || end > bdevfs_setup.base_addr + bdevfs_setup.size) {
                    fail(ERROR_NOT_POSSIBLE, "Start or end is out of range");
                    return RES_PARERR;
                }
                for (uint32_t addr = start; addr < end; addr += FLASH_SECTOR_ERASE_SIZE) {
                    bdevfs_setup.connection->flash_erase(addr, FLASH_SECTOR_ERASE_SIZE);
                }
                return RES_OK;
            } else {
                fail(ERROR_NOT_POSSIBLE, "This block device is not writeable");
                return RES_WRPRT;
            }
        }

        default:
            fail(ERROR_NOT_POSSIBLE, "Unknown ioctl %d", cmd);
            return RES_PARERR;
    }
}

void fatfs_ls(FATFS *fatfs, const char *path, bool recursive=false) {
    int err;
    FF_DIR dir;
    err = f_opendir(fatfs, &dir, path);
    if (err) {
        fos << "FatFS Error: " << fatfs_err_str(err) << "\n";
    }

    FILINFO info;
    while (true) {
        int res = f_readdir(&dir, &info);
        if (res != 0) {
            fos << "FatFS Res: " << fatfs_err_str(res) << "\n";
            break;
        }

        if (info.fname[0] == 0) {
            break;
        }

        if (strcmp(info.fname, ".") == 0 || strcmp(info.fname, "..") == 0) {
            continue;
        }

        fos << info.fname;

        if (info.fattrib & AM_DIR) {
            fos << "/\n";
            if (recursive) {
                int fr_col = fos.first_column();
                fos.first_column(fr_col + 4);
                std::stringstream ss;
                ss << path << "/" << info.fname;
                fatfs_ls(fatfs, ss.str().c_str(), recursive);
                fos.first_column(fr_col);
            }
        } else {
            std::stringstream ss;
            int year = (info.fdate >> 9) & 0x7f;
            int mon = (info.fdate >> 5) & 0xf;
            int day = info.fdate & 0x1f;
            int hour = (info.ftime >> 11) & 0x1f;
            int min = (info.ftime >> 5) & 0x3f;
            int sec = info.ftime & 0x1f;
            ss << " " << day << "-" << mon << "-" << year + 1980;
            ss << " " << std::setfill('0') << std::setw(2) << hour;
            ss << ":" << std::setfill('0') << std::setw(2) << min;
            ss << ":" << std::setfill('0') << std::setw(2) << sec;
            ss << " " << info.fsize << "B\n";
            fos << ss.str();
        }
    }

    err = f_closedir(&dir);
    if (err) {
        fos << "FatFS Error: " << fatfs_err_str(err) << "\n";
    }
}

typedef std::function<void(FATFS* fatfs)> fatfs_op_fn;

void do_fatfs_op(fatfs_op_fn fatfs_op) {
    FATFS fatfs;

    // Enable auto-erase, as FatFS has no Flash Translation Layer
    bdevfs_setup.access->enable_ftl = true;

    int err = f_mount(&fatfs);
    if (err == FR_NO_FILESYSTEM || settings.bdev.always_format) {
        if (settings.bdev.format) {
            if (bdevfs_setup.formattable) {
                fos << "Formatting FatFS file system\n";
                uint8_t work_buf[SECTOR_SIZE];
                err = f_mkfs(&fatfs, FM_ANY | FM_SFD, 0, work_buf, sizeof(work_buf));
                if (err) {
                    fail(ERROR_CONNECTION, "FatFS Format Error: %s", fatfs_err_str(err).c_str());
                }
                err = f_mount(&fatfs);
                if (err) {
                    fail(ERROR_CONNECTION, "FatFS Format Failed with: %s", fatfs_err_str(err).c_str());
                }
            } else {
                fail(ERROR_NOT_POSSIBLE, "This block device is not formattable");
            }
        } else {
            fail(ERROR_CONNECTION, "FatFS file system is corrupted - add --format flag to format it (this may result in data loss)");
        }
    } else if (err) {
        fail(ERROR_CONNECTION, "FatFS Mount Error: %s", fatfs_err_str(err).c_str());
    }

    fatfs_op(&fatfs);
}

// LittleFS Functions
string lfs_err_str(int err) {
    switch (err) {
        case LFS_ERR_OK: return "No error";
        case LFS_ERR_IO: return "Error during device operation";
        case LFS_ERR_CORRUPT: return "Corrupted";
        case LFS_ERR_NOENT: return "No directory entry";
        case LFS_ERR_EXIST: return "Entry already exists";
        case LFS_ERR_NOTDIR: return "Entry is not a dir";
        case LFS_ERR_ISDIR: return "Entry is a dir";
        case LFS_ERR_NOTEMPTY: return "Dir is not empty";
        case LFS_ERR_BADF: return "Bad file number";
        case LFS_ERR_FBIG: return "File too large";
        case LFS_ERR_INVAL: return "Invalid parameter";
        case LFS_ERR_NOSPC: return "No space left on device";
        case LFS_ERR_NOMEM: return "No more memory available";
        case LFS_ERR_NOATTR: return "No data/attr available";
        case LFS_ERR_NAMETOOLONG: return "File name too long";
        default: return "Unknown error";
    }
}

int lfs_read(const struct lfs_config *c, lfs_block_t block, lfs_off_t off, void *buffer, lfs_size_t size) {
    if (block >= bdevfs_setup.size / c->block_size) {
        fail(ERROR_NOT_POSSIBLE, "Block %d is out of range", block);
        return LFS_ERR_INVAL;
    }
    bdevfs_setup.access->read(bdevfs_setup.base_addr + (block * c->block_size) + off, (uint8_t*)buffer, size, false);
    return LFS_ERR_OK;
}

int lfs_prog(const struct lfs_config *c, lfs_block_t block, lfs_off_t off, const void *buffer, lfs_size_t size) {
    if (block >= bdevfs_setup.size / c->block_size) {
        fail(ERROR_NOT_POSSIBLE, "Block %d is out of range", block);
        return LFS_ERR_INVAL;
    }
    if (bdevfs_setup.writeable) {
        bdevfs_setup.access->write(bdevfs_setup.base_addr + (block * c->block_size) + off, (uint8_t*)buffer, size);
        return LFS_ERR_OK;
    } else {
        fail(ERROR_NOT_POSSIBLE, "This block device is not writeable");
        return LFS_ERR_IO;
    }
};

int lfs_erase(const struct lfs_config *c, lfs_block_t block) {
    if (block >= bdevfs_setup.size / c->block_size) {
        fail(ERROR_NOT_POSSIBLE, "Block %d is out of range", block);
        return LFS_ERR_INVAL;
    }
    if (bdevfs_setup.writeable) {
        bdevfs_setup.connection->flash_erase(bdevfs_setup.base_addr + (block * c->block_size), c->block_size);
        return LFS_ERR_OK;
    } else {
        fail(ERROR_NOT_POSSIBLE, "This block device is not writeable");
        return LFS_ERR_IO;
    }
};

int lfs_sync(const struct lfs_config *c) {
    return LFS_ERR_OK;
};

void lfs_ls(lfs_t *lfs, const char *path, bool recursive=false) {
    int err;
    lfs_dir_t dir;
    err = lfs_dir_open(lfs, &dir, path);
    if (err) {
        fos << "LittleFS Error: " << lfs_err_str(err) << "\n";
    }

    struct lfs_info info;
    while (true) {
        int res = lfs_dir_read(lfs, &dir, &info);
        if (res < 0) {
            fos << "LittleFS Res: " << lfs_err_str(res) << "\n";
            break;
        }

        if (res == 0) {
            break;
        }

        if (strcmp(info.name, ".") == 0 || strcmp(info.name, "..") == 0) {
            continue;
        }

        fos << info.name;

        if (info.type == LFS_TYPE_DIR) {
            fos << "/\n";
            if (recursive) {
                int fr_col = fos.first_column();
                fos.first_column(fr_col + 4);
                std::stringstream ss;
                ss << path << "/" << info.name;
                lfs_ls(lfs, ss.str().c_str(), recursive);
                fos.first_column(fr_col);
            }
        } else if (info.type == LFS_TYPE_REG) {
            std::stringstream ss;
            ss << " " << info.size << "B\n";
            fos << ss.str();
        }
    }

    err = lfs_dir_close(lfs, &dir);
    if (err) {
        fos << "LittleFS Error: " << lfs_err_str(err) << "\n";
    }
}

typedef std::function<void(lfs_t *lfs)> lfs_op_fn;

void do_lfs_op(lfs_op_fn lfs_op) {
    lfs_t lfs;

    const struct lfs_config cfg = {
        // block device operations
        .read  = lfs_read,
        .prog  = lfs_prog,
        .erase = lfs_erase,
        .sync  = lfs_sync,

        // block device configuration
        .read_size = 16,
        .prog_size = PAGE_SIZE,
        .block_size = FLASH_SECTOR_ERASE_SIZE,
        .block_count = bdevfs_setup.size / FLASH_SECTOR_ERASE_SIZE, // 0x160 (352) on micropython
        .block_cycles = -1,
        .cache_size = PAGE_SIZE,
        .lookahead_size = 16,
    };

    int err = lfs_mount(&lfs, &cfg);
    if (err == LFS_ERR_CORRUPT || settings.bdev.always_format) {
        if (settings.bdev.format) {
            if (bdevfs_setup.formattable) {
                fos << "Formatting LittleFS file system\n";
                lfs_format(&lfs, &cfg);
                lfs_mount(&lfs, &cfg);
            } else {
                fail(ERROR_NOT_POSSIBLE, "This block device is not formattable");
            }
        } else {
            fail(ERROR_CONNECTION, "LittleFS file system is corrupted - add --format flag to format it (this may result in data loss)");
        }
    } else if (err) {
        fail(ERROR_CONNECTION, "LittleFS Mount Error: %s", lfs_err_str(err).c_str());
    }

    lfs_op(&lfs);
}

bool bdev_ls_command::execute(device_map &devices) {
    auto con = get_single_bootsel_device_connection(devices);
    setup_bdevfs(con);

    // Remove starting ':' if present, as ls only operates on the device
    if ((char)(settings.filenames[0].front()) == ':') settings.filenames[0].erase(0, 1);

    string dir = "/";
    if (settings.filenames[0].length() > 0) {
        if (settings.filenames[0].front() == '/') {
            dir = settings.filenames[0];
        } else {
            dir += settings.filenames[0];
        }
    }
    if ((char)(dir.back()) == '/' && dir.length() > 1) {
        // FatFS doesn't like trailing slashes except for root, and LittleFS requires / for root
        dir.pop_back();
    }

    fos << dir << (dir.back() == '/' ? "" : "/") << "\n";

    switch (settings.bdev.fs) {
        case fs_littlefs: {
            lfs_op_fn lfs_op = [&](lfs_t *lfs) {
                lfs_ls(lfs, dir.c_str(), settings.bdev.recursive);
            };
            do_lfs_op(lfs_op);
            break;
        }

        case fs_fatfs: {
            fatfs_op_fn fatfs_op = [&](FATFS *fatfs) {
                fatfs_ls(fatfs, dir.c_str(), settings.bdev.recursive);
            };
            do_fatfs_op(fatfs_op);
            break;
        }

        default:
            fail(ERROR_ARGS, "Unknown filesystem specified");
    }
    return false;
}

bool bdev_mkdir_command::execute(device_map &devices) {
    auto con = get_single_bootsel_device_connection(devices);
    setup_bdevfs(con);

    // Remove starting ':' if present, as mkdir only operates on the device
    if ((char)(settings.filenames[0].front()) == ':') settings.filenames[0].erase(0, 1);

    switch (settings.bdev.fs) {
        case fs_littlefs: {
            lfs_op_fn lfs_op = [&](lfs_t *lfs) {
                int err = lfs_mkdir(lfs, settings.filenames[0].c_str());
                if (err == LFS_ERR_EXIST) {
                    fos << "Directory already exists\n";
                } else if (err) {
                    fos << "LittleFS Error: " << lfs_err_str(err) << "\n";
                }
            };
            do_lfs_op(lfs_op);
            break;
        }

        case fs_fatfs: {
            fatfs_op_fn fatfs_op = [&](FATFS *fatfs) {
                int err = f_mkdir(fatfs, settings.filenames[0].c_str());
                if (err == FR_EXIST) {
                    fos << "Directory already exists\n";
                } else if (err) {
                    fos << "FatFS Error: " << fatfs_err_str(err) << "\n";
                }
            };
            do_fatfs_op(fatfs_op);
            break;
        }

        default:
            fail(ERROR_ARGS, "Unknown filesystem specified");
    }

    fos << "Created directory " << settings.filenames[0] << " on device\n";

    return false;
}

bool bdev_cp_command::execute(device_map &devices) {
    auto con = get_single_bootsel_device_connection(devices);
    setup_bdevfs(con);

    if ((char)(settings.filenames[1].back()) == '/') {
        int filenamestart = std::max(settings.filenames[0].find_last_of("/") + 1, settings.filenames[0].find_last_of(":") + 1);
        int filenamelen = settings.filenames[0].length() - filenamestart;
        settings.filenames[1] += settings.filenames[0].substr(filenamestart, filenamelen);
    }

    // Remote files start with ':', so detect and remove the ':'
    bool srcRemote = false;
    bool destRemote = false;
    if ((char)(settings.filenames[0].front()) == ':') {
        srcRemote = true;
        settings.filenames[0].erase(0, 1);
    }
    if ((char)(settings.filenames[1].front()) == ':') {
        destRemote = true;
        settings.filenames[1].erase(0, 1);
    }

    fos << "Copying " << (srcRemote ? "device " : "local ") << settings.filenames[0] << " to " << (destRemote ? "device " : "local ") << settings.filenames[1] << "\n";

    std::vector<char> data_buf;

    // Read local file
    if (!srcRemote) {
        auto infile = get_file_idx(ios::in|ios::binary|ios::ate, 0);
        auto size = infile->tellg();
        uint32_t max_size = bdevfs_setup.access->get_model()->flash_end() - bdevfs_setup.access->get_model()->flash_start();
        if (size > max_size) {
            fail(ERROR_NOT_POSSIBLE, "File size %dMB is too large for flash device (max %dMB)", size / (1024 * 1024), max_size / (1024 * 1024));
            return false;
        }
        infile->seekg(0, std::ios::beg);
        data_buf.resize(size);
        infile->read(data_buf.data(), size);
        infile->close();
    }

    switch (settings.bdev.fs) {
        case fs_littlefs: {
            lfs_op_fn lfs_op = [&](lfs_t *lfs) {
                if (srcRemote) {
                    lfs_file_t file;
                    int err = lfs_file_open(lfs, &file, settings.filenames[0].c_str(), LFS_O_RDONLY);
                    if (err) {
                        fail(ERROR_READ_FAILED, "LittleFS Open Error: %s", lfs_err_str(err).c_str());
                    }
                    auto size = lfs_file_size(lfs, &file);
                    err = lfs_file_rewind(lfs, &file);
                    data_buf.resize(size);
                    err = lfs_file_read(lfs, &file, data_buf.data(), data_buf.size());
                    if (err < 0) {
                        fail(ERROR_READ_FAILED, "LittleFS Read Error: %s", lfs_err_str(err).c_str());
                    } else if (err != data_buf.size()) {
                        fail(ERROR_READ_FAILED, "LittleFS Read too short - got %d bytes expected %d bytes", err, data_buf.size());
                    }
                    err = lfs_file_close(lfs, &file);
                }

                if (destRemote) {
                    lfs_file_t file;
                    int err = lfs_file_open(lfs, &file, settings.filenames[1].c_str(), LFS_O_WRONLY | LFS_O_CREAT);
                    if (err) {
                        fail(ERROR_WRITE_FAILED, "LittleFS Open Error: %s", lfs_err_str(err).c_str());
                    }
                    err = lfs_file_write(lfs, &file, data_buf.data(), data_buf.size());
                    if (err < 0) {
                        fail(ERROR_WRITE_FAILED, "LittleFS Write Error: %s", lfs_err_str(err).c_str());
                    } else if (err != data_buf.size()) {
                        fail(ERROR_WRITE_FAILED, "LittleFS Write too short - wrote %d bytes expected %d bytes", err, data_buf.size());
                    }
                    err = lfs_file_close(lfs, &file);
                }
            };
            do_lfs_op(lfs_op);
            break;
        };

        case fs_fatfs: {
            fatfs_op_fn fatfs_op = [&](FATFS* fatfs) {
                if (srcRemote) {
                    FIL file;
                    FRESULT err = f_open(fatfs, &file, settings.filenames[0].c_str(), FA_READ);
                    if (err) {
                        fail(ERROR_READ_FAILED, "FatFS Open Error: %s", fatfs_err_str(err).c_str());
                    }
                    auto size = f_size(&file);
                    data_buf.resize(size);
                    UINT bytes_read;
                    err = f_read(&file, data_buf.data(), data_buf.size(), &bytes_read);
                    if (err) {
                        fail(ERROR_READ_FAILED, "FatFS Read Error: %s", fatfs_err_str(err).c_str());
                    } else if (bytes_read != data_buf.size()) {
                        fail(ERROR_READ_FAILED, "FatFS Read too short - got %d bytes expected %d bytes", bytes_read, data_buf.size());
                    }
                    err = f_close(&file);
                }

                if (destRemote) {
                    FIL file;
                    FRESULT err = f_open(fatfs, &file, settings.filenames[1].c_str(), FA_WRITE | FA_OPEN_ALWAYS);
                    if (err) {
                        fail(ERROR_WRITE_FAILED, "FatFS Open Error: %s", fatfs_err_str(err).c_str());
                    }
                    UINT bytes_written;
                    err = f_write(&file, data_buf.data(), data_buf.size(), &bytes_written);
                    if (err) {
                        fail(ERROR_WRITE_FAILED, "FatFS Write Error: %s", fatfs_err_str(err).c_str());
                    } else if (bytes_written != data_buf.size()) {
                        fail(ERROR_WRITE_FAILED, "FatFS Write too short - wrote %d bytes expected %d bytes", bytes_written, data_buf.size());
                    }
                    err = f_close(&file);
                }
            };
            do_fatfs_op(fatfs_op);
            break;
        };

        default:
            fail(ERROR_ARGS, "Unknown filesystem specified");
    }

    // Write local file
    if (!destRemote) {
        auto outfile = get_file_idx(ios::out|ios::binary, 1);
        outfile->write(data_buf.data(), data_buf.size());
        outfile->close();
    }

    return false;
}

bool bdev_rm_command::execute(device_map &devices) {
    auto con = get_single_bootsel_device_connection(devices);
    setup_bdevfs(con);

    // Remove starting ':' if present, as rm only operates on the device
    if ((char)(settings.filenames[0].front()) == ':') settings.filenames[0].erase(0, 1);

    switch (settings.bdev.fs) {
        case fs_littlefs: {
            lfs_op_fn lfs_op = [&](lfs_t *lfs) {
                int err = lfs_remove(lfs, settings.filenames[0].c_str());
                if (err == LFS_ERR_NOTEMPTY) {
                    fail(ERROR_NOT_POSSIBLE, "Directory to remove is not empty");
                } else if (err == LFS_ERR_NOENT) {
                    fail(ERROR_NOT_POSSIBLE, "File to remove does not exist");
                } else if (err) {
                    fail(ERROR_WRITE_FAILED, "LittleFS Error: %s", lfs_err_str(err).c_str());
                }
            };
            do_lfs_op(lfs_op);
            break;
        }

        case fs_fatfs: {
            fatfs_op_fn fatfs_op = [&](FATFS *fatfs) {
                int err = f_unlink(fatfs, settings.filenames[0].c_str());
                if (err == FR_DENIED) {
                    fail(ERROR_NOT_POSSIBLE, "Directory to remove is not empty");
                } else if (err == FR_NO_FILE) {
                    fail(ERROR_NOT_POSSIBLE, "File to remove does not exist");
                } else if (err) {
                    fail(ERROR_WRITE_FAILED, "FatFS Error: %s", fatfs_err_str(err).c_str());
                }
            };
            do_fatfs_op(fatfs_op);
            break;
        }

        default:
            fail(ERROR_ARGS, "Unknown filesystem specified");
    }

    fos << "Removed " << settings.filenames[0] << " from device\n";

    return false;
}

bool bdev_cat_command::execute(device_map &devices) {
    // Quieten all output, so you can use `cat > file.txt`
    fos_ptr = fos_null_ptr;

    auto con = get_single_bootsel_device_connection(devices);
    setup_bdevfs(con);

    // Remove starting ':' if present, as cat only operates on the device
    if ((char)(settings.filenames[0].front()) == ':') settings.filenames[0].erase(0, 1);

    switch (settings.bdev.fs) {
        case fs_littlefs: {
            lfs_op_fn lfs_op = [&](lfs_t *lfs) {
                lfs_file_t file;
                int err = lfs_file_open(lfs, &file, settings.filenames[0].c_str(), LFS_O_RDONLY);
                if (err) {
                    fail(ERROR_READ_FAILED, "LittleFS Open Error: %s", lfs_err_str(err).c_str());
                }
                auto size = lfs_file_size(lfs, &file);
                err = lfs_file_rewind(lfs, &file);
                std::vector<char> data_buf(size);
                err = lfs_file_read(lfs, &file, data_buf.data(), data_buf.size());
                if (err < 0) {
                    fail(ERROR_READ_FAILED, "LittleFS Read Error: %s", lfs_err_str(err).c_str());
                } else if (err != data_buf.size()) {
                    fail(ERROR_READ_FAILED, "LittleFS Read too short - got %d bytes expected %d bytes", err, data_buf.size());
                }
                err = lfs_file_close(lfs, &file);

                string out(data_buf.begin(), data_buf.end());
                printf("%s", out.c_str());
            };
            do_lfs_op(lfs_op);
            break;
        }

        case fs_fatfs: {
            fatfs_op_fn fatfs_op = [&](FATFS *fatfs) {
                FIL file;
                int err = f_open(fatfs, &file, settings.filenames[0].c_str(), FA_READ);
                if (err) {
                    fail(ERROR_READ_FAILED, "FatFS Open Error: %s", fatfs_err_str(err).c_str());
                }
                auto size = f_size(&file);
                err = f_rewind(&file);
                std::vector<char> data_buf(size);
                UINT bytes_read;
                err = f_read(&file, data_buf.data(), data_buf.size(), &bytes_read);
                if (err) {
                    fail(ERROR_READ_FAILED, "FatFS Read Error: %s", fatfs_err_str(err).c_str());
                } else if (bytes_read != data_buf.size()) {
                    fail(ERROR_READ_FAILED, "FatFS Read too short - got %d bytes expected %d bytes", bytes_read, data_buf.size());
                }
                err = f_close(&file);

                string out(data_buf.begin(), data_buf.end());
                printf("%s", out.c_str());
            };
            do_fatfs_op(fatfs_op);
            break;
        }

        default:
            fail(ERROR_ARGS, "Unknown filesystem specified");
    }
    return false;
}

bool bdev_format_command::execute(device_map &devices) {
    settings.bdev.always_format = true;
    settings.bdev.format = true;

    auto con = get_single_bootsel_device_connection(devices);
    setup_bdevfs(con);

    switch (settings.bdev.fs) {
        case fs_littlefs: {
            lfs_op_fn lfs_op = [&](lfs_t *lfs) {
                printf("LittleFS formatting complete\n");
            };
            do_lfs_op(lfs_op);
            break;
        }

        case fs_fatfs: {
            fatfs_op_fn fatfs_op = [&](FATFS *fatfs) {
                printf("FatFS formatting complete\n");
            };
            do_fatfs_op(fatfs_op);
            break;
        }

        default:
            fail(ERROR_ARGS, "Unknown filesystem specified");
    }
    return false;
}

bool verify_command::execute(device_map &devices) {
    auto file_access = get_file_memory_access(0);
    auto con = get_single_bootsel_device_connection(devices);
    picoboot_memory_access raw_access(con);
    model_t model = raw_access.get_model();
    if (settings.offset_set && get_file_type() != filetype::bin && model->chip() == rp2040) {
        fail(ERROR_ARGS, "Offset only valid for BIN files");
    }
    auto ranges = get_coalesced_ranges(file_access, model);
    if (settings.range_set) {
        range filter(settings.from, settings.to);
        for(auto& range : ranges) {
            range.intersect(filter);
        }
    }
    ranges.erase(std::remove_if(ranges.begin(), ranges.end(), std::mem_fn(&range::empty)), ranges.end());
    if (ranges.empty()) {
        std::cout << "No ranges to verify.\n";
    } else {
        for (auto mem_range : ranges) {
            enum memory_type t1 = get_memory_type(mem_range.from, model);
            enum memory_type t2 = get_memory_type(mem_range.to, model);
            if (t1 != t2 || t1 == invalid || t1 == sram_unstriped) {
                fail(ERROR_NOT_POSSIBLE, "invalid memory range for verification %08x-%08x", mem_range.from, mem_range.to);
            } else {
                bool ok = true;
                uint32_t pos = mem_range.from;
                {
                    progress_bar bar("Verifying " + memory_names[t1] + ": ");
                    vector<uint8_t> file_buf;
                    vector<uint8_t> device_buf;
                    uint32_t batch_size = calculate_chunk_size(mem_range.len());
                    for(uint32_t base = mem_range.from; base < mem_range.to && ok; base += batch_size) {
                        uint32_t this_batch = std::min(mem_range.to - base, batch_size);
                        // note we pass zero_fill = true in case the file has holes, but this does
                        // mean that the verification will fail if those holes are not filled with zeros
                        // on the device
                        file_access.read_into_vector(base, this_batch, file_buf, true);
                        raw_access.read_into_vector(base, this_batch, device_buf);
                        assert(file_buf.size() == device_buf.size());
                        for(unsigned int i=0;i<this_batch;i++) {
                            if (file_buf[i] != device_buf[i]) {
                                pos = base + i;
                                ok = false;
                                break;
                            }
                        }
                        if (ok) {
                            pos = base + this_batch;
                        }
                        bar.progress(pos - mem_range.from, mem_range.to - mem_range.from);
                    }
                }
                if (ok) {
                    std::cout << "  OK\n";
                } else {
                    std::cout << "  First mismatch at " << hex_string(pos) << "\n";
                    uint32_t display_from = (pos - 15) & ~15;
                    uint32_t display_to = display_from + 48;
                    range valid(display_from, display_to);
                    valid.intersect(mem_range);
                    vector<uint8_t> file_buf;
                    vector<uint8_t> device_buf;
                    file_access.read_into_vector(valid.from, valid.to - valid.from, file_buf);
                    raw_access.read_into_vector(valid.from, valid.to - valid.from, device_buf);
                    assert(file_buf.size() == device_buf.size());
                    for(unsigned int l=0;l<3;l++, display_from+=16) {
                        range this_range(display_from, display_from + 16);
                        this_range.intersect(mem_range);
                        if (this_range.empty()) continue;

                        fos.first_column(4);
                        fos.hanging_indent(0);
                        fos << hex_string(display_from);
                        fos.first_column(15);
                        for(int w=0;w<2;w++) {
                            const auto& buf = w ? device_buf : file_buf;
                            std::stringstream line;
                            line << "| ";
                            for (unsigned int p = 0; p < 16; p++) {
                                if (valid.contains(display_from + p)) {
                                    line << hex_string(buf[display_from + p - valid.from], 2, false) << " ";
                                } else {
                                    line << "   ";
                                }
                            }
                            fos << line.str() << "\n";
                        }

                        std::stringstream line;
                        line << "| "; // fos ignores leading whitespace!
                        for (unsigned int p = 0; p < 16; p++) {
                            if (valid.contains(display_from + p) &&
                                file_buf[display_from + p - valid.from] != device_buf[display_from + p - valid.from]) {
                               line << "~~ ";
                            } else {
                               line << "   ";
                            }
                        }
                        fos << line.str() << "\n\n";
                    }
                    fail(ERROR_VERIFICATION_FAILED, "The device contents did not match the file");
                }
            }
        }
    }
    return false;
}
#endif

bool get_json_bcd(json value, int& out) {
    int tmp = 0;
    if (value.is_number_float()) {
        tmp = round(100.0 * value.template get<double>());
    } else if (get_json_int(value, tmp)) {
        tmp *= 100;
    } else {
        return false;
    }
    if (tmp > 9999) {
        return false;
    }

    int rev = 0;
    out = 0;
    int shift = tmp >= 1000 ? 12 : 8;

    // Reverse the number
    while (tmp > 0) {
        rev = rev * 10 + (tmp % 10);
        tmp /= 10;
    }

    // Output each remainder
    while (rev > 0) {
        uint8_t n = (rev % 10) & 0xf;
        out |= n << shift;

        rev /= 10;
        shift -= 4;
    }
    return true;
}

bool utf8_to_utf16(string utf8, vector<uint16_t>& utf16) {
    // Check valid utf8, and check len
    bool unicode = false;
    bool done = true;
    unsigned int ex_len = 0;
    vector<char> cur;
    for (char c : utf8) {
        if (done) {
            if ((c & 0x80) == 0) {
                ex_len = 1;
            } else if ((c & 0xE0) == 0xC0) {
                ex_len = 2;
            } else if ((c & 0xF0) == 0xE0) {
                ex_len = 3;
            } else if ((c & 0xF8) == 0xF0) {
                ex_len = 4;
            } else {
                fail(ERROR_INCOMPATIBLE, "%x is not a valid first character for a unicode string", c);
            }
            done = false;
            if (ex_len > 1) {
                unicode = true;
            }
        } else {
            if ((c & 0xC0) != 0x80) {
                fail(ERROR_INCOMPATIBLE, "%x is not a valid character in a unicode sequence", c);
            }
        }
        cur.push_back(c);
        if (cur.size() == ex_len) {
            done = true;
            switch (ex_len) {
                case 1: {
                    utf16.push_back(cur[0] & 0x7F);
                    break;
                }
                case 2: {
                    utf16.push_back((cur[0] & 0x1F) << 6 | (cur[1] & 0x3F));
                    break;
                }
                case 3: {
                    utf16.push_back((cur[0] & 0x0F) << 12 | (cur[1] & 0x3F) << 6 | (cur[2] & 0x3F));
                    break;
                }
                case 4: {
                    int32_t cp = (cur[0] & 0x07) << 18 | (cur[1] & 0x3F) << 12 | (cur[2] & 0x3F) << 6 | (cur[3] & 0x3F);
                    cp -= 0x10000;
                    utf16.push_back(0xD800 + ((cp >> 10) & 0x3FF));
                    utf16.push_back(0xDC00 + (cp & 0x3FF));
                    break;
                }
            }
            cur.clear();
        }
    }
    return unicode;
}

bool get_json_strdef(json value, int& out, vector<uint16_t>& data, uint8_t max_strlen) {
    if (!value.is_string()) {
        return false;
    }

    uint8_t max_size = 0;

    // Decode as if unicode into UTF-16
    vector<uint16_t> tmp;
    std::setlocale(LC_ALL, "en_US.utf8");
    string str = value;
    bool unicode = utf8_to_utf16(str, tmp);

    if (tmp.size() > max_strlen) {
        DEBUG_LOG("String is too long (%d) - max length is %d\n", (int)tmp.size(), (int)max_strlen);
        return false;
    }

    out = (data.size() << 8) | (tmp.size() & 0x7f) | (unicode ? 0x80 : 0);
    if (unicode) {
        data.insert(data.end(), tmp.begin(), tmp.end());
    } else {
        size_t data_old_size = data.size();
        data.resize(data.size() + ceil(tmp.size()/2.0));
        vector<uint8_t> new_tmp;
        for (auto x : tmp) new_tmp.push_back(x);
        memcpy(data.data() + data_old_size, new_tmp.data(), new_tmp.size());
    }
    return true;
}

otp_field *select_field(uint32_t offset, const std::string& field_selector) {
    return nullptr;
}

struct otp_match {
    explicit otp_match() = default;
    uint32_t reg_row;
    uint32_t mask = 0xffffffff; // 0xffffffff means unknown
    const otp_reg *reg = nullptr;
    const otp_field *field = nullptr;
};

bool get_mask(const std::string& sel, uint32_t &mask, int max_bit) {
    auto dash = sel.find_first_of('-');
    int from = 0, to = 0;
    if (dash != string::npos) {
        bool ok = get_int(sel.substr(0, dash), from) &&
                  get_int(sel.substr(dash+1), to);
        if (!ok || to < from || from < 0 || to >= max_bit) {
            fail(ERROR_ARGS, "Invalid bit-range in selector: %s; expect 'm-n' where m >= 0, n >= m and n <= %d", sel.c_str(), max_bit-1);
        }
        mask = (2u << to) - (1u << from);
        return true;
    }
    return false;
}

void init_matches(const otp_reg *reg, uint32_t reg_row, const std::string& field_sel, int max_bit,
                  std::function<void(otp_match)> func, bool fuzzy = true) {
    if (!reg) {
        auto f = otp_regs.find(reg_row);
        if (f != otp_regs.end()) {
            reg = &f->second;
        }
    }
    std::vector<otp_match> matches;
    otp_match m;
    m.reg_row = reg_row;
    m.reg = reg;
    m.field = nullptr;
    m.mask = 0; // not matching field
    if (field_sel.empty()) {
        if (reg) m.mask = reg->mask;
        else m.mask = 0xffffffff;
    } else {
        int from = 0;
        if (!get_mask(field_sel, m.mask, max_bit)) {
            if (get_int(field_sel, from)) {
                if (from < 0 || from >= max_bit) {
                    fail(ERROR_ARGS, "Invalid bit-index in selector: %s; expect value from 0 to %d", field_sel.c_str(), max_bit - 1);
                }
                m.mask = 1u << from;
            } else if (reg) {
                // field name
                auto upper_field = uppercase(field_sel);
                for(const auto &f : reg->fields) {
                    if (f.upper_name.find(upper_field) != string::npos && fuzzy) {
                        m.field = &f;
                        m.mask = f.mask;
                        func(m);
                        m.mask = 0;
                    } else if (f.upper_name == upper_field) {
                        m.field = &f;
                        m.mask = f.mask;
                        func(m);
                        m.mask = 0;
                    }
                }
            }
        }
    }
    if (m.mask) func(m);
}

std::map<std::pair<uint32_t,uint32_t>, otp_match> filter_otp(std::vector<string> selectors, int max_bit, bool fuzzy) {
    // inefficient but who cares!?
    std::map<std::pair<uint32_t,uint32_t>, otp_match> matches;
    auto match_adder = [&matches](const otp_match &m) {
        matches.emplace(std::make_pair(m.reg_row, m.mask), m);
    };
    for(const auto &sel : selectors) {
        std::string reg_sel;
        std::string field_sel;
        auto period = sel.find_first_of('.');
        if (period == string::npos) {
            reg_sel = sel;
        } else {
            reg_sel = sel.substr(0, period);
            field_sel = sel.substr(period+1);
        }
        int reg_row = 0;
        if (get_int(reg_sel, reg_row)) {
            // absolute offset
            if (reg_row < 0 || reg_row >= OTP_ROW_COUNT) {
                fail(ERROR_ARGS, "Invalid selector %s; absolute row number must be even and between 0 and 0x%x", sel.c_str(), OTP_ROW_COUNT);
            }
            init_matches(nullptr, reg_row, field_sel, max_bit, match_adder);
        } else {
            auto colon = reg_sel.find_first_of(':');
            if (colon != string::npos) {
                int single_page = 0;
                auto page_sel = reg_sel.substr(0, colon);
                std::vector<int> pages;
                if (page_sel.empty()) {
                    pages.reserve(OTP_PAGE_COUNT);
                    std::generate_n(std::back_insert_iterator<std::vector<int>>(pages), OTP_PAGE_COUNT,
                                    [n = 0]() mutable { return n++; });
                } else if (!get_int(page_sel, single_page) || single_page < 0 || single_page >= OTP_PAGE_COUNT) {
                    fail(ERROR_ARGS, "Invalid selector %s; expected valid page number (0-%d) before ':'", sel.c_str(),
                         OTP_PAGE_COUNT - 1);
                } else {
                    pages.push_back(single_page);
                }
                std::string offset_sel = reg_sel.substr(colon + 1);
                int page_row = 0;
                if (offset_sel.empty()) {
                    for(auto page : pages) {
                        for (reg_row = page * OTP_PAGE_ROWS; reg_row < (page + 1) * OTP_PAGE_ROWS; reg_row++) {
                            init_matches(nullptr, reg_row, field_sel, max_bit, match_adder);
                        }
                    }
                } else if (!get_int(offset_sel, page_row) || page_row < 0 || page_row >= OTP_PAGE_ROWS) {
                    fail(ERROR_ARGS, "Invalid selector %s; page row number must be even and between 0 and 0x%x", sel.c_str(), OTP_PAGE_ROWS);
                } else {
                    for(auto page : pages) {
                        init_matches(nullptr, page * OTP_PAGE_ROWS + page_row, field_sel, max_bit, match_adder);
                    }
                }
            } else {
                auto upper = uppercase(reg_sel);
                for(const auto &e : otp_regs) {
                    if (e.second.upper_name.find(upper, 0) != string::npos && fuzzy) {
                        init_matches(&e.second, e.second.row, field_sel, max_bit, match_adder);
                    } else if (e.second.upper_name == upper || e.second.upper_name == "OTP_DATA_" + upper) {
                        init_matches(&e.second, e.second.row, field_sel, max_bit, match_adder, false);
                    }
                }
            }
        }
    }
    return matches;
}

#if HAS_LIBUSB
bool partition_info_command::execute(device_map &devices) {
    auto con = get_single_picoboot_cmd_compatible_device_connection("partition info", devices, {PC_GET_INFO}, false);

    auto partition_info = get_partition_info(con);
    if (partition_info == nullptr) {
        // get_partition_info had an error
        return -1;
    }
    auto unpartitioned = std::get<0>(*partition_info);
    auto partitions = std::get<1>(*partition_info);

    if (!partitions) {
        printf("there is no partition table\n");
    } else if (partitions->size() == 0) {
        printf("the partition table is empty\n");
    }
    printf("un-partitioned_space : ");
    fos << str_permissions(unpartitioned.permissions_and_flags);
    std::vector<std::string> family_names;
    insert_default_families(unpartitioned.permissions_and_flags, family_names);
    printf(", uf2 { %s }\n", cli::join(family_names, ", ").c_str());

    if (partitions) {
        printf("partitions:\n");
        for (int i = 0; i < (*partitions).size(); i++) {
            auto partition = (*partitions)[i];
            uint32_t flags_and_permissions = partition.flags_and_permissions;
            uint64_t id = partition.id;
            printf("  %d", i);
            if ((flags_and_permissions & PICOBIN_PARTITION_FLAGS_LINK_TYPE_BITS) ==
                PICOBIN_PARTITION_FLAGS_LINK_TYPE_AS_BITS(A_PARTITION)) {
                printf("(B w/ %d) ", (flags_and_permissions & PICOBIN_PARTITION_FLAGS_LINK_VALUE_BITS)
                        >> PICOBIN_PARTITION_FLAGS_LINK_VALUE_LSB);
            } else if ((flags_and_permissions & PICOBIN_PARTITION_FLAGS_LINK_TYPE_BITS) ==
                    PICOBIN_PARTITION_FLAGS_LINK_TYPE_AS_BITS(OWNER_PARTITION)) {
                    printf("(A ob/ %d)", (flags_and_permissions & PICOBIN_PARTITION_FLAGS_LINK_VALUE_BITS)
                            >> PICOBIN_PARTITION_FLAGS_LINK_VALUE_LSB);
            } else {
                // b_partition doesn't work without the PT loaded
                printf("(A)      ");
            }
            printf(" %08x->%08x",
                   partition.start,
                   partition.end);
            unsigned int permissions = flags_and_permissions & PICOBIN_PARTITION_PERMISSIONS_BITS;
            fos << str_permissions(permissions);
            if (partition.has_id) {
                printf(", id=%016" PRIx64, id);
            }
            family_names.clear();
            insert_default_families(flags_and_permissions, family_names);
            for (auto extra_family : partition.extra_families) {
                family_names.emplace_back(family_name(extra_family));
            }
            if (partition.has_name) {
                printf(", \"%s\"", partition.name.c_str());
            }
            printf(", uf2 { %s }", cli::join(family_names, ", ").c_str());
            printf(", arm_boot %d", !(flags_and_permissions & PICOBIN_PARTITION_FLAGS_IGNORED_DURING_ARM_BOOT_BITS));
            printf(", riscv_boot %d", !(flags_and_permissions & PICOBIN_PARTITION_FLAGS_IGNORED_DURING_RISCV_BOOT_BITS));
            printf("\n");
        }
    }
    if (settings.family_id) {
        get_target_partition(con);
    }
    return false;
}
#endif

uint32_t permissions_to_flags(json permissions) {
    uint32_t ret = 0;
    if (permissions.contains("secure")) {
        string perms = permissions["secure"];
        if (perms.find("r") != string::npos) ret |= PICOBIN_PARTITION_PERMISSION_S_R_BITS;
        if (perms.find("w") != string::npos) ret |= PICOBIN_PARTITION_PERMISSION_S_W_BITS;
    }
    if (permissions.contains("nonsecure")) {
        string perms = permissions["nonsecure"];
        if (perms.find("r") != string::npos) ret |= PICOBIN_PARTITION_PERMISSION_NS_R_BITS;
        if (perms.find("w") != string::npos) ret |= PICOBIN_PARTITION_PERMISSION_NS_W_BITS;
    }
    if (permissions.contains("bootloader")) {
        string perms = permissions["bootloader"];
        if (perms.find("r") != string::npos) ret |= PICOBIN_PARTITION_PERMISSION_NSBOOT_R_BITS;
        if (perms.find("w") != string::npos) ret |= PICOBIN_PARTITION_PERMISSION_NSBOOT_W_BITS;
    }
    return ret;
}

uint32_t families_to_flags(std::vector<string> families, bool fail_invalid = false) {
    uint32_t ret = 0;
    uint32_t id = 0;
    auto family_id_getter = family_id("family_id").set(id);
    for (auto family : families) {
        auto family_id_ret = family_id_getter.action(family);
        if (family_id_ret.size() > 0) {
            if (fail_invalid) {
                fail(ERROR_FORMAT, "Could not parse family ID from %s: %s", family.c_str(), family_id_ret.c_str());
            }
            continue;
        }
        if (id == DATA_FAMILY_ID) {
            ret |= PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_DATA_BITS;
        } else if (id == ABSOLUTE_FAMILY_ID) {
            ret |= PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_ABSOLUTE_BITS;
        } else if (id == RP2040_FAMILY_ID) {
            ret |= PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_RP2040_BITS;
        } else if (id == RP2350_ARM_S_FAMILY_ID) {
            ret |= PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_RP2350_ARM_S_BITS;
        } else if (id == RP2350_ARM_NS_FAMILY_ID) {
            ret |= PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_RP2350_ARM_NS_BITS;
        } else if (id == RP2350_RISCV_FAMILY_ID) {
            ret |= PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_RP2350_RISCV_BITS;
        } else if (fail_invalid) {
            fail(ERROR_FORMAT, "Invalid family ID for bootrom flags: %s", family.c_str());
        }
    }
    return ret;
}

uint32_t round_up_power_2(uint32_t v) {
    v--;
    v |= v >> 1;
    v |= v >> 2;
    v |= v >> 4;
    v |= v >> 8;
    v |= v >> 16;
    v++;
    return v;
}

bool partition_create_command::execute(device_map &devices) {
    if (get_file_type_idx(0) != filetype::json) {
        fail(ERROR_ARGS, "json must be a json file\n");
    }
    if (settings.filenames[2].empty()) {
        if (!(get_file_type_idx(1) == filetype::bin || get_file_type_idx(1) == filetype::uf2)) {
            fail(ERROR_ARGS, "output must be a BIN/UF2\n");
        }
    } else {
        if (get_file_type_idx(2) != filetype::elf) {
        fail(ERROR_ARGS, "bootloader must be an ELF\n");
    }
    }

    auto file = get_file(ios::in);
    json pt_json = json::parse(*file.get());
    file->close();

    auto partitions = pt_json["partitions"];

    elf_file source_file(settings.verbose);
    elf_file *elf = &source_file;
    std::shared_ptr<block> pt_block;
    if (!settings.filenames[2].empty()) {
        model_t model = get_model(2);
        elf->read_file(get_file_idx(ios::in|ios::binary, 2));
        std::unique_ptr<block> first_block = find_first_block(elf);
        if (!first_block) {
            fail(ERROR_FORMAT, "No first block found");
        }

        block new_block = place_new_block(elf, first_block, model);
        new_block.items.erase(new_block.items.begin(), new_block.items.end());
        pt_block = std::make_shared<block>(new_block);
    } else {
        pt_block = std::make_shared<block>(FLASH_START);
    }

    uint32_t unpartitioned_flags = permissions_to_flags(pt_json["unpartitioned"]["permissions"]) | families_to_flags(pt_json["unpartitioned"]["families"], true);
    partition_table_item pt(unpartitioned_flags, settings.partition.singleton);

#if SUPPORT_RP2350_A2
    // todo fix test
    if (!(unpartitioned_flags & PICOBIN_PARTITION_FLAGS_ACCEPTS_DEFAULT_FAMILY_ABSOLUTE_BITS)) {
        fail(ERROR_INCOMPATIBLE, "Unpartitioned space must accept the absolute family, for the RP2350-E10 fix to work");
    }
#endif

    uint32_t cur_pos = 2;
    uint32_t max_pos = 0;

    for (auto p : partitions) {
        partition_table_item::partition new_p;
        uint32_t start = cur_pos;
        if (p.contains("start")) get_json_int(p["start"], start);
        int size; get_json_int(p["size"], size);

        if (start >= 4096 || size >= 4096) {
            if (start == cur_pos) start *= 0x1000;
            if (start % 0x1000 || size % 0x1000) {
                fail(ERROR_INCOMPATIBLE, "Partition start (%dK) and size (%dK) must be 4K aligned", start/1024, size/1024);
            }
            start /= 0x1000;
            size /= 0x1000;
        }

        cur_pos = start + size;
        if (cur_pos > max_pos) max_pos = cur_pos;
    #if SUPPORT_RP2350_A2
        if (start <= (settings.uf2.abs_block_loc - FLASH_START)/0x1000 && start + size > (settings.uf2.abs_block_loc - FLASH_START)/0x1000) {
            fail(ERROR_INCOMPATIBLE, "The address %" PRIx32 " cannot be in a partition for the RP2350-E10 fix to work", settings.uf2.abs_block_loc);
        }
    #endif
        new_p.first_sector = start;
        new_p.last_sector = start + size - 1;
        new_p.permissions = permissions_to_flags(p["permissions"]) >> PICOBIN_PARTITION_PERMISSIONS_LSB;
        new_p.flags = families_to_flags(p["families"]);
        uint32_t id = 0;
        auto id_getter = family_id("family_id").set(id);
        for (string family : p["families"]) {
            DEBUG_LOG("Checking %s\n", family.c_str());
            auto ret = id_getter.action(family);
            if (ret.size() > 0) {
                fail(ERROR_FORMAT, "Could not parse family ID from %s: %s", family.c_str(), ret.c_str());
            }
            DEBUG_LOG("Got ID %08x\n", id);
            if (id < BOOTROM_FAMILY_ID_MIN || id > BOOTROM_FAMILY_ID_MAX) {
                DEBUG_LOG("Adding extra family\n");
                new_p.extra_families.push_back(id);
            }
            if (new_p.extra_families.size() > PICOBIN_PARTITION_MAX_EXTRA_FAMILIES) {
                fail(ERROR_NOT_POSSIBLE, "Too many extra families - max permitted is %d", PICOBIN_PARTITION_MAX_EXTRA_FAMILIES);
            }
            DEBUG_LOG("Num extra families %d %x\n", (int)new_p.extra_families.size(), (int)(new_p.extra_families.size() << PICOBIN_PARTITION_FLAGS_ACCEPTS_NUM_EXTRA_FAMILIES_LSB));
            new_p.flags |= new_p.extra_families.size() << PICOBIN_PARTITION_FLAGS_ACCEPTS_NUM_EXTRA_FAMILIES_LSB;
        }
        DEBUG_LOG("Permissions %08x\n", new_p.permissions);
        if (p.contains("link")) {
            string link_type = p["link"][0];
            int link_value = p["link"][1];
            if (link_type == "a") {
                new_p.flags |= PICOBIN_PARTITION_FLAGS_LINK_TYPE_AS_BITS(A_PARTITION);
            } else if (link_type == "owner") {
                new_p.flags |= PICOBIN_PARTITION_FLAGS_LINK_TYPE_AS_BITS(OWNER_PARTITION);
            } else if (link_type == "none") {

            } else {
                fail(ERROR_INCOMPATIBLE, "Link type \"%s\" is not recognised\n", link_type.c_str());
            }
            new_p.flags |= (link_value << PICOBIN_PARTITION_FLAGS_LINK_VALUE_LSB) & PICOBIN_PARTITION_FLAGS_LINK_VALUE_BITS;
        }
        if (p.contains("name")) {
            new_p.name = p["name"];
            new_p.flags |= PICOBIN_PARTITION_FLAGS_HAS_NAME_BITS;
            if (new_p.name.size() > 127) {
                fail(ERROR_INCOMPATIBLE, "Partition name \"%s\" is %d characters long - max length is 127 characters\n", new_p.name.c_str(), new_p.name.size());
            }
        }
        if (p.contains("id")) {
            if (get_json_int(p["id"], new_p.id)) {new_p.flags |= PICOBIN_PARTITION_FLAGS_HAS_ID_BITS;}
            else {string p_id = p["id"]; fail(ERROR_INCOMPATIBLE, "Partition ID \"%s\" is not a valid 64bit integer\n", p_id.c_str());}
        }

        bool tmp_bool;
        for (const auto& pair : std::vector<std::pair<std::string, uint32_t>>{
            {"no_reboot_on_uf2_download", PICOBIN_PARTITION_FLAGS_UF2_DOWNLOAD_NO_REBOOT_BITS},
            {"ab_non_bootable_owner_affinity", PICOBIN_PARTITION_FLAGS_UF2_DOWNLOAD_AB_NON_BOOTABLE_OWNER_AFFINITY},
            {"ignored_during_riscv_boot", PICOBIN_PARTITION_FLAGS_IGNORED_DURING_RISCV_BOOT_BITS},
            {"ignored_during_arm_boot", PICOBIN_PARTITION_FLAGS_IGNORED_DURING_ARM_BOOT_BITS},
        }) {
            const std::string json_flag = pair.first;
            const uint32_t FLAG_BITS = pair.second;
            if(p.contains(json_flag)) {  
                if (get_json_bool(p[json_flag], tmp_bool)) {  
                    new_p.flags |= (tmp_bool ? FLAG_BITS : 0);  
                } else {  
                    fail(ERROR_INCOMPATIBLE, "Partition %d %s value is not a valid boolean\n", pt.partitions.size(), json_flag.c_str());  
                }  
            }  
        }
        pt.partitions.push_back(new_p);
    }

    // Catch partition tables which cover the end of Flash, which the SDK uses for the BTStack flash bank
    uint num_end_sectors = 0;
    string end_sectors_warning_output = "";
    if (!settings.partition.no_btstack_flash_bank) {
        num_end_sectors += 2; // for BTStack flash bank
        end_sectors_warning_output += "the BTStack flash bank";
    }
#if SUPPORT_RP2350_A2
    if (settings.uf2.abs_block_loc) {
        uint32_t abs_block_flash_loc = settings.uf2.abs_block_loc - FLASH_START;
        if (round_up_power_2(abs_block_flash_loc) <= abs_block_flash_loc + 0x1000) {
            // if abs_block within 4k of end, also keep space for that
            num_end_sectors += 1;
            if (!end_sectors_warning_output.empty()) end_sectors_warning_output += ", and ";
            end_sectors_warning_output += "the RP2350-E10 absolute block";
        }
    }
#endif

    if (round_up_power_2(max_pos) - max_pos < num_end_sectors) {
        // Likely this partition table doesn't account for BTStack flash bank at end
        fos << "WARNING: This partition table covers the last " << num_end_sectors;
        fos << " Flash sectors for flash size " << round_up_power_2(max_pos) / (1024/4);
        fos << "M, which will interfere with ";
        fos << end_sectors_warning_output;
        fos << " if that is the actual flash size\n";
    }

    pt_block->items.push_back(std::make_shared<partition_table_item>(pt));

    if (pt_json.contains("version")) {
        version_item v(pt_json["version"][0], pt_json["version"][1]);
        pt_block->items.push_back(std::make_shared<version_item>(v));
    }

    // todo workaround for this not being set
    settings.partition.sign = !settings.filenames[3].empty();
    if (settings.partition.hash || settings.partition.sign) {
    #if HAS_MBEDTLS
        DEBUG_LOG(
            "%s%s%s partition table\n",
            settings.partition.hash ? "Hashing" : "",
            (settings.partition.hash && settings.partition.sign) ? " and " : "",
            settings.partition.sign ? "Signing" : ""
        );
        private_t private_key = {};
        public_t public_key = {};
        if (settings.partition.sign) {
            read_keys(settings.filenames[3], &public_key, &private_key);
        }
        hash_andor_sign_block(pt_block.get(), public_key, private_key, settings.partition.hash, settings.partition.sign);
    #else
        fail(ERROR_ARGS, "Cannot sign/hash partition table with no mbedtls\n");
    #endif
    }

    auto out = get_file_idx(ios::out|ios::binary, 1);
    auto tmp = pt_block->to_words();
    std::vector<uint8_t> data = words_to_lsb_bytes(tmp.begin(), tmp.end());
    if (settings.filenames[2].empty()) {
        if (get_file_type_idx(1) == filetype::uf2) {
            uint32_t family_id = ABSOLUTE_FAMILY_ID;
            if (settings.family_id) family_id = settings.family_id;
            uint32_t address = settings.offset_set ? settings.offset : FLASH_START;
            auto tmp = std::make_shared<std::stringstream>();
            tmp->write(reinterpret_cast<const char*>(data.data()), data.size());
            bin2uf2(tmp, out, address, family_id, models::largest);
        } else {
            out->write(reinterpret_cast<const char*>(data.data()), data.size());
        }
    } else {
        elf->append_segment(pt_block->physical_addr, pt_block->physical_addr, data.size(), ".pt");
        auto pt_section = elf->get_section(".pt");
        assert(pt_section);
        assert(pt_section->virtual_address() == pt_block->physical_addr);

        if (pt_section->size < data.size()) {
            fail(ERROR_UNKNOWN, "Partition Table block is too big for elf section\n");
        }
        DEBUG_LOG("Partition Table section requires %lu bytes of padding\n", pt_section->size - data.size());
        while (data.size() < pt_section->size) {
            data.push_back(0);
        }

        elf->content(*pt_section, data);
        elf->write(out);
    }
    out->close();
    return false;
}

#if HAS_LIBUSB
bool uf2_info_command::execute(device_map &devices) {
    auto con = get_single_picoboot_cmd_compatible_device_connection("uf2 info", devices, {PC_GET_INFO}, false);
    uint32_t buf[5];
    picoboot_get_info_cmd cmd;
    cmd.bType = PICOBOOT_GET_INFO_UF2_STATUS;
    con.get_info(&cmd, (uint8_t *)buf, sizeof(buf));
    vector<pair<string,string>> infos;
    auto info_pair = [&](const string &name, const string &value) {
        if (!value.empty()) {
            infos.emplace_back(std::make_pair(name, value));
        }
    };
    assert(buf[0] == 4);
    uint32_t status = (uint16_t)buf[1];
    uint32_t family_id = buf[2];
    uint32_t valid_block_count = buf[3];
    uint32_t total_block_count = buf[4];
    const int uf2_status_all = UF2_STATUS_ABORT_BAD_ADDRESS | UF2_STATUS_ABORT_EXCLUSIVELY_LOCKED | UF2_STATUS_IGNORED_FAMILY | UF2_STATUS_ABORT_WRITE_ERROR | UF2_STATUS_ABORT_REBOOT_FAILED;
    if (status & ~uf2_status_all) {
        fos << "<invalid>\n";
    } else if (!status && (!family_id || !total_block_count)) {
        fos << "no info found\n";
    } else {
        std::vector<string> aborts;
        info_pair("uf2 family", family_name(family_id));
        info_pair("uf2 blocks downloaded", total_block_count ? (std::to_string(valid_block_count) + " / " + std::to_string(total_block_count)) : "none");
        if (status & UF2_STATUS_ABORT_BAD_ADDRESS) aborts.emplace_back("bad address");
        if (status & UF2_STATUS_ABORT_EXCLUSIVELY_LOCKED) aborts.emplace_back("exclusively locked");
        if (status & UF2_STATUS_ABORT_WRITE_ERROR) aborts.emplace_back("write error");
        if (status & UF2_STATUS_ABORT_REBOOT_FAILED) aborts.emplace_back("reboot failed");
        info_pair("ignored un-placeable family(s)", status & UF2_STATUS_IGNORED_FAMILY ? "true" : "false");
        info_pair("abort reason", aborts.empty() ? "none" : cli::join(aborts, ", "));
    }
    int tab = 0;
    for(const auto& item : infos) {
        tab = std::max(tab, 3 + (int)item.first.length()); // +3 for ":  "
    }
    for(const auto& item : infos) {
        fos.first_column(1);
        fos << (item.first + ":");
        fos.first_column(1 + tab);
        fos << (item.second + "\n");
    }
    return false;
}
#endif


#ifndef count_of
#define count_of(x) (sizeof(x) / sizeof((x)[0]))
#endif

bool uf2_convert_command::execute(device_map &devices) {
    if (get_file_type_idx(1) != filetype::uf2) {
        fail(ERROR_ARGS, "Output must be a UF2 file\n");
    }

    if (get_file_type_idx(0) != filetype::elf && get_file_type_idx(0) != filetype::bin) {
        fail(ERROR_ARGS, "Input must be an ELF or BIN file\n");
    }

    uint32_t family_id = get_family_id(0);
    model_t model = get_model(0);

    auto in = get_file(ios::in|ios::binary);
    auto out = get_file_idx(ios::out|ios::binary, 1);
    #if SUPPORT_RP2350_A2
    // RP2350-E10 : add absolute block
    if (settings.uf2.abs_block) {
        fos << "RP2350-E10: Adding absolute block to UF2 targeting " << hex_string(settings.uf2.abs_block_loc) << "\n";
    } else {
        settings.uf2.abs_block_loc = 0;
    }
    #endif
    if (get_file_type() == filetype::elf) {
        uint32_t package_address = settings.offset_set ? settings.offset : 0;
        elf2uf2(in, out, family_id, model, package_address, settings.uf2.abs_block_loc, settings.verbose);
    } else if (get_file_type() == filetype::bin) {
        uint32_t address = settings.offset_set ? settings.offset : FLASH_START;
        bin2uf2(in, out, address, family_id, model, settings.uf2.abs_block_loc, settings.verbose);
    } else {
        fail(ERROR_ARGS, "Convert currently only from ELF/BIN to UF2\n");
    }
    out->close();

    return false;
}

bool uf2_combine_command::execute(device_map &devices) {
    if (get_file_type_idx(0) != filetype::uf2 || get_file_type_idx(1) != filetype::uf2) {
        fail(ERROR_ARGS, "All files must be UF2 files\n");
    }

    if (settings.uf2.partition_set && settings.uf2.offset_set) {
        fail(ERROR_ARGS, "Cannot use both partition and offset options together\n");
    }

    auto out = get_file_idx(ios::out|ios::binary, 0);
    auto file1 = get_file_idx(ios::in|ios::binary, 1);
    auto file2 = get_file_idx(ios::in|ios::binary, 2);

    if (settings.uf2.partition_set) {
        auto access = get_file_memory_access(1);

        vector<uint8_t> bin;
        auto blocks = find_all_blocks(access, bin);
        for (auto &block : blocks) {
            auto partition_table = block->get_item<partition_table_item>();
            if (partition_table == nullptr) {
                continue;
            }

            if (settings.uf2.partition < 0 || settings.uf2.partition >= partition_table->partitions.size()) {
                fail(ERROR_ARGS, "Partition table only contains partitions 0 -> %d\n", partition_table->partitions.size() - 1);
            }

            settings.uf2.offset = partition_table->partitions[settings.uf2.partition].first_sector * 4096;
            settings.uf2.offset_set = true;
            break;
        }

        if (!settings.uf2.offset_set) {
            fail(ERROR_ARGS, "No partition table found in first UF2\n");
        }
    }

    out->seekp(0, ios::beg);

    unsigned int num_blocks = 0;
    for (auto file : {file1, file2}) {
        uf2_block block;
        // Seek to 2nd block, in case 1st is abs_block
        file->seekg(0, ios::beg);
        file->read((char*)&block, sizeof(uf2_block));
        if (file->fail()) {
            fail(ERROR_READ_FAILED, "unexpected end of input file");
        }

    #if SUPPORT_RP2350_A2
        if (check_abs_block(block)) {
            // save abs block address
            settings.uf2.abs_block = true;
            settings.uf2.abs_block_loc = block.target_addr;
            file->read((char*)&block, sizeof(uf2_block));
        }
    #endif

        num_blocks += block.num_blocks;

        if (!settings.family_id) {
            settings.family_id = block.file_size;
        }
    }

#if SUPPORT_RP2350_A2
    if (settings.uf2.abs_block) {
        uf2_block block = gen_abs_block(settings.uf2.abs_block_loc);
        out->write((char*)&block, sizeof(uf2_block));
    }
#endif

    unsigned int block_no = 0;
    unsigned int file_no = 0;
    for (auto file : {file1, file2}) {
        file->seekg(0, ios::beg);
        uf2_block block;
        unsigned int pos = 0;
        uint32_t next_family_id = 0;
        do {
            file->read((char*)&block, sizeof(uf2_block));
            if (file->fail()) {
                if (file->eof()) { file->clear(); break; }
                fail(ERROR_READ_FAILED, "unexpected end of input file");
            }
            if (block.magic_start0 == UF2_MAGIC_START0 && block.magic_start1 == UF2_MAGIC_START1 &&
                block.magic_end == UF2_MAGIC_END) {
                if (block.flags & UF2_FLAG_FAMILY_ID_PRESENT &&
                    !(block.flags & UF2_FLAG_NOT_MAIN_FLASH) && block.payload_size == PAGE_SIZE) {
                    // ignore the absolute block
                    if (check_abs_block(block)) {
                        DEBUG_LOG("Ignoring RP2350-E10 absolute block\n");
                    } else {
                        block.block_no = block_no; block_no++;
                        block.num_blocks = num_blocks;
                        block.file_size = settings.family_id;
                        if (settings.uf2.offset_set && file_no == 1) {
                            block.target_addr += settings.uf2.offset;
                        }
                        out->write((char*)&block, sizeof(uf2_block));
                    }
                }
            }
            pos += sizeof(uf2_block);
        } while (true);
        file_no++;
    }

    out->close();

    return false;
}


// Dissassembly helpers
string gpiodir(int val) {
    switch(val/4) {
        case 0: return "out";
        case 1: return "oe";
        case 2: return "in";
        default: return "unknown";
    }
}

string gpiohilo(int val) {
    switch(val % 4) {
        case 0: return "lo_" + gpiodir(val);
        case 1: return "hi_" + gpiodir(val);
        default: return "unknown";
    }
}

string gpiopxsc(int val) {
    switch(val) {
        case 0: return "put";
        case 1: return "xor";
        case 2: return "set";
        case 3: return "clr";
        default: return "unknown";
    }
}
    
string gpioxsc2(int val) {
    return gpiopxsc(val - 4) + (val > 4 ? "2" : "");
}

string gpioxsc(int val) {
    return gpiopxsc(val - 4);
}

const char *cpu_reg(int val) {
    switch (val) {
        case 0x0: return "r0";
        case 0x1: return "r1";
        case 0x2: return "r2";
        case 0x3: return "r3";
        case 0x4: return "r4";
        case 0x5: return "r5";
        case 0x6: return "r6";
        case 0x7: return "r7";
        case 0x8: return "r8";
        case 0x9: return "r9";
        case 0xa: return "sl";
        case 0xb: return "fp";
        case 0xc: return "ip";
        case 0xd: return "sp";
        case 0xe: return "lr";
        // PC is not valid for coprocessor operations - the value 15 corresponds to APSR_nzcv instead
        case 0xf: return "APSR_nzcv";
        default: return "unknown";
    }
}

bool coprodis_command::decode_line(uint32_t val, char *buf, size_t buf_len) {
    enum {
        NONE = 0,
        MCR = 0x01,
        MCR2 = 0x02,
        MRC = 0x04,
        MRC2 = 0x08,
        MCRR = 0x10,
        MCRR2 = 0x20,
        MRRC = 0x40,
        MRRC2 = 0x80,
        CDP = 0x100,
    } type = NONE;

    uint32_t mcrbits = val & 0xff100010;
    uint32_t mccrbits = (val & 0xfff00000) >> 20;
    uint32_t cdpbits = val & 0xff000010;
    const char *inst = "";

    switch (mcrbits) {
        case 0xee000010:
            type = MCR;
            inst = "mcr";
            break;
        case 0xfe000010:
            type = MCR2;
            inst = "mcr2";
            break;
        case 0xee100010:
            type = MRC;
            inst = "mrc";
            break;
        case 0xfe100010:
            type = MRC2;
            inst = "mrc2";
            break;
        default:
            break;
    }

    switch (mccrbits) {
        case 0xec4:
            type = MCRR;
            inst = "mcrr";
            break;
        case 0xfc4:
            type = MCRR2;
            inst = "mcrr2";
            break;
        case 0xec5:
            type = MRRC;
            inst = "mrrc";
            break;
        case 0xfc5:
            type = MRRC2;
            inst = "mrrc2";
            break;
        default:
            break;
    }

    switch (cdpbits) {
        case 0xee000000:
            type = CDP;
            inst = "cdp";
            break;
        default:
            break;
    }

    if (type & (MCR | MCR2 | MRC | MRC2)) {
        uint32_t opc1 = (val >> (16+5)) & 0x7u;
        uint32_t CRn = (val >> 16) & 0xfu;
        uint32_t Rt = (val >> 12) & 0xfu;
        uint32_t coproc = (val >> 8) & 0xfu;
        uint32_t opc2 = (val >> 5) & 0x7u;
        uint32_t CRm = val & 0xfu;

        if (coproc == 0) {
            // GPIO
            if (CRn != 0 || opc1 >= 8) {
//                    fail(ERROR_INCOMPATIBLE,
//                         "Instruction %s %d, #%d, %s, c%d, c%d, #%d is not supported by GPIO Coprocessor",
//                         inst, coproc, opc1, cpu_reg(Rt), CRn, CRm, opc2
//                    );
                printf("WARNING: Instruction %s %d, #%d, %s, c%d, c%d, #%d is not supported by GPIO Coprocessor\n",
                     inst, coproc, opc1, cpu_reg(Rt), CRn, CRm, opc2
                );
                return false;
            }
            if (type == MCR) {
                if (opc1 < 4) {
                    snprintf(buf, buf_len, "gpioc_%s_%s %s",
                        gpiohilo(CRm).c_str(), gpiopxsc(opc1).c_str(), cpu_reg(Rt)
                    );
                } else {
                    snprintf(buf, buf_len, "gpioc_%s_%s %s",
                        gpiodir(CRm).c_str(), gpioxsc(opc1).c_str(), cpu_reg(Rt)
                    );
                }
            } else {
                snprintf(buf, buf_len, "gpioc_%s_get %s",
                    gpiohilo(CRm).c_str(), cpu_reg(Rt)
                );
            }
        } else if (coproc == 4 || coproc == 5) {
            // DCP
            bool ns = coproc == 5;
            if (type & (MRC | MRC2)) {
                bool isP = type == MRC2;
                switch (CRm) {
                    case 0:
                        switch (opc2) {
                            case 0:
                                snprintf(buf, buf_len, "dcp%s_%sxvd %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            case 1:
                                snprintf(buf, buf_len, "dcp%s_%scmp %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            default:
                                return false;
                        }
                        break;
                    case 2:
                        switch (opc2) {
                            case 0:
                                snprintf(buf, buf_len, "dcp%s_%sdfa %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            case 1:
                                snprintf(buf, buf_len, "dcp%s_%sdfs %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            case 2:
                                snprintf(buf, buf_len, "dcp%s_%sdfm %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            case 3:
                                snprintf(buf, buf_len, "dcp%s_%sdfd %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            case 4:
                                snprintf(buf, buf_len, "dcp%s_%sdfq %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            case 5:
                                snprintf(buf, buf_len, "dcp%s_%sdfg %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            default:
                                return false;
                        }
                        break;
                    case 3:
                        switch (opc2) {
                            case 0:
                                snprintf(buf, buf_len, "dcp%s_%sdic %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            case 1:
                                snprintf(buf, buf_len, "dcp%s_%sduc %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt)
                                );
                                break;
                            default:
                                return false;
                        }
                        break;
                    default:
                        return false;
                }
            }
        } else if (coproc == 7) {
            // RCP
            if (type & (MCR | MCR2)) {
                bool delay = type == MCR;
                switch (opc1) {
                    case 0:
                        snprintf(buf, buf_len, "rcp_canary_check %s, 0x%02x (%d), %sdelay",
                            cpu_reg(Rt), CRn*16 + CRm, CRn*16 + CRm, delay ? "" : "no"
                        );
                        break;
                    case 1:
                        snprintf(buf, buf_len, "rcp_bvalid %s, %sdelay",
                            cpu_reg(Rt), delay ? "" : "no"
                        );
                        break;
                    case 2:
                        snprintf(buf, buf_len, "rcp_btrue %s, %sdelay",
                            cpu_reg(Rt), delay ? "" : "no"
                        );
                        break;
                    case 3:
                        snprintf(buf, buf_len, "rcp_bfalse %s, %sdelay",
                            cpu_reg(Rt), delay ? "" : "no"
                        );
                        break;
                    case 4:
                        snprintf(buf, buf_len, "rcp_count_set 0x%02x (%d), %sdelay",
                            CRn*16 + CRm, CRn*16 + CRm, delay ? "" : "no"
                        );
                        break;
                    case 5:
                        snprintf(buf, buf_len, "rcp_count_check 0x%02x (%d), %sdelay",
                            CRn*16 + CRm, CRn*16 + CRm, delay ? "" : "no"
                        );
                        break;
                    default:
                        return false;
                }
            } else {
                bool delay = type == MRC;
                switch (opc1) {
                    case 0:
                        snprintf(buf, buf_len, "rcp_canary_get %s, 0x%02x (%d), %sdelay",
                            cpu_reg(Rt), CRn*16 + CRm, CRn*16 + CRm, delay ? "" : "no"
                        );
                        break;
                    case 1:
                        snprintf(buf, buf_len, "rcp_canary_status %s, %sdelay",
                            cpu_reg(Rt), delay ? "" : "no"
                        );
                        break;
                    default:
                        return false;
                }
            }
        }
    } else if (type & (MCRR | MCRR2 | MRRC | MRRC2)) {
        uint32_t Rt2 = (val >> 16) & 0xfu;
        uint32_t Rt = (val >> 12) & 0xfu;
        uint32_t coproc = (val >> 8) & 0xfu;
        uint32_t opc1 = (val >> 4) & 0xfu;
        uint32_t CRm = val & 0xfu;

        if (coproc == 0) {
            // GPIO
            if (opc1 >= 12) { // || CRm not in [0, 4, 8]):
//                    fail(ERROR_INCOMPATIBLE,
//                         "Instruction %s %d, #%d, %s, %s, c%d is not supported by GPIO Coprocessor",
//                         inst.c_str(), coproc, opc1, cpu_reg(Rt), cpu_reg(Rt2), CRm
//                    );
#ifndef NDEBUG
                printf("WARNING: Instruction %s %d, #%d, %s, %s, c%d is not supported by GPIO Coprocessor\n",
                     inst, coproc, opc1, cpu_reg(Rt), cpu_reg(Rt2), CRm
                );
#endif
                return false;
            }
            if (type == MCRR) {
                if (opc1 < 4) {
                    snprintf(buf, buf_len, "gpioc_hilo_%s_%s %s, %s",
                        gpiodir(CRm).c_str(), gpiopxsc(opc1).c_str(), cpu_reg(Rt), cpu_reg(Rt2)
                    );
                } else if (opc1 < 8) {
                    snprintf(buf, buf_len, "gpioc_bit_%s_%s %s, %s",
                        gpiodir(CRm).c_str(), gpioxsc2(opc1).c_str(), cpu_reg(Rt), cpu_reg(Rt2)
                    );
                } else {
                    snprintf(buf, buf_len, "gpioc_index_%s_%s %s, %s",
                        gpiodir(CRm).c_str(), gpiopxsc(opc1 - 8).c_str(), cpu_reg(Rt), cpu_reg(Rt2)
                    );
                }
            } else {
                snprintf(buf, buf_len, "gpioc_index_%s_get %s, %s",
                    gpiodir(CRm).c_str(), cpu_reg(Rt), cpu_reg(Rt2)
                );
            }
        } else if (coproc == 4 || coproc == 5) {
            // DCP
            bool ns = coproc == 5;
            if (type == MCRR) {
                switch (opc1) {
                    case 0:
                        switch (CRm) {
                            case 0:
                                snprintf(buf, buf_len, "dcp%s_wxmd %s, %s",
                                    ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 1:
                                snprintf(buf, buf_len, "dcp%s_wymd %s, %s",
                                    ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 2:
                                snprintf(buf, buf_len, "dcp%s_wefd %s, %s",
                                    ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            default:
                                return false;
                        }
                        break;
                    case 1:
                        switch (CRm) {
                            case 0:
                                snprintf(buf, buf_len, "dcp%s_wxup %s, %s",
                                    ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 1:
                                snprintf(buf, buf_len, "dcp%s_wyup %s, %s",
                                    ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 2:
                                snprintf(buf, buf_len, "dcp%s_wxyu %s, %s",
                                    ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            default:
                                return false;
                        }
                        break;
                    case 2:
                        snprintf(buf, buf_len, "dcp%s_wxms %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 3:
                        snprintf(buf, buf_len, "dcp%s_wxmo %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 4:
                        snprintf(buf, buf_len, "dcp%s_wxdd %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 5:
                        snprintf(buf, buf_len, "dcp%s_wxdq %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 6:
                        snprintf(buf, buf_len, "dcp%s_wxuc %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 7:
                        snprintf(buf, buf_len, "dcp%s_wxic %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 8:
                        snprintf(buf, buf_len, "dcp%s_wxdc %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 9:
                        snprintf(buf, buf_len, "dcp%s_wxfc %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 10:
                        snprintf(buf, buf_len, "dcp%s_wxfm %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 11:
                        snprintf(buf, buf_len, "dcp%s_wxfd %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 12:
                        snprintf(buf, buf_len, "dcp%s_wxfq %s, %s",
                            ns ? "ns" : "", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    default:
                        return false;
                }
            } else if (type & (MRRC | MRRC2)) {
                bool isP = type == MRRC2;
                switch (CRm) {
                    case 0:
                        switch (opc1) {
                            case 1:
                                snprintf(buf, buf_len, "dcp%s_%sdda %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 3:
                                snprintf(buf, buf_len, "dcp%s_%sdds %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 5:
                                snprintf(buf, buf_len, "dcp%s_%sddm %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 7:
                                snprintf(buf, buf_len, "dcp%s_%sddd %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 9:
                                snprintf(buf, buf_len, "dcp%s_%sddq %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 11:
                                snprintf(buf, buf_len, "dcp%s_%sddg %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            default:
                                return false;
                        }
                        break;
                    case 1:
                        switch (opc1) {
                            case 1:
                                snprintf(buf, buf_len, "dcp%s_%sxyh %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 2:
                                snprintf(buf, buf_len, "dcp%s_%symr %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            case 4:
                                snprintf(buf, buf_len, "dcp%s_%sxmq %s, %s",
                                    ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                                );
                                break;
                            default:
                                return false;
                        }
                        break;
                    case 4:
                        snprintf(buf, buf_len, "dcp%s_%sxms %s, %s, #0x%01x",
                            ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2), opc1
                        );
                        break;
                    case 5:
                        snprintf(buf, buf_len, "dcp%s_%syms %s, %s, #0x%01x",
                            ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2), opc1
                        );
                        break;
                    case 8:
                        snprintf(buf, buf_len, "dcp%s_%sxmd %s, %s",
                            ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 9:
                        snprintf(buf, buf_len, "dcp%s_%symd %s, %s",
                            ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    case 10:
                        snprintf(buf, buf_len, "dcp%s_%sefd %s, %s",
                            ns ? "ns" : "", isP ? "p" : "r", cpu_reg(Rt), cpu_reg(Rt2)
                        );
                        break;
                    default:
                        return false;
                }
            }
        } else if (coproc == 7) {
            // RCP
            if (type & (MCRR | MCRR2)) {
                bool delay = type == MCRR;
                switch (opc1) {
                    case 0:
                        snprintf(buf, buf_len, "rcp_b2valid %s, %s, %sdelay",
                            cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    case 1:
                        snprintf(buf, buf_len, "rcp_b2and %s, %s, %sdelay",
                            cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    case 2:
                        snprintf(buf, buf_len, "rcp_b2or %s, %s, %sdelay",
                            cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    case 3:
                        snprintf(buf, buf_len, "rcp_bxorvalid %s, %s, %sdelay",
                            cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    case 4:
                        snprintf(buf, buf_len, "rcp_bxortrue %s, %s, %sdelay",
                            cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    case 5:
                        snprintf(buf, buf_len, "rcp_bxorfalse %s, %s, %sdelay",
                            cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    case 6:
                        snprintf(buf, buf_len, "rcp_ivalid %s, %s, %sdelay",
                            cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    case 7:
                        snprintf(buf, buf_len, "rcp_iequal %s, %s, %sdelay",
                            cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    case 8:
                        snprintf(buf, buf_len, "rcp_salt_core%d %s, %s, %sdelay",
                            CRm, cpu_reg(Rt), cpu_reg(Rt2), delay ? "" : "no"
                        );
                        break;
                    default:
                        return false;
                }
            }
        }
    } else if (type == CDP) {
        uint8_t opc1 = (val >> 20) & 0xf;
        uint8_t CRn = (val >> 16) & 0xf;
        uint8_t CRd = (val >> 12) & 0xf;
        uint8_t coproc = (val >> 8) & 0xf;
        uint8_t opc2 = (val >> 5) & 0x7;
        uint8_t CRm = val & 0xf;

        if (coproc == 0) {
            // GPIO has no cdp instructions
            return false;
        } else if (coproc == 4|| coproc == 5) {
            // DCP
            bool ns = coproc == 5;
            switch (opc1) {
                case 0:
                    if (CRm == 0) {
                        snprintf(buf, buf_len, "dcp%s_init", ns ? "ns" : "");
                    } else {
                        snprintf(buf, buf_len, "dcp%s_add0", ns ? "ns" : "");
                    }
                break;
                case 1:
                    if (opc2 == 0) {
                        snprintf(buf, buf_len, "dcp%s_add1", ns ? "ns" : "");
                    } else {
                        snprintf(buf, buf_len, "dcp%s_sub1", ns ? "ns" : "");
                    }
                break;
                case 2:
                    snprintf(buf, buf_len, "dcp%s_sqr0", ns ? "ns" : "");
                break;
                case 8:
                    if (CRm == 2 && opc2 == 0) {
                        snprintf(buf, buf_len, "dcp%s_norm", ns ? "ns" : "");
                    } else if (CRm == 2 && opc2 == 1) {
                        snprintf(buf, buf_len, "dcp%s_nrdf", ns ? "ns" : "");
                    } else if (CRm == 0 && opc2 == 1) {
                        snprintf(buf, buf_len, "dcp%s_nrdd", ns ? "ns" : "");
                    } else if (CRm == 0 && opc2 == 2) {
                        snprintf(buf, buf_len, "dcp%s_ntdc", ns ? "ns" : "");
                    } else if (CRm == 0 && opc2 == 3) {
                        snprintf(buf, buf_len, "dcp%s_nrdc", ns ? "ns" : "");
                    } else {
                        return false;
                    }
                break;
                default:
                    return false;
            }
        } else if (coproc == 7) {
            // RCP
            if (opc1 == 0 && CRd == 0 && CRn == 0 && CRm == 0) {
                snprintf(buf, buf_len, "rcp_panic");
            } else {
                return false;
            }
        }
    } else {
        return false;
    }
    return true;
}

bool coprodis_command::execute(device_map &devices) {
    auto in = get_file(ios::in);
    std::stringstream buffer;
    buffer << in->rdbuf();

    auto out = get_file_idx(ios::out, 1);

    string line;
    static char buf[512];
    buf[sizeof(buf)-1] = 0;
    std::smatch sm;
    while (std::getline(buffer, line)) {
        size_t len = line.length();
        bool replaced = false;
        auto consume_whitespace = [&](size_t &pos) {
            while (pos < len) {
                char c = line[pos];
                if (c != ' ' && c !='\t') break;
                pos++;
            }
        };
        auto consume_hex = [&](size_t &pos, uint32_t& val) {
            val = 0;
            size_t pos0 = pos;
            while (pos < len) {
                char c = line[pos];
                if (c >= '0' && c <= '9') val = (val << 4) | (c - '0');
                else if (c >= 'a' && c <= 'f') val = (val << 4) | (c + 10 - 'a');
                else if (c >= 'A' && c <= 'F') val = (val << 4) | (c + 10 - 'A');
                else break;
                pos++;
            }
            return pos - pos0;
        };

        size_t pos = 0;
        do {
            // regex is too slow on some platforms, so hand code
            if (len < 16 || len >= sizeof(buf) || line[8]!=':') break;
            uint32_t val;
            consume_whitespace(pos);
            consume_hex(pos, val);
            if (pos != 8) break;
            pos = 9;
            consume_whitespace(pos);
            // note we only care about 32 bit instructions
            uint32_t instr, tmp;
            size_t first_hex_size = consume_hex(pos, instr);
            if (first_hex_size == 2) {
                // some clang have LL HH ll hh
                consume_whitespace(pos);
                if (2 != consume_hex(pos, tmp)) break;
                instr |= (tmp << 8u);
                consume_whitespace(pos);
                if (2 != consume_hex(pos, tmp)) break;
                instr = (instr << 16) | tmp;
                consume_whitespace(pos);
                if (2 != consume_hex(pos, tmp)) break;
                instr |= (tmp << 8u);
            } else if (first_hex_size == 4) {
                // GCC has HHLL hhll
                consume_whitespace(pos);
                if (4 != consume_hex(pos, tmp)) break;
                instr = (instr << 16) | tmp;
            } else {
                break;
            }
            consume_whitespace(pos);
            if (line.compare(pos, 5, ".word") == 0) {
                // This is a .word, not a coprocessor instruction
                break;
            }
            if (pos < sizeof(buf)-1) {
                strncpy(buf, line.c_str(), sizeof(buf)-1);
                replaced = decode_line(instr, buf + pos, sizeof(buf) - pos - 1);
            }
        } while (false);
        if (replaced) {
            *out << buf << "\n";
        } else {
            *out << line << "\n";
        }
    }
    return false;
}

#if HAS_LIBUSB
static void check_otp_write_error(picoboot::command_failure &e, struct picoboot_otp_cmd *otp_cmd, model_t model) {
    if (e.get_code() == PICOBOOT_UNSUPPORTED_MODIFICATION) {
        if (otp_cmd->bEcc) fail(ERROR_NOT_POSSIBLE, "Attempted to modify OTP ECC row(s)\n");
        else     fail(ERROR_NOT_POSSIBLE, "Attempted to clear bits in OTP row(s)\n");
    } else if (
        e.get_code() == PICOBOOT_NOT_PERMITTED
        && model->chip() == rp2350
        && model->chip_revision() == rp2350_a2
        && otp_cmd->wRow >= OTP_PAGE_COUNT * (OTP_PAGE_ROWS - 1)
    ) {
        fail(ERROR_NOT_POSSIBLE, "Cannot write to page locks for pages 32-63 on RP2350 A2 due to errata RP2350-E15 - use otp permissions command instead\n");
    }
}

bool settings_select_ecc(void) {
    return settings.otp.ecc && !settings.otp.raw;
}

uint8_t otp_cmd_max_bits(void) {
    return settings_select_ecc() ? 16 : 24;
}

typedef std::function<void(uint8_t *buffer, uint32_t len, picoboot_otp_cmd &otp_cmd)> otp_read_func_t;
typedef std::function<void(uint8_t *buffer, uint32_t len, picoboot_otp_cmd &otp_cmd)> otp_write_func_t;
void process_otp_json(json &otp_json, model_t model, otp_read_func_t read_func, otp_write_func_t write_func) {
    int raw_max_bits = 24;
    for (auto row : otp_json.items()) {
        fos.first_column(0);
        string row_key = row.key();
        auto row_value = row.value();
        fos << row_key << ":\n";

        // Find matching OTP row
        bool is_sequence = false;
        auto row_matches = filter_otp({row_key}, raw_max_bits, true);
        if (row_matches.empty()) {
            fail(ERROR_INCOMPATIBLE, "%s does not match an otp row", row_key.c_str());
        } else if (row_matches.size() != 1) {
            // Check if it is a sequence
            auto row_seq0_matches = filter_otp({row_key + "0"}, raw_max_bits, false);
            auto row_seq_0_matches = filter_otp({row_key + "_0"}, raw_max_bits, false);
            if (row_seq0_matches.size() == 1) {
                row_matches = row_seq0_matches;
            } else if (row_seq_0_matches.size() == 1) {
                row_matches = row_seq_0_matches;
            }
            else if (row_matches.size() != (*row_matches.begin()).second.reg->seq_length) {
                for (auto x : row_matches) {
                    DEBUG_LOG("Matches %s\n", x.second.reg->name.c_str());
                }
                fail(ERROR_INCOMPATIBLE, "%s matches multiple otp rows or sequences", row_key.c_str());
            }
            is_sequence = true;
        }
        auto row_match = *row_matches.begin();
        auto reg = row_match.second.reg;
        vector<uint8_t> data;
        struct picoboot_otp_cmd otp_cmd;
        int hex_val = 0;
        unsigned int row_size = 0;

        fos.first_column(2);

        if (reg != nullptr) {
            otp_cmd.wRow = row_match.second.reg_row;
            otp_cmd.wRowCount = is_sequence ? reg->seq_length : (reg->redundancy ? reg->redundancy : 1);
            otp_cmd.bEcc = reg->ecc;
            row_size = otp_cmd.bEcc ? 2 : 4;

            // Calculate row value
            if (row_value.is_object()) {
                // Specified fields
                struct picoboot_otp_cmd tmp_otp_cmd = otp_cmd;
                tmp_otp_cmd.wRowCount = 1;
                tmp_otp_cmd.bEcc = 0;
                uint32_t old_raw_value;
                read_func((uint8_t*)&old_raw_value, sizeof(old_raw_value), tmp_otp_cmd);

                uint32_t reg_value = 0;
                uint32_t full_mask = 0;
                for (auto field_val : row_value.items()) {
                    string key = field_val.key();
                    auto value = field_val.value();
                    if (!get_json_int(value, hex_val)) {
                        fail(ERROR_FORMAT, "Values must be integers");
                    }

                    // Find matching OTP field
                    auto field_matches = filter_otp({row_key + "." + key}, raw_max_bits, false);
                    if (field_matches.size() != 1) {
                        fail(ERROR_INCOMPATIBLE, "%s is not a single otp field", key.c_str());
                    }
                    auto field_match = *field_matches.begin();
                    auto field = field_match.second.field;

                    fos << key << ": " << hex_string(hex_val) << "\n";

                    int low = __builtin_ctz(field->mask);

                    if (hex_val & (~field->mask >> low)) {
                        fail(ERROR_NOT_POSSIBLE, "Value to set does not fit in field: value %06x, mask %06x\n", hex_val, field->mask >> low);
                    }

                    hex_val <<= low;
                    hex_val &= field->mask;
                    full_mask |= field->mask;
                    reg_value |= hex_val;
                }
                reg_value |= old_raw_value & ~full_mask;
                vector<uint8_t> tmp((uint8_t*)(&reg_value), (uint8_t*)(&reg_value) + row_size);
                data.insert(data.begin(), tmp.begin(), tmp.end());
            } else if (row_value.is_array()) {
                for (auto val : row_value)  {
                    if (!get_json_int(val, hex_val)) {
                        fail(ERROR_FORMAT, "Values must be integers");
                    }
                    fos << hex_string(hex_val, 2) << ", ";
                    data.push_back(hex_val);
                }
                fos << "\n";
            } else {
                if (!get_json_int(row_value, hex_val)) {
                    fail(ERROR_FORMAT, "Values must be integers");
                }
                fos << hex_string(hex_val) << "\n";
                vector<uint8_t> tmp((uint8_t*)(&hex_val), (uint8_t*)(&hex_val) + row_size);
                data.insert(data.begin(), tmp.begin(), tmp.end());
            }
        } else {
            // Must be a raw address
            otp_cmd.wRow = row_match.second.reg_row;
            otp_cmd.wRowCount = 1;
            otp_cmd.bEcc = row_value["ecc"].is_boolean() ? (bool)row_value["ecc"] : false;
            row_size = otp_cmd.bEcc ? 2 : 4;

            auto val = row_value["value"];
            if (val.is_array()) {
                for (auto v : val)  {
                    if (!get_json_int(v, hex_val)) {
                        fail(ERROR_FORMAT, "Values must be integers");
                    }
                    fos << hex_string(hex_val, 2) << ", ";
                    data.push_back(hex_val);
                }
                fos << "\n";
                otp_cmd.wRowCount = data.size() / row_size;
            } else {
                if (!get_json_int(val, hex_val)) {
                    fail(ERROR_FORMAT, "Values must be integers");
                }
                fos << hex_string(hex_val) << "\n";
                vector<uint8_t> tmp((uint8_t*)(&hex_val), (uint8_t*)(&hex_val) + row_size);
                data.insert(data.begin(), tmp.begin(), tmp.end());
                if (get_json_int(row_value["redundancy"], hex_val)) otp_cmd.wRowCount = hex_val;
            }
        }

        if (data.size() % row_size) {
            fail(ERROR_FORMAT, "Data size must be a multiple of selected row data size (%d)", row_size);
        }
        if (data.size() == row_size && otp_cmd.wRowCount > 1) {
            // Repeat the data for each redundant row
            data.resize(otp_cmd.wRowCount * row_size);
            for (int i=1; i < otp_cmd.wRowCount; i++) std::copy_n(data.begin(), row_size, data.begin() + row_size*i);
        }

        if (data.size() != row_size * otp_cmd.wRowCount) {
            fail(ERROR_FORMAT, "Data size must be selected row data size * row count (%d*%d)", row_size, otp_cmd.wRowCount);
        }

        write_func(data.data(), data.size(), otp_cmd);
    }
}

static void hack_init_otp_regs() {
    // build map of OTP regs by offset
    if (settings.otp.extra_files.size() > 0) {
        DEBUG_LOG("Using extra OTP files:\n");
        for (auto file : settings.otp.extra_files) {
            DEBUG_LOG("%s\n", file.c_str());
        }
    }
    init_otp(otp_regs, settings.otp.extra_files);
}


bool otp_get_command::execute(device_map &devices) {
    auto con = get_single_picoboot_cmd_compatible_device_connection("otp get", devices, {PC_OTP_READ}, false);
    hack_init_otp_regs();
    picoboot_memory_access raw_access(con);
    auto model = raw_access.get_model();
    auto matches = filter_otp(settings.otp.selectors, otp_cmd_max_bits(), settings.otp.fuzzy);
    uint32_t last_reg_row = UINT32_MAX; // invalid
    bool first = true;
    char buf[512];
    uint32_t raw_buffer[OTP_PAGE_ROWS];
    memset(raw_buffer, 0xaa, sizeof(raw_buffer));
    int indent0 = settings.otp.list_pages ? 18 : 8;
    uint32_t last_page = -1;
    for (const auto& e : matches) {
        const auto &m = e.second;
        bool do_ecc = settings.otp.ecc;
        int redundancy = settings.otp.redundancy;
        uint32_t corrected_val = 0;
        if (m.reg_row / OTP_PAGE_ROWS != last_page) {
            // todo pre-check page lock
            struct picoboot_otp_cmd otp_cmd;
            if (m.reg_row / OTP_PAGE_ROWS >= OTP_PAGE_COUNT - OTP_SPECIAL_PAGES) {
                // Read individual rows for lock words
                otp_cmd.wRow = m.reg_row;
                otp_cmd.wRowCount = 1;
                otp_cmd.bEcc = 0;
                con.otp_read(&otp_cmd, (uint8_t *)&(raw_buffer[m.reg_row % OTP_PAGE_ROWS]), sizeof(raw_buffer[0]));
            } else {
                // Otherwise read a page at a time
                last_page = m.reg_row / OTP_PAGE_ROWS;
                otp_cmd.wRow = last_page * OTP_PAGE_ROWS;
                otp_cmd.wRowCount = OTP_PAGE_ROWS;
                otp_cmd.bEcc = 0;
                con.otp_read(&otp_cmd, (uint8_t *)raw_buffer, sizeof(raw_buffer));
            }
        }
        if (m.reg_row != last_reg_row) {
            last_reg_row = m.reg_row;
            // Write out header for row
            fos.first_column(0);
            if (!first) fos.wrap_hard();
            first = false;
            fos.hanging_indent(7);
            snprintf(buf, sizeof(buf), "ROW 0x%04x", m.reg_row);
            fos << buf;
            if (settings.otp.list_pages) {
                snprintf(buf, sizeof(buf), " (0x%02x:0x%02x)", m.reg_row / OTP_PAGE_ROWS,
                         m.reg_row % OTP_PAGE_ROWS);
                fos << buf;
            }
            if (m.reg) {
                fos << ": " << m.reg->name;
                if (settings.otp.list_no_descriptions) {
                    if (m.reg->ecc) {
                        fos << " (ECC)";
                    } else if (m.reg->crit) {
                        fos << " (CRIT)";
                    } else if (m.reg->redundancy) {
                        fos << " (RBIT-" << m.reg->redundancy << ")";
                    }
                }
                if (m.reg->seq_length) {
                    fos << " (Part " << (m.reg->seq_index + 1) << "/" << m.reg->seq_length << ")";
                }
                // Set the ecc and redundancy, unless the user specified values
                do_ecc |= (m.reg->ecc && !settings.otp.raw);
                if (redundancy < 0) redundancy = m.reg->redundancy;
            }
            fos << "\n";
            if (m.reg && !settings.otp.list_no_descriptions && !m.reg->description.empty()) {
                fos.first_column(indent0);
                fos.hanging_indent(0);
                fos << "\"" << m.reg->description << "\"";
                fos.first_column(0);
                fos << "\n";
            }
            fos.first_column(4);
            fos.hanging_indent(10);
            uint32_t raw_value = raw_buffer[m.reg_row % OTP_PAGE_ROWS];
            char raw_buf[16 * 1024];
            uint8_t buf_pos = 0;
            buf_pos += snprintf(raw_buf+buf_pos, sizeof(raw_buf), "RAW_VALUE=0x%06x", raw_value);
            for (int i=1; i < std::max(redundancy, 1); i++) {
                raw_value = raw_buffer[(m.reg_row % OTP_PAGE_ROWS) + i];
                buf_pos += snprintf(raw_buf+buf_pos, sizeof(raw_buf) - buf_pos, ";0x%06x", raw_value);
                if (3 == (raw_value >> 22)) {
                    raw_value ^= 0xffffff;
                    snprintf(buf, sizeof(buf), "(flipping raw value to 0x%08x)", raw_value);
                    fos << buf;
                }
            }
            if (do_ecc) {
                corrected_val = otp_calculate_ecc(raw_value &0xffff);
                snprintf(buf, sizeof(buf), "\nVALUE 0x%04x\n", corrected_val &0xffff);
                fos << buf;
                // todo more clarity over ECC settings
                // todo recovery
                if (corrected_val != raw_value) {
                    fos << raw_buf;
                    fos << " (WARNING - ECC IS INVALID)";
                }
            } else if (redundancy > 0) {
                uint8_t sets[24] = {};
                uint8_t clears[24] = {};
                bool diff = false;
                bool crit = m.reg ? m.reg->crit : false;
                for (int i=0; i < redundancy; i++) {
                    raw_value = raw_buffer[(m.reg_row % OTP_PAGE_ROWS) + i];
                    for (int b=0; b < 24; b++) raw_value & (1 << b) ? sets[b]++ : clears[b]++;
                }
                for (int b=0; b < 24; b++){
                    if(sets[b] >= clears[b] || (crit && sets[b] >= 3)) corrected_val |= (1 << b);
                    if (sets[b] && clears[b]) diff = true;
                }
                snprintf(buf, sizeof(buf), "\nVALUE 0x%06x\n", corrected_val);
                if (diff) {
                    fos << raw_buf;
                    fos << " (WARNING - REDUNDANT ROWS AREN'T EQUAL)";
                }
                fos << buf;
            } else {
                corrected_val = raw_value;
                snprintf(buf, sizeof(buf), "\nVALUE 0x%06x\n", corrected_val);
                fos << buf;
            }
            fos << "\n";
        }
        if (m.reg) {
            for (const auto &f: m.reg->fields) {
                if (f.mask & m.mask) {
                    fos.first_column(4);
                    fos.hanging_indent(10);
                    // todo should we care if the fields overlap - i think this is fine for here in list
                    int low = __builtin_ctz(f.mask);
                    int high = 31 - __builtin_clz(f.mask);
                    fos << "field " << f.name;
                    if (low == high) {
                        fos << " (bit " << low << ")";
                    } else {
                        fos << " (bits " << low << "-" << high << ")";
                    }
                    snprintf(buf, sizeof(buf), " = %x\n", (corrected_val & f.mask) >> low);
                    fos << buf;
                    if (!settings.otp.list_no_descriptions && !f.description.empty()) {
                        fos.first_column(indent0);
                        fos.hanging_indent(0);
                        fos << "\"" << f.description << "\"";
                        fos.first_column(0);
                        fos << "\n";
                    }
                }
            }
        }
    #if ENABLE_DEBUG_LOG
        if (do_ecc) {
            struct picoboot_otp_cmd otp_cmd;
            otp_cmd.wRow = m.reg_row;
            otp_cmd.bEcc = 1;
            otp_cmd.wRowCount = 1;
            uint16_t val = 0xaaaa;
            con.otp_read(&otp_cmd, (uint8_t *)&val, sizeof(val));
            snprintf(buf, sizeof(buf), "EXTRA ECC READ: %04x\n", val);
            fos << buf;
        }
    #endif
    }
    return false;
}

bool otp_dump_command::execute(device_map &devices) {
    // todo pre-check page lock
    bool do_ecc = settings.otp.ecc && !settings.otp.raw;
    vector<uint8_t> raw_buffer;
    uint8_t row_size = do_ecc ? 2 : 4;
    raw_buffer.resize(OTP_ROW_COUNT * row_size);
    std::map<int, string> page_errors;
    std::map<int, string> row_errors;

    if (!settings.filenames[0].empty()) {
        std::shared_ptr<std::fstream> file = get_file(ios::in|ios::binary);
        if (get_file_type() != filetype::json) {
            fail(ERROR_ARGS, "Input file must be a JSON file");
        }
        hack_init_otp_regs();
        json otp_json = json::parse(*file);

        // Prevent outputting to the console
        fos_ptr = fos_null_ptr;
        auto model = models::largest;

        process_otp_json(otp_json, model,
            [&](uint8_t *buffer, uint32_t len, picoboot_otp_cmd &otp_cmd) {
                memset(buffer, 0, len);
            }, [&](uint8_t *buffer, uint32_t len, picoboot_otp_cmd &otp_cmd) {
                uint32_t offset = otp_cmd.wRow * row_size;
                uint8_t write_row_size = len / otp_cmd.wRowCount;
                if (otp_cmd.wRowCount * row_size == len) {
                    memcpy(raw_buffer.data() + offset, buffer, len);
                } else {
                    for (int i=0; i < otp_cmd.wRowCount; i++) {
                        if (do_ecc) {
                            // only copy row_size bytes from the buffer, as we're ignoring the ECC bits
                            memcpy(raw_buffer.data() + offset + i * row_size, buffer + i * write_row_size, row_size);
                        } else {
                            // calculate the ECC for the row and write it to the raw buffer
                            uint32_t val = otp_calculate_ecc(*(uint16_t*)(buffer + i * write_row_size));
                            memcpy(raw_buffer.data() + offset + i * row_size, &val, row_size);
                        }
                    }
                }
            }
        );
        fos_ptr = fos_base_ptr;
    } else {
        auto con = get_single_picoboot_cmd_compatible_device_connection("otp dump", devices, {PC_OTP_READ}, false);
        struct picoboot_otp_cmd otp_cmd;
        otp_cmd.bEcc = do_ecc;
        // Read most pages by page, as permissions are per page
        otp_cmd.wRowCount = OTP_PAGE_ROWS;
        for (int i=0; i < OTP_PAGE_COUNT - OTP_SPECIAL_PAGES; i++) {
            otp_cmd.wRow = i * OTP_PAGE_ROWS;
            try {
                con.otp_read(&otp_cmd, raw_buffer.data() + i*(raw_buffer.size() / OTP_PAGE_COUNT), raw_buffer.size() / OTP_PAGE_COUNT);
            } catch (picoboot::command_failure& e) {
                if (e.get_code() == PICOBOOT_NOT_PERMITTED) {
                    page_errors[i] = e.what();
                } else {
                    throw e;
                }
            }
        }

        // Read special pages by row, as permissions are special
        otp_cmd.wRowCount = 1;
        for (int i=(OTP_PAGE_COUNT - OTP_SPECIAL_PAGES) * OTP_PAGE_ROWS; i < OTP_PAGE_COUNT * OTP_PAGE_ROWS; i++) {
            otp_cmd.wRow = i;
            try {
                con.otp_read(&otp_cmd, raw_buffer.data() + i * row_size, row_size);
            } catch (picoboot::command_failure& e) {
                if (e.get_code() == PICOBOOT_NOT_PERMITTED) {
                    row_errors[i] = e.what();
                } else {
                    throw e;
                }
            }
        }
    }

    fos.first_column(0);

    if (!settings.filenames[1].empty()) {
        fos << "Outputting to " << settings.filenames[1] << "\n";
        std::shared_ptr<std::fstream> file = get_file_idx(ios::out|ios::binary, 1);
        file->write((char*)raw_buffer.data(), raw_buffer.size());
        file->close();
    } else {
        char buf[256];
        for(int i=0;i<OTP_ROW_COUNT;i+=8) {
            if (settings.otp.dump_pages) {
                snprintf(buf, sizeof(buf), "%02d:%02d: ", i / OTP_PAGE_ROWS, i % OTP_PAGE_ROWS);
                fos << buf;
            } else {
                snprintf(buf, sizeof(buf), "%04x: ", i);
                fos << buf;
            }

            for (int j = i; j < i + 8; j++) {
                if (row_errors.find(j) != row_errors.end() || page_errors.find(j / OTP_PAGE_ROWS) != page_errors.end()) {
                    snprintf(buf, sizeof(buf), "%s, ", do_ecc ? "XXXX" : "XXXXXXXX");
                } else if (do_ecc) {
                    snprintf(buf, sizeof(buf), "%04x, ", ((uint16_t *) raw_buffer.data())[j]);
                } else {
                    snprintf(buf, sizeof(buf), "%08x, ", ((uint32_t *) raw_buffer.data())[j]);
                }
                fos << buf;
            }
            fos << "\n";
        }
    }
    return false;
}

bool otp_load_command::execute(device_map &devices) {
    auto con = get_single_picoboot_cmd_compatible_device_connection("otp load", devices, {PC_OTP_READ, PC_OTP_WRITE});
    picoboot_memory_access raw_access(con);
    auto model = raw_access.get_model();
    // todo pre-check page lock
    struct picoboot_otp_cmd otp_cmd;
    std::shared_ptr<std::fstream> file = get_file(ios::in|ios::binary);
    if (get_file_type() == filetype::json) {
        hack_init_otp_regs();
        json otp_json = json::parse(*file);
        // todo validation on json
        process_otp_json(otp_json, model,
            [&](uint8_t *buffer, uint32_t len, picoboot_otp_cmd &otp_cmd) {
                picoboot_memory_access raw_access(con);
                con.otp_read(&otp_cmd, buffer, len);
            }, [&](uint8_t *buffer, uint32_t len, picoboot_otp_cmd &otp_cmd) {
                try {
                    con.otp_write(&otp_cmd, buffer, len);
                } catch (picoboot::command_failure &e) {
                    check_otp_write_error(e, &otp_cmd, model);
                    throw e;
                }
        });

        // Return now, don't do rest of function
        return false;
    }
    otp_cmd.wRow = settings.otp.row;
    otp_cmd.bEcc = settings_select_ecc();
    unsigned int row_size = otp_cmd.bEcc ? 2 : 4;
    file->seekg(0, ios::end);
    uint32_t file_size = file->tellg();
    if (file_size % row_size) {
        fail(ERROR_FORMAT, "File size must be a multiple of selected row data size (%d)", row_size);
    }
    unsigned int rows = file_size / row_size;
    if (rows < 1 || otp_cmd.wRow + rows > OTP_ROW_COUNT) {
        fail(ERROR_FORMAT, "OTP data will not fit starting at row %d\n", otp_cmd.wRow);
    }
    otp_cmd.wRowCount = rows;
    file->seekg(0, ios::beg);

    std::unique_ptr<uint8_t[]> unique_file_buffer(new uint8_t[file_size]());
    uint8_t* file_buffer = unique_file_buffer.get();
    file->read((char*)file_buffer, file_size);
    try {
        con.otp_write(&otp_cmd, (uint8_t *)file_buffer, file_size);
    } catch (picoboot::command_failure &e) {
        check_otp_write_error(e, &otp_cmd, model);
        throw e;
    }

    std::unique_ptr<uint8_t[]> unique_verify_buffer(new uint8_t[file_size]());
    uint8_t* verify_buffer = unique_verify_buffer.get();
    con.otp_read(&otp_cmd, (uint8_t *)verify_buffer, file_size);
    unsigned int i;
    for(i=0;i<file_size;i++) {
        if (file_buffer[i] != verify_buffer[i]) {
            std::cout << "  Mismatch at row " << hex_string(i/row_size) << "\n";
            break;
        }
    }
    if (i == file_size) {
        std::cout << "  Verified OK\n";
    }
    return false;
}
#endif

bool otp_list_command::execute(device_map &devices) {
    init_otp(otp_regs, settings.otp.extra_files);

    auto matches = filter_otp(settings.otp.selectors.empty() ? std::vector<string>({":"}) : settings.otp.selectors, 24, true);
    uint32_t last_reg_row = UINT32_MAX; // invalid
    bool first = true;
    char buf[512];
    int indent0 = settings.otp.list_pages ? 18 : 8;
    for (const auto& e : matches) {
        const auto &m = e.second;
        if (!m.reg) continue;
        auto write_reg_header_if_necessary = [&] {
            // lazy row header output
            if (m.reg_row != last_reg_row && m.reg) {
                last_reg_row = m.reg_row;
                // Write out header for row
                fos.first_column(0);
                if (!first) fos.wrap_hard();
                first = false;
                fos.hanging_indent(7);
                snprintf(buf, sizeof(buf), "ROW 0x%04x", m.reg_row);
                fos << buf;
                if (settings.otp.list_pages) {
                    snprintf(buf, sizeof(buf), " (0x%02x:0x%02x)", m.reg_row / OTP_PAGE_ROWS, m.reg_row % OTP_PAGE_ROWS);
                    fos << buf;
                }
                fos << ": " << m.reg->name;
                if (settings.otp.list_no_descriptions) {
                    if (m.reg->ecc) {
                        fos << " (ECC)";
                    } else if (m.reg->crit) {
                        fos << " (CRIT)";
                    } else if (m.reg->redundancy) {
                        fos << " (RBIT-" << m.reg->redundancy << ")";
                    }
                }
                if (m.reg->seq_length) {
                    fos << " (Part " << (m.reg->seq_index + 1) << "/" << m.reg->seq_length << ")";
                }
                fos << "\n";
                if (!settings.otp.list_no_descriptions && !m.reg->description.empty()) {
                    fos.first_column(indent0);
                    fos.hanging_indent(0);
                    fos << "\"" << m.reg->description << "\"";
                    fos.first_column(0);
                    fos << "\n";
                }
            }
        };
        if (m.reg->fields.empty()) {
            write_reg_header_if_necessary();
            fos.first_column(4);
            fos << "(row has no sub-fields)\n";
        }
        for(const auto &f : m.reg->fields) {
            if (f.mask & m.mask) {
                write_reg_header_if_necessary();
                // todo should we care if the fields overlap - i think this is fine for here in list
                fos.first_column(4);
                fos.hanging_indent(10);
                int low = __builtin_ctz(f.mask);
                int high = 31 - __builtin_clz(f.mask);
                fos << "field " << f.name ;
                if (low == high) {
                    fos << " (bit " << low << ")\n";
                } else {
                    fos << " (bits " << low << "-" << high << ")\n";
                }
                if ((m.field || settings.otp.list_field_descriptions) && !settings.otp.list_no_descriptions && !f.description.empty()) {
                    // Only print field descriptors if matching a field, or if list_field_descriptions is set
                    fos.first_column(indent0);
                    fos.hanging_indent(0);
                    fos << "\"" << f.description << "\"";
                    fos.first_column(0);
                    fos << "\n";
                }
            }
        }
    }
    return false;
}

#if HAS_LIBUSB
bool otp_set_command::execute(device_map &devices) {
    auto con = get_single_picoboot_cmd_compatible_device_connection("otp set", devices, {PC_OTP_READ, PC_OTP_WRITE});
    hack_init_otp_regs();
    picoboot_memory_access raw_access(con);
    auto model = raw_access.get_model();
    auto matches = filter_otp(settings.otp.selectors, otp_cmd_max_bits(), settings.otp.fuzzy);
    // baing lazy to count
    std::set<uint32_t> unique_rows;
    std::transform(matches.begin(), matches.end(), std::inserter(unique_rows, unique_rows.begin()), [](const auto&e) { return e.first.first; });
    if (unique_rows.empty()) {
        fail(ERROR_ARGS, " no OTP rows matched for writing.");
    }
    if (unique_rows.size() != 1) {
        fail(ERROR_ARGS, " multiple OTP rows matched, so write is not allowed.");
    }
    if (matches.size() != 1) {
        fail(ERROR_ARGS, " multiple OTP fields matched, so write is not allowed.");
    }
    uint32_t mask = 0;
    for(const auto& e : matches) {
        mask |= e.second.mask;
    }
    uint32_t reg_row = *unique_rows.begin();
    char buf[512];
    int indent0 = settings.otp.list_pages ? 18 : 8;
    // todo check page permissions
    // todo write only
    struct picoboot_otp_cmd otp_cmd;
    otp_cmd.wRow = reg_row;
    otp_cmd.wRowCount = 1;
    otp_cmd.bEcc = 0;
    uint32_t old_raw_value;
    con.otp_read(&otp_cmd, (uint8_t *)&old_raw_value, sizeof(old_raw_value));
    fos.first_column(0);
    fos.hanging_indent(7);
    snprintf(buf, sizeof(buf), "ROW 0x%04x", reg_row);
    fos << buf;
    snprintf(buf, sizeof(buf), "  OLD_VALUE=0x%06x", old_raw_value);
    fos << buf;
    const otp_reg *reg = matches.cbegin()->second.reg;
    if (settings.otp.list_pages) {
        snprintf(buf, sizeof(buf), " (0x%02x:0x%02x)", reg_row / OTP_PAGE_ROWS, reg_row % OTP_PAGE_ROWS);
        fos << buf;
    }
    if (reg) {
        fos << ": " << reg->name;
        if (settings.otp.list_no_descriptions) {
            if (reg->ecc) {
                fos << " (ECC)";
            } else if (reg->crit) {
                fos << " (CRIT)";
            } else if (reg->redundancy) {
                fos << " (RBIT-" << reg->redundancy << ")";
            }
        }
        if (reg->seq_length) {
            fos << " (Part " << (reg->seq_index + 1) << "/" << reg->seq_length << ")";
        }
        // Set the ecc and redundancy, unless the user specified values
        settings.otp.ecc |= (reg->ecc && !settings.otp.raw);
        if (settings.otp.redundancy < 0) settings.otp.redundancy = reg->redundancy;
    }
    fos << "\n";
    if (reg && !settings.otp.list_no_descriptions && !reg->description.empty()) {
        fos.first_column(indent0);
        fos.hanging_indent(0);
        fos << "\"" << reg->description << "\"";
        fos.first_column(0);
        fos << "\n";
    }
#if 0
    fos.first_column(4);
    fos.hanging_indent(10);
    fos << buf;
    fos << "\n";
    for (const auto &f: m.reg->fields) {
        if (f.mask & m.mask) {
            fos.first_column(4);
            fos.hanging_indent(10);
            // todo should we care if the fields overlap - i think this is fine for here in list
            int low = __builtin_ctz(f.mask);
            int high = 31 - __builtin_clz(f.mask);
            fos << "field " << f.name;
            if (low == high) {
                fos << " (bit " << low << ")\n";
            } else {
                fos << " (bits " << low << "-" << high << ")\n";
            }
        }
    }
#endif
    // if writing field, only write that field
    const otp_field *field = matches.cbegin()->second.field;
    if (field) {
        fos.first_column(4);
        fos.hanging_indent(10);
        int low = __builtin_ctz(field->mask);
        int high = 31 - __builtin_clz(field->mask);
        fos << "field " << field->name;
        if (low == high) {
            fos << " (bit " << low << ")\n";
        } else {
            fos << " (bits " << low << "-" << high << ")\n";
        }

        if (settings.otp.value & (~field->mask >> low)) {
            fail(ERROR_NOT_POSSIBLE, "Value to set does not fit in field: value %06x, mask %06x\n", settings.otp.value, field->mask >> low);
        }

        settings.otp.value <<= low;
        settings.otp.value &= field->mask;
        settings.otp.value |= old_raw_value & ~field->mask;
    }
    if (settings.otp.ignore_set) {
        // OR with current value, to ignore any already-set bits
        settings.otp.value |= old_raw_value;
    }
    if (~settings.otp.value & old_raw_value) {
        fail(ERROR_NOT_POSSIBLE, "Cannot clear bits in OTP row(s): current value %06x, new value %06x\n", old_raw_value, settings.otp.value);
    }
    otp_cmd.bEcc = settings_select_ecc();
    // todo check for clearing bits instead
    if (old_raw_value && otp_cmd.bEcc) {
        fail(ERROR_NOT_POSSIBLE, "Cannot modify OTP ECC row(s)\n");
    }
    try {
        if (otp_cmd.bEcc) {
            uint16_t write_value = settings.otp.value;
            con.otp_write(&otp_cmd, (uint8_t *) &write_value, sizeof(write_value));
        } else if (settings.otp.redundancy > 0) {
            otp_cmd.wRowCount = settings.otp.redundancy;
            vector<uint32_t> write_value;
            for (int i=0; i < otp_cmd.wRowCount; i++) write_value.push_back(settings.otp.value);
            con.otp_write(&otp_cmd, (uint8_t *)write_value.data(), write_value.size() * sizeof(uint32_t));
        } else {
            uint32_t write_value = settings.otp.value;
            con.otp_write(&otp_cmd, (uint8_t *)&write_value, sizeof(write_value));
        }
    } catch (picoboot::command_failure &e) {
        check_otp_write_error(e, &otp_cmd, model);
        throw e;
    }

    return false;
}

bool otp_permissions_command::execute(device_map &devices) {
    auto con = get_single_picoboot_cmd_compatible_device_connection("otp permissions", devices, {PC_OTP_READ, PC_OTP_WRITE});
    picoboot_memory_access raw_access(con);
    auto model = raw_access.get_model();

    json perms_json = json::parse(*get_file(ios::in|ios::binary));

    auto tmp = std::make_shared<std::stringstream>();
    auto file = get_xip_ram_perms();
    *tmp << file->rdbuf();

    auto program = get_iostream_memory_access<iostream_memory_access>(tmp, filetype::elf, true);
    // todo what do we need from this model - if it is generic, we should make a generic one
    program.set_model(std::make_unique<model_rp2350>());

    settings.config.group = "otp_page_permissions";
    for (auto it = perms_json.begin(); it != perms_json.end(); ++it) {
        settings.otp.lock0 = 0;
        settings.otp.lock1 = 0;
        int page;
        if (!get_int(it.key(), page)) continue;
        std::stringstream ss;
        ss << "page" << page;
        settings.config.key = ss.str();
        std::cout << ss.str() << std::endl;
        auto perms = it.value();
        int tmp;
        if (perms.contains("no_key_state")) {
            get_json_int(perms["no_key_state"], tmp);
            settings.otp.lock0 |= (tmp << OTP_DATA_PAGE0_LOCK0_NO_KEY_STATE_LSB);
        }
        if (perms.contains("key_r")) {
            get_json_int(perms["key_r"], tmp);
            settings.otp.lock0 |= (tmp << OTP_DATA_PAGE0_LOCK0_KEY_R_LSB);
        }
        if (perms.contains("key_w")) {
            get_json_int(perms["key_w"], tmp);
            settings.otp.lock0 |= (tmp << OTP_DATA_PAGE0_LOCK0_KEY_W_LSB);
        }
        if (perms.contains("lock_bl")) {
            get_json_int(perms["lock_bl"], tmp);
            settings.otp.lock1 |= (tmp << OTP_DATA_PAGE0_LOCK1_LOCK_BL_LSB);
        }
        if (perms.contains("lock_ns")) {
            get_json_int(perms["lock_ns"], tmp);
            settings.otp.lock1 |= (tmp << OTP_DATA_PAGE0_LOCK1_LOCK_NS_LSB);
        }
        if (perms.contains("lock_s")) {
            get_json_int(perms["lock_s"], tmp);
            settings.otp.lock1 |= (tmp << OTP_DATA_PAGE0_LOCK1_LOCK_S_LSB);
        }
        settings.config.value = hex_string(settings.otp.lock1 << 16 | settings.otp.lock0);
        config_guts(program);
    }

    settings.config.group = "led_config";
    // UART Config
    if (settings.otp.led_pin != -1) {
        settings.config.key = "led";
        settings.config.value = hex_string(settings.otp.led_pin);
        config_guts(program);
    }

#if HAS_MBEDTLS
    private_t private_key = {};
    public_t public_key = {};

    if (settings.seal.sign && settings.filenames[2].empty()) {
        fail(ERROR_ARGS, "missing key file for signing");
    }

    if (!settings.filenames[2].empty() && get_file_type_idx(2) != filetype::pem) {
        fail(ERROR_ARGS, "Can only read pem keys");
    }

    if (settings.seal.sign) read_keys(settings.filenames[2], &public_key, &private_key);

    elf_file source_file(settings.verbose);
    elf_file *elf = &source_file;
    elf->read_file(tmp);
    sign_guts_elf(elf, private_key, public_key, program.get_model());
    auto out = std::make_shared<std::stringstream>();
    elf->write(out);

    auto signed_program = get_iostream_memory_access<iostream_memory_access>(out, filetype::elf, true);
#else
    if (settings.seal.sign) fail(ERROR_NOT_POSSIBLE, "Cannot sign binaries without mbedtls");
    auto signed_program = get_iostream_memory_access<iostream_memory_access>(tmp, filetype::elf, true);
#endif

    settings.load.execute = true;
    load_guts(con, signed_program);

    // todo: read back after reboot (requires lots of stuff)

    return true;
}

enum wl_type {
    wl_value,
    wl_bcd,
    wl_strdef,
    wl_unistrdef
};

void wl_do_field(json json_data, vector<uint16_t>& data, uint32_t& flags, const otp_reg* flags_reg, tuple<string, string, wl_type, uint8_t> field, int idx) {
    auto cat = std::get<0>(field);
    auto sub = std::get<1>(field);
    auto type = std::get<2>(field);
    auto max_strlen = std::get<3>(field);

    int hex_val = 0;

    if (json_data.contains(cat) && json_data[cat].contains(sub)) {
        auto val = json_data[cat][sub];
        switch (type)
        {
        case wl_value:
            if (!get_json_int(val, hex_val)) {
                fail(ERROR_FORMAT, "%s.%s must be an integer", cat.c_str(), sub.c_str());
            }
            break;
        
        case wl_bcd:
            if (!get_json_bcd(val, hex_val)) {
                fail(ERROR_FORMAT, "%s.%s must be a float or integer less than 100", cat.c_str(), sub.c_str());
            }
            break;

        case wl_strdef:
            if (get_json_strdef(val, hex_val, data, max_strlen)) {
                if (hex_val & 0x80) {
                    fail(ERROR_FORMAT, "%s.%s must be an ascii string", cat.c_str(), sub.c_str());
                }
            } else {
                fail(ERROR_FORMAT, "%s.%s must be a string with < %d characters", cat.c_str(), sub.c_str(), max_strlen);
            }
            break;
        
        case wl_unistrdef:
            if (!get_json_strdef(val, hex_val, data, max_strlen)) {
                fail(ERROR_FORMAT, "%s.%s must be a string with < %d characters", cat.c_str(), sub.c_str(), max_strlen);
            }
            break;
        }

        data[idx] = hex_val;
        flags |= 1 << idx;
    }
}

bool otp_white_label_command::execute(device_map &devices) {
    auto con = get_single_picoboot_cmd_compatible_device_connection("otp white-label", devices, {PC_OTP_READ, PC_OTP_WRITE});
    hack_init_otp_regs();
    picoboot_memory_access raw_access(con);
    auto model = raw_access.get_model();
    const otp_reg* flags_reg;
    const otp_reg* addr_reg;
    {
        auto matches = filter_otp({"usb_boot_flags"}, 24, false);
        auto row_match = *matches.begin();
        flags_reg = row_match.second.reg;
    }
    {
        auto matches = filter_otp({"usb_white_label_addr"}, 16, false);
        auto row_match = *matches.begin();
        addr_reg = row_match.second.reg;
    }

    json wl_json = json::parse(*get_file(ios::in|ios::binary));

    if (wl_json.contains("device") && !wl_json["device"].contains("config_attributes_max_power")) {
        // Check for separate max_power and attributes
        uint16_t val = 0;
        int hex_val = 0;
        if (wl_json["device"].contains("max_power") && wl_json["device"].contains("attributes")) {
            if (!get_json_int(wl_json["device"]["max_power"], hex_val)) {
                fail(ERROR_FORMAT, "MaxPower must be an integer");
            }
            val |= (hex_val << 8);

            if (!get_json_int(wl_json["device"]["attributes"], hex_val)) {
                fail(ERROR_FORMAT, "Device Attributes must be an integer");
            } else if (hex_val & 0b11111 || ~hex_val & 0x80) {
                fail(ERROR_FORMAT, "Device Attributes must have bit 7 set (0x80), and bits 4-0 clear");
            }
            val |= hex_val;
        } else if (wl_json["device"].contains("max_power") || wl_json["device"].contains("attributes")) {
            fail(ERROR_INCOMPATIBLE, "Must specify both max_power and attributes in the JSON file");
        }
        if (val) {
            fos << "Setting attributes " << hex_string(val, 4) << "\n";
            wl_json["device"]["config_attributes_max_power"] = val;
        }
    }

    vector<uint16_t> data;
    data.resize(16);

    uint32_t flags = 0;
    flags |= (flags_reg->fields.begin() + 1)->mask;

    vector<tuple<string, string, wl_type, uint8_t>> wl_fields = {
        {"device", "vid", wl_value, 0},
        {"device", "pid", wl_value, 0},
        {"device", "bcd", wl_bcd, 0},
        {"device", "lang_id", wl_value, 0},
        {"device", "manufacturer", wl_unistrdef, 30},
        {"device", "product", wl_unistrdef, 30},
        {"device", "serial_number", wl_unistrdef, 30},
        {"device", "config_attributes_max_power", wl_value, 0},
        {"volume", "label", wl_strdef, 11},
        {"scsi", "vendor", wl_strdef, 8},
        {"scsi", "product", wl_strdef, 16},
        {"scsi", "version", wl_strdef, 4},
        {"volume", "redirect_url", wl_strdef, 0x7f},
        {"volume", "redirect_name", wl_strdef, 0x7f},
        {"volume", "model", wl_strdef, 0x7f},
        {"volume", "board_id", wl_strdef, 0x7f},
    };

    for (unsigned int i=0; i < wl_fields.size(); i++) {
        wl_do_field(wl_json, data, flags, flags_reg, wl_fields[i], i);
    }

    struct picoboot_otp_cmd otp_cmd;

    uint16_t struct_row = settings.otp.row ? settings.otp.row : 0x100;

    fos << "Writing white-label data to row " << hex_string(struct_row, 4) << "\n";
    if (fos.last_column() > 8*8 - 1) fos.last_column(8*8 - 1);
    for (auto x : data) {
        fos << hex_string(x, 4) << ", ";
    }
    fos << "\n";

    // Write struct
    otp_cmd.bEcc = true;
    otp_cmd.wRow = struct_row;
    otp_cmd.wRowCount = data.size();

    try {
        con.otp_write(&otp_cmd, (uint8_t*)data.data(), data.size()*sizeof(data[0]));
    } catch (picoboot::command_failure &e) {
        check_otp_write_error(e, &otp_cmd, model);
        throw e;
    }

    // Write addr
    otp_cmd.bEcc = true;
    otp_cmd.wRow = addr_reg->row;
    otp_cmd.wRowCount = 1;
    try {
        con.otp_write(&otp_cmd, (uint8_t*)&struct_row, sizeof(struct_row));
    } catch (picoboot::command_failure &e) {
        check_otp_write_error(e, &otp_cmd, model);
        throw e;
    }

    // Write flags
    otp_cmd.bEcc = false;
    otp_cmd.wRow = flags_reg->row;
    otp_cmd.wRowCount = flags_reg->redundancy;

    vector<uint32_t> tmp_data = {flags};

    tmp_data.resize(otp_cmd.wRowCount);
    for (int i=1; i < otp_cmd.wRowCount; i++) std::copy_n(tmp_data.begin(), 1, tmp_data.begin() + i);
    try {
        con.otp_write(&otp_cmd, (uint8_t*)tmp_data.data(), tmp_data.size()*sizeof(flags));
    } catch (picoboot::command_failure &e) {
        check_otp_write_error(e, &otp_cmd, model);
        throw e;
    }

    return false;
}
#endif


#if HAS_LIBUSB
static int reboot_device(libusb_device *device, libusb_device_handle *dev_handle, bool bootsel, unsigned int disable_mask=0) {
    // ok, the device isn't in USB boot mode, let's try to reboot via vendor interface
    struct libusb_config_descriptor *config;
    int ret = libusb_get_active_config_descriptor(device, &config);
    if (ret) {
        fail(ERROR_USB, "Failed to get descriptor %d\n", ret);
    }
    for (int i = 0; i < config->bNumInterfaces; i++) {
        if (0xff == config->interface[i].altsetting[0].bInterfaceClass &&
            RESET_INTERFACE_SUBCLASS == config->interface[i].altsetting[0].bInterfaceSubClass &&
            RESET_INTERFACE_PROTOCOL == config->interface[i].altsetting[0].bInterfaceProtocol) {
            ret = libusb_claim_interface(dev_handle, i);
            if (ret) {
                fail(ERROR_USB, "Failed to claim interface\n");
            }
            if (bootsel) {
                if (settings.led >= 0) {
                    disable_mask |= (settings.led << 9u) | (settings.active_low << 7u) | (1u << 8u);
                }
                ret = libusb_control_transfer(dev_handle, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE,
                                              RESET_REQUEST_BOOTSEL, disable_mask, i, nullptr, 0, 2000);
            } else {
                ret = libusb_control_transfer(dev_handle, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE,
                                              RESET_REQUEST_FLASH, 0, i, nullptr, 0, 2000);
            }
//            if (ret != 0 ) {
//                fail(ERROR_UNKNOWN, "Unable to reset the device %d\n", ret);
//            }
            return 0;
//            return ret;
        }
    }
    fail(ERROR_USB, "Unable to locate reset interface on the device");
    return -1;
}

bool reboot_command::execute(device_map &devices) {
    if (settings.force) {
        if (!settings.switch_cpu.empty()) {
            fail(ERROR_ARGS, "--cpu may not be specified for forced reboot");
        }
        selected_chip = std::get<0>(devices[dr_vidpid_stdio_usb][0]);
        reboot_device(std::get<1>(devices[dr_vidpid_stdio_usb][0]), std::get<2>(devices[dr_vidpid_stdio_usb][0]), settings.reboot_usb);
        if (!quiet) {
            if (settings.reboot_usb) {
                std::cout << "The device was asked to reboot into BOOTSEL mode.\n";
            } else {
                std::cout << "The device was asked to reboot into application mode.\n";
            }
        }
    } else {
        // not exclusive, because restoring un-exclusive could fail; also if we're rebooting, we don't much
        // care what else is happening.
        auto con = get_single_bootsel_device_connection(devices, false);
        picoboot_memory_access raw_access(con);
        model_t model = raw_access.get_model();
        if (model->supports_picoboot_cmd(PC_REBOOT2)) {
            uint32_t usb_flags = 0u;
            if (settings.led >= 0) {
                usb_flags |= BOOTSEL_FLAG_GPIO_PIN_SPECIFIED;
                if (settings.active_low)
                    usb_flags |= BOOTSEL_FLAG_GPIO_PIN_ACTIVE_LOW;
            }
            struct picoboot_reboot2_cmd cmd = {
                    .dFlags = (uint8_t)(settings.reboot_usb ? REBOOT2_FLAG_REBOOT_TYPE_BOOTSEL : REBOOT2_FLAG_REBOOT_TYPE_NORMAL),
                    .dDelayMS = 500,
                    .dParam0 = settings.reboot_usb ? usb_flags : (unsigned int)settings.reboot_diagnostic_partition,
                    .dParam1 = (settings.reboot_usb && (settings.led >= 0)) ? settings.led : 0u,
            };
            if (!settings.switch_cpu.empty()) {
                if (settings.switch_cpu == "arm")
                    cmd.dFlags |= REBOOT2_FLAG_REBOOT_TO_ARM;
                else if (settings.switch_cpu == "riscv")
                    cmd.dFlags |= REBOOT2_FLAG_REBOOT_TO_RISCV;
                else
                    fail(ERROR_ARGS, "--cpu CPU type must be 'arm' or 'riscv'");
            }
            try {
                con.reboot2(&cmd);
            } catch (picoboot::command_failure &e) {
                if (e.get_code() == PICOBOOT_NOT_PERMITTED) {
                    fail(ERROR_NOT_POSSIBLE, "Unable to reboot - architecture unavailable");
                } else {
                    throw;
                }
            }
        } else if (!settings.reboot_usb) {
            // on RP2040 pass 0 to reboot in to flash
            con.reboot(0, 0, 500);
        } else {
            uint32_t led_flag = (settings.led >= 0) ? (1u << settings.led) : 0u;
            unsigned int program_base = SRAM_START;
            std::vector<uint32_t> program = {
                    0x21004802, // ldr r0, [pc, #8]; movs r1, #0
                    0x47104a00, // ldr r2, [pc, #0]; bx r2
                    bootrom_func_lookup_rp2040(raw_access, rom_table_code('U', 'B')),
                    led_flag,
            };

            raw_access.write_vector(program_base, program);
            try {
                con.exec(program_base);
            } catch (picoboot::connection_error &e) {
                // the reset_usb_boot above has a very short delay, so it frequently causes libusb to return
                // fairly unpredictable errors... i think it is best to ignore them, because catching a rare
                // case where the reboot command fails, is probably less important than potentially confusing
                // the user with spurious error messages
            }
        }
        if (!quiet) {
            if (settings.reboot_usb) {
                std::cout << "The device was rebooted into BOOTSEL mode.\n";
            } else {
                std::cout << "The device was rebooted into application mode.\n";
            }
        }
    }
    return true;
}
#endif

#if defined(_WIN32)
#define WIN32_LEAN_AND_MEAN
#define VC_EXTRALEAN
#include <windows.h>
#elif defined(__linux__) || defined(__APPLE__)
#include <sys/ioctl.h>
#endif

// tsk namespace is polluted on windows
#ifdef _WIN32
#undef min
#undef max

#define _CRT_SECURE_NO_WARNINGS
#endif

static void sleep_ms(int ms) {
#if defined(__unix__) || defined(__APPLE__)
    usleep(ms * 1000);
#else
    Sleep(ms);
#endif
}

void get_terminal_size(int& width, int& height) {
#if defined(DOCS_WIDTH)
    width = DOCS_WIDTH;
    height = 24;
#elif defined(_WIN32)
    CONSOLE_SCREEN_BUFFER_INFO csbi;
    GetConsoleScreenBufferInfo(GetStdHandle(STD_OUTPUT_HANDLE), &csbi);
    width = (int)(csbi.dwSize.X);
    height = (int)(csbi.dwSize.Y);
#elif defined(__linux__) || defined(__APPLE__)
    winsize w;
        if (ioctl(fileno(stdout), TIOCGWINSZ, &w) == 0) {
        width = (int)(w.ws_col);
        height = (int)(w.ws_row);
    } else {
        // default values if ioctl failed, for example if stdout is not
        // connected to an interactive tty
        width = 80;
        height = 24;
    }
#endif // Windows/Linux
}

void cancelled(int) {
    throw cancelled_exception();
}

int main(int argc, char **argv) {
    int tw=0, th=0;
    get_terminal_size(tw, th);
    if (tw) {
        fos.last_column(std::max(tw, 40));
    }

    int rc = parse(argc, argv);
    if (rc) return rc;
    if (!selected_cmd) {
        return 0;
    }

    if (settings.quiet) {
        fos_ptr = fos_null_ptr;
    }

#if HAS_LIBUSB
    libusb_context *ctx = nullptr;

    // save complicating the grammar
    if (settings.force_no_reboot) settings.force = true;

    struct libusb_device **devs = nullptr;
    device_map devices;
    vector<libusb_device_handle *> to_close;

    try {
        signal(SIGINT, cancelled);
        signal(SIGTERM, cancelled);

        if (settings.reboot_usb && settings.reboot_app_specified) {
            fail(ERROR_ARGS, "Cannot specify both -u and -a reboot options");
        }

        if (selected_cmd->get_device_support() != cmd::none) {
            if (libusb_init(&ctx)) {
                fail(ERROR_USB, "Failed to initialise libUSB\n");
            }
        }

        // we only loop a second time if we want to reboot some devices (which may cause device
        for (int tries = 0; !rc && tries <= MAX_REBOOT_TRIES; tries++) {
            if (ctx) {
                if (libusb_get_device_list(ctx, &devs) < 0) {
                    fail(ERROR_USB, "Failed to enumerate USB devices\n");
                }
                for (libusb_device **dev = devs; *dev; dev++) {
                    if (settings.bus != -1 && settings.bus != libusb_get_bus_number(*dev)) continue;
                    if (settings.address != -1 && settings.address != libusb_get_device_address(*dev)) continue;
                    libusb_device_handle *handle = nullptr;
                    chip_t chip = unknown;
                    auto result = picoboot_open_device(*dev, &handle, &chip, settings.vid, settings.pid, settings.ser.c_str());
                    if (handle) {
                        to_close.push_back(handle);
                    }
                    if (result != dr_error) {
                        devices[result].emplace_back(std::make_tuple(chip, *dev, handle));
                    }

                    if (settings.vid == 0 && !settings.ser.empty() && !devices[dr_vidpid_bootrom_ok].empty()) {
                        // Searching with no vid/pid filtering (ie attempting to open all USB devices to look for a PICOBOOT interface)
                        // can cause issues, so stop searching when we have found a device with the correct serial number, as we know we have
                        // the correct device
                        DEBUG_LOG("Found bootrom device with serial number, so stopping search");
                        break;
                    }
                }
            }
            auto supported = selected_cmd->get_device_support();
            switch (supported) {
                case cmd::device_support::zero_or_more:
                    if (!settings.filenames[0].empty()) break;
                    // fall thru
                case cmd::device_support::one:
                    if (devices[dr_vidpid_bootrom_ok].empty() &&
                        (!settings.force || devices[dr_vidpid_stdio_usb].empty())) {
                        if (tries == 0 || tries == MAX_REBOOT_TRIES) {
                            if (tries) {
                                fos << "\n\n";
                            }
                            bool had_note = false;
                            fos << missing_device_string(tries>0, selected_cmd->requires_rp2350());
                            if (tries) {
#if defined(_WIN32)
                                if (settings.force_rp2040) {
                                    fos << " You may need to install a driver via Zadig. See Zadig in the README (https://github.com/raspberrypi/picotool#zadig) for more information.";
                                }
#endif
                                fos << " It is possible the device is not responding, and will have to be manually entered into BOOTSEL mode.\n";
                                had_note = true; // suppress "but:" in this case
                            }
                            fos << "\n";
                            fos.first_column(0);
                            fos.hanging_indent(4);
                            auto printer = [&](enum picoboot_device_result r, const string &description) {
                                if (!had_note && !devices[r].empty()) {
                                    fos << "\nbut:\n\n";
                                    had_note = true;
                                }
                                for (auto d : devices[r]) {
                                    fos << bus_device_string(std::get<1>(d), std::get<0>(d)) << description << "\n";
                                }
                            };
    #if defined(__linux__) || defined(__APPLE__)
                            printer(dr_vidpid_bootrom_cant_connect,
                                    " appears to be in BOOTSEL mode, but picotool was unable to connect. Maybe try 'sudo' or check your permissions.");
                            printer(dr_vidpid_stdio_usb_cant_connect,
                                    " appears to have a USB serial connection, but picotool was unable to connect. Maybe try 'sudo' or check your permissions.");
    #else
                            printer(dr_vidpid_bootrom_cant_connect,
                                    " appears to be in BOOTSEL mode, but picotool was unable to connect. You may need to install a driver via Zadig. See Zadig in the README (https://github.com/raspberrypi/picotool#zadig) for more information");
                            printer(dr_vidpid_stdio_usb_cant_connect,
                                    " appears to have a USB serial connection, but picotool was unable to connect.");
    #endif
                            printer(dr_vidpid_picoprobe,
                                    " appears to be an RP-series PicoProbe device not in BOOTSEL mode.");
                            printer(dr_vidpid_micropython,
                                    " appears to be an RP-series MicroPython device not in BOOTSEL mode.");
                            if (selected_cmd->force_requires_pre_reboot()) {
                                printer(dr_vidpid_stdio_usb,
                                        " appears to have a USB serial connection, so consider -f (or -F) to force reboot in order to run the command.");
                            } else {
                                // special case message for what is actually just reboot (the only command that doesn't require reboot first)
                                printer(dr_vidpid_stdio_usb,
                                        " appears to have a USB serial connection, so consider -f to force the reboot.");
                            }
                            rc = ERROR_NO_DEVICE;
                        } else {
                            // waiting for rebooted device to show up
                            break;
                        }
                    } else if (supported == cmd::device_support::one) {
                        if (devices[dr_vidpid_bootrom_ok].size() > 1 ||
                            (devices[dr_vidpid_bootrom_ok].empty() && devices[dr_vidpid_stdio_usb].size() > 1)) {
                            fail(ERROR_NOT_POSSIBLE, "Command requires a single RP-series device to be targeted.");
                        }
                        if (!devices[dr_vidpid_bootrom_ok].empty()) {
                            settings.force = false; // we have a device, so we're not forcing
                        }
                    } else if (supported == cmd::device_support::zero_or_more && settings.force && !devices[dr_vidpid_bootrom_ok].empty()) {
                        // we have usable devices, so lets use them without force
                        settings.force = false;
                    }
                    fos.first_column(0);
                    fos.hanging_indent(0);
                    break;
                default:
                    break;
            }
            if (!rc) {
                if (settings.force && ctx) { // actually ctx should never be null as we are targeting device if force is set, but still
                    if (devices[dr_vidpid_stdio_usb].size() != 1 && !tries) {
                        fail(ERROR_NOT_POSSIBLE,
                             "Forced command requires a single rebootable RP-series device to be targeted.");
                    }
                    if (selected_cmd->force_requires_pre_reboot()) {
                        if (!tries) {
                            // we reboot into BOOTSEL mode and disable MSC interface (the 1 here)
                            auto &to_reboot = std::get<1>(devices[dr_vidpid_stdio_usb][0]);
                            auto &to_reboot_handle = std::get<2>(devices[dr_vidpid_stdio_usb][0]);
                            unsigned int disable_mask = 1;  // disable MSC interface
    #if defined(_WIN32)
                            {
                                struct libusb_device_descriptor desc;
                                libusb_get_device_descriptor(to_reboot, &desc);
                                if (desc.idProduct == PRODUCT_ID_RP2040_STDIO_USB || settings.force_rp2040) {
                                    // the Zadig driver should be setup for the device in BOOTSEL mode with no interfaces disabled,  
                                    // as all the interfaces are enabled when you plug it in while holding down the BOOTSEL button  
                                    disable_mask = 0;
                                    settings.force_rp2040 = true;
                                }
                            }
    #endif
                            if (settings.ser.empty() && to_reboot_handle) {
                                // store USB serial number, to pick correct device after reboot
                                struct libusb_device_descriptor desc;
                                libusb_get_device_descriptor(to_reboot, &desc);
                                char ser_str[128];
                                libusb_get_string_descriptor_ascii(to_reboot_handle, desc.iSerialNumber, (unsigned char*)ser_str, sizeof(ser_str));
                                if (strcmp(ser_str, "EEEEEEEEEEEEEEEE") != 0) {
                                    // don't store EEs serial number, as that is an RP2040 running a no_flash binary
                                    settings.ser = ser_str;
                                    fos << "Tracking device serial number " << ser_str << " for reboot\n";
                                }
                            }

                            reboot_device(to_reboot, to_reboot_handle, true, disable_mask);
                            fos << "The device was asked to reboot into BOOTSEL mode so the command can be executed.";
                        } else if (tries == 1) {
                            fos << "\nWaiting for device to reboot";
                        } else {
                            fos << "...";
                        }
                        fos.flush();
                        for (const auto &handle : to_close) {
                            libusb_close(handle);
                        }
                        libusb_free_device_list(devs, 1);
                        devs = nullptr;
                        to_close.clear();
                        devices.clear();
                        sleep_ms(1200);

                        // we now clear bus/address filters, because the device may have moved, so the only way we can find it
                        // again is to assume it has the same serial number.
                        settings.address = -1;
                        settings.bus = -1;
                        if (settings.pid != -1 || settings.vid != -1) {
                            // vid/pid filtering was enabled, but may change in BOOTSEL mode, so needs to be disabled
                            if (settings.ser.empty()) {
                                // this is an RP2040 running a no_flash binary, so will have a standard RP2040 vid/pid in BOOTSEL mode
                                settings.vid = -1; // -1 means filter for standard vid/pid
                                settings.pid = -1;
                            } else {
                                // skip vid/pid filtering, as it can be white-labelled on RP2350, and we know the serial number
                                settings.vid = 0; // 0 means skip vid/pid filtering entirely
                                settings.pid = -1;
                            }
                        }
                        continue;
                    }
                }
                if (tries) {
                    fos << "\n\n";
                }
                if (!selected_cmd->execute(devices) && tries) {
                    if (settings.force_no_reboot) {
                        fos << "\nThe device has been left accessible, but without the drive mounted; use 'picotool reboot' to reboot into regular BOOTSEL mode or application mode.\n";
                    } else {
                        // can only really do this with one device
                        if (devices[dr_vidpid_bootrom_ok].size() == 1) {
                            reboot_cmd->quiet = true;
                            reboot_cmd->execute(devices);
                            fos << "\nThe device was asked to reboot back into application mode.\n";
                        }
                    }
                }
                break;
            }
        }
    } catch (failure_error &e) {
        std::cout << "ERROR: " << e.what() << "\n";
        rc = e.code();
    } catch (picoboot::command_failure& e) {
        std::cout << "ERROR: The " << chip_name(selected_chip) << " device returned an error: " << e.what() << "\n";
        rc = ERROR_UNKNOWN;
    } catch (picoboot::connection_error&) {
        std::cout << "ERROR: Communication with " << chip_name(selected_chip) << " device failed\n";
        rc = ERROR_CONNECTION;
    } catch (cancelled_exception&) {
        rc = ERROR_CANCELLED;
    } catch (std::exception &e) {
        std::cout << "ERROR: " << e.what() << "\n";
        rc = ERROR_UNKNOWN;
    }

    for(const auto &handle : to_close) {
        libusb_close(handle);
    }
    if (devs) libusb_free_device_list(devs, 1);
    if (ctx) libusb_exit(ctx);

#else
    device_map devices;

    if (selected_cmd->get_device_support() != cmd::none) {
        fail(ERROR_USB, "No libUSB\n");
    }
    try {
        rc = selected_cmd->execute(devices);
    } catch (failure_error &e) {
        std::cout << "ERROR: " << e.what() << "\n";
        rc = e.code();
    } catch (cancelled_exception&) {
        rc = ERROR_CANCELLED;
    } catch (std::exception &e) {
        std::cout << "ERROR: " << e.what() << "\n";
        rc = ERROR_UNKNOWN;
    }
#endif

    return rc;
}