[PATCH 3/4] Support for device capability

[Sheng Yang <sheng@linux.intel.com>]

This framework can be easily extended to support device capability, like
MSI/MSI-x.

Signed-off-by: Sheng Yang <sheng@linux.intel.com>
---
 qemu/hw/device-assignment.c |   96 ++++++++++++++++++++++++++++++++++++++++--
 qemu/hw/device-assignment.h |    3 +
 2 files changed, 94 insertions(+), 5 deletions(-)

diff --git a/qemu/hw/device-assignment.c b/qemu/hw/device-assignment.c
index c7e92a7..14b5911 100644
--- a/qemu/hw/device-assignment.c
+++ b/qemu/hw/device-assignment.c
@@ -245,11 +245,41 @@ uint8_t pci_find_cap_offset(struct pci_dev *pci_dev, uint8_t cap)
     return 0;
 }
 
+static int access_capability_config(AssignedDevice *pci_dev,
+				    uint32_t address, int len)
+{
+    if (address >= pci_dev->cap.start &&
+	    (address + len) < pci_dev->cap.start + pci_dev->cap.length)
+	return 1;
+    return 0;
+}
+
+static void check_cap_state(AssignedDevice *pci_dev,
+			    uint32_t address, uint32_t* val)
+{
+    return;
+}
+
+static void assigned_dev_pci_write_cap_config(AssignedDevice *pci_dev,
+				uint32_t address, uint32_t val, int len)
+{
+    if (access_capability_config(pci_dev, address, len)) {
+	int i;
+	for (i = 0; i < len; i++) {
+	    check_cap_state(pci_dev, address + i, &val);
+	    pci_dev->cap.config[address + i - pci_dev->cap.start] = val;
+	    val >>= 8;
+	}
+	return;
+    }
+}
+
 static void assigned_dev_pci_write_config(PCIDevice *d, uint32_t address,
                                           uint32_t val, int len)
 {
     int fd;
     ssize_t ret;
+    AssignedDevice *pci_dev = (AssignedDevice *)d;
 
     DEBUG("(%x.%x): address=%04x val=0x%08x len=%d\n",
           ((d->devfn >> 3) & 0x1F), (d->devfn & 0x7),
@@ -267,11 +297,16 @@ static void assigned_dev_pci_write_config(PCIDevice *d, uint32_t address,
         return;
     }
 
+    if (pci_dev->cap.available && access_capability_config(pci_dev, address, len)) {
+	assigned_dev_pci_write_cap_config(pci_dev, address, val, len);
+	return;
+    }
+
     DEBUG("NON BAR (%x.%x): address=%04x val=0x%08x len=%d\n",
           ((d->devfn >> 3) & 0x1F), (d->devfn & 0x7),
           (uint16_t) address, val, len);
 
-    fd = ((AssignedDevice *)d)->real_device.config_fd;
+    fd = pci_dev->real_device.config_fd;
 
 again:
     ret = pwrite(fd, &val, len, address);
@@ -286,14 +321,46 @@ again:
     }
 }
 
+static uint32_t assigned_dev_pci_read_cap_config(AssignedDevice *pci_dev,
+					uint32_t address, int len)
+{
+    uint32_t val = 0;
+
+    if (access_capability_config(pci_dev, address, len)) {
+	switch(len) {
+	default:
+	case 4:
+	    if (address < pci_dev->cap.start + pci_dev->cap.length - 4) {
+		val = le32_to_cpu(*(uint32_t *)(pci_dev->cap.config
+			    + address - pci_dev->cap.start));
+		break;
+	    }
+	    /* fall through */
+	case 2:
+	    if (address < pci_dev->cap.start + pci_dev->cap.length - 2) {
+		val = le16_to_cpu(*(uint16_t *)(pci_dev->cap.config
+			    + address - pci_dev->cap.start));
+		break;
+	    }
+	    /* fall through */
+	case 1:
+	    val = pci_dev->cap.config[address - pci_dev->cap.start];
+	    break;
+	}
+    }
+    return val;
+}
+
 static uint32_t assigned_dev_pci_read_config(PCIDevice *d, uint32_t address,
                                              int len)
 {
     uint32_t val = 0;
     int fd;
     ssize_t ret;
+    AssignedDevice *pci_dev = (AssignedDevice *)d;
 
-    if ((address >= 0x10 && address <= 0x24) || address == 0x34 ||
+    if ((address >= 0x10 && address <= 0x24) ||
+	(!pci_dev->cap.available && address == 0x34) ||
         address == 0x3c || address == 0x3d) {
         val = pci_default_read_config(d, address, len);
         DEBUG("(%x.%x): address=%04x val=0x%08x len=%d\n",
@@ -305,7 +372,18 @@ static uint32_t assigned_dev_pci_read_config(PCIDevice *d, uint32_t address,
     if (address == 0xFC)
         goto do_log;
 
-    fd = ((AssignedDevice *)d)->real_device.config_fd;
+    if (pci_dev->cap.available) {
+	if (address == 0x34) {
+	    val = pci_dev->cap.start;
+	    goto do_log;
+	}
+	if (access_capability_config(pci_dev, address, len)) {
+	    val = assigned_dev_pci_read_cap_config(pci_dev, address, len);
+	    goto do_log;
+	}
+    }
+
+    fd = pci_dev->real_device.config_fd;
 
 again:
     ret = pread(fd, &val, len, address);
@@ -325,9 +403,9 @@ do_log:
 
     /* kill the special capabilities */
     if (address == 4 && len == 4)
-        val &= ~0x100000;
+	val &= ~0x100000;
     else if (address == 6)
-        val &= ~0x10;
+	val &= ~0x10;
 
     return val;
 }
@@ -535,6 +613,12 @@ void assigned_dev_update_irq(PCIDevice *d)
     }
 }
 
+static void init_dev_cap_config(AssignedDevice *dev)
+{
+#define ASSIGNED_DEVICE_CAPABILITY_START_ADDR 0x40
+    dev->cap.start = ASSIGNED_DEVICE_CAPABILITY_START_ADDR;
+}
+
 struct PCIDevice *init_assigned_device(AssignedDevInfo *adev, PCIBus *bus)
 {
     int r;
@@ -576,6 +660,8 @@ struct PCIDevice *init_assigned_device(AssignedDevInfo *adev, PCIBus *bus)
     dev->h_busnr = adev->bus;
     dev->h_devfn = PCI_DEVFN(adev->dev, adev->func);
 
+    init_dev_cap_config(dev);
+
     memset(&assigned_dev_data, 0, sizeof(assigned_dev_data));
     assigned_dev_data.assigned_dev_id  =
 	calc_assigned_dev_id(dev->h_busnr, (uint32_t)dev->h_devfn);
diff --git a/qemu/hw/device-assignment.h b/qemu/hw/device-assignment.h
index d0667be..531a489 100644
--- a/qemu/hw/device-assignment.h
+++ b/qemu/hw/device-assignment.h
@@ -84,6 +84,9 @@ typedef struct {
     struct {
 #define ASSIGNED_DEVICE_CAP_MSI (1 << 0)
 	int available;
+#define ASSIGNED_DEVICE_CAPABILITY_LENGTH 0x60
+	uint8_t config[ASSIGNED_DEVICE_CAPABILITY_LENGTH];
+	unsigned int start, length;
     } cap;
 } AssignedDevice;
 
-- 
1.5.4.5