diff --git a/cpu-all.h b/cpu-all.h
index 56a768add4..e0c3efd557 100644
--- a/cpu-all.h
+++ b/cpu-all.h
@@ -909,6 +909,7 @@ int cpu_register_io_memory(int io_index,
                            CPUReadMemoryFunc **mem_read,
                            CPUWriteMemoryFunc **mem_write,
                            void *opaque);
+void cpu_unregister_io_memory(int table_address);
 CPUWriteMemoryFunc **cpu_get_io_memory_write(int io_index);
 CPUReadMemoryFunc **cpu_get_io_memory_read(int io_index);
 
diff --git a/exec.c b/exec.c
index 9852bdcd31..7ed7e3ed32 100644
--- a/exec.c
+++ b/exec.c
@@ -179,7 +179,7 @@ static void io_mem_init(void);
 CPUWriteMemoryFunc *io_mem_write[IO_MEM_NB_ENTRIES][4];
 CPUReadMemoryFunc *io_mem_read[IO_MEM_NB_ENTRIES][4];
 void *io_mem_opaque[IO_MEM_NB_ENTRIES];
-static int io_mem_nb;
+char io_mem_used[IO_MEM_NB_ENTRIES];
 static int io_mem_watch;
 #endif
 
@@ -2799,12 +2799,28 @@ static void *subpage_init (target_phys_addr_t base, ram_addr_t *phys,
     return mmio;
 }
 
+static int get_free_io_mem_idx(void)
+{
+    int i;
+
+    for (i = 0; i<IO_MEM_NB_ENTRIES; i++)
+        if (!io_mem_used[i]) {
+            io_mem_used[i] = 1;
+            return i;
+        }
+
+    return -1;
+}
+
 static void io_mem_init(void)
 {
+    int i;
+
     cpu_register_io_memory(IO_MEM_ROM >> IO_MEM_SHIFT, error_mem_read, unassigned_mem_write, NULL);
     cpu_register_io_memory(IO_MEM_UNASSIGNED >> IO_MEM_SHIFT, unassigned_mem_read, unassigned_mem_write, NULL);
     cpu_register_io_memory(IO_MEM_NOTDIRTY >> IO_MEM_SHIFT, error_mem_read, notdirty_mem_write, NULL);
-    io_mem_nb = 5;
+    for (i=0; i<5; i++)
+        io_mem_used[i] = 1;
 
     io_mem_watch = cpu_register_io_memory(0, watch_mem_read,
                                           watch_mem_write, NULL);
@@ -2829,9 +2845,9 @@ int cpu_register_io_memory(int io_index,
     int i, subwidth = 0;
 
     if (io_index <= 0) {
-        if (io_mem_nb >= IO_MEM_NB_ENTRIES)
-            return -1;
-        io_index = io_mem_nb++;
+        io_index = get_free_io_mem_idx();
+        if (io_index == -1)
+            return io_index;
     } else {
         if (io_index >= IO_MEM_NB_ENTRIES)
             return -1;
@@ -2847,6 +2863,19 @@ int cpu_register_io_memory(int io_index,
     return (io_index << IO_MEM_SHIFT) | subwidth;
 }
 
+void cpu_unregister_io_memory(int io_table_address)
+{
+    int i;
+    int io_index = io_table_address >> IO_MEM_SHIFT;
+
+    for (i=0;i < 3; i++) {
+        io_mem_read[io_index][i] = unassigned_mem_read[i];
+        io_mem_write[io_index][i] = unassigned_mem_write[i];
+    }
+    io_mem_opaque[io_index] = NULL;
+    io_mem_used[io_index] = 0;
+}
+
 CPUWriteMemoryFunc **cpu_get_io_memory_write(int io_index)
 {
     return io_mem_write[io_index >> IO_MEM_SHIFT];