[PATCH 06/16] 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/pci.c |   77 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 qemu/hw/pci.h |   29 +++++++++++++++++++++
 2 files changed, 104 insertions(+), 2 deletions(-)

diff --git a/qemu/hw/pci.c b/qemu/hw/pci.c
index 821646c..eca0517 100644
--- a/qemu/hw/pci.c
+++ b/qemu/hw/pci.c
@@ -427,8 +427,8 @@ static void pci_update_mappings(PCIDevice *d)
     }
 }
 
-uint32_t pci_default_read_config(PCIDevice *d,
-                                 uint32_t address, int len)
+static uint32_t pci_read_config(PCIDevice *d,
+                                uint32_t address, int len)
 {
     uint32_t val;
 
@@ -453,6 +453,45 @@ uint32_t pci_default_read_config(PCIDevice *d,
     return val;
 }
 
+static void pci_write_config(PCIDevice *pci_dev,
+                             uint32_t address, uint32_t val, int len)
+{
+    int i;
+    for (i = 0; i < len; i++) {
+        pci_dev->config[address + i] = val & 0xff;
+        val >>= 8;
+    }
+}
+
+int pci_access_cap_config(PCIDevice *pci_dev, uint32_t address, int len)
+{
+    if (pci_dev->cap.supported && address >= pci_dev->cap.start &&
+            (address + len) < pci_dev->cap.start + pci_dev->cap.length)
+        return 1;
+    return 0;
+}
+
+uint32_t pci_default_cap_read_config(PCIDevice *pci_dev,
+                                     uint32_t address, int len)
+{
+    return pci_read_config(pci_dev, address, len);
+}
+
+void pci_default_cap_write_config(PCIDevice *pci_dev,
+                                  uint32_t address, uint32_t val, int len)
+{
+    pci_write_config(pci_dev, address, val, len);
+}
+
+uint32_t pci_default_read_config(PCIDevice *d,
+                                 uint32_t address, int len)
+{
+    if (pci_access_cap_config(d, address, len))
+        return d->cap.config_read(d, address, len);
+
+    return pci_read_config(d, address, len);
+}
+
 void pci_default_write_config(PCIDevice *d,
                               uint32_t address, uint32_t val, int len)
 {
@@ -485,6 +524,11 @@ void pci_default_write_config(PCIDevice *d,
         return;
     }
  default_config:
+    if (pci_access_cap_config(d, address, len)) {
+        d->cap.config_write(d, address, val, len);
+        return;
+    }
+
     /* not efficient, but simple */
     addr = address;
     for(i = 0; i < len; i++) {
@@ -905,3 +949,32 @@ PCIBus *pci_bridge_init(PCIBus *bus, int devfn, uint16_t vid, uint16_t did,
     s->bus = pci_register_secondary_bus(&s->dev, map_irq);
     return s->bus;
 }
+
+int pci_enable_capability_support(PCIDevice *pci_dev,
+                                  uint32_t config_start,
+                                  PCICapConfigReadFunc *config_read,
+                                  PCICapConfigWriteFunc *config_write,
+                                  PCICapConfigInitFunc *config_init)
+{
+    if (!pci_dev)
+        return -ENODEV;
+
+    if (config_start == 0)
+	pci_dev->cap.start = PCI_CAPABILITY_CONFIG_DEFAULT_START_ADDR;
+    else if (config_start >= 0x40 && config_start < 0xff)
+        pci_dev->cap.start = config_start;
+    else
+        return -EINVAL;
+
+    if (config_read)
+        pci_dev->cap.config_read = config_read;
+    else
+        pci_dev->cap.config_read = pci_default_cap_read_config;
+    if (config_write)
+        pci_dev->cap.config_write = config_write;
+    else
+        pci_dev->cap.config_write = pci_default_cap_write_config;
+    pci_dev->cap.supported = 1;
+    pci_dev->config[PCI_CAPABILITY_LIST] = pci_dev->cap.start;
+    return config_init(pci_dev);
+}
diff --git a/qemu/hw/pci.h b/qemu/hw/pci.h
index 0558821..4f494e7 100644
--- a/qemu/hw/pci.h
+++ b/qemu/hw/pci.h
@@ -79,6 +79,12 @@ typedef void PCIMapIORegionFunc(PCIDevice *pci_dev, int region_num,
                                 uint32_t addr, uint32_t size, int type);
 typedef int PCIUnregisterFunc(PCIDevice *pci_dev);
 
+typedef void PCICapConfigWriteFunc(PCIDevice *pci_dev,
+                                   uint32_t address, uint32_t val, int len);
+typedef uint32_t PCICapConfigReadFunc(PCIDevice *pci_dev,
+                                      uint32_t address, int len);
+typedef int PCICapConfigInitFunc(PCIDevice *pci_dev);
+
 #define PCI_ADDRESS_SPACE_MEM		0x00
 #define PCI_ADDRESS_SPACE_IO		0x01
 #define PCI_ADDRESS_SPACE_MEM_PREFETCH	0x08
@@ -137,6 +143,10 @@ typedef struct PCIIORegion {
 
 #define PCI_COMMAND_RESERVED_MASK_HI (PCI_COMMAND_RESERVED >> 8)
 
+#define PCI_CAPABILITY_CONFIG_MAX_LENGTH 0x60
+#define PCI_CAPABILITY_CONFIG_DEFAULT_START_ADDR 0x40
+#define PCI_CAPABILITY_CONFIG_MSI_LENGTH 0x10
+
 struct PCIDevice {
     /* PCI config space */
     uint8_t config[256];
@@ -159,6 +169,14 @@ struct PCIDevice {
 
     /* Current IRQ levels.  Used internally by the generic PCI code.  */
     int irq_state[4];
+
+    /* Device capability configuration space */
+    struct {
+        int supported;
+        unsigned int start, length;
+        PCICapConfigReadFunc *config_read;
+        PCICapConfigWriteFunc *config_write;
+    } cap;
 };
 
 PCIDevice *pci_register_device(PCIBus *bus, const char *name,
@@ -171,6 +189,12 @@ void pci_register_io_region(PCIDevice *pci_dev, int region_num,
                             uint32_t size, int type,
                             PCIMapIORegionFunc *map_func);
 
+int pci_enable_capability_support(PCIDevice *pci_dev,
+                                  uint32_t config_start,
+                                  PCICapConfigReadFunc *config_read,
+                                  PCICapConfigWriteFunc *config_write,
+                                  PCICapConfigInitFunc *config_init);
+
 int pci_map_irq(PCIDevice *pci_dev, int pin);
 uint32_t pci_default_read_config(PCIDevice *d,
                                  uint32_t address, int len);
@@ -178,6 +202,11 @@ void pci_default_write_config(PCIDevice *d,
                               uint32_t address, uint32_t val, int len);
 void pci_device_save(PCIDevice *s, QEMUFile *f);
 int pci_device_load(PCIDevice *s, QEMUFile *f);
+uint32_t pci_default_cap_read_config(PCIDevice *pci_dev,
+                                     uint32_t address, int len);
+void pci_default_cap_write_config(PCIDevice *pci_dev,
+                                  uint32_t address, uint32_t val, int len);
+int pci_access_cap_config(PCIDevice *pci_dev, uint32_t address, int len);
 
 typedef void (*pci_set_irq_fn)(qemu_irq *pic, int irq_num, int level);
 typedef int (*pci_map_irq_fn)(PCIDevice *pci_dev, int irq_num);
-- 
1.5.4.5