[PATCH 3/5] Figure out device capability

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

Try to figure out device capability in update_dev_cap(). Now we are only care
about MSI capability.

The function pci_find_cap_offset original function wrote by Allen for Xen.
Notice the function need root privilege to work. This depends on libpci to work.

(Update: Make update_dev_cap() more generic.)

Signed-off-by: Allen Kay <allen.m.kay@intel.com>
Signed-off-by: Sheng Yang <sheng@linux.intel.com>
---
 qemu/hw/device-assignment.c |   50 +++++++++++++++++++++++++++++++++++++++++++
 qemu/hw/device-assignment.h |    5 ++++
 2 files changed, 55 insertions(+), 0 deletions(-)

diff --git a/qemu/hw/device-assignment.c b/qemu/hw/device-assignment.c
index 786b2f0..f79cc67 100644
--- a/qemu/hw/device-assignment.c
+++ b/qemu/hw/device-assignment.c
@@ -216,6 +216,35 @@ static void assigned_dev_ioport_map(PCIDevice *pci_dev, int region_num,
                           (r_dev->v_addrs + region_num));
 }
 
+uint8_t pci_find_cap_offset(struct pci_dev *pci_dev, uint8_t cap)
+{
+    int id;
+    int max_cap = 48;
+    int pos = PCI_CAPABILITY_LIST;
+    int status;
+
+    status = pci_read_byte(pci_dev, PCI_STATUS);
+    if ((status & PCI_STATUS_CAP_LIST) == 0)
+        return 0;
+
+    while (max_cap--) {
+        pos = pci_read_byte(pci_dev, pos);
+        if (pos < 0x40)
+            break;
+
+        pos &= ~3;
+        id = pci_read_byte(pci_dev, pos + PCI_CAP_LIST_ID);
+
+        if (id == 0xff)
+            break;
+        if (id == cap)
+            return pos;
+
+        pos += PCI_CAP_LIST_NEXT;
+    }
+    return 0;
+}
+
 static void assigned_dev_pci_write_config(PCIDevice *d, uint32_t address,
                                           uint32_t val, int len)
 {
@@ -367,6 +396,25 @@ static int assigned_dev_register_regions(PCIRegion *io_regions,
     return 0;
 }
 
+static void update_dev_cap(AssignedDevice *pci_dev, uint8_t r_bus,
+                           uint8_t r_dev, uint8_t r_func)
+{
+    struct pci_access *pacc;
+    struct pci_dev *pdev;
+    int r;
+
+    pacc = pci_alloc();
+    pci_init(pacc);
+    pdev = pci_get_dev(pacc, 0, r_bus, r_dev, r_func);
+    pci_cleanup(pacc);
+#ifdef KVM_CAP_DEVICE_MSI
+    r = pci_find_cap_offset(pdev, PCI_CAP_ID_MSI);
+    if (r)
+        pci_dev->cap.available |= ASSIGNED_DEVICE_CAP_MSI;
+#endif
+    pci_free_dev(pdev);
+}
+
 static int get_real_device(AssignedDevice *pci_dev, uint8_t r_bus,
                            uint8_t r_dev, uint8_t r_func)
 {
@@ -436,6 +484,8 @@ again:
     fclose(f);
 
     dev->region_number = r;
+
+    update_dev_cap(pci_dev, r_bus, r_dev, r_func);
     return 0;
 }
 
diff --git a/qemu/hw/device-assignment.h b/qemu/hw/device-assignment.h
index d6caa67..de60988 100644
--- a/qemu/hw/device-assignment.h
+++ b/qemu/hw/device-assignment.h
@@ -29,6 +29,7 @@
 #define __DEVICE_ASSIGNMENT_H__
 
 #include <sys/mman.h>
+#include <pci/pci.h>
 #include "qemu-common.h"
 #include "sys-queue.h"
 #include "pci.h"
@@ -80,6 +81,10 @@ typedef struct {
     unsigned char h_busnr;
     unsigned int h_devfn;
     int bound;
+    struct {
+#define ASSIGNED_DEVICE_CAP_MSI (1 << 0)
+        int available;
+    } cap;
 } AssignedDevice;
 
 typedef struct AssignedDevInfo AssignedDevInfo;
-- 
1.5.4.5