[PATCH 2/4] kvm tools: Fix PCI probing

[Sasha Levin <levinsasha928@gmail.com>]

PCI BAR probing is done in four steps:

 1. Read address (and flags).
 2. Mask BAR.
 3. Read BAR again - Now the expected result is the size of the BAR.
 4. Mask BAR with address.

So far, we have only took care of the first step. This means that the kernel
was using address as the size, causing a PCI allocation blunder.

This patch fixes the issue by passing a proper size after masking.

Signed-off-by: Sasha Levin <levinsasha928@gmail.com>
---
 tools/kvm/include/kvm/pci.h |    1 +
 tools/kvm/pci.c             |   57 +++++++++++++++++++++++++++++++++++++++----
 2 files changed, 53 insertions(+), 5 deletions(-)

diff --git a/tools/kvm/include/kvm/pci.h b/tools/kvm/include/kvm/pci.h
index 6ad4426..a7532e3 100644
--- a/tools/kvm/include/kvm/pci.h
+++ b/tools/kvm/include/kvm/pci.h
@@ -51,5 +51,6 @@ struct pci_device_header {
 
 void pci__init(void);
 void pci__register(struct pci_device_header *dev, u8 dev_num);
+u32 pci_get_io_space_block(void);
 
 #endif /* KVM__PCI_H */
diff --git a/tools/kvm/pci.c b/tools/kvm/pci.c
index a1ad8ba..799536e3 100644
--- a/tools/kvm/pci.c
+++ b/tools/kvm/pci.c
@@ -5,11 +5,23 @@
 #include <assert.h>
 
 #define PCI_MAX_DEVICES			256
+#define PCI_IO_SIZE			0x100
 
 static struct pci_device_header		*pci_devices[PCI_MAX_DEVICES];
 
 static struct pci_config_address	pci_config_address;
 
+/* This is within our PCI gap */
+static u32 io_space_blocks		= 0xE1000000;
+
+u32 pci_get_io_space_block(void)
+{
+	u32 block = io_space_blocks;
+	io_space_blocks += PCI_IO_SIZE;
+
+	return block;
+}
+
 static void *pci_config_address_ptr(u16 port)
 {
 	unsigned long offset;
@@ -44,11 +56,6 @@ static struct ioport_operations pci_config_address_ops = {
 	.io_out		= pci_config_address_out,
 };
 
-static bool pci_config_data_out(struct ioport *ioport, struct kvm *kvm, u16 port, void *data, int size, u32 count)
-{
-	return true;
-}
-
 static bool pci_device_exists(u8 bus_number, u8 device_number, u8 function_number)
 {
 	struct pci_device_header *dev;
@@ -67,6 +74,46 @@ static bool pci_device_exists(u8 bus_number, u8 device_number, u8 function_numbe
 	return dev != NULL;
 }
 
+static bool pci_config_data_out(struct ioport *ioport, struct kvm *kvm, u16 port, void *data, int size, u32 count)
+{
+	unsigned long start;
+	u8 dev_num;
+
+	/*
+	 * If someone accesses PCI configuration space offsets that are not
+	 * aligned to 4 bytes, it uses ioports to signify that.
+	 */
+	start = port - PCI_CONFIG_DATA;
+
+	dev_num		= pci_config_address.device_number;
+
+	if (pci_device_exists(0, dev_num, 0)) {
+		unsigned long offset;
+
+		offset = start + (pci_config_address.register_number << 2);
+		if (offset < sizeof(struct pci_device_header)) {
+			void *p = pci_devices[dev_num];
+			u32 sz = PCI_IO_SIZE;
+
+			/*
+			 * If the kernel masks the BAR it would expect to find the
+			 * size of the BAR there next time it reads from it.
+			 * When the kernel got the size it would write the address
+			 * back.
+			 */
+			if ((offset >= offsetof(struct pci_device_header, bar[0])) &&
+			(offset <= offsetof(struct pci_device_header, bar[6]))) {
+				if (*(u32 *)(p + offset))
+					memcpy(p + offset, &sz, sizeof(sz));
+			} else if (*(u32 *)(p + offset)) {
+				memcpy(p + offset, data, size);
+			}
+		}
+	}
+
+	return true;
+}
+
 static bool pci_config_data_in(struct ioport *ioport, struct kvm *kvm, u16 port, void *data, int size, u32 count)
 {
 	unsigned long start;
-- 
1.7.6