Userspace for MSI support of KVM

index 9a790c6..c7e92a7 100644
@@ -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)
 {

@@ -365,6 +394,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)
+{
+#ifdef KVM_CAP_DEVICE_MSI
+    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);
+    r = pci_find_cap_offset(pdev, PCI_CAP_ID_MSI);
+    if (r)
+	pci_dev->cap.available |= ASSIGNED_DEVICE_CAP_MSI;
+    pci_free_dev(pdev);
+#endif
+}
+
 static int get_real_device(AssignedDevice *pci_dev, uint8_t r_bus,
                            uint8_t r_dev, uint8_t r_func)
 {

@@ -434,6 +482,8 @@ again:
     fclose(f);
 
     dev->region_number = r;
+
+    update_dev_cap(pci_dev, r_bus, r_dev, r_func);
     return 0;
 }
 

index d6caa67..d0667be 100644
@@ -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;

index c7e92a7..14b5911 100644
@@ -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);

index d0667be..531a489 100644
@@ -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;
 

index 14b5911..daa0e32 100644
@@ -254,9 +254,26 @@ static int access_capability_config(AssignedDevice *pci_dev,
     return 0;
 }
 
+static void assigned_dev_enable_msi(AssignedDevice *assigned_dev);
+
 static void check_cap_state(AssignedDevice *pci_dev,
 			    uint32_t address, uint32_t* val)
 {
+    uint32_t pos = pci_dev->cap.start;
+
+#ifdef KVM_CAP_DEVICE_MSI
+    /* Check if guest want to enable MSI */
+    if (pci_dev->cap.available & ASSIGNED_DEVICE_CAP_MSI) {
+	if (address == pos + 2 && (uint8_t)*val == 1) {
+	    pci_dev->cap.enabled |= ASSIGNED_DEVICE_MSI_ENABLED;
+	    assigned_dev_enable_msi(pci_dev);
+	    if (!pci_dev->cap.enabled & ASSIGNED_DEVICE_MSI_ENABLED)
+		*val &= ~1ul;
+	    return;
+	}
+	pos += ASSIGNED_DEVICE_CONFIG_MSI_LENGTH;
+    }
+#endif
     return;
 }
 

@@ -401,11 +418,13 @@ do_log:
     DEBUG("(%x.%x): address=%04x val=0x%08x len=%d\n",
           (d->devfn >> 3) & 0x1F, (d->devfn & 0x7), address, val, len);
 
-    /* kill the special capabilities */
-    if (address == 4 && len == 4)
-	val &= ~0x100000;
-    else if (address == 6)
-	val &= ~0x10;
+    if (!pci_dev->cap.available) {
+	/* kill the special capabilities */
+	if (address == 4 && len == 4)
+	    val &= ~0x100000;
+	else if (address == 6)
+	    val &= ~0x10;
+    }
 
     return val;
 }

@@ -583,6 +602,8 @@ void assigned_dev_update_irq(PCIDevice *d)
 
     LIST_FOREACH(adev, &adev_head, next) {
         assigned_dev = adev->assigned_dev;
+	if (assigned_dev->cap.enabled & ASSIGNED_DEVICE_MSI_ENABLED)
+	    continue;
         irq = pci_map_irq(&assigned_dev->dev, assigned_dev->intpin);
         irq = piix_get_irq(irq);
 

@@ -613,10 +634,40 @@ void assigned_dev_update_irq(PCIDevice *d)
     }
 }
 
+#ifdef KVM_CAP_DEVICE_MSI
+static void assigned_dev_enable_msi(AssignedDevice *assigned_dev)
+{
+    int r;
+    struct kvm_assigned_irq assigned_irq_data;
+
+    memset(&assigned_irq_data, 0, sizeof assigned_irq_data);
+    assigned_irq_data.assigned_dev_id  =
+	    calc_assigned_dev_id(assigned_dev->h_busnr,
+			         (uint8_t)assigned_dev->h_devfn);
+    assigned_irq_data.guest_msi.addr_lo = *(uint32_t *)
+	    (assigned_dev->cap.config + 4);
+    assigned_irq_data.guest_msi.data = *(uint16_t *)
+	    (assigned_dev->cap.config + 8);
+    assigned_irq_data.flags |= KVM_DEV_IRQ_ASSIGN_ENABLE_MSI;
+    r = kvm_assign_irq(kvm_context, &assigned_irq_data);
+    if (r < 0) {
+	perror("assigned_dev_enable_msi");
+	assigned_dev->cap.enabled &= ~ASSIGNED_DEVICE_MSI_ENABLED;
+	/* Fail to enable MSI, enable INTx instead */
+	assigned_dev_update_irq((PCIDevice *)assigned_dev);
+    }
+}
+#endif
+
 static void init_dev_cap_config(AssignedDevice *dev)
 {
 #define ASSIGNED_DEVICE_CAPABILITY_START_ADDR 0x40
     dev->cap.start = ASSIGNED_DEVICE_CAPABILITY_START_ADDR;
+#ifdef KVM_CAP_DEVICE_MSI
+    /* Expose MSI capability */
+    dev->cap.config[0] = 0x5;
+    dev->cap.length += ASSIGNED_DEVICE_CONFIG_MSI_LENGTH;
+#endif
 }
 
 struct PCIDevice *init_assigned_device(AssignedDevInfo *adev, PCIBus *bus)

index 531a489..7e74287 100644
@@ -86,7 +86,10 @@ typedef struct {
 	int available;
 #define ASSIGNED_DEVICE_CAPABILITY_LENGTH 0x60
 	uint8_t config[ASSIGNED_DEVICE_CAPABILITY_LENGTH];
+#define ASSIGNED_DEVICE_CONFIG_MSI_LENGTH 0x10
 	unsigned int start, length;
+#define ASSIGNED_DEVICE_MSI_ENABLED (1 << 0)
+	int enabled;
     } cap;
 } AssignedDevice;