Read AP fw ident better from hfwget

[agere_fw_utils.git] / hfwget.c

/*
 * Hermes AP firmware extractor for Windows drivers (c) 2003 by Mark Smith
 * This may be distributed freely under the GPL v2 so long as this copyright
 * notice is included.
 *
 * Following modifications (c) 2008 David Kilroy
 *  primary plug data
 *  compatibility info
 *  firmware identification
 *  carry on without filename (wldel48b, and old wlluc48)
 *  binary output format for linux kernel driver
 *  big endian translations
 *  refactorring
 *
 * These modifications may be distributed freely under the GPL v2 so
 * long as this copyright notice is included.
 */
#include <stddef.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <memory.h>
#include <string.h>

#if __STDC_VERSION__>=199901L
# include <stdbool.h>
#else
typedef int bool;

# define false   0
# define true    (!0)
#endif

/* Typedefs for little and big endian values */
typedef uint32_t __le32;
typedef uint16_t __le16;
typedef uint32_t __be32;
typedef uint16_t __be16;

/* Driver endianness */
typedef uint32_t __de32;
typedef uint16_t __de16;


/* Typedefs for sized integers */
typedef uint32_t u32;
typedef uint16_t u16;
typedef uint8_t u8;

#define ARRAY_SIZE(x) (sizeof(x)/sizeof((x)[0]))

/*** Macros to deal with different endianess ***/

/* 0xAABB to 0xBBAA */
#define swap_bytes_16(value)                    \
    ((((value) >> 8) & 0xFF) |                  \
     (((value) & 0xFF) << 8))

/* 0xAABBCCDD to 0xDDCCBBAA */
#define reverse_bytes_32(value)                 \
    ((((value) >> 24) & 0x0000FF) |             \
     (((value) >>  8) & 0x00FF00) |             \
     (((value) <<  8) & 0xFF0000) |             \
     (((value) & 0xFF) << 24))

/* 0xAABBCCDD to 0xBBAADDCC */
#define swap_bytes_32(value)                    \
    ((((value) >>  8) & 0x00FF00FF) |           \
     (((value) <<  8) & 0xFF00FF00))

/* 0xAABBCCDD to 0xCCDDAABB */
#define swap_words_32(value)                    \
    ((((value) >> 16) & 0x0000FFFF) |           \
     (((value) << 16) & 0xFFFF0000))

/*                    address ->   0    1    2    3
 * Pure LE stores 0x12345678 as 0x78 0x56 0x34 0x12
 * Pure BE stores 0x12345678 as 0x12 0x34 0x56 0x78
 * BEW+LEB stores 0x12345678 as 0x34 0x12 0x78 0x56
 * LEW+BEB stores 0x12345678 as 0x56 0x78 0x12 0x34
 */
static bool host_bytes_in_word_be = false;
static bool host_words_in_dword_be = false;
static bool driver_is_be = false;

#define host_to_le16(value) (__le16)                            \
    (host_bytes_in_word_be ? swap_bytes_16(value) : (value))
#define host_to_le32(value) (__le32)                            \
    (host_words_in_dword_be ?                                   \
     (host_bytes_in_word_be ? reverse_bytes_32(value)           \
                            : swap_bytes_32(value)) :           \
     (host_bytes_in_word_be ? swap_words_32(value) : (value)))

#define le16_to_host(value) (u16)                               \
    (host_bytes_in_word_be ? swap_bytes_16(value) : (value))
#define le32_to_host(value) (u32)                               \
    (host_words_in_dword_be ?                                   \
     (host_bytes_in_word_be ? reverse_bytes_32(value)           \
                            : swap_bytes_32(value)) :           \
     (host_bytes_in_word_be ? swap_words_32(value) : (value)))

#define host_to_be16(value) (__be16)                            \
    (host_bytes_in_word_be ? (value) : swap_bytes_16(value))
#define host_to_be32(value) (__be32)                            \
    (host_words_in_dword_be ?                                   \
     (host_bytes_in_word_be ? (value) : swap_bytes_32(value)) : \
     (host_bytes_in_word_be ? swap_words_32(value)              \
                            : reverse_bytes_32(value)))

#define be16_to_host(value) (u16)                               \
    (host_bytes_in_word_be ? (value) : swap_bytes_16(value))
#define be32_to_host(value) (u32)                               \
    (host_words_in_dword_be ?                                   \
     (host_bytes_in_word_be ? (value) : swap_bytes_32(value)) : \
     (host_bytes_in_word_be ? swap_words_32(value)              \
                            : reverse_bytes_32(value)))

#define driver_to_host_16(value) (u16)                          \
    (driver_is_be ? be16_to_host(value) : le16_to_host(value))
#define driver_to_host_32(value) (u32)                          \
    (driver_is_be ? be32_to_host(value) : le32_to_host(value))
#define host_to_driver_16(value) (__de16)                       \
    (driver_is_be ? host_to_be16(value) : host_to_le16(value))
#define host_to_driver_32(value) (__de32)                       \
    (driver_is_be ? host_to_be32(value) : host_to_le32(value))

/**** Structures to read image data from driver ****/

/* Decode Windows firmware blocks */
struct fwblock_wdrv {
    __le32 offset;
    __le16 size;
    __le16 flags;
    __le32 data_p;
};

/* Decode Mac firmware blocks */
struct fwblock_mdrv {
    __be16 len;
    __be16 code;
    __be16 prg_mode;
    __be16 size;
    __be32 offset;
    __be32 flags;
    __be32 data_p;
};

union fwblock_drv {
    struct fwblock_wdrv *w;
    struct fwblock_mdrv *m;
};

struct plugarray_drv {
    __de32 code;
    __de32 targ_off;
    __de32 length;
};

struct ident_info_drv {
    __de16 size;
    __de16 code;
    __de16 comp_id;
    __de16 variant;
    __de16 version_major;
    __de16 version_minor;
};

struct compat_info_drv {
    __de16 size;
    __de16 code;
    __de16 role;
    __de16 id;
    struct {
        __de16 variant;
        __de16 bottom;
        __de16 top;
    } range[20];
};

struct fwtable_drv {
    __de32 segarray_p;
    __de32 halfentry;
    __de32 plugarray_p;
    __de32 pri_plugarray_p;
    __de32 compat_p;
    __de32 ident_p;
};

/*** Structures to use on host. ***/
struct fwblock {
    uint32_t offset;
    uint16_t size;
    uint16_t flags;
    uint8_t *data;
};

struct plugarray {
    u32 code;
    u32 targ_off;
    u32 length;
};

struct ident_info {
    u16 size;
    u16 code;
    u16 comp_id;
    u16 variant;
    u16 version_major;
    u16 version_minor;
};

struct compat_info {
    u16 size;
    u16 code;
    u16 role;
    u16 id;
    struct {
        u16 variant;
        u16 bottom;
        u16 top;
    } range[20];
};

struct fwtable {
    struct fwblock *segarray;
    u32 halfentry;
    struct plugarray *plugarray;
    struct plugarray *pri_plugarray;
    struct compat_info *compat;
    struct ident_info *ident;
};

/* Structure detailing firmware differences between Windows and Mac */
struct fw_layout {
    const int block_prefix;     /* Bytes before the start of a firmware block */
    const size_t lead_block_bytes;      /* Bytes used by lead blocks */
    const size_t datap_offset;  /* Offset of data_p in fw_block struct */
    size_t max_offset;          /* No firmware after this offset */
    ptrdiff_t addr_delta;       /* Difference between addresses encoded in the
                                 * driver and the file offset of the associated
                                 * data */
    bool mac;                   /* Use mac structures */
};

static struct fw_layout firmware_layout[] =
{
    {   /* Windows */
        4, 0 * sizeof(struct fwblock_wdrv),
        offsetof(struct fwblock_wdrv, data_p)
    },
    {   /* Mac */
        0, 1 * sizeof(struct fwblock_mdrv),
        offsetof(struct fwblock_mdrv, data_p)
    }
};

/* Structure to map firmware identifiers to description strings */
static const struct {
    u16 id;
    char *comp_string;
} compat_table[] =
{
    /* Firmware type */
    { 21, "Primary firmware" },
    { 22, "Intermediate firmware" },
    { 31, "Station firmware" },
    { 32, "AP firmware" },
    { 0x14B, "AP firmware" },

    /* Driver type */
    { 41, "Windows 9x/NT Miniport NDIS 3.1" },
    { 42, "Packet" },
    { 43, "DOS ODI" },
    { 44, "32 bit ODI" },
    { 45, "Mac OS" },
    { 46, "Windows CE Miniport" },
    { 47, "Linux HCF-light based (public domain)" },
    { 48, "Windows 9x/NT Miniport NDIS 5.0" },
    { 49, "Linux HCF-library based" },
    { 50, "QNX" },
    { 51, "Windows 9x/NT Miniport NDIS 5.0 USB" },
    { 52, "Windows 9x/NT Miniport NDIS 4.0" },
    { 53, "VxWorks END Station driver" },
    { 54, "VxWorks END Access Point driver" },
    { 55, "Mac OS?" },
    { 56, "VxWorks END Station/AP driver" },

    /* Others */
    { 63, "WaveLAN Station FW update utility" },
    { 81, "WaveLAN/IEEE AP" },
    { 83, "WaveLAN/IEEE Ethernet Converter" },
    { 87, "USB Boot Loader" },
    { 0xFF, "Unknown" }
};

/* Checking endianess at runtime because performance isn't an issue,
 * and I'd rather not add a configure step */
static void check_endianess(void)
{
    union {
        u32 dword;
        u16 word[2];
        u8 byte[4];
    } data;

    data.dword = 0x12345678;
    if (data.word[0] == 0x1234)
    {
        host_words_in_dword_be = true;
    }
    else if (data.word[0] == 0x5678)
    {
        host_words_in_dword_be = false;
    }
    else
    {
        fprintf(stderr, "Can't determine endianess of host!\n");
        exit(EXIT_FAILURE);
    }

    data.word[0] = 0x1234;
    if (data.byte[0] == 0x12)
    {
        host_bytes_in_word_be = true;
    }
    else if (data.byte[0] == 0x34)
    {
        host_bytes_in_word_be = false;
    }
    else
    {
        fprintf(stderr, "Can't determine endianess of host!\n");
        exit(EXIT_FAILURE);
    }

    if (host_bytes_in_word_be == host_words_in_dword_be)
    {
        fprintf(stdout, "Detected %s host\n",
                host_bytes_in_word_be ? "big endian" : "little endian");
    }
    else
    {
        fprintf(stdout, "Detected host with mixed endianess\n");
    }
    return;
}

/* Locate firmware by looking for a T???????.HEX filename */
static char* find_fw_filename(const u8 *hostdriver, size_t flen, char hexchar)
{
    const u8 *p, *end;
    bool found;

    /* Find the ?1XXYYZZ.HEX string */
    p = hostdriver;
    end = hostdriver + flen;

    for (found = false; (found == false) && (p != NULL); )
    {
        p = memchr(p, hexchar, (end - p));

        if (p != NULL)
        {
            if (memcmp(".HEX", p + 8, 4) == 0)
            {
                found = true;
            }
            else
            {
                p++;
            }
        }
    }

    if (p != NULL)
    {
        printf("Found firmware %s at file offset 0x%08x\n",
               p, (p - hostdriver));
    }
    else
    {
        printf("%c-firmware not found!\n", hexchar);
    }

    return (char*) p;
}

/* Find the start of the firmware based on a hint as to where the end
 * of the firmware image is. The start of the firmware image is
 * defined by a signature. */
static u8* find_fw(const u8 *hostdriver, size_t flen,
                   const u8 *signature, size_t slen,
                   const u8 *hint)
{
    const u8 *p = hint - slen;
    bool found = false;
    size_t i;

    printf("Searching for firmware from offset 0x%08x, start signature",
           hint - hostdriver);
    for (i = 0; i < slen; i++)
    {
        printf(" %02x", signature[i]);
    }
    printf("...\n");

    /* Really should use a mask here, but its not necessary for the moment. */
    for (found = false; (p > hostdriver) && (found == false); p--)
        if (memcmp(p, signature, slen) == 0)
            found = true;

    if (!found)
    {
        printf("Signature not found!\n");
        return NULL;
    }

    p++;
    printf("Found signature at file offset 0x%08x\n", p - hostdriver);

    return (u8*) p;
}

/* Returns a pointer to the PE header */
static void* pe_header(const void *data)
{
    __le32 *e_lfanew = (__le32 *) (data + 0x3c);

    /* data + *e_lfanew gives us the NT SIGNATURE
     * The NT signature is 4 bytes long.
     * The PE header follows immediately.
     */
    return (void *)(data + (le32_to_host(*e_lfanew) + 4));
}

/* returns the expected imagebase */
static u32 pe_imagebase(const void *data)
{
    void *pe_hdr = pe_header(data);

    return le32_to_host(*((u32 *) (pe_hdr + 0x30)));
}

typedef __be32 mac_cpu_type_t;
typedef __be32 mac_cpu_subtype_t;
typedef __be32 mac_vmprot_t;

struct mach_header {
    __be32 magic;
    mac_cpu_type_t cputype;
    mac_cpu_subtype_t cpusubtype;
    __be32 filetype;
    __be32 ncmds;
    __be32 sizeofcmds;
    __be32 flags;
};

struct mach_load_command {
    __be32 cmd;
    __be32 cmdsize;
};

struct mach_segment_command {
    __be32 cmd;
    __be32 cmdsize;
    char segname[16];
    __be32 vmaddr;
    __be32 vmsize;
    __be32 fileoff;
    __be32 filesize;
    mac_vmprot_t maxprot;
    mac_vmprot_t initprot;
    __be32 nsects;
    __be32 flags;
};

struct mach_section {
    char sectname[16];
    char segname[16];
    __be32 addr;
    __be32 size;
    __be32 offset;
    __be32 align;
    __be32 reloff;
    __be32 nreloc;
    __be32 flags;
    __be32 reserved1;
    __be32 reserved2;
};

/* Returns the start of the segment that contains the firmware.  Also
 * identifies the end of the section. It appears that the firmware
 * contains another second copy of our pointers later on (at about
 * offset 0x12330) which interfere with our search.
 */
static const struct mach_section* macho_fw_section(const void *data)
{
    const struct mach_header *hdr = data;
    u32 i, j;
    const void *p = data + sizeof(struct mach_header);

    for (i = 0; i < be32_to_host(hdr->ncmds); i++)
    {
        const struct mach_load_command *load_cmd = p;

        if (be32_to_host(load_cmd->cmd) == 0x0001)
        {
            /* LC_SEGMENT */

            const struct mach_segment_command *seg_cmd = p;
            p += sizeof(struct mach_segment_command);
            for (j = 0; j < be32_to_host(seg_cmd->nsects); j++)
            {
                const struct mach_section *sect = p;

                if ((strcmp(sect->sectname, "__data") == 0) &&
                    (strcmp(sect->segname, "__DATA") == 0))
                {
                    return sect;
                }

                p += sizeof(struct mach_section);
            }
        }

        /* advance to past the load command */
        p += sizeof(struct mach_load_command);
        p += be32_to_host(load_cmd->cmdsize);
    }

    printf("Couldn't find Mach-O __data/__DATA section\n");
    return NULL;
}

#define MH_MAGIC    0xfeedface  /* BE Mach-O magic number */
#define MH_CIGAM    0xcefaedfe  /* LE Mach-O magic number */
#define MH_MAGIC_64 0xfeedfacf  /* BE Mach-O 64-bit magic number */
#define MH_CIGAM_64 0xcffaedfe  /* LE Mach-O 64-bit magic number */

/* Validates the Mach-O object file; only accepts 32-bit BE
 * PPC Mach-O object files because classic AirPort was only
 * ever used on 32-bit PPC machines.
 *
 * Returns:
 *   0 = success
 *  -1 = Not a 32-bit PPC Mach-O object file
 *  -2 = Not a Mach-O object file
 */
static int macho_validate(const void *data)
{
    const struct mach_header *hdr = data;

    switch (be32_to_host(hdr->magic))
    {
    case MH_MAGIC:
        /* Yay, what we need */
        break;
    case MH_MAGIC_64:
    case MH_CIGAM_64:
    case MH_CIGAM:
        /* 64-bit or LE 32-bit, can't use it */
        return -1;
    default:
        /* Not a Mach-O file at all */
        return -2;
    }

    /* PPC */
    if (be32_to_host(hdr->cputype) != 0x12)
        return -1;

    /* MH_OBJECT */
    if (be32_to_host(hdr->filetype) != 0x0001)
        return -1;

    return 0;
}

/* Returns a pointer within data, to the fwblock pointing containing a
 * pointer to addr (in driver address space)*/
static u8* find_fwblock_entry(const u8 *data, const struct fw_layout *layout,
                              u32 addr)
{
    u32 *p = (u32*) ((unsigned int)(data + layout->max_offset) & 0xFFFFFFFCu);
    bool found = false;

    printf("Now searching for driver's firmware block entry (0x%08x)...\n",
           addr);

    /* Convert to driver endianness to compare against file data */
    addr = host_to_driver_32(addr);

    /* Note that we're not searching each byte position for a match.
     * This should be fine because the data should have been placed on
     * a 4-byte boundary */

    for (found = false; ((u8*)p >= data); p--)
    {
        if (*p == addr)
        {
            found = true;
            break;
        }
    }

    /* Compensate for the fields before the data_p pointer */
    p -= layout->datap_offset / sizeof(*p);

    return (u8*) p;
}

static struct fwtable_drv* find_fwtable_entry(const u8 *data,
                                              const struct fw_layout *layout,
                                              u32 fwblock)
{
    u32 *p = (u32*) ((unsigned int)(data + layout->max_offset) & 0xFFFFFFFCu);
    struct fwtable_drv *firmware;
    bool found = false;

    printf("Looking for main firmware table....\n");

    /* Convert to driver endianess to compare against file data */
    fwblock = host_to_driver_32(fwblock);

    for (found = false; ((u8*)p >= data); p--)
    {
        if (*p == fwblock)
        {
            found = true;
            break;
        }
    }

    firmware = (struct fwtable_drv *)p;
    if (found == false)
    {
        printf("Main table not found - contact Mark!\n");
    }
    else
    {
        printf("Found at file offset 0x%08x\n", (u8*)p - data);
    }
    return firmware;
}

/* Copy all data in firmware block from virtual address space to
 * mapped file address space.
 *
 * Also convert from little endian to host endian while we're at it.
 * Some data will need to be converted back to LE when written out,
 * but it will be easier than trying to kepp track of the endianness.
 */
static void copy_fw_data(struct fwtable *firmware,
                         const struct fwtable_drv *driver_fw,
                         const void *data,
                         const struct fw_layout *layout)
{
    union fwblock_drv block;
    const struct plugarray_drv *pdr;
    const struct plugarray_drv *pri;
    const struct ident_info_drv *ident;
    const struct compat_info_drv *compat;

    /* Static data structures to write host endian data to */
    static struct fwblock block_data[4] = {{ 0, 0, 0, 0 }};
    static struct plugarray plug_data[64] = {{ 0, 0, 0 }};
    static struct plugarray pri_plug_data[64] = {{ 0, 0, 0 }};
    static struct ident_info ident_data = { 0, 0, 0, 0, 0, 0 };
    static struct compat_info compat_data[16] = {{ 0 }};
    u32 delta = (u32)data - layout->addr_delta;
    size_t i;

    /* Calculate valid pointers to driver data */
    block.w = (struct fwblock_wdrv *)
        (driver_to_host_32(driver_fw->segarray_p) + delta);
    pdr = (struct plugarray_drv *)
        (driver_to_host_32(driver_fw->plugarray_p) + delta);
    pri = (struct plugarray_drv *)
        (driver_to_host_32(driver_fw->pri_plugarray_p) + delta);
    ident = (struct ident_info_drv *)
        (driver_to_host_32(driver_fw->ident_p) + delta);
    compat = (struct compat_info_drv *)
        (driver_to_host_32(driver_fw->compat_p) + delta);

    /* Setup pointers to host data */
    firmware->segarray = &block_data[0];
    firmware->plugarray = &plug_data[0];
    firmware->pri_plugarray = &pri_plug_data[0];
    firmware->compat = &compat_data[0];
    firmware->ident = &ident_data;

    firmware->halfentry = driver_to_host_32(driver_fw->halfentry);

    for (i = 0; i < ARRAY_SIZE(block_data); i++)
    {
        u32 offset = layout->mac ? driver_to_host_32(block.m[i].offset) :
                                   driver_to_host_32(block.w[i].offset);
        u32 data_p = ((layout->mac ? driver_to_host_32(block.m[i].data_p) :
                                     driver_to_host_32(block.w[i].data_p)) +
                      layout->block_prefix);
        u16 size = layout->mac ? driver_to_host_16(block.m[i].size) :
                                 driver_to_host_16(block.w[i].size);
        u16 flags = layout->mac ? (u16) driver_to_host_32(block.m[i].flags) :
                                  driver_to_host_16(block.w[i].flags);

        if (offset != 0)
        {
            firmware->segarray[i].data = (uint8_t *)(data_p + delta);
            firmware->segarray[i].offset = offset;
            firmware->segarray[i].size = size;
            firmware->segarray[i].flags = flags;
            printf("Segment: %d File offs: 0x%08x Target mem: 0x%08x "
                   "Length 0x%04x%s\n",
                   i,
                   (void *)(&block.w[i]) - data,
                   firmware->segarray[i].offset,
                   firmware->segarray[i].size,
                   (firmware->segarray[i].size == 0) ? " (ignored)" : "");
        }
        else
        {
            firmware->segarray[i].data = NULL;
            firmware->segarray[i].offset = 0;
            firmware->segarray[i].size = 0;
            firmware->segarray[i].flags = 0;
            break;
        }
    }

    printf("Production Data plugrecords at file offset 0x%08x\n",
           (void *)pdr - data);
    printf("Primary plugrecords at file offset 0x%08x\n",
           (void *)pri - data);
    printf("Compatibility info at file offset 0x%08x\n",
           (void *)compat - data);
    printf("Identity info at file offset 0x%08x\n",
           (void *)ident - data);

    /* Copy plugarray */
    for (i = 0; (pdr[i].code != 0) && (i < ARRAY_SIZE(plug_data)); i++)
    {
        firmware->plugarray[i].code = driver_to_host_32(pdr[i].code);
        firmware->plugarray[i].targ_off = driver_to_host_32(pdr[i].targ_off);
        firmware->plugarray[i].length = driver_to_host_32(pdr[i].length);
    }

    /* Copy pri_array */
    for (i = 0; (pri[i].code != 0) && (i < ARRAY_SIZE(pri_plug_data)); i++)
    {
        firmware->pri_plugarray[i].code = driver_to_host_32(pri[i].code);
        firmware->pri_plugarray[i].targ_off =
            driver_to_host_32(pri[i].targ_off);
        firmware->pri_plugarray[i].length = driver_to_host_32(pri[i].length);
    }

    /* Copy identifiers */
    firmware->ident->size = driver_to_host_16(ident->size);
    firmware->ident->code = driver_to_host_16(ident->code);
    firmware->ident->comp_id = driver_to_host_16(ident->comp_id);
    firmware->ident->variant = driver_to_host_16(ident->variant);
    firmware->ident->version_major = driver_to_host_16(ident->version_major);
    firmware->ident->version_minor = driver_to_host_16(ident->version_minor);

    /* Copy compat_info */
    for (i = 0; (compat[i].size != 0) && (i < ARRAY_SIZE(compat_data)); i++)
    {
        size_t j;

        firmware->compat[i].size = driver_to_host_16(compat[i].size);
        firmware->compat[i].code = driver_to_host_16(compat[i].code);
        firmware->compat[i].role = driver_to_host_16(compat[i].role);
        firmware->compat[i].id = driver_to_host_16(compat[i].id);
        for (j = 0; j < ARRAY_SIZE(compat[i].range); j++)
        {
            firmware->compat[i].range[j].variant =
                driver_to_host_16(compat[i].range[j].variant);
            firmware->compat[i].range[j].bottom =
                driver_to_host_16(compat[i].range[j].bottom);
            firmware->compat[i].range[j].top =
                driver_to_host_16(compat[i].range[j].top);
        }
    }
}

static void print_fw_ident(const struct fwtable *firmware)
{
    size_t i;

    if ((firmware->ident->code == 0xFD20u) || /* FW_IDENTITY */
        (firmware->ident->code == 0x014Bu))   /* AP_IDENTITY */
    {
        for (i = 0; i < ARRAY_SIZE(compat_table); i++)
        {
            if (compat_table[i].id == firmware->ident->comp_id)
                break;
        }
        if (i == ARRAY_SIZE(compat_table))
            i--;

        printf("Firmware identity: %s, Variant %d Version %d.%2d\n",
               compat_table[i].comp_string,
               firmware->ident->variant,
               firmware->ident->version_major,
               firmware->ident->version_minor);
    }
}

static int write_hermesap_fw(FILE *output, const struct fwtable *firmware)
{
    unsigned int i;

    fprintf(output, "HFW1\nENTRY %08X\n", firmware->halfentry * 2);

    for (i = 0; firmware->plugarray[i].code != 0; i++)
    {
        fprintf(output, "PLUG %08X %08X %08X\n",
                firmware->plugarray[i].code,
                firmware->plugarray[i].targ_off,
                firmware->plugarray[i].length);
    }

    for (i = 0; firmware->segarray[i].offset != 0; i++)
    {
        u16 j;

        if (i != 0)
            fprintf(output, "\n");
        fprintf(output, "SEG %08X %08X %08X",
                firmware->segarray[i].offset,
                firmware->segarray[i].size,
                0);

        for (j = 0; j < firmware->segarray[i].size; j += 2)
        {
            if ((j % 16) == 0)
                fprintf(output, "\nDATA");

            fprintf(output, " %02X%02X",
                    firmware->segarray[i].data[j],
                    firmware->segarray[i].data[j + 1]);
        }
    }

    fputc('\n', output);

    return 0;
}

static size_t count_blocks(const struct fwblock *first_block)
{
    const struct fwblock *block = first_block;
    size_t count = 0;

    while (block->offset != 0)
    {
        if (block->size > 0)
            count++;
        block++;
    }
    return count;
}

static size_t acc_block_size(const struct fwblock *first_block)
{
    const struct fwblock *block = first_block;
    size_t len = 0;

    while (block->offset != 0)
    {
        len += block->size;
        block++;
    }
    return len;
}

static size_t count_pdr(const struct plugarray *first_pdr)
{
    const struct plugarray *pdr = first_pdr;
    size_t count = 0;

    while (pdr->code)
    {
        count++;
        pdr++;
    }
    return count;
}

static void dump_blocks(FILE *output, const struct fwblock *first_block)
{
    const struct fwblock *block = first_block;
    u8 block_hdr[sizeof(block->offset) + sizeof(block->size)];
    __le32 *addr = (__le32 *) &block_hdr[0];
    __le16 *size = (__le16 *) &block_hdr[sizeof(block->offset)];

    while (block->offset != 0)
    {
        if (block->size > 0)
        {
            *addr = host_to_le32(block->offset);
            *size = host_to_le16(block->size);

            (void)fwrite(&block_hdr, 1, sizeof(block_hdr), output);
            (void)fwrite(block->data, 1, block->size, output);  /* data */
        }
        block++;
    }
    *addr = host_to_le32(0xFFFFFFFFu);  /* set block end */
    *size = host_to_le16(0);
    (void)fwrite(&block_hdr, 1, sizeof(block_hdr), output);
}

static void dump_pdr(FILE *output, const struct plugarray *first_pdr)
{
    const struct plugarray *r = first_pdr;
    u8 pdr[sizeof(r->code) + sizeof(r->targ_off) + sizeof(r->length)];
    __le32 *code = (__le32*) &pdr[0];
    __le32 *addr = (__le32*) &pdr[sizeof(r->code)];
    __le32 *len = (__le32*) &pdr[sizeof(r->code) + sizeof(r->targ_off)];

    if (r)
    {
        while (r->code != 0)
        {
            *code = host_to_le32(r->code);
            *addr = host_to_le32(r->targ_off);
            *len = host_to_le32(r->length);
            (void)fwrite(&pdr, 1, sizeof(pdr), output);
            r++;
        }
    }
    *code = *addr = *len = host_to_le32(0);     /* pdr end */
    (void)fwrite(&pdr, 1, sizeof(pdr), output);
}

static void dump_compat(FILE *output, const struct compat_info *compat)
{
    __le16 buf[sizeof(*compat) / sizeof(__le16)];

    /* Dump non-zero length blocks.
     * No need to reformat. */
    while (compat->size != 0)
    {
        size_t i;

        for (i = 0; i < ARRAY_SIZE(buf); i++)
        {
            buf[i] = host_to_le16(((u16 *) compat)[i]);
        }
        (void)fwrite(buf, 1, sizeof(buf), output);
        compat++;
    }
    /* sentinel */
    memset(&buf, 0, sizeof(buf));
    (void)fwrite(&buf, 1, sizeof(buf), output);
}

#define VERSION "HFW000"
/* Returns zero, or a negative number to indicate an error */
static int write_kernel_fw(FILE *output, const struct fwtable *firmware)
{
    /* Note: does not deal with BE/LE issues */
    u32 image_header[6];
    u32 *ptr;
    u16 headersize = ((sizeof(VERSION) - 1) + sizeof(u16) + (sizeof(u32) * 6));
    u32 blocks = count_blocks(&firmware->segarray[0]);
    u32 blk_offset = 0; /* Immediately after header */
    u32 pdr_offset = (acc_block_size(&firmware->segarray[0]) +
                      ((blocks + 1) * (sizeof(u32) + sizeof(u16))));
    u32 pri_offset = pdr_offset +
        ((count_pdr(&firmware->plugarray[0]) + 1) * sizeof(u32) * 3);
    u32 cpt_offset = pri_offset +
        ((count_pdr(&firmware->pri_plugarray[0]) + 1) * sizeof(u32) * 3);


    (void)fwrite(VERSION, 1, sizeof(VERSION) - 1, output);

    headersize = host_to_le16(headersize);
    (void)fwrite(&headersize, 1, sizeof(headersize), output);

    ptr = &image_header[0];
    *ptr = host_to_le32(firmware->halfentry);   /* entrypoint */
    ptr++;
    *ptr = host_to_le32(blocks);
    ptr++;
    *ptr = host_to_le32(blk_offset);
    ptr++;
    *ptr = host_to_le32(pdr_offset);
    ptr++;
    *ptr = host_to_le32(pri_offset);
    ptr++;
    *ptr = host_to_le32(cpt_offset);
    ptr++;
    (void)fwrite(&image_header, 1, sizeof(image_header), output);

    dump_blocks(output, firmware->segarray);
    dump_pdr(output, firmware->plugarray);
    dump_pdr(output, firmware->pri_plugarray);
    dump_compat(output, firmware->compat);
    return 0;
}

static int dump_fw(const char *basename, char hexchar,
                   const void *data,
                   const struct fw_layout *layout,
                   const u8 *signature, size_t slen)
{
    struct fwtable_drv *fw_image;       /* structure all elements of a given firmware */
    struct fwtable firmware;
    char *fwname;
    char *filename;
    const void *hint;
    void *fw;                   /* pointer to the actual blocks for programming */
    void *fwblock;              /* location of structure listing blocks to program for a given firmware */
    u32 vaddr;
    FILE *output;

    printf("\nAttempting to dump %c firmware:\n", hexchar);
    fwname = find_fw_filename(data, layout->max_offset, hexchar);
    if (fwname)
    {
        /* The filename is towards the end of the FW block,
         * so use it as a hint to locate the start of the block.
         */
        hint = (u8*) fwname;
    }
    else
    {
        hint = data + layout->max_offset;
    }

    /* Now find the first firmware blob using the signature */
    fw = find_fw(data, layout->max_offset, signature, slen, hint);
    if (!fw)
        return -1;

    vaddr = (fw - data) + layout->addr_delta;

    printf("Driver address of first firmware blob is 0x%08x\n", vaddr);

    /* Some drivers fwtables point before the actual block start. */
    vaddr -= layout->block_prefix;

    fwblock = find_fwblock_entry(data, layout, vaddr);

    vaddr = (fwblock - data) + layout->addr_delta;

    if (!fwblock)
    {
        printf("Firmware block entry not found - contact Mark!\n");
        return -2;
    }
    else
    {
        printf("Found firmware block entry at virtual location 0x%08x, "
               "file offset 0x%08x\n", vaddr, fwblock - data);
    }

    /* Got to the first fwblock. Static offset per arch */
    fwblock -= layout->lead_block_bytes;
    vaddr = (fwblock - data) + layout->addr_delta;

    fw_image = find_fwtable_entry(data, layout, vaddr);
    if (!fw_image)
        return -3;

    copy_fw_data(&firmware, fw_image, data, layout);

    /* Print FW ident information */
    printf("Entry point at 0x%08x\n", firmware.halfentry * 2);
    print_fw_ident(&firmware);

    filename = malloc(strlen(basename) + 12);
    strcpy(filename, basename);
    strcat(filename,
           (firmware.ident->comp_id == 31) ? "_sta_fw.bin" : "_ap_fw.bin");

    printf("Dumping to %s...\n", filename);
    if ((output = fopen(filename, "wb")) == NULL)
    {
        printf("Unable to open %s, aborting.\n", filename);
        free(filename);
        return -4;
    }

#if 0
    write_hermesap_fw(output, &firmware);
#else
    write_kernel_fw(output, &firmware);
#endif

    fclose(output);
    printf("Dump of %s complete.\n", filename);
    free(filename);

    return 0;
}

struct fw_layout* detect_fw_layout(const void *data, size_t flen)
{
    int macho = macho_validate(data);
    struct fw_layout *layout;
    bool mac = false;

    if (macho == 0)
    {
        const struct mach_section *fw_section;

        printf("Driver looks like Apple Mach-O format\n");

        mac = true;
        driver_is_be = true;
        layout = &firmware_layout[mac];

        fw_section = macho_fw_section(data);

        layout->addr_delta = (be32_to_host(fw_section->addr) -
                              be32_to_host(fw_section->offset));

        /* restrict search area */
        layout->max_offset = (size_t) (be32_to_host(fw_section->offset) +
                                       be32_to_host(fw_section->size));
        layout->mac = mac;
    }
    else if (macho == -1)
    {
        printf("Driver looks like Apple Mach-O format\n"
               "But only a 32-bit PPC Mach-O format driver is supported.\n");
        return NULL;
    }
    else if (memcmp(data, "MZ", 2) == 0)
    {
        printf("Driver looks like Microsoft PE format\n");
        driver_is_be = false;
        mac = false;

        layout = &firmware_layout[mac];

        layout->addr_delta = (ptrdiff_t) pe_imagebase(data);
        layout->max_offset = flen;
        layout->mac = mac;
    }
    else if (memcmp(data, "Joy!", 4) == 0)
    {
        printf("Driver looks like Apple PEF format\n");
        printf("I don't know how to extract for this format.\n");

        /* Need to look at each of the section headers.
         * Number of section headers if given at offset 0x20 (32) (__be16?)
         * First section header starts at offset 0x28 (40)
         * Each section header is 0x1C (28) bytes long
         * Subsequent section headers follow immediately after.
         * Each section header specifies its imagebase at offset 0x4 (__be32?)
         * We need to use the imagebase of the section in which the firmware is found.
         * The offset to the section data is at offset 0x14 (20) (__be32?).
         *    This offset is relative to the beginning of the file.
         * The length of each sections data is at offset 0x10 (16) (__be32?)
         */
        /* layout->addr_delta = pef_imagebase(data); */
        return NULL;
    }
    else
    {
        printf("Unknown object file format\n");
        return NULL;
    }

    return layout;
}

/*
 * Main
 */
int main(int argc, char *argv[])
{
    FILE *input;
    void *data;
    size_t read_bytes;
    size_t flen;

    printf("Lucent Firmware Extractor v1.1\n"
           "(c) 2003 Mark Smith (username 'Mark' on HermesAP board)\n"
           "(c) 2007,2008 Dave Kilroy\n");

    check_endianess();

    /* Attempt to load file */
    if (argc != 3)
    {
        printf("Usage: %s <driver> <output_base>\n", argv[0]);
        return -1;
    }

    /* TODO: parse options better
     * Want to be able to specify:
     *   input file (WLAGS49B.SYS)
     *   output file base name (wlags_)
     *   desired output format --hermesap or --kernel
     * Others?
     */

    if ((input = fopen(argv[1], "rb")) == NULL)
    {
        printf("Unable to open %s, aborting.\n", argv[1]);
        return -1;
    }

    /* Get file length */
    fseek(input, 0L, SEEK_END);
    flen = ftell(input);
    printf("File %s length %u (0x%08x)\n", argv[1], flen, flen);

    /* Rewind file pointer */
    fseek(input, 0L, SEEK_SET);

    /* Allocate memory and load the file */
    data = malloc(flen);
    read_bytes = fread(data, 1, flen, input);
    fclose(input);

    if (read_bytes == flen)
    {
        u8 t_sig[4] = { 0x61, 0x44, 0xfe, 0xfb };
        u8 r_sig[4] = { 0x0f, 0x60, 0xfc, 0x63 };
        struct fw_layout *layout;

        printf("Memory allocated and file read OK\n");

        layout = detect_fw_layout(data, flen);
        if (layout)
        {
            dump_fw(argv[2], 'T', data, layout, t_sig, sizeof(t_sig));
            dump_fw(argv[2], 'R', data, layout, r_sig, sizeof(r_sig));
        }
    }
    else
    {
        printf("Only read %d out of %d bytes\n", read_bytes, flen);
    }

    free(data);
    printf("\nAll dumps complete.\n\n");
    return 0;
}