Add ability to read non-VGA roms and refactor Joan's codehurd-pciarbiter

6 files changed, 565 insertions, 243 deletions

diff --git a/include/pciaccess.h b/include/pciaccess.h
index 8167be6..53a404b 100644
--- a/include/pciaccess.h
+++ b/include/pciaccess.h
@@ -76,6 +76,16 @@ struct pci_slot_match;
 extern "C" {
 #endif
 
+enum access_method {
+	PCI_ACCESS_LINUX = 0,
+	PCI_ACCESS_FREEBSD,
+	PCI_ACCESS_NETBSD,
+	PCI_ACCESS_OPENBSD,
+	PCI_ACCESS_SUN,
+	PCI_ACCESS_HURD,
+	PCI_ACCESS_RAW_X86
+};
+
 int pci_device_has_kernel_driver(struct pci_device *dev);
 
 int pci_device_is_boot_vga(struct pci_device *dev);
@@ -115,6 +125,8 @@ int pci_device_get_bridge_buses(struct pci_device *dev, int *primary_bus,
 
 int pci_system_init(void);
 
+int pci_system_init_force(enum access_method method);
+
 void pci_system_init_dev_mem(int fd);
 
 void pci_system_cleanup(void);
diff --git a/src/common_init.c b/src/common_init.c
index d83eb33..511bb91 100644
--- a/src/common_init.c
+++ b/src/common_init.c
@@ -76,6 +76,48 @@ pci_system_init( void )
     return err;
 }
 
+int
+pci_system_init_force( enum access_method method)
+{
+    int err = ENOSYS;
+#ifdef __linux__
+    if (method == PCI_ACCESS_LINUX) {
+        err = pci_system_linux_sysfs_create();
+    } else if (method == PCI_ACCESS_RAW_X86) {
+        err = pci_system_x86_create();
+    }
+#elif defined(__FreeBSD__) || defined(__FreeBSD_kernel__) || defined(__DragonFly__)
+    if (method == PCI_ACCESS_FREEBSD) {
+        err = pci_system_freebsd_create();
+    }
+#elif defined(__NetBSD__)
+    if (method == PCI_ACCESS_NETBSD) {
+        err = pci_system_netbsd_create();
+    }
+#elif defined(__OpenBSD__)
+    if (method == PCI_ACCESS_OPENBSD) {
+        err = pci_system_openbsd_create();
+    }
+#elif defined(__sun)
+    if (method == PCI_ACCESS_SUN) {
+        err = pci_system_solx_devfs_create();
+    }
+#elif defined(__GNU__)
+    if (method == PCI_ACCESS_HURD) {
+        err = pci_system_hurd_create();
+    } else if (method == PCI_ACCESS_RAW_X86) {
+        err = pci_system_x86_create();
+    }
+#elif defined(__CYGWIN__)
+    if (method == PCI_ACCESS_RAW_X86) {
+        err = pci_system_x86_create();
+    }
+#else
+# error "Unsupported OS"
+#endif
+    return err;
+}
+
 void
 pci_system_init_dev_mem(int fd)
 {
diff --git a/src/common_interface.c b/src/common_interface.c
index cb95e90..23806ec 100644
--- a/src/common_interface.c
+++ b/src/common_interface.c
@@ -480,7 +480,7 @@ pci_device_cfg_read( struct pci_device * dev, void * data,
 		     pciaddr_t offset, pciaddr_t size,
 		     pciaddr_t * bytes_read )
 {
-    pciaddr_t  scratch;
+    pciaddr_t  scratch = 0;
 
     if ( (dev == NULL) || (data == NULL) ) {
 	return EFAULT;
@@ -496,7 +496,7 @@ int
 pci_device_cfg_read_u8( struct pci_device * dev, uint8_t * data,
 			pciaddr_t offset )
 {
-    pciaddr_t bytes;
+    pciaddr_t bytes = 0;
     int err = pci_device_cfg_read( dev, data, offset, 1, & bytes );
 
     if ( (err == 0) && (bytes != 1) ) {
@@ -511,7 +511,7 @@ int
 pci_device_cfg_read_u16( struct pci_device * dev, uint16_t * data,
 			 pciaddr_t offset )
 {
-    pciaddr_t bytes;
+    pciaddr_t bytes = 0;
     int err = pci_device_cfg_read( dev, data, offset, 2, & bytes );
 
     if ( (err == 0) && (bytes != 2) ) {
@@ -527,7 +527,7 @@ int
 pci_device_cfg_read_u32( struct pci_device * dev, uint32_t * data,
 			 pciaddr_t offset )
 {
-    pciaddr_t bytes;
+    pciaddr_t bytes = 0;
     int err = pci_device_cfg_read( dev, data, offset, 4, & bytes );
 
     if ( (err == 0) && (bytes != 4) ) {
@@ -567,7 +567,7 @@ pci_device_cfg_write( struct pci_device * dev, const void * data,
 		      pciaddr_t offset, pciaddr_t size,
 		      pciaddr_t * bytes_written )
 {
-    pciaddr_t  scratch;
+    pciaddr_t  scratch = 0;
 
     if ( (dev == NULL) || (data == NULL) ) {
 	return EFAULT;
@@ -583,7 +583,7 @@ int
 pci_device_cfg_write_u8(struct pci_device *dev, uint8_t data,
 			pciaddr_t offset)
 {
-    pciaddr_t bytes;
+    pciaddr_t bytes = 0;
     int err = pci_device_cfg_write(dev, & data, offset, 1, & bytes);
 
     if ( (err == 0) && (bytes != 1) ) {
@@ -599,7 +599,7 @@ int
 pci_device_cfg_write_u16(struct pci_device *dev, uint16_t data,
 			 pciaddr_t offset)
 {
-    pciaddr_t bytes;
+    pciaddr_t bytes = 0;
     const uint16_t temp = HTOLE_16(data);
     int err = pci_device_cfg_write( dev, & temp, offset, 2, & bytes );
 
@@ -616,7 +616,7 @@ int
 pci_device_cfg_write_u32(struct pci_device *dev, uint32_t data,
 			 pciaddr_t offset)
 {
-    pciaddr_t bytes;
+    pciaddr_t bytes = 0;
     const uint32_t temp = HTOLE_32(data);
     int err = pci_device_cfg_write( dev, & temp, offset, 4, & bytes );
 
@@ -633,7 +633,7 @@ int
 pci_device_cfg_write_bits( struct pci_device * dev, uint32_t mask,
 			   uint32_t data, pciaddr_t offset )
 {
-    uint32_t  temp;
+    uint32_t  temp = 0;
     int err;
 
     err = pci_device_cfg_read_u32( dev, & temp, offset );
diff --git a/src/hurd_pci.c b/src/hurd_pci.c
index abdcb0e..eefb618 100644
--- a/src/hurd_pci.c
+++ b/src/hurd_pci.c
@@ -460,14 +460,17 @@ pci_system_hurd_create(void)
     pci_sys->methods = &hurd_pci_methods;
 
     pci_server_port = file_name_lookup(_SERVERS_BUS_PCI, 0, 0);
-    if (pci_server_port == MACH_PORT_NULL)
-        return errno;
+    if (!pci_server_port) {
+        /* Fall back to x86 access method */
+        return pci_system_x86_create();
+    }
 
     /* The server gives us the number of available devices for us */
     err = pci_get_ndevs (pci_server_port, &ndevs);
     if (err) {
         mach_port_deallocate (mach_task_self (), pci_server_port);
-        return err;
+        /* Fall back to x86 access method */
+        return pci_system_x86_create();
     }
     mach_port_deallocate (mach_task_self (), pci_server_port);
 
@@ -484,7 +487,7 @@ pci_system_hurd_create(void)
     err = enum_devices(_SERVERS_BUS_PCI, &device, -1, -1, -1, -1,
                        LEVEL_DOMAIN);
     if (err)
-        return err;
+        return pci_system_x86_create();
 
     return 0;
 }
diff --git a/src/x86_pci.c b/src/x86_pci.c
index 650e98a..bf627bd 100644
--- a/src/x86_pci.c
+++ b/src/x86_pci.c
@@ -1,4 +1,6 @@
 /*
+ * Copyright (c) 2018 Damien Zammit
+ * Copyright (c) 2017 Joan Lledó
  * Copyright (c) 2009, 2012 Samuel Thibault
  * Heavily inspired from the freebsd, netbsd, and openbsd backends
  * (C) Copyright Eric Anholt 2006
@@ -209,12 +211,6 @@ outl(uint32_t value, uint16_t port)
 
 #endif
 
-struct pci_system_x86 {
-    struct pci_system system;
-    int (*read)(unsigned bus, unsigned dev, unsigned func, pciaddr_t reg, void *data, unsigned size);
-    int (*write)(unsigned bus, unsigned dev, unsigned func, pciaddr_t reg, const void *data, unsigned size);
-};
-
 static int
 pci_system_x86_conf1_probe(void)
 {
@@ -387,76 +383,6 @@ pci_system_x86_conf2_write(unsigned bus, unsigned dev, unsigned func, pciaddr_t
     return ret;
 }
 
-/* Check that this really looks like a PCI configuration. */
-static int
-pci_system_x86_check(struct pci_system_x86 *pci_sys_x86)
-{
-    int dev;
-    uint16_t class, vendor;
-
-    /* Look on bus 0 for a device that is a host bridge, a VGA card,
-     * or an intel or compaq device.  */
-
-    for (dev = 0; dev < 32; dev++) {
-	if (pci_sys_x86->read(0, dev, 0, PCI_CLASS_DEVICE, &class, sizeof(class)))
-	    continue;
-	if (class == PCI_CLASS_BRIDGE_HOST || class == PCI_CLASS_DISPLAY_VGA)
-	    return 0;
-	if (pci_sys_x86->read(0, dev, 0, PCI_VENDOR_ID, &vendor, sizeof(vendor)))
-	    continue;
-	if (vendor == PCI_VENDOR_ID_INTEL || class == PCI_VENDOR_ID_COMPAQ)
-	    return 0;
-    }
-
-    return ENODEV;
-}
-
-static int
-pci_nfuncs(struct pci_system_x86 *pci_sys_x86, int bus, int dev)
-{
-    uint8_t hdr;
-    int err;
-
-    err = pci_sys_x86->read(bus, dev, 0, PCI_HDRTYPE, &hdr, sizeof(hdr));
-
-    if (err)
-	return err;
-
-    return hdr & 0x80 ? 8 : 1;
-}
-
-/**
- * Read a VGA rom using the 0xc0000 mapping.
- */
-static int
-pci_device_x86_read_rom(struct pci_device *dev, void *buffer)
-{
-    void *bios;
-    int memfd;
-
-    if ((dev->device_class & 0x00ffff00) !=
-	 ((PCIC_DISPLAY << 16) | ( PCIS_DISPLAY_VGA << 8))) {
-	return ENOSYS;
-    }
-
-    memfd = open("/dev/mem", O_RDONLY | O_CLOEXEC);
-    if (memfd == -1)
-	return errno;
-
-    bios = mmap(NULL, dev->rom_size, PROT_READ, 0, memfd, 0xc0000);
-    if (bios == MAP_FAILED) {
-	close(memfd);
-	return errno;
-    }
-
-    memcpy(buffer, bios, dev->rom_size);
-
-    munmap(bios, dev->rom_size);
-    close(memfd);
-
-    return 0;
-}
-
 /** Returns the number of regions (base address registers) the device has */
 static int
 pci_device_x86_get_num_regions(uint8_t header_type)
@@ -507,77 +433,380 @@ get_test_val_size( uint32_t testval )
 }
 
 static int
-pci_device_x86_probe(struct pci_device *dev)
+pci_nfuncs(struct pci_device *dev, uint8_t *nfuncs)
 {
-    uint8_t irq, hdrtype;
-    int err, i, bar;
+    uint8_t hdr;
+    int err;
+    struct pci_device tmpdev = *dev;
+    
+    tmpdev.func = 0;
 
-    /* Many of the fields were filled in during initial device enumeration.
-     * At this point, we need to fill in regions, rom_size, and irq.
-     */
+    err = pci_device_cfg_read_u8 (&tmpdev, &hdr, PCI_HDRTYPE);
 
-    err = pci_device_cfg_read_u8(dev, &irq, PCI_IRQ);
     if (err)
 	return err;
-    dev->irq = irq;
 
-    err = pci_device_cfg_read_u8(dev, &hdrtype, PCI_HDRTYPE);
-    if (err)
-	return err;
+    *nfuncs = hdr & 0x80 ? 8 : 1;
+    return err;
+}
 
-    bar = 0x10;
-    for (i = 0; i < pci_device_x86_get_num_regions(hdrtype); i++, bar += 4) {
-	uint32_t addr, testval;
-
-	/* Get the base address */
-	err = pci_device_cfg_read_u32(dev, &addr, bar);
-	if (err != 0)
-	    continue;
-
-	/* Test write all ones to the register, then restore it. */
-	err = pci_device_cfg_write_u32(dev, 0xffffffff, bar);
-	if (err != 0)
-	    continue;
-	pci_device_cfg_read_u32(dev, &testval, bar);
-	err = pci_device_cfg_write_u32(dev, addr, bar);
-
-	if (addr & 0x01)
-	    dev->regions[i].is_IO = 1;
-	if (addr & 0x04)
-	    dev->regions[i].is_64 = 1;
-	if (addr & 0x08)
-	    dev->regions[i].is_prefetchable = 1;
-
-	/* Set the size */
-	dev->regions[i].size = get_test_val_size(testval);
-
-	/* Set the base address value */
-	if (dev->regions[i].is_64) {
-	    uint32_t top;
-
-	    err = pci_device_cfg_read_u32(dev, &top, bar + 4);
-	    if (err != 0)
-		continue;
-
-	    dev->regions[i].base_addr = ((uint64_t)top << 32) |
-					get_map_base(addr);
-	    bar += 4;
-	    i++;
-	} else {
-	    dev->regions[i].base_addr = get_map_base(addr);
-	}
+/* Read BAR `reg_num' in `dev' and map the data if any */
+static error_t
+pci_device_x86_region_probe (struct pci_device *dev, int reg_num)
+{
+  error_t err;
+  uint8_t offset;
+  uint32_t reg, addr, testval;
+  int memfd;
+
+  offset = PCI_BAR_ADDR_0 + 0x4 * reg_num;
+
+  /* Get the base address */
+  err = pci_device_cfg_read_u32 (dev, &addr, offset);
+  if (err)
+    return err;
+
+  /* Test write all ones to the register, then restore it. */
+  reg = 0xffffffff;
+  err = pci_device_cfg_write_u32 (dev, reg, offset);
+  if (err)
+    return err;
+  err = pci_device_cfg_read_u32 (dev, &testval, offset);
+  if (err)
+    return err;
+  err = pci_device_cfg_write_u32 (dev, addr, offset);
+  if (err)
+    return err;
+
+  if (addr & 0x01)
+    dev->regions[reg_num].is_IO = 1;
+  if (addr & 0x04)
+    dev->regions[reg_num].is_64 = 1;
+  if (addr & 0x08)
+    dev->regions[reg_num].is_prefetchable = 1;
+
+  /* Set the size */
+  dev->regions[reg_num].size = get_test_val_size (testval);
+
+  /* Set the base address value */
+  dev->regions[reg_num].base_addr = get_map_base (addr);
+
+  if (dev->regions[reg_num].is_64)
+    {
+      err = pci_device_cfg_read_u32 (dev, &addr, offset + 4);
+      if (err)
+       return err;
+
+      dev->regions[reg_num].base_addr |= ((uint64_t) addr << 32);
+    }
+
+  if (dev->regions[reg_num].is_IO)
+    {
+      /* Enable the I/O Space bit */
+      err = pci_device_cfg_read_u32 (dev, &reg, PCI_COMMAND);
+      if (err)
+       return err;
+
+      if (!(reg & 0x1))
+       {
+         reg |= 0x1;
+
+         err = pci_device_cfg_write_u32 (dev, reg, PCI_COMMAND);
+         if (err)
+           return err;
+       }
+
+      /* Clear the map pointer */
+      dev->regions[reg_num].memory = 0;
+    }
+  else if (dev->regions[reg_num].size > 0)
+    {
+      /* Enable the Memory Space bit */
+      err = pci_device_cfg_read_u32 (dev, &reg, PCI_COMMAND);
+      if (err)
+       return err;
+
+      if (!(reg & 0x2))
+       {
+         reg |= 0x2;
+
+         err = pci_device_cfg_write_u32 (dev, reg, PCI_COMMAND);
+         if (err)
+           return err;
+       }
+
+      /* Map the region in our space */
+      memfd = open ("/dev/mem", O_RDONLY | O_CLOEXEC);
+      if (memfd == -1)
+       return errno;
+
+      dev->regions[reg_num].memory =
+       mmap (NULL, dev->regions[reg_num].size, PROT_READ | PROT_WRITE, 0,
+             memfd, dev->regions[reg_num].base_addr);
+      if (dev->regions[reg_num].memory == MAP_FAILED)
+       {
+         dev->regions[reg_num].memory = 0;
+         close (memfd);
+         return errno;
+       }
+
+      close (memfd);
     }
 
-    /* If it's a VGA device, set up the rom size for read_rom using the
-     * 0xc0000 mapping.
-     */
-    if ((dev->device_class & 0x00ffff00) ==
-	((PCIC_DISPLAY << 16) | (PCIS_DISPLAY_VGA << 8)))
+  return 0;
+}
+
+/* Read the XROMBAR in `dev' and save the rom size and rom base */
+static error_t 
+pci_device_x86_probe_rom (struct pci_device *dev)
+{
+  error_t err;
+  uint8_t reg_8, xrombar_addr;
+  uint32_t reg, reg_back;
+  pciaddr_t rom_size;
+  pciaddr_t rom_base;
+  struct pci_device_private *d = (struct pci_device_private *)dev;
+
+  /* First we need to know which type of header is this */
+  err = pci_device_cfg_read_u8 (dev, &reg_8, PCI_HDRTYPE);
+  if (err)
+    return err;
+
+  /* Get the XROMBAR register address */
+  switch (reg_8 & 0x3)
     {
-	dev->rom_size = 64 * 1024;
+    case PCI_HDRTYPE_DEVICE:
+      xrombar_addr = PCI_XROMBAR_ADDR_00;
+      break;
+    case PCI_HDRTYPE_BRIDGE:
+      xrombar_addr = PCI_XROMBAR_ADDR_01;
+      break;
+    default:
+      return -1;
     }
 
+  /* Get size and physical address */
+  err = pci_device_cfg_read_u32 (dev, &reg, xrombar_addr);
+  if (err)
+    return err;
+
+  reg_back = reg;
+  reg = 0xFFFFF800;            /* Base address: first 21 bytes */
+  err = pci_device_cfg_write_u32 (dev, reg, xrombar_addr);
+  if (err)
+    return err;
+  err = pci_device_cfg_read_u32 (dev, &reg, xrombar_addr);
+  if (err)
+    return err;
+
+  rom_size = (~reg + 1);
+  rom_base = reg_back & reg;
+
+  if (rom_size == 0)
     return 0;
+
+  /* Enable the address decoder and write the physical address back */
+  reg_back |= 0x1;
+  err = pci_device_cfg_write_u32 (dev, reg_back, xrombar_addr);
+  if (err)
+    return err;
+
+  /* Enable the Memory Space bit */
+  err = pci_device_cfg_read_u32 (dev, &reg, PCI_COMMAND);
+  if (err)
+    return err;
+
+  if (!(reg & 0x2))
+    {
+      reg |= 0x2;
+
+      err = pci_device_cfg_write_u32 (dev, reg, PCI_COMMAND);
+      if (err)
+       return err;
+    }
+
+  dev->rom_size = rom_size;
+  d->rom_base = rom_base;
+
+  return 0;
+}
+
+static error_t
+pci_device_x86_read_rom (struct pci_device *dev, void *buffer)
+{
+  int memfd;
+  void *rom_mapped;
+  struct pci_device_private *d = (struct pci_device_private *)dev;
+ 
+  /* Map the ROM in our space */
+  memfd = open ("/dev/mem", O_RDONLY | O_CLOEXEC);
+  if (memfd == -1)
+    return errno;
+
+  rom_mapped = mmap (NULL, dev->rom_size, PROT_READ, 0, memfd, d->rom_base);
+  if (rom_mapped == MAP_FAILED)
+    {
+      close (memfd);
+      return errno;
+    }
+
+  close (memfd);
+
+  memcpy(buffer, rom_mapped, dev->rom_size);
+  munmap(rom_mapped, dev->rom_size);
+
+  return 0;
+}
+
+/* Configure BARs and ROM */
+static error_t
+pci_device_x86_probe (struct pci_device *dev)
+{
+  error_t err;
+  uint8_t hdrtype;
+  int i;
+
+  /* Probe BARs */
+  err = pci_device_cfg_read_u8 (dev, &hdrtype, PCI_HDRTYPE);
+  if (err)
+    return err;
+
+  for (i = 0; i < pci_device_x86_get_num_regions (hdrtype); i++)
+    {
+      err = pci_device_x86_region_probe (dev, i);
+      if (err)
+       return err;
+
+      if (dev->regions[i].is_64)
+       /* Move the pointer one BAR ahead */
+       i++;
+    }
+
+  /* Probe ROM */
+  pci_device_x86_probe_rom(dev);
+
+  return 0;
+}
+
+/* Check that this really looks like a PCI configuration. */
+static error_t
+pci_system_x86_check (void)
+{
+  int dev;
+  uint16_t class, vendor;
+  struct pci_device tmpdev = { 0 };
+
+  /* Look on bus 0 for a device that is a host bridge, a VGA card,
+   * or an intel or compaq device.  */
+  tmpdev.bus = 0;
+  tmpdev.func = 0;
+  class = 0;
+  vendor = 0;
+
+  for (dev = 0; dev < 32; dev++)
+    {
+      tmpdev.dev = dev;
+
+      if (pci_device_cfg_read_u16 (&tmpdev, &class, PCI_CLASS_DEVICE))
+        continue;
+      if (class == PCI_CLASS_BRIDGE_HOST || class == PCI_CLASS_DISPLAY_VGA)
+        return 0;
+      if (pci_device_cfg_read_u16 (&tmpdev, &vendor, PCI_VENDOR_ID))
+        continue;
+      if (vendor == PCI_VENDOR_ID_INTEL || class == PCI_VENDOR_ID_COMPAQ)
+        return 0;
+    }
+
+  return ENODEV;
+}
+
+/* Recursively scan bus number `bus' */
+static error_t
+pci_system_x86_scan_bus (uint8_t bus)
+{
+  error_t err;
+  uint8_t dev, func, nfuncs, hdrtype, secbus;
+  uint32_t reg;
+  struct pci_device_private *d, *devices;
+  struct pci_device scratchdev;
+
+  scratchdev.bus = bus;
+
+  for (dev = 0; dev < 32; dev++)
+    {
+      scratchdev.dev = dev;
+      scratchdev.func = 0;
+      err = pci_nfuncs (&scratchdev, &nfuncs);
+      if (err)
+       return err;
+
+      for (func = 0; func < nfuncs; func++)
+       {
+         scratchdev.func = func;
+	 err = pci_device_cfg_read_u32 (&scratchdev, &reg, PCI_VENDOR_ID);
+         if (err)
+           return err;
+
+         if (PCI_VENDOR (reg) == PCI_VENDOR_INVALID || PCI_VENDOR (reg) == 0)
+           continue;
+
+         err = pci_device_cfg_read_u32 (&scratchdev, &reg, PCI_CLASS);
+         if (err)
+           return err;
+
+         err = pci_device_cfg_read_u8 (&scratchdev, &hdrtype, PCI_HDRTYPE);
+         if (err)
+           return err;
+
+         devices =
+           realloc (pci_sys->devices,
+                    (pci_sys->num_devices + 1) * sizeof (struct pci_device_private));
+         if (!devices)
+           return ENOMEM;
+
+         d = devices + pci_sys->num_devices;
+         memset (d, 0, sizeof (struct pci_device_private));
+
+         /* Fixed values as PCI express is still not supported */
+         d->base.domain = 0;
+         d->base.bus = bus;
+         d->base.dev = dev;
+         d->base.func = func;
+
+         d->base.device_class = reg >> 8;
+
+         err = pci_device_x86_probe (&d->base);
+         if (err)
+           return err;
+
+         pci_sys->devices = devices;
+         pci_sys->num_devices++;
+
+         switch (hdrtype & 0x3)
+           {
+           case PCI_HDRTYPE_DEVICE:
+             break;
+           case PCI_HDRTYPE_BRIDGE:
+           case PCI_HDRTYPE_CARDBUS:
+             {
+               err = pci_device_cfg_read_u8 (&scratchdev, &secbus, PCI_SECONDARY_BUS);
+               if (err)
+                 return err;
+
+               err = pci_system_x86_scan_bus (secbus);
+               if (err)
+                 return err;
+
+               break;
+             }
+           default:
+             /* Unknown header, do nothing */
+             break;
+           }
+       }
+    }
+
+  return 0;
 }
 
 #if defined(__CYGWIN__)
@@ -646,10 +875,9 @@ pci_device_x86_unmap_range(struct pci_device *dev,
 #endif
 
 static int
-pci_device_x86_read(struct pci_device *dev, void *data,
+pci_device_x86_read_conf1(struct pci_device *dev, void *data,
     pciaddr_t offset, pciaddr_t size, pciaddr_t *bytes_read)
 {
-    struct pci_system_x86 *pci_sys_x86 = (struct pci_system_x86 *) pci_sys;
     int err;
 
     *bytes_read = 0;
@@ -658,7 +886,7 @@ pci_device_x86_read(struct pci_device *dev, void *data,
 	if (toread > size)
 	    toread = size;
 
-	err = pci_sys_x86->read(dev->bus, dev->dev, dev->func, offset, data, toread);
+	err = pci_system_x86_conf1_read(dev->bus, dev->dev, dev->func, offset, data, toread);
 	if (err)
 	    return err;
 
@@ -671,10 +899,33 @@ pci_device_x86_read(struct pci_device *dev, void *data,
 }
 
 static int
-pci_device_x86_write(struct pci_device *dev, const void *data,
+pci_device_x86_read_conf2(struct pci_device *dev, void *data,
+    pciaddr_t offset, pciaddr_t size, pciaddr_t *bytes_read)
+{
+    int err;
+
+    *bytes_read = 0;
+    while (size > 0) {
+	int toread = 1 << (ffs(0x4 + (offset & 0x03)) - 1);
+	if (toread > size)
+	    toread = size;
+
+	err = pci_system_x86_conf2_read(dev->bus, dev->dev, dev->func, offset, data, toread);
+	if (err)
+	    return err;
+
+	offset += toread;
+	data = (char*)data + toread;
+	size -= toread;
+	*bytes_read += toread;
+    }
+    return 0;
+}
+
+static int
+pci_device_x86_write_conf1(struct pci_device *dev, const void *data,
     pciaddr_t offset, pciaddr_t size, pciaddr_t *bytes_written)
 {
-    struct pci_system_x86 *pci_sys_x86 = (struct pci_system_x86 *) pci_sys;
     int err;
 
     *bytes_written = 0;
@@ -685,7 +936,33 @@ pci_device_x86_write(struct pci_device *dev, const void *data,
 	if (towrite > 4 - (offset & 0x3))
 	    towrite = 4 - (offset & 0x3);
 
-	err = pci_sys_x86->write(dev->bus, dev->dev, dev->func, offset, data, towrite);
+	err = pci_system_x86_conf1_write(dev->bus, dev->dev, dev->func, offset, data, towrite);
+	if (err)
+	    return err;
+
+	offset += towrite;
+	data = (const char*)data + towrite;
+	size -= towrite;
+	*bytes_written += towrite;
+    }
+    return 0;
+}
+
+static int
+pci_device_x86_write_conf2(struct pci_device *dev, const void *data,
+    pciaddr_t offset, pciaddr_t size, pciaddr_t *bytes_written)
+{
+    int err;
+
+    *bytes_written = 0;
+    while (size > 0) {
+	int towrite = 4;
+	if (towrite > size)
+	    towrite = size;
+	if (towrite > 4 - (offset & 0x3))
+	    towrite = 4 - (offset & 0x3);
+
+	err = pci_system_x86_conf2_write(dev->bus, dev->dev, dev->func, offset, data, towrite);
 	if (err)
 	    return err;
 
@@ -792,14 +1069,35 @@ pci_device_x86_unmap_legacy(struct pci_device *dev, void *addr,
     return pci_device_x86_unmap_range(dev, &map);
 }
 
-static const struct pci_system_methods x86_pci_methods = {
+static const struct pci_system_methods x86_pci_method_conf1 = {
+    .destroy = pci_system_x86_destroy,
+    .read_rom = pci_device_x86_read_rom,
+    .probe = pci_device_x86_probe,
+    .map_range = pci_device_x86_map_range,
+    .unmap_range = pci_device_x86_unmap_range,
+    .read = pci_device_x86_read_conf1,
+    .write = pci_device_x86_write_conf1,
+    .fill_capabilities = pci_fill_capabilities_generic,
+    .open_legacy_io = pci_device_x86_open_legacy_io,
+    .close_io = pci_device_x86_close_io,
+    .read32 = pci_device_x86_read32,
+    .read16 = pci_device_x86_read16,
+    .read8 = pci_device_x86_read8,
+    .write32 = pci_device_x86_write32,
+    .write16 = pci_device_x86_write16,
+    .write8 = pci_device_x86_write8,
+    .map_legacy = pci_device_x86_map_legacy,
+    .unmap_legacy = pci_device_x86_unmap_legacy,
+};
+
+static const struct pci_system_methods x86_pci_method_conf2 = {
     .destroy = pci_system_x86_destroy,
     .read_rom = pci_device_x86_read_rom,
     .probe = pci_device_x86_probe,
     .map_range = pci_device_x86_map_range,
     .unmap_range = pci_device_x86_unmap_range,
-    .read = pci_device_x86_read,
-    .write = pci_device_x86_write,
+    .read = pci_device_x86_read_conf2,
+    .write = pci_device_x86_write_conf2,
     .fill_capabilities = pci_fill_capabilities_generic,
     .open_legacy_io = pci_device_x86_open_legacy_io,
     .close_io = pci_device_x86_close_io,
@@ -813,109 +1111,63 @@ static const struct pci_system_methods x86_pci_methods = {
     .unmap_legacy = pci_device_x86_unmap_legacy,
 };
 
-static int pci_probe(struct pci_system_x86 *pci_sys_x86)
+static int pci_probe(void)
 {
+    pci_sys->methods = &x86_pci_method_conf1;
     if (pci_system_x86_conf1_probe() == 0) {
-	pci_sys_x86->read = pci_system_x86_conf1_read;
-	pci_sys_x86->write = pci_system_x86_conf1_write;
-	if (pci_system_x86_check(pci_sys_x86) == 0)
-	    return 0;
+	if (pci_system_x86_check() == 0)
+	    return 1;
     }
 
+    pci_sys->methods = &x86_pci_method_conf2;
     if (pci_system_x86_conf2_probe() == 0) {
-	pci_sys_x86->read = pci_system_x86_conf2_read;
-	pci_sys_x86->write = pci_system_x86_conf2_write;
-	if (pci_system_x86_check(pci_sys_x86) == 0)
-	    return 0;
+	if (pci_system_x86_check() == 0)
+	    return 2;
     }
 
-    return ENODEV;
+    pci_sys->methods = NULL;
+    return 0;
 }
 
 _pci_hidden int
-pci_system_x86_create(void)
+pci_system_x86_create (void)
 {
-    struct pci_device_private *device;
-    int ret, bus, dev, ndevs, func, nfuncs;
-    struct pci_system_x86 *pci_sys_x86;
-    uint32_t reg;
+  error_t err;
+  int confx;
 
-    ret = x86_enable_io();
-    if (ret)
-	return ret;
-
-    pci_sys_x86 = calloc(1, sizeof(struct pci_system_x86));
-    if (pci_sys_x86 == NULL) {
-	x86_disable_io();
-	return ENOMEM;
-    }
-    pci_sys = &pci_sys_x86->system;
-
-    ret = pci_probe(pci_sys_x86);
-    if (ret) {
-	x86_disable_io();
-	free(pci_sys_x86);
-	pci_sys = NULL;
-	return ret;
-    }
+  err = x86_enable_io ();
+  if (err)
+    return err;
 
-    pci_sys->methods = &x86_pci_methods;
-
-    ndevs = 0;
-    for (bus = 0; bus < 256; bus++) {
-	for (dev = 0; dev < 32; dev++) {
-	    nfuncs = pci_nfuncs(pci_sys_x86, bus, dev);
-	    for (func = 0; func < nfuncs; func++) {
-		if (pci_sys_x86->read(bus, dev, func, PCI_VENDOR_ID, &reg, sizeof(reg)) != 0)
-		    continue;
-		if (PCI_VENDOR(reg) == PCI_VENDOR_INVALID ||
-		    PCI_VENDOR(reg) == 0)
-		    continue;
-		ndevs++;
-	    }
-	}
+  pci_sys = calloc (1, sizeof (struct pci_system));
+  if (pci_sys == NULL)
+    {
+      x86_disable_io ();
+      return ENOMEM;
     }
 
-    pci_sys->num_devices = ndevs;
-    pci_sys->devices = calloc(ndevs, sizeof(struct pci_device_private));
-    if (pci_sys->devices == NULL) {
-	x86_disable_io();
-	free(pci_sys_x86);
-	pci_sys = NULL;
-	return ENOMEM;
+  confx = pci_probe ();
+  if (!confx)
+    {
+      x86_disable_io ();
+      free (pci_sys);
+      return ENOSYS;
     }
-
-    device = pci_sys->devices;
-    for (bus = 0; bus < 256; bus++) {
-	for (dev = 0; dev < 32; dev++) {
-	    nfuncs = pci_nfuncs(pci_sys_x86, bus, dev);
-	    for (func = 0; func < nfuncs; func++) {
-		if (pci_sys_x86->read(bus, dev, func, PCI_VENDOR_ID, &reg, sizeof(reg)) != 0)
-		    continue;
-		if (PCI_VENDOR(reg) == PCI_VENDOR_INVALID ||
-		    PCI_VENDOR(reg) == 0)
-		    continue;
-		device->base.domain = device->base.domain_16 = 0;
-		device->base.bus = bus;
-		device->base.dev = dev;
-		device->base.func = func;
-		device->base.vendor_id = PCI_VENDOR(reg);
-		device->base.device_id = PCI_DEVICE(reg);
-
-		if (pci_sys_x86->read(bus, dev, func, PCI_CLASS, &reg, sizeof(reg)) != 0)
-		    continue;
-		device->base.device_class = reg >> 8;
-		device->base.revision = reg & 0xFF;
-
-		if (pci_sys_x86->read(bus, dev, func, PCI_SUB_VENDOR_ID, &reg, sizeof(reg)) != 0)
-		    continue;
-		device->base.subvendor_id = PCI_VENDOR(reg);
-		device->base.subdevice_id = PCI_DEVICE(reg);
-
-		device++;
-	    }
-	}
+  else if (confx == 1)
+    pci_sys->methods = &x86_pci_method_conf1;
+  else
+    pci_sys->methods = &x86_pci_method_conf2;
+
+  /* Recursive scan */
+  pci_sys->num_devices = 0;
+  err = pci_system_x86_scan_bus (0);
+  if (err)
+    {
+      x86_disable_io ();
+      free (pci_sys);
+      pci_sys = NULL;
+      return err;
     }
 
-    return 0;
+  return 0;
 }
diff --git a/src/x86_pci.h b/src/x86_pci.h
index 2e00ba6..c441442 100644
--- a/src/x86_pci.h
+++ b/src/x86_pci.h
@@ -1,4 +1,5 @@
 /*
+ * Copyright (c) 2017 Joan Lledó
  * Copyright (c) 2009, 2012 Samuel Thibault
  * Heavily inspired from the freebsd, netbsd, and openbsd backends
  * (C) Copyright Eric Anholt 2006
@@ -47,8 +48,20 @@
 #define	PCIS_DISPLAY_VGA	0x00
 
 #define PCI_HDRTYPE	0x0E
+#define PCI_HDRTYPE_DEVICE     0x00
+#define PCI_HDRTYPE_BRIDGE     0x01
+#define PCI_HDRTYPE_CARDBUS    0x02
 #define PCI_IRQ		0x3C
 
+#define PCI_BAR_ADDR_0         0x10
+#define PCI_XROMBAR_ADDR_00    0x30
+#define PCI_XROMBAR_ADDR_01    0x38
+
+#define PCI_COMMAND            0x04
+#define PCI_SECONDARY_BUS      0x19
+
+#define PCI_CONFIG_SIZE  256
+
 int x86_enable_io(void);
 int x86_disable_io(void);
 void pci_system_x86_destroy(void);