diff --git a/hw/device-assignment.c b/hw/device-assignment.c
index 179c7dc..85fa50d 100644
--- a/hw/device-assignment.c
+++ b/hw/device-assignment.c
@@ -63,6 +63,11 @@ static void assigned_dev_load_option_rom(AssignedDevice *dev);
 
 static void assigned_dev_unregister_msix_mmio(AssignedDevice *dev);
 
+static void assigned_device_pci_cap_write_config(PCIDevice *pci_dev,
+                                                 uint8_t cap_id,
+                                                 uint32_t address,
+                                                 uint32_t val, int len);
+
 static uint32_t assigned_dev_ioport_rw(AssignedDevRegion *dev_region,
                                        uint32_t addr, int len, uint32_t *val)
 {
@@ -400,20 +405,25 @@ static void assigned_dev_pci_write_config(PCIDevice *d, uint32_t address,
 {
     int fd;
     ssize_t ret;
+    uint8_t cap_id = pci_get_cap_id(d, address);
     AssignedDevice *pci_dev = container_of(d, AssignedDevice, dev);
 
     DEBUG("(%x.%x): address=%04x val=0x%08x len=%d\n",
           ((d->devfn >> 3) & 0x1F), (d->devfn & 0x7),
           (uint16_t) address, val, len);
 
+    if (cap_id && cap_id != PCI_CAP_ID_BASIC) {
+        return assigned_device_pci_cap_write_config(d, cap_id, address,
+                                                    val, len);
+    }
+
     if (address == 0x4) {
         pci_default_write_config(d, address, val, len);
         /* Continue to program the card */
     }
 
     if ((address >= 0x10 && address <= 0x24) || address == 0x30 ||
-        address == 0x34 || address == 0x3c || address == 0x3d ||
-        pci_access_cap_config(d, address, len)) {
+        address == 0x34 || address == 0x3c || address == 0x3d) {
         /* used for update-mappings (BAR emulation) */
         pci_default_write_config(d, address, val, len);
         return;
@@ -443,13 +453,14 @@ static uint32_t assigned_dev_pci_read_config(PCIDevice *d, uint32_t address,
 {
     uint32_t val = 0;
     int fd;
+    uint8_t cap_id = pci_get_cap_id(d, address);
     ssize_t ret;
     AssignedDevice *pci_dev = container_of(d, AssignedDevice, dev);
 
     if (address < 0x4 || (pci_dev->need_emulate_cmd && address == 0x4) ||
 	(address >= 0x10 && address <= 0x24) || address == 0x30 ||
         address == 0x34 || address == 0x3c || address == 0x3d ||
-        pci_access_cap_config(d, address, len)) {
+        (cap_id && cap_id != PCI_CAP_ID_BASIC)) {
         val = pci_default_read_config(d, address, len);
         DEBUG("(%x.%x): address=%04x val=0x%08x len=%d\n",
               (d->devfn >> 3) & 0x1F, (d->devfn & 0x7), address, val, len);
@@ -1249,7 +1260,7 @@ static void assigned_device_pci_cap_write_config(PCIDevice *pci_dev,
                                                  uint32_t address,
                                                  uint32_t val, int len)
 {
-    pci_default_cap_write_config(pci_dev, cap_id, address, val, len);
+    pci_default_write_config(pci_dev, address, val, len);
 
     switch (cap_id) {
 #ifdef KVM_CAP_IRQ_ROUTING
@@ -1466,9 +1477,6 @@ static int assigned_initfn(struct PCIDevice *pci_dev)
     dev->h_busnr = dev->host.bus;
     dev->h_devfn = PCI_DEVFN(dev->host.dev, dev->host.func);
 
-    pci_register_capability_handlers(pci_dev, NULL,
-                                     assigned_device_pci_cap_write_config);
-
     if (assigned_device_pci_cap_init(pci_dev) < 0)
         goto out;
 
diff --git a/hw/pci.c b/hw/pci.c
index 337afc4..bc25be7 100644
--- a/hw/pci.c
+++ b/hw/pci.c
@@ -730,7 +730,7 @@ static void pci_config_alloc(PCIDevice *pci_dev)
     pci_dev->config = qemu_mallocz(config_size);
     pci_dev->cmask = qemu_mallocz(config_size);
     pci_dev->wmask = qemu_mallocz(config_size);
-    pci_dev->cap_map = qemu_mallocz(config_size);
+    pci_dev->config_map = qemu_mallocz(config_size);
 }
 
 static void pci_config_free(PCIDevice *pci_dev)
@@ -738,7 +738,7 @@ static void pci_config_free(PCIDevice *pci_dev)
     qemu_free(pci_dev->config);
     qemu_free(pci_dev->cmask);
     qemu_free(pci_dev->wmask);
-    qemu_free(pci_dev->cap_map);
+    qemu_free(pci_dev->config_map);
 }
 
 /* -1 for devfn means auto assign */
@@ -767,6 +767,7 @@ static PCIDevice *do_pci_register_device(PCIDevice *pci_dev, PCIBus *bus,
     pstrcpy(pci_dev->name, sizeof(pci_dev->name), name);
     pci_dev->irq_state = 0;
     pci_config_alloc(pci_dev);
+    memset(pci_dev->config_map, PCI_CAP_ID_BASIC, PCI_CONFIG_HEADER_SIZE);
 
     if (!is_bridge) {
         pci_set_default_subsystem_id(pci_dev);
@@ -787,8 +788,6 @@ static PCIDevice *do_pci_register_device(PCIDevice *pci_dev, PCIBus *bus,
         config_write = pci_default_write_config;
     pci_dev->config_read = config_read;
     pci_dev->config_write = config_write;
-    pci_dev->cap.config_read = pci_default_cap_read_config;
-    pci_dev->cap.config_write = pci_default_cap_write_config;
     bus->devices[devfn] = pci_dev;
     pci_dev->irq = qemu_allocate_irqs(pci_set_irq, pci_dev, PCI_NUM_PINS);
     pci_dev->version_id = 2; /* Current pci device vmstate version */
@@ -1168,13 +1167,8 @@ static uint32_t pci_read_config(PCIDevice *d,
 uint32_t pci_default_read_config(PCIDevice *d,
                                  uint32_t address, int len)
 {
-    uint8_t cap_id;
     assert(len == 1 || len == 2 || len == 4);
 
-    if ((cap_id = pci_access_cap_config(d, address, len))) {
-        return d->cap.config_read(d, cap_id, address, len);
-    }
-
     return pci_read_config(d, address, len);
 }
 
@@ -1190,32 +1184,14 @@ static void pci_write_config_with_mask(PCIDevice *d, uint32_t addr,
     }
 }
 
-int pci_access_cap_config(PCIDevice *pci_dev, uint32_t address, int len)
-{
-    return pci_dev->cap_map[address];
-}
-
-uint32_t pci_default_cap_read_config(PCIDevice *pci_dev, uint8_t cap_id,
-                                     uint32_t address, int len)
-{
-    return pci_read_config(pci_dev, address, len);
-}
-
-void pci_default_cap_write_config(PCIDevice *pci_dev, uint8_t cap_id,
-                                  uint32_t address, uint32_t val, int len)
+uint8_t pci_get_cap_id(PCIDevice *pci_dev, uint32_t addr)
 {
-    pci_write_config_with_mask(pci_dev, address, val, len);
+    return pci_dev->config_map[addr];
 }
 
 void pci_default_write_config(PCIDevice *d, uint32_t addr, uint32_t val, int l)
 {
     int was_irq_disabled = pci_irq_disabled(d);
-    uint8_t cap_id;
-
-    if ((cap_id = pci_access_cap_config(d, addr, l))) {
-        d->cap.config_write(d, cap_id, addr, val, l);
-        return;
-    }
 
     pci_write_config_with_mask(d, addr, val, l);
 
@@ -1895,23 +1871,6 @@ PCIDevice *pci_create_simple_multifunction(PCIBus *bus, int devfn,
     return dev;
 }
 
-void pci_register_capability_handlers(PCIDevice *pdev,
-                                      PCICapConfigReadFunc *config_read,
-                                      PCICapConfigWriteFunc *config_write)
-{
-    if (config_read) {
-        pdev->cap.config_read = config_read;
-    } else {
-        pdev->cap.config_read = pci_default_cap_read_config;
-    }
-
-    if (config_write) {
-        pdev->cap.config_write = config_write;
-    } else {
-        pdev->cap.config_write = pci_default_cap_write_config;
-    }
-}
-
 PCIDevice *pci_create(PCIBus *bus, int devfn, const char *name)
 {
     return pci_create_multifunction(bus, devfn, false, name);
@@ -1928,7 +1887,7 @@ static int pci_find_space(PCIDevice *pdev, uint8_t size)
     int offset = PCI_CONFIG_HEADER_SIZE;
     int i;
     for (i = PCI_CONFIG_HEADER_SIZE; i < config_size; ++i)
-        if (pdev->cap_map[i])
+        if (pdev->config_map[i])
             offset = i + 1;
         else if (i - offset + 1 == size)
             return offset;
@@ -2029,11 +1988,18 @@ static void pci_del_option_rom(PCIDevice *pdev)
 int pci_add_capability_at_offset(PCIDevice *pdev, uint8_t cap_id,
                                  uint8_t offset, uint8_t size)
 {
-    uint8_t *config = pdev->config + offset;
+    uint8_t i, *config = pdev->config + offset;
+
+    for (i = 0; i < size; i++) {
+        if (pdev->config_map[offset + i]) {
+            return -EFAULT;
+        }
+    }
+
     config[PCI_CAP_LIST_ID] = cap_id;
     config[PCI_CAP_LIST_NEXT] = pdev->config[PCI_CAPABILITY_LIST];
     pdev->config[PCI_CAPABILITY_LIST] = offset;
-    memset(pdev->cap_map + offset, cap_id, size);
+    memset(pdev->config_map + offset, cap_id, size);
     /* Make capability read-only by default */
     memset(pdev->wmask + offset, 0, size);
     /* Check capability by default */
@@ -2066,7 +2032,7 @@ void pci_del_capability(PCIDevice *pdev, uint8_t cap_id, uint8_t size)
     memset(pdev->wmask + offset, 0xff, size);
     /* Clear cmask as device-specific registers can't be checked */
     memset(pdev->cmask + offset, 0, size);
-    memset(pdev->cap_map + offset, 0, size);
+    memset(pdev->config_map + offset, 0, size);
 
     if (!pdev->config[PCI_CAPABILITY_LIST]) {
         pdev->config[PCI_STATUS] &= ~PCI_STATUS_CAP_LIST;
diff --git a/hw/pci.h b/hw/pci.h
index 3f0b4e0..cea1c3a 100644
--- a/hw/pci.h
+++ b/hw/pci.h
@@ -83,11 +83,6 @@ typedef void PCIMapIORegionFunc(PCIDevice *pci_dev, int region_num,
                                 pcibus_t addr, pcibus_t size, int type);
 typedef int PCIUnregisterFunc(PCIDevice *pci_dev);
 
-typedef void PCICapConfigWriteFunc(PCIDevice *pci_dev, uint8_t cap_id,
-                                   uint32_t address, uint32_t val, int len);
-typedef uint32_t PCICapConfigReadFunc(PCIDevice *pci_dev, uint8_t cap_id,
-                                      uint32_t address, int len);
-
 typedef struct PCIIORegion {
     pcibus_t addr; /* current PCI mapping address. -1 means not mapped */
 #define PCI_BAR_UNMAPPED (~(pcibus_t)0)
@@ -114,6 +109,8 @@ typedef struct PCIIORegion {
 
 #define PCI_NUM_PINS 4 /* A-D */
 
+#define PCI_CAP_ID_BASIC 0xff
+
 /* Bits in cap_present field. */
 enum {
     QEMU_PCI_CAP_MSIX = 0x1,
@@ -152,7 +149,7 @@ struct PCIDevice {
     uint8_t *wmask;
 
     /* Used to allocate config space and track capabilities. */
-    uint8_t *cap_map;
+    uint8_t *config_map;
 
     /* the following fields are read only */
     PCIBus *bus;
@@ -205,12 +202,6 @@ struct PCIDevice {
     struct kvm_msix_message *msix_irq_entries;
 
     msix_mask_notifier_func msix_mask_notifier;
-
-    /* Device capability configuration space */
-    struct {
-        PCICapConfigReadFunc *config_read;
-        PCICapConfigWriteFunc *config_write;
-    } cap;
 };
 
 PCIDevice *pci_register_device(PCIBus *bus, const char *name,
@@ -225,10 +216,6 @@ void pci_register_bar(PCIDevice *pci_dev, int region_num,
 void pci_map_option_rom(PCIDevice *pdev, int region_num, pcibus_t addr,
                         pcibus_t size, int type);
 
-void pci_register_capability_handlers(PCIDevice *pci_dev,
-                                      PCICapConfigReadFunc *config_read,
-                                      PCICapConfigWriteFunc *config_write);
-
 int pci_map_irq(PCIDevice *pci_dev, int pin);
 
 int pci_add_capability(PCIDevice *pci_dev, uint8_t cap_id, uint8_t cap_size);
@@ -245,11 +232,7 @@ 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, uint8_t cap_id,
-                                     uint32_t address, int len);
-void pci_default_cap_write_config(PCIDevice *pci_dev, uint8_t cap_id,
-                                  uint32_t address, uint32_t val, int len);
-int pci_access_cap_config(PCIDevice *pci_dev, uint32_t address, int len);
+uint8_t pci_get_cap_id(PCIDevice *pci_dev, uint32_t addr);
 
 typedef void (*pci_set_irq_fn)(void *opaque, int irq_num, int level);
 typedef int (*pci_map_irq_fn)(PCIDevice *pci_dev, int irq_num);
