Rewritten the PCI Type 2 access handling.

src/include/86box/pci.h

@@ -91,13 +91,30 @@ enum {
 
 typedef union {
     uint32_t addr;
-    uint8_t  addr_regs[4];
+    uint8_t addr_regs[4];
 } bar_t;
 
+
+#define PCI_IO_ON   0x01
+#define PCI_IO_DEV0 0x02
+
+
 extern int pci_burst_time;
 extern int agp_burst_time;
 extern int pci_nonburst_time;
 extern int agp_nonburst_time;
+extern int pci_take_over_io;
+
+extern uint32_t pci_base;
+extern uint32_t pci_size;
+
+
+extern void     pci_type2_write(uint16_t port, uint8_t val, void *priv);
+extern void     pci_type2_writew(uint16_t port, uint16_t val, void *priv);
+extern void     pci_type2_writel(uint16_t port, uint32_t val, void *priv);
+extern uint8_t  pci_type2_read(uint16_t port, void *priv);
+extern uint16_t pci_type2_readw(uint16_t port, void *priv);
+extern uint32_t pci_type2_readl(uint16_t port, void *priv);
 
 extern void pci_set_irq_routing(int pci_int, int irq);
 extern void pci_set_irq_level(int pci_int, int level);

src/io.c

@@ -29,6 +29,7 @@
 #include <86box/timer.h>
 #include "cpu.h"
 #include <86box/m_amstrad.h>
+#include <86box/pci.h>
 
 #define NPORTS 65536 /* PC/AT supports 64K ports */
 
@@ -286,15 +287,25 @@ inb(uint16_t port)
     int     found  = 0;
     int     qfound = 0;
 
-    p = io[port];
+    if ((pci_take_over_io & PCI_IO_ON) && (port >= pci_base) && (port < (pci_base + pci_size))) {
-    while (p) {
+        ret = pci_type2_read(port, NULL);
-        q = p->next;
+        found = 1;
-        if (p->inb) {
+        qfound = 1;
-            ret &= p->inb(port, p->priv);
+    } else if ((pci_take_over_io & PCI_IO_DEV0) && (port >= 0xc000) && (port < 0xc100)) {
-            found |= 1;
+        ret = pci_type2_read(port, NULL);
-            qfound++;
+        found = 1;
+        qfound = 1;
+    } else {
+        p = io[port];
+        while (p) {
+            q = p->next;
+            if (p->inb) {
+                ret &= p->inb(port, p->priv);
+                found |= 1;
+                qfound++;
+            }
+            p = q;
         }
-        p = q;
     }
 
     if (amstrad_latch & 0x80000000) {
@@ -326,15 +337,25 @@ outb(uint16_t port, uint8_t val)
     int   found  = 0;
     int   qfound = 0;
 
-    p = io[port];
+    if ((pci_take_over_io & PCI_IO_ON) && (port >= pci_base) && (port < (pci_base + pci_size))) {
-    while (p) {
+        pci_type2_write(port, val, NULL);
-        q = p->next;
+        found = 1;
-        if (p->outb) {
+        qfound = 1;
-            p->outb(port, val, p->priv);
+    } else if ((pci_take_over_io & PCI_IO_DEV0) && (port >= 0xc000) && (port < 0xc100)) {
-            found |= 1;
+        pci_type2_write(port, val, NULL);
-            qfound++;
+        found = 1;
+        qfound = 1;
+    } else {
+        p = io[port];
+        while (p) {
+            q = p->next;
+            if (p->outb) {
+                p->outb(port, val, p->priv);
+                found |= 1;
+                qfound++;
+            }
+            p = q;
         }
-        p = q;
     }
 
     if (!found) {
@@ -360,32 +381,42 @@ inw(uint16_t port)
     int      qfound = 0;
     uint8_t  ret8[2];
 
-    p = io[port];
+    if ((pci_take_over_io & PCI_IO_ON) && (port >= pci_base) && (port < (pci_base + pci_size))) {
-    while (p) {
+        ret = pci_type2_readw(port, NULL);
-        q = p->next;
+        found = 2;
-        if (p->inw) {
+        qfound = 1;
-            ret &= p->inw(port, p->priv);
+    } else if ((pci_take_over_io & PCI_IO_DEV0) && (port >= 0xc000) && (port < 0xc100)) {
-            found |= 2;
+        ret = pci_type2_readw(port, NULL);
-            qfound++;
+        found = 2;
-        }
+        qfound = 1;
-        p = q;
+    } else {
-    }
+        p = io[port];
-
-    ret8[0] = ret & 0xff;
-    ret8[1] = (ret >> 8) & 0xff;
-    for (uint8_t i = 0; i < 2; i++) {
-        p = io[(port + i) & 0xffff];
         while (p) {
             q = p->next;
-            if (p->inb && !p->inw) {
+            if (p->inw) {
-                ret8[i] &= p->inb(port + i, p->priv);
+                ret &= p->inw(port, p->priv);
-                found |= 1;
+                found |= 2;
                 qfound++;
             }
             p = q;
         }
+
+        ret8[0] = ret & 0xff;
+        ret8[1] = (ret >> 8) & 0xff;
+        for (uint8_t i = 0; i < 2; i++) {
+            p = io[(port + i) & 0xffff];
+            while (p) {
+                q = p->next;
+                if (p->inb && !p->inw) {
+                    ret8[i] &= p->inb(port + i, p->priv);
+                    found |= 1;
+                    qfound++;
+                }
+                p = q;
+            }
+        }
+        ret = (ret8[1] << 8) | ret8[0];
     }
-    ret = (ret8[1] << 8) | ret8[0];
 
     if (amstrad_latch & 0x80000000) {
         if (port & 0x80)
@@ -412,28 +443,38 @@ outw(uint16_t port, uint16_t val)
     int   found  = 0;
     int   qfound = 0;
 
-    p = io[port];
+    if ((pci_take_over_io & PCI_IO_ON) && (port >= pci_base) && (port < (pci_base + pci_size))) {
-    while (p) {
+        pci_type2_writew(port, val, NULL);
-        q = p->next;
+        found = 2;
-        if (p->outw) {
+        qfound = 1;
-            p->outw(port, val, p->priv);
+    } else if ((pci_take_over_io & PCI_IO_DEV0) && (port >= 0xc000) && (port < 0xc100)) {
-            found |= 2;
+        pci_type2_writew(port, val, NULL);
-            qfound++;
+        found = 2;
-        }
+        qfound = 1;
-        p = q;
+    } else {
-    }
+        p = io[port];
-
-    for (uint8_t i = 0; i < 2; i++) {
-        p = io[(port + i) & 0xffff];
         while (p) {
             q = p->next;
-            if (p->outb && !p->outw) {
+            if (p->outw) {
-                p->outb(port + i, val >> (i << 3), p->priv);
+                p->outw(port, val, p->priv);
-                found |= 1;
+                found |= 2;
                 qfound++;
             }
             p = q;
         }
+
+        for (uint8_t i = 0; i < 2; i++) {
+            p = io[(port + i) & 0xffff];
+            while (p) {
+                q = p->next;
+                if (p->outb && !p->outw) {
+                    p->outb(port + i, val >> (i << 3), p->priv);
+                    found |= 1;
+                    qfound++;
+                }
+                p = q;
+            }
+        }
     }
 
     if (!found) {
@@ -460,59 +501,69 @@ inl(uint16_t port)
     int      found  = 0;
     int      qfound = 0;
 
-    p = io[port];
+    if ((pci_take_over_io & PCI_IO_ON) && (port >= pci_base) && (port < (pci_base + pci_size))) {
-    while (p) {
+        ret = pci_type2_readl(port, NULL);
-        q = p->next;
+        found = 4;
-        if (p->inl) {
+        qfound = 1;
-            ret &= p->inl(port, p->priv);
+    } else if ((pci_take_over_io & PCI_IO_DEV0) && (port >= 0xc000) && (port < 0xc100)) {
-            found |= 4;
+        ret = pci_type2_readl(port, NULL);
-            qfound++;
+        found = 4;
-        }
+        qfound = 1;
-        p = q;
+    } else {
-    }
+        p = io[port];
-
-    ret16[0] = ret & 0xffff;
-    ret16[1] = (ret >> 16) & 0xffff;
-    p        = io[port & 0xffff];
-    while (p) {
-        q = p->next;
-        if (p->inw && !p->inl) {
-            ret16[0] &= p->inw(port, p->priv);
-            found |= 2;
-            qfound++;
-        }
-        p = q;
-    }
-
-    p = io[(port + 2) & 0xffff];
-    while (p) {
-        q = p->next;
-        if (p->inw && !p->inl) {
-            ret16[1] &= p->inw(port + 2, p->priv);
-            found |= 2;
-            qfound++;
-        }
-        p = q;
-    }
-    ret = (ret16[1] << 16) | ret16[0];
-
-    ret8[0] = ret & 0xff;
-    ret8[1] = (ret >> 8) & 0xff;
-    ret8[2] = (ret >> 16) & 0xff;
-    ret8[3] = (ret >> 24) & 0xff;
-    for (uint8_t i = 0; i < 4; i++) {
-        p = io[(port + i) & 0xffff];
         while (p) {
             q = p->next;
-            if (p->inb && !p->inw && !p->inl) {
+            if (p->inl) {
-                ret8[i] &= p->inb(port + i, p->priv);
+                ret &= p->inl(port, p->priv);
-                found |= 1;
+                found |= 4;
                 qfound++;
             }
             p = q;
         }
+
+        ret16[0] = ret & 0xffff;
+        ret16[1] = (ret >> 16) & 0xffff;
+        p        = io[port & 0xffff];
+        while (p) {
+            q = p->next;
+            if (p->inw && !p->inl) {
+                ret16[0] &= p->inw(port, p->priv);
+                found |= 2;
+                qfound++;
+            }
+            p = q;
+        }
+
+        p = io[(port + 2) & 0xffff];
+        while (p) {
+            q = p->next;
+            if (p->inw && !p->inl) {
+                ret16[1] &= p->inw(port + 2, p->priv);
+                found |= 2;
+                qfound++;
+            }
+            p = q;
+        }
+        ret = (ret16[1] << 16) | ret16[0];
+
+        ret8[0] = ret & 0xff;
+        ret8[1] = (ret >> 8) & 0xff;
+        ret8[2] = (ret >> 16) & 0xff;
+        ret8[3] = (ret >> 24) & 0xff;
+        for (uint8_t i = 0; i < 4; i++) {
+            p = io[(port + i) & 0xffff];
+            while (p) {
+                q = p->next;
+                if (p->inb && !p->inw && !p->inl) {
+                    ret8[i] &= p->inb(port + i, p->priv);
+                    found |= 1;
+                    qfound++;
+                }
+                p = q;
+            }
+        }
+        ret = (ret8[3] << 24) | (ret8[2] << 16) | (ret8[1] << 8) | ret8[0];
     }
-    ret = (ret8[3] << 24) | (ret8[2] << 16) | (ret8[1] << 8) | ret8[0];
 
     if (amstrad_latch & 0x80000000) {
         if (port & 0x80)
@@ -540,42 +591,52 @@ outl(uint16_t port, uint32_t val)
     int   qfound = 0;
     int   i      = 0;
 
-    p = io[port];
+    if ((pci_take_over_io & PCI_IO_ON) && (port >= pci_base) && (port < (pci_base + pci_size))) {
-    if (p) {
+        pci_type2_writel(port, val, NULL);
-        while (p) {
+        found = 4;
-            q = p->next;
+        qfound = 1;
-            if (p->outl) {
+    } else if ((pci_take_over_io & PCI_IO_DEV0) && (port >= 0xc000) && (port < 0xc100)) {
-                p->outl(port, val, p->priv);
+        pci_type2_writel(port, val, NULL);
-                found |= 4;
+        found = 4;
-                qfound++;
+        qfound = 1;
+    } else {
+        p = io[port];
+        if (p) {
+            while (p) {
+                q = p->next;
+                if (p->outl) {
+                    p->outl(port, val, p->priv);
+                    found |= 4;
+                    qfound++;
+                }
+                p = q;
             }
-            p = q;
         }
-    }
 
-    for (i = 0; i < 4; i += 2) {
+        for (i = 0; i < 4; i += 2) {
-        p = io[(port + i) & 0xffff];
+            p = io[(port + i) & 0xffff];
-        while (p) {
+            while (p) {
-            q = p->next;
+                q = p->next;
-            if (p->outw && !p->outl) {
+                if (p->outw && !p->outl) {
-                p->outw(port + i, val >> (i << 3), p->priv);
+                    p->outw(port + i, val >> (i << 3), p->priv);
-                found |= 2;
+                    found |= 2;
-                qfound++;
+                    qfound++;
+                }
+                p = q;
             }
-            p = q;
         }
-    }
 
-    for (i = 0; i < 4; i++) {
+        for (i = 0; i < 4; i++) {
-        p = io[(port + i) & 0xffff];
+            p = io[(port + i) & 0xffff];
-        while (p) {
+            while (p) {
-            q = p->next;
+                q = p->next;
-            if (p->outb && !p->outw && !p->outl) {
+                if (p->outb && !p->outw && !p->outl) {
-                p->outb(port + i, val >> (i << 3), p->priv);
+                    p->outb(port + i, val >> (i << 3), p->priv);
-                found |= 1;
+                    found |= 1;
-                qfound++;
+                    qfound++;
+                }
+                p = q;
             }
-            p = q;
         }
     }
 

src/machine/machine.c

@@ -41,6 +41,7 @@
 #include <86box/video.h>
 #include <86box/machine.h>
 #include <86box/isamem.h>
+#include <86box/pci.h>
 
 int bios_only = 0;
 int machine;
@@ -103,6 +104,8 @@ machine_init_ex(int m)
 
         /* Reset the fast off stuff. */
         cpu_fast_off_reset();
+
+        pci_take_over_io = 0x00000000;
     }
 
     /* All good, boot the machine! */

src/pci.c

@@ -53,6 +53,10 @@ int pci_burst_time;
 int agp_burst_time;
 int pci_nonburst_time;
 int agp_nonburst_time;
+int pci_take_over_io;
+
+uint32_t pci_base = 0xc000;
+uint32_t pci_size = 0x1000;
 
 static pci_card_t pci_cards[32];
 static uint8_t    pci_pmc = 0;
@@ -74,8 +78,6 @@ static int        pci_bus;
 static int        pci_enable;
 static int        pci_key;
 static int        trc_reg = 0;
-static uint32_t   pci_base = 0xc000;
-static uint32_t   pci_size = 0x1000;
 
 static void pci_reset_regs(void);
 
@@ -389,9 +391,6 @@ pci_readl(uint16_t port, void *priv)
     return ret;
 }
 
-static void    pci_type2_write(uint16_t port, uint8_t val, void *priv);
-static uint8_t pci_type2_read(uint16_t port, void *priv);
-
 void
 pci_set_pmc(uint8_t pmc)
 {
@@ -399,9 +398,8 @@ pci_set_pmc(uint8_t pmc)
     // pci_reset_regs();
 
     if (!pci_pmc && (pmc & 0x01)) {
-        io_removehandler(pci_base, pci_size,
+        pci_log("PMC: Dellocating ports %04X-%04X...\n", pci_base, pci_base + pci_size - 1);
-                         pci_type2_read, NULL, NULL,
+        pci_take_over_io &= ~PCI_IO_ON;
-                         pci_type2_write, NULL, NULL, NULL);
 
         io_removehandler(0x0cf8, 1,
                          pci_type2_read, NULL, NULL, pci_type2_write, NULL, NULL, NULL);
@@ -414,13 +412,11 @@ pci_set_pmc(uint8_t pmc)
         io_sethandler(0x0cfc, 4,
                       pci_read, pci_readw, pci_readl, pci_write, pci_writew, pci_writel, NULL);
     } else if (pci_pmc && !(pmc & 0x01)) {
-        io_removehandler(pci_base, pci_size,
+        pci_log("PMC: Redellocating ports %04X-%04X...\n", pci_base, pci_base + pci_size - 1);
-                         pci_type2_read, NULL, NULL,
+        pci_take_over_io &= ~PCI_IO_ON;
-                         pci_type2_write, NULL, NULL, NULL);
         if (pci_key) {
-            io_sethandler(pci_base, pci_size,
+            pci_log("PMC: Allocating ports %04X-%04X...\n", pci_base, pci_base + pci_size - 1);
-                          pci_type2_read, NULL, NULL,
+            pci_take_over_io |= PCI_IO_ON;
-                          pci_type2_write, NULL, NULL, NULL);
         }
 
         io_removehandler(0x0cf8, 1,
@@ -439,67 +435,97 @@ pci_set_pmc(uint8_t pmc)
 }
 
 static void
-pci_type2_write(uint16_t port, uint8_t val, void *priv)
+pci_type2_write_reg(uint16_t port, uint8_t val)
 {
     uint8_t slot = 0;
 
-    if (port == 0xcf8) {
+    pci_card  = (port >> 8) & 0xf;
-        pci_func = (val >> 1) & 7;
+    pci_index = port & 0xff;
 
-        if (!pci_key && (val & 0xf0)) {
+    slot = pci_card_to_slot_mapping[pci_bus_number_to_index_mapping[pci_bus]][pci_card];
-            io_removehandler(pci_base, pci_size,
+    if (slot != 0xff) {
-                             pci_type2_read, NULL, NULL,
+        if (pci_cards[slot].write)
-                             pci_type2_write, NULL, NULL, NULL);
+            pci_cards[slot].write(pci_func, pci_index | (port & 3), val, pci_cards[slot].priv);
-            io_sethandler(pci_base, pci_size,
-                          pci_type2_read, NULL, NULL,
-                          pci_type2_write, NULL, NULL, NULL);
-        } else if (pci_key && !(val & 0xf0))
-            io_removehandler(pci_base, pci_size,
-                             pci_type2_read, NULL, NULL,
-                             pci_type2_write, NULL, NULL, NULL);
-
-        pci_key = val & 0xf0;
-    } else if (port == 0xcfa) {
-        pci_bus = val;
-
-        pci_log("Allocating ports %04X-%04X...\n", pci_base, pci_base + pci_size - 1);
-
-        /* Evidently, writing here, we should also enable the
-           configuration space. */
-        io_removehandler(pci_base, pci_size,
-                         pci_type2_read, NULL, NULL,
-                         pci_type2_write, NULL, NULL, NULL);
-        io_sethandler(pci_base, pci_size,
-                      pci_type2_read, NULL, NULL,
-                      pci_type2_write, NULL, NULL, NULL);
-
-        /* Mark as enabled. */
-        pci_key |= 0x100;
-    } else if (port == 0xcfb) {
-        pci_log("Write %02X to port 0CFB\n", val);
-        pci_set_pmc(val);
-    } else {
-        pci_card  = (port >> 8) & 0xf;
-        pci_index = port & 0xff;
-
-        slot = pci_card_to_slot_mapping[pci_bus_number_to_index_mapping[pci_bus]][pci_card];
-        if (slot != 0xff) {
-            if (pci_cards[slot].write)
-                pci_cards[slot].write(pci_func, pci_index | (port & 3), val, pci_cards[slot].priv);
-#ifdef ENABLE_PCI_LOG
-            else
-                pci_log("Writing to empty PCI card on slot %02X (pci_cards[%i]) (%02X:%02X)...\n", pci_card, slot, pci_func, pci_index);
-#endif
-        }
 #ifdef ENABLE_PCI_LOG
         else
-            pci_log("Writing to unassigned PCI card on slot %02X (pci_cards[%i]) (%02X:%02X)...\n", pci_card, slot, pci_func, pci_index);
+            pci_log("Writing to empty PCI card on slot %02X:%02X (pci_cards[%i]) (%02X:%02X)...\n", pci_bus, pci_card, slot, pci_func, pci_index);
 #endif
     }
+#ifdef ENABLE_PCI_LOG
+    else
+        pci_log("Writing to unassigned PCI card on slot %02X:%02X (pci_cards[%i]) (%02X:%02X)...\n", pci_bus, pci_card, slot, pci_func, pci_index);
+#endif
+}
+
+void
+pci_type2_write(uint16_t port, uint8_t val, void *priv)
+{
+    switch (port) {
+        case 0xcf8:
+            pci_func = (val >> 1) & 7;
+
+            if (val & 0xf0) {
+                pci_log("CF8: Allocating ports %04X-%04X...\n", pci_base, pci_base + pci_size - 1);
+                pci_take_over_io |= PCI_IO_ON;
+            } else {
+                pci_log("CF8: Dellocating ports %04X-%04X...\n", pci_base, pci_base + pci_size - 1);
+                pci_take_over_io &= ~PCI_IO_ON;
+            }
+
+            pci_key = val & 0xf0;
+            break;
+        case 0xcfa:
+            pci_bus = val;
+
+            pci_log("CFA: Allocating ports %04X-%04X...\n", pci_base, pci_base + pci_size - 1);
+
+            /* Evidently, writing here, we should also enable the
+               configuration space. */
+            pci_take_over_io |= PCI_IO_ON;
+
+            /* Mark as enabled. */
+            pci_key |= 0x100;
+            break;
+        case 0xcfb:
+            pci_log("Write %02X to port 0CFB\n", val);
+            pci_set_pmc(val);
+            break;
+
+        case 0xc000 ... 0xc0ff:
+            if (pci_take_over_io == 0x00000000)
+                break;
+
+            pci_type2_write_reg(port, val);
+            break;
+
+        case 0xc100 ... 0xcfff:
+            if (!(pci_take_over_io & PCI_IO_ON))
+                break;
+
+            pci_type2_write_reg(port, val);
+            break;
+
+        default:
+            break;
+    }
 }
 
-static void
+void
+pci_type2_writew(uint16_t port, uint16_t val, void *priv)
+{
+    pci_type2_write(port, val & 0xff, priv);
+    pci_type2_write(port + 1, val >> 8, priv);
+}
+
+void
 pci_type2_writel(uint16_t port, uint32_t val, void *priv)
+{
+    pci_type2_writew(port, val & 0xffff, priv);
+    pci_type2_writew(port + 2, val >> 16, priv);
+}
+
+static void
+pci_type2_cfb_writel(uint16_t port, uint32_t val, void *priv)
 {
     for (uint8_t i = 0; i < 4; i++) {
         /* Make sure to have the DWORD write not pass through to PMC if mechanism 1 is in use,
@@ -510,41 +536,88 @@ pci_type2_writel(uint16_t port, uint32_t val, void *priv)
 }
 
 static uint8_t
-pci_type2_read(uint16_t port, void *priv)
+pci_type2_read_reg(uint16_t port)
 {
     uint8_t slot = 0;
     uint8_t ret  = 0xff;
 
-    if (port == 0xcf8)
+    pci_card  = (port >> 8) & 0xf;
-        ret = pci_key | (pci_func << 1);
+    pci_index = port & 0xff;
-    else if (port == 0xcfa)
-        ret = pci_bus;
-    else if (port == 0xcfb)
-        ret = pci_pmc;
-    else {
-        pci_card  = (port >> 8) & 0xf;
-        pci_index = port & 0xff;
 
-        slot = pci_card_to_slot_mapping[pci_bus_number_to_index_mapping[pci_bus]][pci_card];
+    slot = pci_card_to_slot_mapping[pci_bus_number_to_index_mapping[pci_bus]][pci_card];
-        if (slot != 0xff) {
+    if (slot != 0xff) {
-            if (pci_cards[slot].read)
+        if (pci_cards[slot].read)
-                ret = pci_cards[slot].read(pci_func, pci_index | (port & 3), pci_cards[slot].priv);
+            ret = pci_cards[slot].read(pci_func, pci_index | (port & 3), pci_cards[slot].priv);
-#ifdef ENABLE_PCI_LOG
-            else
-                pci_log("Reading from empty PCI card on slot %02X (pci_cards[%i]) (%02X:%02X)...\n", pci_card, slot, pci_func, pci_index);
-#endif
-        }
 #ifdef ENABLE_PCI_LOG
         else
-            pci_log("Reading from unasisgned PCI card on slot %02X (pci_cards[%i]) (%02X:%02X)...\n", pci_card, slot, pci_func, pci_index);
+            pci_log("Reading from empty PCI card on slot %02X:%02X (pci_cards[%i]) (%02X:%02X)...\n", pci_bus, pci_card, slot, pci_func, pci_index);
+#endif
+    }
+#ifdef ENABLE_PCI_LOG
+    else
+        pci_log("Reading from unasisgned PCI card on slot %02X:%02X (pci_cards[%i]) (%02X:%02X)...\n", pci_bus, pci_card, slot, pci_func, pci_index);
 #endif
 
-        pci_log("Reading %02X at PCI register %02X at bus %02X, card %02X, function %02X\n", ret, pci_index, pci_bus, pci_card, pci_func);
+    pci_log("Reading %02X at PCI register %02X at bus %02X, card %02X, function %02X\n", ret, pci_index, pci_bus, pci_card, pci_func);
+
+    return ret;
+}
+
+uint8_t
+pci_type2_read(uint16_t port, void *priv)
+{
+    uint8_t ret  = 0xff;
+
+    switch (port) {
+        case 0xcf8:
+            ret = pci_key | (pci_func << 1);
+            break;
+        case 0xcfa:
+            ret = pci_bus;
+            break;
+       case 0xcfb:
+            ret = pci_pmc;
+            break;
+
+        case 0xc000 ... 0xc0ff:
+            if (pci_take_over_io == 0x00000000)
+                break;
+
+            ret = pci_type2_read_reg(port);
+            break;
+
+        case 0xc100 ... 0xcfff:
+            if (!(pci_take_over_io & PCI_IO_ON))
+                break;
+
+            ret = pci_type2_read_reg(port);
+            break;
+
+        default:
+            break;
     }
 
     return ret;
 }
 
+uint16_t
+pci_type2_readw(uint16_t port, void *priv)
+{
+    uint16_t ret = pci_type2_read(port, priv);
+    ret |= ((uint16_t) pci_type2_read(port + 1, priv)) << 8;
+
+    return ret;
+}
+
+uint32_t
+pci_type2_readl(uint16_t port, void *priv)
+{
+    uint32_t ret = pci_type2_readw(port, priv);
+    ret |= ((uint32_t) pci_type2_readw(port + 2, priv)) << 16;
+
+    return ret;
+}
+
 void
 pci_set_irq_routing(int pci_int, int irq)
 {
@@ -826,9 +899,7 @@ pci_reset_regs(void)
 {
     pci_index = pci_card = pci_func = pci_bus = pci_key = 0;
 
-    io_removehandler(pci_base, pci_size,
+    pci_take_over_io &= ~PCI_IO_ON;
-                     pci_type2_read, NULL, NULL,
-                     pci_type2_write, NULL, NULL, NULL);
 }
 
 void
@@ -993,7 +1064,7 @@ pci_init(int type)
         pci_pmc = 0x00;
 
         io_sethandler(0x0cfb, 1,
-                      pci_type2_read, NULL, NULL, pci_type2_write, NULL, pci_type2_writel, NULL);
+                      pci_type2_read, NULL, NULL, pci_type2_write, NULL, pci_type2_cfb_writel, NULL);
     }
 
     if (type & PCI_NO_IRQ_STEERING) {
@@ -1004,6 +1075,8 @@ pci_init(int type)
         pic_elcr_set_enabled(1);
     }
 
+    pci_take_over_io = 0x00000000;
+
     if ((type & PCI_CONFIG_TYPE_MASK) == PCI_CONFIG_TYPE_1) {
         pci_log("PCI: Configuration mechanism #1\n");
         io_sethandler(0x0cf8, 1,
@@ -1024,9 +1097,7 @@ pci_init(int type)
             pci_base = 0xc100;
             pci_size = 0x0f00;
 
-            io_sethandler(0xc000, 0x0100,
+            pci_take_over_io |= PCI_IO_DEV0;
-                          pci_type2_read, NULL, NULL,
-                          pci_type2_write, NULL, NULL, NULL);
         }
     }