/*! @file       i386/libsaio/pxe.c
    @copyright  2007,2009 David F. Elliott
    @abstract   Basic PXE Interface
    @discussion
    Provides the low-level interface to the PXE API.
 */

#include "libsaio.h"

#include "pxe.h"

///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////

uint32_t pxeCall(uint16_t command, void *pData) __attribute__((noinline));

extern struct {
    uint8_t     call_op; // should be 0x9a
    SEGOFF16    call_ptr; // m16:16
    uint8_t     nop_1;
    uint8_t     nop_2;
} __attribute__((packed)) pxeCallInstruction asm("L_pxeCallInstruction");

uint32_t gPXE_real_entry_ebp;

/*!
    @abstract   32-bit pointer to '!PXE' structure
 */
static t_BANGPXE *gPXE_bangpxe;

/*!
    @abstract   32-bit pointer to 'PXENV+' structure
 */
static t_PXENV *gPXE_pxenv;

static inline bool isBangpxeSigValid(t_BANGPXE *bangpxe)
{
    return  bangpxe->Signature[0] == '!' && bangpxe->Signature[1] == 'P'
            && bangpxe->Signature[2] == 'X' && bangpxe->Signature[3] == 'E';
}

static inline bool isBangpxeChecksumValid(t_BANGPXE *bangpxe)
{
    return checksum8(bangpxe, bangpxe->StructLength) == 0;
}

static inline bool isPxenvSigValid(t_PXENV *pxenv)
{
    return  pxenv->Signature[0] == 'P' && pxenv->Signature[1] == 'X'
            && pxenv->Signature[2] == 'E' && pxenv->Signature[3] == 'N'
            && pxenv->Signature[4] == 'V' && pxenv->Signature[5] == '+';
}

static inline bool isPxenvChecksumValid(t_PXENV *pxenv)
{
    return checksum8(pxenv, pxenv->Length) == 0;
}

static bool initBangpxe(t_BANGPXE *bangpxe)
{
    if(bangpxe != NULL)
    {
        // It isn't really an error for the sig not to be valid because
        // we expect to be called with a pointer to garbage when not
        // having booted from PXE.
        if(isBangpxeSigValid(bangpxe))
        {
            if(isBangpxeChecksumValid(bangpxe))
            {
                gPXE_bangpxe = bangpxe;
                return true;
            }
            else
                printf("'!PXE' @0x%08x has bad checksum\n", (uint32_t)bangpxe);
        }
    }
    return false;
}

static bool initPxenv(t_PXENV *pxenv)
{
    if(pxenv != NULL)
    {
        // It isn't really an error for the sig not to be valid because
        // we expect to be called with a pointer to garbage when not
        // having booted from PXE.
        if(isPxenvSigValid(pxenv))
        {
            if(isPxenvChecksumValid(pxenv))
            {
                gPXE_pxenv = pxenv;
                if(pxenv->Version >= 0x0201)
                    initBangpxe((void*)ADDR32((uint32_t)pxenv->PXEPtr.segment, pxenv->PXEPtr.offset));
                return true;
            }
            else
                printf("'PXENV+' @0x%08x has bad checksum\n", (uint32_t)pxenv);
        }
    }
    return false;
}

bool initPxeFromBootstrap(t_BANGPXE *bangpxe, t_PXENV *pxenv)
{
    // Try '!PXE' first.  If we find it, no need to check 'PXENV+'
    return initBangpxe(bangpxe) || initPxenv(pxenv);
}

/*  Executes a PXE command by number with the command-specific data
    This is (basically) the same signature that the real callout has except
    it takes a 32-bit flat data pointer instead of a 16:16 segoff
 */
uint32_t pxeCall(uint16_t command, void *pData)
{
    uint32_t pData_offset = NORMALIZED_OFFSET((uint32_t)pData);
    uint32_t pData_segment = NORMALIZED_SEGMENT((uint32_t)pData);

    // NOTE: Self-modifying code of the call ptr16:16
    pxeCallInstruction.call_ptr = gPXE_bangpxe->EntryPointSP;

#if DEBUG
    printf("About to make call to %04x:%04xh\n", gPXE_bangpxe->EntryPointSP.segment, gPXE_bangpxe->EntryPointSP.offset);
    printf("Using command structure at %04x:%04xh\n", pData_segment, pData_offset);
#endif

    // NOTE: No need to flush pipeline, we jump from protected to real mode
    // anyway which of course flushes the pipeline.

    register uint32_t retval;
    __asm__(
        // push flags
        "pushf\n\t"
        // Push ESI and EDI because they definitely get clobbered by __switch_stack
        // We've already declared to GCC we use EAX, EBX, ECX, EDX
        "pushl %%esi\n\t"
        "pushl %%edi\n\t"
        // Push EBP
        "pushl %%ebp\n\t"

        // Set EBP to the one we entered with.
        // PXE (at least the Intel implementation used for VMware e1000) will hang
        // if you give it an EBP with high bits set.  It does work to just zero it
        // but it seems better to set it so that it makes a real-mode stack frame.
        // That is, it points to saved BP then return IP then return CS.
        "movl   _gPXE_real_entry_ebp, %%ebp\n\t"

        // Back to real mode
        "call __prot_to_real\n\t"

        ".code16\n\t"

        "pushw %%dx\n\t" // push data segment
        "pushw %%cx\n\t" // push data offset
        "pushw %%bx\n\t" // push command code

        // uint16_t far __cdecl EntryPoint(uint16_t cmd, SEGOFF16 cmdStruct);
        "\nL_pxeCallInstruction:\n\t"
        "callw $0x0,$0\n\t"

        "\nL_pxeCallDone:\n\t"

        // Restore SP from real-mode C call.  3 16-bit args were pushed.
        "addw $6, %%sp\n\t"

        // NOTE: __real_to_prot clobbers ax.. we need it
        "movw %%ax, %%bx\n\t" // Stuff return value (ax) into bx

        // Call __real_to_prot using 32-bit operand size which is its calling convention
        "calll   __real_to_prot\n\t"
        ".code32\n\t"

        // Restore EBP
        "popl   %%ebp\n\t"

        "movl %%ebx, %%eax\n\t" // restore return value stuffed into ebx back into normal eax
        "popl %%edi\n\t"
        "popl %%esi\n\t"
        // pop flags
        "popf\n\t"
    :   "=a"(retval) // output comes in ax
    :   "c"(pData_offset), "d"(pData_segment), "b"(command)
    :   "esi", "edi" // Clobbers these
    );
    return retval & 0xffff;
}

///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////

const char * getCachedInfoBufferTypeString(uint16_t packetType)
{
    if(packetType == PXENV_PACKET_TYPE_DHCP_DISCOVER)
        return "DHCP Discover";
    else if(packetType == PXENV_PACKET_TYPE_DHCP_ACK)
        return "DHCP ACK";
    else if(packetType == PXENV_PACKET_TYPE_CACHED_REPLY)
        return "Cached Reply";
    else
        return "<unknown>";
}

// Helper function to return the cached buffer from a PXENV_GET_CACHED_INFO result
void * getPxeCachedBuffer(t_PXENV_GET_CACHED_INFO *pGetCachedInfoCommand)
{
    // Check that command record makes sense
    if(pGetCachedInfoCommand->Status != PXENV_STATUS_SUCCESS)
    {
        printf("PXENV_GET_CACHED_INFO failed with status 0x%x\n", pGetCachedInfoCommand->Status);
        return NULL;
    }
    printf("%s buffer at %04x:%04x with size %d\n", getCachedInfoBufferTypeString(pGetCachedInfoCommand->PacketType), pGetCachedInfoCommand->Buffer.segment, pGetCachedInfoCommand->Buffer.offset, pGetCachedInfoCommand->BufferSize);
    void *pxe_buffer = (void*)(ADDR32(pGetCachedInfoCommand->Buffer.segment, pGetCachedInfoCommand->Buffer.offset));
    return pxe_buffer;

}

// Helper function to return a new malloc'd buffer based off of PXENV_GET_CACHED_INFO result
static void * dupPxeCachedBuffer(t_PXENV_GET_CACHED_INFO *pGetCachedInfoCommand)
{
    void *pxe_buffer = getPxeCachedBuffer(pGetCachedInfoCommand);
    if(pxe_buffer == NULL)
        return NULL;
    // Buffer must be good.. copy it out:
    void *newbuf = malloc(pGetCachedInfoCommand->BufferSize);
//    printf("Copying to %08x\n", newbuf);
    if(newbuf != NULL)
        memcpy(newbuf, pxe_buffer, pGetCachedInfoCommand->BufferSize);
    return newbuf;
}

// NOTE: Size may be larger, but the first part of the data will be a BOOTPLAYER
static BOOTPLAYER *sDhcpDiscover;

uint16_t pxenvGetCachedInfo(t_PXENV_GET_CACHED_INFO *pGetCachedInfoCommand, uint16_t packetType)
{
    uint16_t pxenv_exit;

    bzero(pGetCachedInfoCommand, sizeof(*pGetCachedInfoCommand));
    pGetCachedInfoCommand->PacketType = packetType;
    // NOTE: Don't set BufferSize nor Buffer so we can just use the existing buffer

    
    pxenv_exit = pxeCall(PXENV_GET_CACHED_INFO, pGetCachedInfoCommand);
    if(pxenv_exit != PXENV_EXIT_SUCCESS)
        printf("PXENV_GET_CACHED_INFO failed with exit 0x%x\n", pxenv_exit);
    return pxenv_exit;
}

// Retrieves the DHCP DISCOVER info from PXE and if successful malloc's
// enough space for it and copies the data to the new area.
// The pointer is stashed in the sDhcpDiscover var.
static void allocCachedDhcpDiscover()
{
#if 1
    uint16_t pxenv_exit;
    t_PXENV_GET_CACHED_INFO cachedInfoData;

    // Clear (if applicable) and re-retrieve the data each time
    if(sDhcpDiscover != NULL)
        free(sDhcpDiscover);
    sDhcpDiscover = NULL;

    if((pxenv_exit = pxenvGetCachedInfo(&cachedInfoData, PXENV_PACKET_TYPE_DHCP_DISCOVER)) == PXENV_EXIT_SUCCESS)
        sDhcpDiscover = dupPxeCachedBuffer(&cachedInfoData);
#endif
}

BOOTPLAYER *gDhcpAck;
uint32_t gDhcpAckSize;
char gPXE_BaseFilename[128];

size_t setBaseFilenameFromOptions(char *dst, size_t dstsz, BOOTPLAYER const *dhcp_packet, unsigned char const *option_list[], size_t const option_list_len)
{
    if(dhcp_packet == NULL) {
        if (dstsz > 0) {
            dst[0] = '\0';
        }
        return 0;
    }

    // Option 52 is special.  Its appearance in the options enables reading additional
    // options from the file and/or sname fields.  So those fields won't be consulted
    // at all unless it is present.  However, if those fields in turn contain an option
    // 52 it is simply ignored.  So only consult the first instance of it (if any) and
    // only if it is exactly 1 byte in length.
    unsigned char opt52 = 0;
    int const opt52i = binarySearchOptionWithTag(52, option_list, 0, option_list_len, SEARCH_FIRST);
    if (opt52i >= 0 && opt52i < option_list_len && option_list[opt52i][0] == 52 && option_list[opt52i][1] == 1) {
        opt52 = option_list[opt52i][2];
    }

    int const opt67i = binarySearchOptionWithTag(67, option_list, 0, option_list_len, SEARCH_FIRST);
    size_t opt67len = copyOptionValueForKeyFromOptionList(dst, dstsz - 1, 67, option_list, opt67i, option_list_len);
    // the option 67 length does not include NUL terminator
    size_t tmpbufsz = opt67len + 1;

    // If the tmpbufsz <= dstsz we don't need a temporary buffer, we can output
    // directly into dst (and in fact, we already did)
    if (tmpbufsz <= dstsz) {
        tmpbufsz = 0;
    }
    // If tmpbufsz > dstsz then it is still non-zero and we can use a C99 variable
    // length array to allocate it on the stack at which point we need to actually
    // do the copy.  We need the data because we need to compute the location of '/'
    char tmpbuf[tmpbufsz];
    if (tmpbufsz != 0) {
        // We promise to NUL-terminate any output
        if (dstsz > 0) {
            dst[dstsz - 1] = '\0';
        }
        dstsz = tmpbufsz;
        dst = tmpbuf;
        copyOptionValueForKeyFromOptionList(dst, dstsz - 1, 67, option_list, opt67i, option_list_len);
    }

    // *(dst + opt67len + 1) = '\0';

    char const *in;
    char const *bootfile;
    char const *end;

    // If we have an option 67 file then prefer it
    // If using ISC DHCP the filename statement declares the non-option file
    // and the bootfile-name declares option 67.  Most PXE clients actually
    // prefer to get the filename in options rather than as the BOOTP file.
    if (opt67len > 0) {
        bootfile = dst;
        end = bootfile + opt67len;
    }
    // Otherwise so long as bootfile wasn't overloaded, use it
    else if ((opt52 & 1) == 0) {
        bootfile = (char const *)dhcp_packet->bootfile;
        end = bootfile + sizeof(dhcp_packet->bootfile);
    }
    else {
        bootfile = NULL;
        end = NULL;
    }

    char const *afterslash = bootfile;
    // Look for the slash.  If no slash is found then afterslash = bootfile
    // so the boot file relative path will be used as is.
    for(in = bootfile; in < end && *in != '\0'; ++in)
        if(*in == '/')
            afterslash = in + 1;

    char *out = dst;
    // If in != out we have to actually copy from in to out up to the last slash
    // and not exceeding the buffer size.
    if(bootfile != dst) {
        char *dstEnd = dst + dstsz - 1;
        for(in = (char const*)bootfile; in < afterslash && out < dstEnd; ++in, ++out)
            *out = *in;
    }
    // If in == out then we skip the copy but we still need to position out at
    // the next character to be written.
    else {
        out = (char *)afterslash;
        in = afterslash;
    }

    // Write the NUL terminator without incrementing out
    if (out < dst + dstsz) {
        *out = '\0';
    }
    else if (dstsz > 0) {
        out[dstsz - 1] = '\0';
    }

    // Return length of string (excluding the NUL)
    return (out - dst) + (afterslash - in);
}

static void allocCachedDhcpAck()
{
    uint16_t pxenv_exit;
    t_PXENV_GET_CACHED_INFO cachedInfoData;

    // Clear (if applicable) and re-retrieve the data each time
    if(gDhcpAck != NULL)
    {
        free(gDhcpAck);
    }

    gDhcpAck = NULL;
    gDhcpAckSize = 0;

    if((pxenv_exit = pxenvGetCachedInfo(&cachedInfoData, PXENV_PACKET_TYPE_DHCP_ACK)) == PXENV_EXIT_SUCCESS)
    {
        gDhcpAck = dupPxeCachedBuffer(&cachedInfoData);
        gDhcpAckSize = cachedInfoData.BufferSize;
    }
}

// The boot reply is a DHCP response coming from the boot server.
// When a separate boot server is used this is where you find the TFTP server name and
// the boot filename.  The DHCP ACK from the DHCP server does not have this information.
// FIXME: When the DHCP ACK _does_ have this information and no separate boot server is used
// does the boot reply still exist in the PXE stack?
BOOTPLAYER *gBootReply;
uint32_t gBootReplySize;
static void allocCachedBootReply()
{
    uint16_t pxenv_exit;
    t_PXENV_GET_CACHED_INFO cachedInfoData;

    // Clear (if applicable) and re-retrieve the data each time
    if(gBootReply != NULL)
    {
        free(gBootReply);
    }

    gBootReply = NULL;
    gBootReplySize = 0;

    if((pxenv_exit = pxenvGetCachedInfo(&cachedInfoData, PXENV_PACKET_TYPE_CACHED_REPLY)) == PXENV_EXIT_SUCCESS)
    {
        gBootReply = dupPxeCachedBuffer(&cachedInfoData);
        gBootReplySize = cachedInfoData.BufferSize;
    }
}

static void setBaseFilename() {
    size_t option_list_sz = parseOptionsFromDhcpPacket(NULL, 0, gBootReply, gBootReplySize);
    unsigned char const *option_list[option_list_sz];

    size_t option_list_len = 0;
    option_list_len = parseOptionsFromDhcpPacket(option_list, option_list_sz, gBootReply, gBootReplySize);

    setBaseFilenameFromOptions(gPXE_BaseFilename, sizeof(gPXE_BaseFilename), gBootReply, option_list, option_list_len);
}


BOOL PXEStartup()
{
    if(gPXE_bangpxe == 0)
    {
        // Work backwords from the absolute top of convential memory less
        // the rounded 16-byte space needed for the minimum sized !PXE struct
        // NOTE: Important that *bangpxe_addr is unsigned so the checksum works
        uint8_t *bangpxe_addr = (uint8_t*)VIDEO_ADDR /*- ( (sizeof(t_BANGPXE) + 0xf) / 0x10 )*/;
        for(; bangpxe_addr >= (uint8_t*)0x0030000; bangpxe_addr -= 0x10)
        {
            t_BANGPXE *curr_bang_pxe = (t_BANGPXE*)bangpxe_addr;
            if(curr_bang_pxe->Signature[0] == '!' && curr_bang_pxe->Signature[1] == 'P'
                && curr_bang_pxe->Signature[2] == 'X' && curr_bang_pxe->Signature[3] == 'E')
            {
                if(checksum8(curr_bang_pxe, curr_bang_pxe->StructLength) == 0)
                {
                    gPXE_bangpxe = curr_bang_pxe;
                    break;
                }
//                printf("A '!PXE' Signature was found at %lx, but had a bad checksum.  Continuing.\n", (uint32_t)curr_bang_pxe);
            }
        }
    }
    if(gPXE_bangpxe == 0)
    {
//        printf("Could not find any usable '!PXE' structure\n");
        return false;
    }

#if 0 // This always seems to fail for me.
    uint16_t pxenv_exit;

    t_PXENV_UNDI_GET_STATE undiGetState;
    bzero(&undiGetState, sizeof(undiGetState));
    if((pxenv_exit = pxeCall(PXENV_UNDI_GET_STATE, &undiGetState)) != PXENV_EXIT_SUCCESS)
    {
        printf("PXENV_UNDI_GET_STATE failed with exit 0x%x\n", pxenv_exit);
        sleep(1);
    }
#endif
    
    printf("Retrieving DHCP Discover info\n");
    allocCachedDhcpDiscover();
//    printf("OK\n");
    if(sDhcpDiscover == NULL)
    {
        gPXE_bangpxe = NULL;
        return FALSE;
    }
#if 0
    printf("opcode\t%d\n", sDhcpDiscover->opcode);
    printf("MAC %02x:%02x:%02x:%02x:%02x:%02x\n"
        ,   sDhcpDiscover->CAddr[0]
        ,   sDhcpDiscover->CAddr[1]
        ,   sDhcpDiscover->CAddr[2]
        ,   sDhcpDiscover->CAddr[3]
        ,   sDhcpDiscover->CAddr[4]
        ,   sDhcpDiscover->CAddr[5]
      );
#endif
//    printf("Retrieving DHCP ACK info\n");
    allocCachedDhcpAck();
    allocCachedBootReply();
    setBaseFilename();
    // In order to boot over the network we need the DHCP ACK and the boot server's reply.
    if(gDhcpAck == NULL || gBootReply == NULL)
    {
        sleep(1);
        gPXE_bangpxe = NULL;
    }

    printf("PXE Loaded\n");
    sleep(1);

    // TODO: make sure things are initialized
    return TRUE;
}

int binarySearchOptionWithTag(unsigned char key, unsigned char const *base[], int begin, int end, search_disposition disp) {
    while (begin < end) {
        int mid = (begin + end) / 2;
        unsigned char const *curr = base[mid];
        // See if we found the target value
        if (*curr == key) {
            if (disp == SEARCH_ANY) {
                return mid;
            }
            // If we're looking for the one after the target
            // start searching after this known target.
            else if (disp > SEARCH_LAST) {
                begin = mid + 1;
            }
            // If we're looking for the last one this might be it.
            else if (disp > SEARCH_ANY) {
                // Don't test the same position twice
                // The only time begin == mid is when
                // begin + 1 == end in which case we already know that
                // end cannot possibly be the item because it is
                // either the actual end as input to the function
                // or if it has changed it's because we know
                // that it's definitely greater than what we seek.
                // Therefore knowing that begin + 1 == end and
                // knowing we have found a match we can simply
                // set end to the match which is begin which
                // is the result and we're done.
                if (begin == mid) {
                    end = mid;
                }
                else {
                    begin = mid;
                }
            }
            else { // disp < SEARCH_ANY
                end = mid;
            }
        }
        // See if we're before the target value
        else if (*curr < key) {
            // The target value is greater than the current value so
            // the earliest value it could be is one after the current
            begin = mid + 1;
        }
        // See if we're still after the target value
        else { // *curr > key
            // We're biased towards end already so regardless of
            // disposition assume that the current item cannot
            // be the target because it's after the target
            end = mid;
        }
    }
    // Basically if we want SEARCH_BEFORE behavior we return
    // end - 1 because begin >= end and this will get us the
    // before behavior whereas in all other cases the
    // begin would have the correct answer.
    return disp >= SEARCH_FIRST ? begin : end - 1;
}

unsigned char const * parseVendorOptionBuffer(unsigned char const *out_list[], size_t out_size, size_t *io_len, unsigned char *popt52, unsigned char const *dhcp_options, size_t options_sz) {
    unsigned char const *cp = dhcp_options;
    unsigned char const *ep = dhcp_options + options_sz;

    if (popt52 != NULL) {
        *popt52 = 0;
    }

    while (cp < ep) {
        unsigned char const *opt = cp;
        unsigned char const tag = *cp++;
        if (tag == 255) {
            // We found an end option, return the pointer to it.
            return opt;
        }
        else if (tag == 0) {
            // The pad option does not have a length
            continue;
        }

        unsigned char const len = *cp++;
        cp += len;
        if (cp <= ep) {
            // If output of option 52 is requested, set it the first
            // time we see it, and only the first time we see it.
            if (popt52 != NULL && tag == 52) {
                if (len == 1) {
                    *popt52 = *cp;
                }
                popt52 = NULL;
            }

            if (*io_len < out_size) {
                int insert_idx = binarySearchOptionWithTag(tag, out_list, 0, *io_len, SEARCH_AFTER);

                // Shift each element right one position moving from right to left
                for(int i = ++(*io_len); i > insert_idx; --i) {
                    out_list[i] = out_list[i - 1];
                }

                // Assign the entry in the newly empty spot.
                out_list[insert_idx] = opt;
            }
            else {
                // Ran out of space in output list.
                ++(*io_len);
            }
        }
        else {
            return opt;
        }
    }
    return cp;
}

size_t parseOptionsFromDhcpPacket(unsigned char const *option_list[], size_t option_list_size, BOOTPLAYER const *dhcp_ack_packet, size_t dhcp_ack_packet_sz) {
    unsigned char opt52 = 0;

    size_t option_list_len = 0;

    uint8_t const *magic = dhcp_ack_packet->vendor.v.magic;
    if (magic[0] == 0x63 && magic[1] == 0x82 && magic[2] == 0x53 && magic[3] == 0x63) {
        unsigned char const *cp = (unsigned char *)dhcp_ack_packet->vendor.d + 4;
        unsigned char const *ep = (unsigned char *)dhcp_ack_packet + dhcp_ack_packet_sz;

        cp = parseVendorOptionBuffer(option_list, option_list_size, &option_list_len, &opt52, cp, ep - cp);

        // RFC 2131: options must end with an end option (255) to be considered valid
        // RFC 2131: Read file then sname
        // That is, if no end option then don't read file/sname and if you do read them
        // then read them in that order
        if ((opt52 & 1) && cp < ep && *cp == 255) {
            cp = dhcp_ack_packet->bootfile;
            ep = cp + sizeof(dhcp_ack_packet->bootfile);
            cp = parseVendorOptionBuffer(option_list, option_list_size, &option_list_len, NULL, cp, ep - cp);
        }
        if ((opt52 & 2) && cp < ep && *cp == 255) {
            cp = dhcp_ack_packet->Sname;
            ep = cp + sizeof(dhcp_ack_packet->Sname);
            cp = parseVendorOptionBuffer(option_list, option_list_size, &option_list_len, NULL, cp, ep - cp);
        }
    }
    return option_list_len;
}

size_t copyOptionValueForKeyFromOptionList(void *dst, size_t dstsz, unsigned char key, unsigned char const *option_list[], size_t start, size_t end) {
    size_t len = 0;
    // The end of the string is 1 less than the given buffer size
    void * const dstend = dst + dstsz;
    for(int i = start; i < end; ++i) {
        if (option_list[i][0] == key) {
            size_t const thislen = option_list[i][1];
            len += thislen;
            unsigned char const *src = option_list[i] + 2;
            unsigned char const * const srcend = src + thislen;
            while (dst < dstend && src < srcend) {
                *((unsigned char *)dst++) = *(src++);
            }
        }
        else {
            break;
        }
    }
    return len;
}

void debugPrintOptions(unsigned char const *option_list[], size_t start, size_t end, BOOTPLAYER const *packet) {
    for(int i = start; i < end; ++i) {
        unsigned char const thistag = option_list[i][0];
        size_t const thislen = option_list[i][1];
        uintptr_t off = (void *)option_list[i] - (void *)packet;
        printf("Option %d length %d @0x%04x\n", thistag, thislen, off);
        unsigned char const *optval = (void *)(option_list[i] + 2);
        unsigned char const *optend = (void *)optval + thislen;
        if (thislen % sizeof(uint32_t) == 0) {
            while (optval < optend) {
                printf("  %d.%d.%d.%d\n", optval[0], optval[1], optval[2], optval[3]);
                optval += 4;
            }
        }
        else if (thislen > 0) {
            unsigned char const *cp = optval;
            for (; cp < optend && (*cp & 0x80) == 0 && (*cp) >= ' '; ++cp) {
            }
            if (cp == optend) {
                printf("  ");
                for (unsigned char const *cp = optval; cp < optend; ++cp) {
                    printf("%c", *cp);
                }
                printf("\n");
            }
            else {
                printf("  Bytes: ");
                for (unsigned char const *cp = optval; cp < optend; ++cp) {
                    if (cp != optval) {
                        printf(", ");
                    }
                    printf("%d", *cp);
                }
                printf("\n");
            }
        }
    }
}