[2/2] Load SMBIOS entries and files from qemu

diff --git a/src/paravirt.c b/src/paravirt.c
index cd1f263..48f10a1 100644
--- a/src/paravirt.c
+++ b/src/paravirt.c
@@ -27,6 +27,13 @@  qemu_cfg_read(u8 *buf, int len)
 }
 
 static void
+qemu_cfg_skip(int len)
+{
+    while (len--)
+        inb(PORT_QEMU_CFG_DATA);
+}
+
+static void
 qemu_cfg_read_entry(void *buf, int e, int len)
 {
     qemu_cfg_select(e);
@@ -99,3 +106,140 @@  void* qemu_cfg_next_acpi_table_load(void *addr, u16 len)
     return addr;
 }
 
+u16 qemu_cfg_smbios_entries(void)
+{
+    u16 cnt;
+
+    if (!qemu_cfg_present)
+        return 0;
+
+    qemu_cfg_read_entry(&cnt, QEMU_CFG_SMBIOS_ENTRIES, sizeof(cnt));
+
+    return cnt;
+}
+
+struct smbios_header {
+    u16 length;
+    u8 type;
+} PACKED;
+
+struct smbios_field {
+    struct smbios_header header;
+    u8 type;
+    u16 offset;
+    u8 data[];
+} PACKED;
+
+struct smbios_table {
+    struct smbios_header header;
+    u8 data[];
+} PACKED;
+
+#define SMBIOS_FIELD_ENTRY 0
+#define SMBIOS_TABLE_ENTRY 1
+
+size_t qemu_cfg_smbios_load_field(int type, size_t offset, void *addr)
+{
+    int i;
+
+    for (i = qemu_cfg_smbios_entries(); i > 0; i--) {
+        struct smbios_field field;
+
+        qemu_cfg_read((u8 *)&field, sizeof(struct smbios_header));
+        field.header.length -= sizeof(struct smbios_header);
+
+        if (field.header.type != SMBIOS_FIELD_ENTRY) {
+            qemu_cfg_skip(field.header.length);
+            continue;
+        }
+
+        qemu_cfg_read((u8 *)&field.type,
+                      sizeof(field) - sizeof(struct smbios_header));
+        field.header.length -= sizeof(field) - sizeof(struct smbios_header);
+
+        if (field.type != type || field.offset != offset) {
+            qemu_cfg_skip(field.header.length);
+            continue;
+        }
+
+        qemu_cfg_read(addr, field.header.length);
+        return (size_t)field.header.length;
+    }
+    return 0;
+}
+
+/* This goes at the beginning of every SMBIOS structure. */
+struct smbios_structure_header {
+	u8 type;
+	u8 length;
+	u16 handle;
+} PACKED;
+
+int qemu_cfg_smbios_load_external(int type, char **p, unsigned *nr_structs,
+                                  unsigned *max_struct_size)
+{
+    static u64 used_bitmap[4] = { 0 };
+    char *start = *p;
+    int i;
+
+    /* Check if we've already reported these tables */
+    if (used_bitmap[(type >> 6) & 0x3] & (1ULL << (type & 0x3f)))
+        return 1;
+
+    /* Don't introduce spurious end markers */
+    if (type == 127)
+        return 0;
+
+    for (i = qemu_cfg_smbios_entries(); i > 0; i--) {
+        struct smbios_table table;
+        struct smbios_structure_header *header = (void *)*p;
+        int string;
+
+        qemu_cfg_read((u8 *)&table, sizeof(struct smbios_header));
+        table.header.length -= sizeof(struct smbios_header);
+
+        if (table.header.type != SMBIOS_TABLE_ENTRY) {
+            qemu_cfg_skip(table.header.length);
+            continue;
+        }
+
+        qemu_cfg_read((u8 *)*p, sizeof(struct smbios_structure_header));
+        table.header.length -= sizeof(struct smbios_structure_header);
+
+        if (header->type != type) {
+            qemu_cfg_skip(table.header.length);
+            continue;
+        }
+
+        *p += sizeof(struct smbios_structure_header);
+
+        /* Entries end with a double NULL char, if there's a string at
+         * the end (length is greater than formatted length), the string
+         * terminator provides the first NULL. */
+        string = header->length < table.header.length +
+                 sizeof(struct smbios_structure_header);
+
+        /* Read the rest and terminate the entry */
+        qemu_cfg_read((u8 *)*p, table.header.length);
+        *p += table.header.length;
+        *((u8*)*p) = 0;
+        (*p)++;
+        if (!string) {
+            *((u8*)*p) = 0;
+            (*p)++;
+        }
+
+        (*nr_structs)++;
+        if (*p - (char *)header > *max_struct_size)
+            *max_struct_size = *p - (char *)header;
+    }
+
+    if (start != *p) {
+        /* Mark that we've reported on this type */
+        used_bitmap[(type >> 6) & 0x3] |= (1ULL << (type & 0x3f));
+        return 1;
+    }
+
+    return 0;
+}
+
diff --git a/src/paravirt.h b/src/paravirt.h
index c2bab71..c2e9470 100644
--- a/src/paravirt.h
+++ b/src/paravirt.h
@@ -43,5 +43,9 @@  void qemu_cfg_get_uuid(u8 *uuid);
 u16 qemu_cfg_acpi_additional_tables(void);
 u16 qemu_cfg_next_acpi_table_len(void);
 void *qemu_cfg_next_acpi_table_load(void *addr, u16 len);
+u16 qemu_cfg_smbios_entries(void);
+size_t qemu_cfg_smbios_load_field(int type, size_t offset, void *addr);
+int qemu_cfg_smbios_load_external(int type, char **p, unsigned *nr_structs,
+                                  unsigned *max_struct_size);
 
 #endif
diff --git a/src/smbios.c b/src/smbios.c
index 30dc853..d4627cf 100644
--- a/src/smbios.c
+++ b/src/smbios.c
@@ -10,25 +10,6 @@ 
 #include "paravirt.h"
 
 /****************************************************************
- * UUID probe
- ****************************************************************/
-
-static void
-uuid_probe(u8 *bios_uuid)
-{
-    // Default to UUID not set
-    memset(bios_uuid, 0, 16);
-
-    if (! CONFIG_UUID_BACKDOOR)
-        return;
-    if (CONFIG_COREBOOT)
-        return;
-
-    qemu_cfg_get_uuid(bios_uuid);
-}
-
-
-/****************************************************************
  * smbios tables
  ****************************************************************/
 
@@ -229,72 +210,121 @@  smbios_entry_point_init(u16 max_structure_size,
     dprintf(1, "SMBIOS ptr=%p table=%p\n", ep, structure_table_address);
 }
 
+#define load_str_field_with_default(type, field, def)                   \
+    do {                                                                \
+        size = qemu_cfg_smbios_load_field(type,                         \
+                                 offsetof(struct smbios_type_##type,    \
+                                          field), end);                 \
+        if (size > 0) {                                                 \
+            end += size;                                                \
+        } else {                                                        \
+            memcpy(end, def, sizeof(def));                              \
+            end += sizeof(def);                                         \
+        }                                                               \
+        p->field = ++str_index;                                         \
+    } while (0)
+
+#define load_str_field_or_skip(type, field) \
+    do {                                                                \
+        size = qemu_cfg_smbios_load_field(type,                         \
+                                 offsetof(struct smbios_type_##type,    \
+                                          field), end);                 \
+        if (size > 0) {                                                 \
+            end += size;                                                \
+            p->field = ++str_index;                                     \
+        } else {                                                        \
+            p->field = 0;                                               \
+        }                                                               \
+    } while (0)
+
 /* Type 0 -- BIOS Information */
 #define RELEASE_DATE_STR "01/01/2007"
 static void *
-smbios_type_0_init(void *start)
+smbios_init_type_0(void *start)
 {
     struct smbios_type_0 *p = (struct smbios_type_0 *)start;
+    char *end = (char *)start + sizeof(struct smbios_type_0);
+    size_t size;
+    int str_index = 0;
 
     p->header.type = 0;
     p->header.length = sizeof(struct smbios_type_0);
     p->header.handle = 0;
 
-    p->vendor_str = 1;
-    p->bios_version_str = 1;
+    load_str_field_with_default(0, vendor_str, CONFIG_APPNAME);
+    load_str_field_with_default(0, bios_version_str, CONFIG_APPNAME);
+
     p->bios_starting_address_segment = 0xe800;
-    p->bios_release_date_str = 2;
+
+    load_str_field_with_default(0, bios_release_date_str, RELEASE_DATE_STR);
+
     p->bios_rom_size = 0; /* FIXME */
 
-    memset(p->bios_characteristics, 0, sizeof(p->bios_characteristics));
+    memset(p->bios_characteristics, 0, 8);
     p->bios_characteristics[0] = 0x08; /* BIOS characteristics not supported */
     p->bios_characteristics_extension_bytes[0] = 0;
     p->bios_characteristics_extension_bytes[1] = 0;
 
-    p->system_bios_major_release = 1;
-    p->system_bios_minor_release = 0;
+    if (!qemu_cfg_smbios_load_field(0, offsetof(struct smbios_type_0,
+                                                system_bios_major_release),
+                                    &p->system_bios_major_release))
+        p->system_bios_major_release = 1;
+
+    if (!qemu_cfg_smbios_load_field(0, offsetof(struct smbios_type_0,
+                                                system_bios_minor_release),
+                                    &p->system_bios_minor_release))
+        p->system_bios_minor_release = 0;
+
     p->embedded_controller_major_release = 0xff;
     p->embedded_controller_minor_release = 0xff;
 
-    start += sizeof(struct smbios_type_0);
-    memcpy((char *)start, CONFIG_APPNAME, sizeof(CONFIG_APPNAME));
-    start += sizeof(CONFIG_APPNAME);
-    memcpy((char *)start, RELEASE_DATE_STR, sizeof(RELEASE_DATE_STR));
-    start += sizeof(RELEASE_DATE_STR);
-    *((u8 *)start) = 0;
+    *end = 0;
+    end++;
 
-    return start+1;
+    return end;
 }
 
 /* Type 1 -- System Information */
 static void *
-smbios_type_1_init(void *start)
+smbios_init_type_1(void *start)
 {
     struct smbios_type_1 *p = (struct smbios_type_1 *)start;
+    char *end = (char *)start + sizeof(struct smbios_type_1);
+    size_t size;
+    int str_index = 0;
+
     p->header.type = 1;
     p->header.length = sizeof(struct smbios_type_1);
     p->header.handle = 0x100;
 
-    p->manufacturer_str = 0;
-    p->product_name_str = 0;
-    p->version_str = 0;
-    p->serial_number_str = 0;
+    load_str_field_or_skip(1, manufacturer_str);
+    load_str_field_or_skip(1, product_name_str);
+    load_str_field_or_skip(1, version_str);
+    load_str_field_or_skip(1, serial_number_str);
 
-    uuid_probe(p->uuid);
+    size = qemu_cfg_smbios_load_field(1, offsetof(struct smbios_type_1,
+                                                  uuid), &p->uuid);
+    if (size == 0)
+        memset(p->uuid, 0, 16);
 
     p->wake_up_type = 0x06; /* power switch */
-    p->sku_number_str = 0;
-    p->family_str = 0;
 
-    start += sizeof(struct smbios_type_1);
-    *((u16 *)start) = 0;
+    load_str_field_or_skip(1, sku_number_str);
+    load_str_field_or_skip(1, family_str);
 
-    return start+2;
+    *end = 0;
+    end++;
+    if (!str_index) {
+        *end = 0;
+        end++;
+    }
+
+    return end;
 }
 
 /* Type 3 -- System Enclosure */
 static void *
-smbios_type_3_init(void *start)
+smbios_init_type_3(void *start)
 {
     struct smbios_type_3 *p = (struct smbios_type_3 *)start;
 
@@ -324,7 +354,7 @@  smbios_type_3_init(void *start)
 
 /* Type 4 -- Processor Information */
 static void *
-smbios_type_4_init(void *start, unsigned int cpu_number)
+smbios_init_type_4(void *start, unsigned int cpu_number)
 {
     struct smbios_type_4 *p = (struct smbios_type_4 *)start;
 
@@ -366,7 +396,7 @@  smbios_type_4_init(void *start, unsigned int cpu_number)
 
 /* Type 16 -- Physical Memory Array */
 static void *
-smbios_type_16_init(void *start, u32 memory_size_mb, int nr_mem_devs)
+smbios_init_type_16(void *start, u32 memory_size_mb, int nr_mem_devs)
 {
     struct smbios_type_16 *p = (struct smbios_type_16*)start;
 
@@ -389,7 +419,7 @@  smbios_type_16_init(void *start, u32 memory_size_mb, int nr_mem_devs)
 
 /* Type 17 -- Memory Device */
 static void *
-smbios_type_17_init(void *start, u32 memory_size_mb, int instance)
+smbios_init_type_17(void *start, u32 memory_size_mb, int instance)
 {
     struct smbios_type_17 *p = (struct smbios_type_17 *)start;
 
@@ -420,7 +450,7 @@  smbios_type_17_init(void *start, u32 memory_size_mb, int instance)
 
 /* Type 19 -- Memory Array Mapped Address */
 static void *
-smbios_type_19_init(void *start, u32 memory_size_mb, int instance)
+smbios_init_type_19(void *start, u32 memory_size_mb, int instance)
 {
     struct smbios_type_19 *p = (struct smbios_type_19 *)start;
 
@@ -441,7 +471,7 @@  smbios_type_19_init(void *start, u32 memory_size_mb, int instance)
 
 /* Type 20 -- Memory Device Mapped Address */
 static void *
-smbios_type_20_init(void *start, u32 memory_size_mb, int instance)
+smbios_init_type_20(void *start, u32 memory_size_mb, int instance)
 {
     struct smbios_type_20 *p = (struct smbios_type_20 *)start;
 
@@ -465,7 +495,7 @@  smbios_type_20_init(void *start, u32 memory_size_mb, int instance)
 
 /* Type 32 -- System Boot Information */
 static void *
-smbios_type_32_init(void *start)
+smbios_init_type_32(void *start)
 {
     struct smbios_type_32 *p = (struct smbios_type_32 *)start;
 
@@ -483,7 +513,7 @@  smbios_type_32_init(void *start)
 
 /* Type 127 -- End of Table */
 static void *
-smbios_type_127_init(void *start)
+smbios_init_type_127(void *start)
 {
     struct smbios_type_127 *p = (struct smbios_type_127 *)start;
 
@@ -514,20 +544,25 @@  smbios_init(void)
     u32 nr_structs = 0, max_struct_size = 0;
     char *q, *p = start;
 
-#define add_struct(fn) { \
-    q = (fn); \
-    nr_structs++; \
-    if ((q - p) > max_struct_size) \
-        max_struct_size = q - p; \
-    p = q; \
-}
+#define add_struct(type, args...)                                       \
+    do {                                                                \
+        if (!qemu_cfg_smbios_load_external(type, &p, &nr_structs,       \
+                                           &max_struct_size)) {         \
+            q = smbios_init_type_##type(args);                          \
+            nr_structs++;                                               \
+            if ((q - p) > max_struct_size)                              \
+                max_struct_size = q - p;                                \
+            p = q;                                                      \
+        }                                                               \
+    } while (0)
+
+    add_struct(0, p);
+    add_struct(1, p);
+    add_struct(3, p);
 
-    add_struct(smbios_type_0_init(p));
-    add_struct(smbios_type_1_init(p));
-    add_struct(smbios_type_3_init(p));
     int cpu_num, smp_cpus = CountCPUs;
     for (cpu_num = 1; cpu_num <= smp_cpus; cpu_num++)
-        add_struct(smbios_type_4_init(p, cpu_num));
+        add_struct(4, p, cpu_num);
     u64 memsize = RamSizeOver4G;
     if (memsize)
         memsize += 0x100000000ull;
@@ -535,17 +570,21 @@  smbios_init(void)
         memsize = RamSize;
     memsize = memsize / (1024 * 1024);
     int nr_mem_devs = (memsize + 0x3fff) >> 14;
-    add_struct(smbios_type_16_init(p, memsize, nr_mem_devs));
+    add_struct(16, p, memsize, nr_mem_devs);
     int i;
     for (i = 0; i < nr_mem_devs; i++) {
         u32 dev_memsize = ((i == (nr_mem_devs - 1))
                            ? (((memsize-1) & 0x3fff)+1) : 0x4000);
-        add_struct(smbios_type_17_init(p, dev_memsize, i));
-        add_struct(smbios_type_19_init(p, dev_memsize, i));
-        add_struct(smbios_type_20_init(p, dev_memsize, i));
+        add_struct(17, p, dev_memsize, i);
+        add_struct(19, p, dev_memsize, i);
+        add_struct(20, p, dev_memsize, i);
     }
-    add_struct(smbios_type_32_init(p));
-    add_struct(smbios_type_127_init(p));
+
+    add_struct(32, p);
+    /* Add any remaining provided entries before the end marker */
+    for (i = 0; i < 256; i++)
+        qemu_cfg_smbios_load_external(i, &p, &nr_structs, &max_struct_size);
+    add_struct(127, p);
 
 #undef add_struct