initial support for pflash / cbfs

Signed-off-by: Paolo Bonzini <pbonzini@redhat.com>
diff --git a/Makefile b/Makefile
index 6db2104..bcefa27 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 obj-y = code16.o entry.o main.o string.o printf.o cstart.o fw_cfg.o
-obj-y += linuxboot.o malloc.o
+obj-y += linuxboot.o malloc.o pflash.o
 
 all-y = bios.bin
 all: $(all-y)
diff --git a/include/pflash.h b/include/pflash.h
new file mode 100644
index 0000000..1dbbeae
--- /dev/null
+++ b/include/pflash.h
@@ -0,0 +1,8 @@
+#ifndef BIOS_PFLASH_H
+#define BIOS_PFLASH_H 1
+
+#include <stdint.h>
+
+void *pflash_base(int n, size_t *size);
+
+#endif
diff --git a/main.c b/main.c
index 74c4057..033ea2f 100644
--- a/main.c
+++ b/main.c
@@ -1,3 +1,4 @@
+#include <stdbool.h>
 #include "bios.h"
 #include "stdio.h"
 #include "e820.h"
@@ -5,6 +6,7 @@
 #include "string.h"
 #include "segment.h"
 #include "fw_cfg.h"
+#include "pflash.h"
 
 #define PCI_VENDOR_ID_INTEL		0x8086
 #define PCI_DEVICE_ID_INTEL_82441	0x1237
@@ -15,6 +17,9 @@
 
 static void make_bios_writable(void)
 {
+	// NOTE: this runs from ROM at 0xFFFF0000, so it is not possible to use any
+	// static data.
+
 	const int bdf = 0;
 	const uint8_t *bios_start = (uint8_t *)0xffff0000;
 	uint8_t *low_start = (uint8_t *)0xf0000;
@@ -39,6 +44,10 @@
 	// We're still running from 0xffff0000
 	pci_config_writeb(bdf, pambase, 0x30);
 	memcpy(low_start, bios_start, 0x10000);
+
+	// Now go to the F-segment: we need to move away from flash area
+	// in order to probe CBFS!
+	asm("ljmp $0x8, $1f; 1:");
 }
 
 static void set_realmode_int(int vec, void *p)
@@ -111,6 +120,18 @@
 	e820_seg = ((uintptr_t) e820) >> 4;
 }
 
+static bool detect_cbfs_and_boot(void)
+{
+	size_t sz;
+	void *base = pflash_base(1, &sz);
+
+	if (!base)
+		return false;
+
+	// return boot_from_cbfs(base, sz);
+	return false;
+}
+
 int main(void)
 {
 	make_bios_writable();
@@ -122,6 +143,7 @@
 	// extract_smbios();
 	// extract_kernel();
 	// make_bios_readonly();
-	boot_from_fwcfg();
+	if (!detect_cbfs_and_boot())
+		boot_from_fwcfg();
 	panic();
 }
diff --git a/pflash.c b/pflash.c
new file mode 100644
index 0000000..950bc74
--- /dev/null
+++ b/pflash.c
@@ -0,0 +1,63 @@
+#include <stddef.h>
+#include "bios.h"
+#include "pflash.h"
+#include "stdio.h"
+
+#define CLEAR_STATUS_CMD         0x50
+#define READ_STATUS_CMD          0x70
+#define QUERY_CMD                0x98
+#define READ_ARRAY_CMD           0xff
+
+static void *pflash_detect(uint8_t *top_addr)
+{
+	volatile uint8_t *p = top_addr;
+	uint8_t save;
+	int i, blocks, sector_len;
+
+	/* The low byte of the address is part of the command, so it must be 0.  */
+	if ((uintptr_t)p & 256)
+		panic();
+
+	p -= 256;
+	for (i = 0; i < 256; i++)
+		if (p[i] != CLEAR_STATUS_CMD)
+			break;
+	if (i == 256)
+		return NULL;
+
+	save = p[i];
+	p[i] = CLEAR_STATUS_CMD;
+	if (p[i] == CLEAR_STATUS_CMD) {
+		/* behaves as RAM */
+		p[i] = save;
+		return NULL;
+	}
+	p[i] = READ_STATUS_CMD;
+	if (p[i] != 0) {
+		/* doesn't behave as flash */
+		return NULL;
+	}
+
+	/* 0x2d-0x2e: blocks_per_device - 1, little endian */
+	/* 0x2f-0x30: sector_len / 256 */
+	p[i] = QUERY_CMD;
+	blocks = p[0x2d] + (p[0x2e] << 8) + 1;
+	sector_len = (p[0x2f] + (p[0x30] << 8)) << 8;
+
+	p[i] = READ_ARRAY_CMD;
+	return top_addr - blocks * sector_len;
+}
+
+void *pflash_base(int n, size_t *size)
+{
+	uint8_t *top = NULL;
+	uint8_t *prev;
+	while (n-- >= 0) {
+		prev = top;
+		top = pflash_detect(top);
+		if (!top)
+			return NULL;
+		*size = prev - top;
+	}
+	return top;
+}