[RFC 05/12] kvm tools: Fixes for ioport module

[Sasha Levin <levinsasha928@gmail.com>]

Fixes include:
 - Error handling
 - Cleanup
 - Standard init/uninit

Signed-off-by: Sasha Levin <levinsasha928@gmail.com>
---
 tools/kvm/builtin-run.c                 |   11 ++++++
 tools/kvm/hw/pci-shmem.c                |    7 +++-
 tools/kvm/hw/vesa.c                     |    6 +++-
 tools/kvm/include/kvm/ioport.h          |    5 ++-
 tools/kvm/include/kvm/rbtree-interval.h |    1 +
 tools/kvm/ioport.c                      |   60 +++++++++++++++++++++++++++++-
 tools/kvm/util/rbtree-interval.c        |    2 -
 tools/kvm/virtio/pci.c                  |    7 +++-
 8 files changed, 91 insertions(+), 8 deletions(-)

diff --git a/tools/kvm/builtin-run.c b/tools/kvm/builtin-run.c
index 37bc3f2..5388171 100644
--- a/tools/kvm/builtin-run.c
+++ b/tools/kvm/builtin-run.c
@@ -986,6 +986,12 @@ static int kvm_cmd_run_init(int argc, const char **argv)
 
 	pci__init();
 
+	r = ioport__init(kvm);
+	if (r < 0) {
+		pr_error("ioport__init() failed with error %d\n", r);
+		goto fail;
+	}
+
 	/*
 	 * vidmode should be either specified
 	 * either set by default
@@ -1205,6 +1211,11 @@ static void kvm_cmd_run_uninit(int guest_ret)
 	virtio_rng__delete_all(kvm);
 
 	disk_image__close_all(kvm->disks, image_count);
+
+	r = ioport__uninit(kvm);
+	if (r < 0)
+		pr_warning("ioport__uninit() failed with error %d\n", r);
+
 	kvm__delete(kvm);
 
 	if (guest_ret == 0)
diff --git a/tools/kvm/hw/pci-shmem.c b/tools/kvm/hw/pci-shmem.c
index 6130131..8bac151 100644
--- a/tools/kvm/hw/pci-shmem.c
+++ b/tools/kvm/hw/pci-shmem.c
@@ -219,6 +219,7 @@ int pci_shmem__init(struct kvm *kvm)
 {
 	u8 dev, line, pin;
 	char *mem;
+	int r;
 
 	if (shmem_region == 0)
 		return 0;
@@ -231,7 +232,11 @@ int pci_shmem__init(struct kvm *kvm)
 	pci_shmem_pci_device.irq_line = line;
 
 	/* Register MMIO space for MSI-X */
-	ivshmem_registers = ioport__register(IOPORT_EMPTY, &shmem_pci__io_ops, IOPORT_SIZE, NULL);
+	r = ioport__register(IOPORT_EMPTY, &shmem_pci__io_ops, IOPORT_SIZE, NULL);
+	if (r < 0)
+		return r;
+	ivshmem_registers = (u16)r;
+
 	msix_block = pci_get_io_space_block(0x1010);
 	kvm__register_mmio(kvm, msix_block, 0x1010, false, callback_mmio_msix, NULL);
 
diff --git a/tools/kvm/hw/vesa.c b/tools/kvm/hw/vesa.c
index 2c0101f..757f0a2 100644
--- a/tools/kvm/hw/vesa.c
+++ b/tools/kvm/hw/vesa.c
@@ -57,9 +57,13 @@ struct framebuffer *vesa__init(struct kvm *kvm)
 	if (r < 0)
 		return ERR_PTR(r);
 
+	r = ioport__register(IOPORT_EMPTY, &vesa_io_ops, IOPORT_SIZE, NULL);
+	if (r < 0)
+		return ERR_PTR(r);
+
 	vesa_pci_device.irq_pin		= pin;
 	vesa_pci_device.irq_line	= line;
-	vesa_base_addr			= ioport__register(IOPORT_EMPTY, &vesa_io_ops, IOPORT_SIZE, NULL);
+	vesa_base_addr			= (u16)r;
 	vesa_pci_device.bar[0]		= cpu_to_le32(vesa_base_addr | PCI_BASE_ADDRESS_SPACE_IO);
 	pci__register(&vesa_pci_device, dev);
 
diff --git a/tools/kvm/include/kvm/ioport.h b/tools/kvm/include/kvm/ioport.h
index 09bf876..f47b347 100644
--- a/tools/kvm/include/kvm/ioport.h
+++ b/tools/kvm/include/kvm/ioport.h
@@ -31,7 +31,10 @@ struct ioport_operations {
 
 void ioport__setup_arch(void);
 
-u16 ioport__register(u16 port, struct ioport_operations *ops, int count, void *param);
+int ioport__register(u16 port, struct ioport_operations *ops, int count, void *param);
+int ioport__unregister(u16 port);
+int ioport__init(struct kvm *kvm);
+int ioport__uninit(struct kvm *kvm);
 
 static inline u8 ioport__read8(u8 *data)
 {
diff --git a/tools/kvm/include/kvm/rbtree-interval.h b/tools/kvm/include/kvm/rbtree-interval.h
index a6688c4..13245ba 100644
--- a/tools/kvm/include/kvm/rbtree-interval.h
+++ b/tools/kvm/include/kvm/rbtree-interval.h
@@ -5,6 +5,7 @@
 #include <linux/types.h>
 
 #define RB_INT_INIT(l, h) (struct rb_int_node){.low = l, .high = h}
+#define rb_int(n) rb_entry(n, struct rb_int_node, node)
 
 struct rb_int_node {
 	struct rb_node	node;
diff --git a/tools/kvm/ioport.c b/tools/kvm/ioport.c
index b417942..19caf6c 100644
--- a/tools/kvm/ioport.c
+++ b/tools/kvm/ioport.c
@@ -52,7 +52,12 @@ static int ioport_insert(struct rb_root *root, struct ioport *data)
 	return rb_int_insert(root, &data->node);
 }
 
-u16 ioport__register(u16 port, struct ioport_operations *ops, int count, void *param)
+static void ioport_remove(struct rb_root *root, struct ioport *data)
+{
+	rb_int_erase(root, &data->node);
+}
+
+int ioport__register(u16 port, struct ioport_operations *ops, int count, void *param)
 {
 	struct ioport *entry;
 
@@ -68,7 +73,7 @@ u16 ioport__register(u16 port, struct ioport_operations *ops, int count, void *p
 
 	entry = malloc(sizeof(*entry));
 	if (entry == NULL)
-		die("Failed allocating new ioport entry");
+		return -ENOMEM;
 
 	*entry = (struct ioport) {
 		.node	= RB_INT_INIT(port, port + count),
@@ -83,6 +88,46 @@ u16 ioport__register(u16 port, struct ioport_operations *ops, int count, void *p
 	return port;
 }
 
+int ioport__unregister(u16 port)
+{
+	struct ioport *entry;
+	int r;
+
+	br_write_lock();
+
+	r = -ENOENT;
+	entry = ioport_search(&ioport_tree, port);
+	if (!entry)
+		goto done;
+
+	ioport_remove(&ioport_tree, entry);
+
+	free(entry);
+
+	r = 0;
+
+done:
+	br_write_unlock();
+
+	return r;
+}
+
+static void ioport__unregister_all(void)
+{
+	struct ioport *entry;
+	struct rb_node *rb;
+	struct rb_int_node *rb_node;
+
+	rb = rb_first(&ioport_tree);
+	while (rb) {
+		rb_node = rb_int(rb);
+		entry = ioport_node(rb_node);
+		ioport_remove(&ioport_tree, entry);
+		free(entry);
+		rb = rb_first(&ioport_tree);
+	}
+}
+
 static const char *to_direction(int direction)
 {
 	if (direction == KVM_EXIT_IO_IN)
@@ -133,3 +178,14 @@ error:
 
 	return !ioport_debug;
 }
+
+int ioport__init(struct kvm *kvm)
+{
+	return 0;
+}
+
+int ioport__uninit(struct kvm *kvm)
+{
+	ioport__unregister_all();
+	return 0;
+}
\ No newline at end of file
diff --git a/tools/kvm/util/rbtree-interval.c b/tools/kvm/util/rbtree-interval.c
index d02fbf0..edc140d 100644
--- a/tools/kvm/util/rbtree-interval.c
+++ b/tools/kvm/util/rbtree-interval.c
@@ -1,8 +1,6 @@
 #include <kvm/rbtree-interval.h>
 #include <stddef.h>
 
-#define rb_int(n) rb_entry(n, struct rb_int_node, node)
-
 struct rb_int_node *rb_int_search_single(struct rb_root *root, u64 point)
 {
 	struct rb_node *node = root->rb_node;
diff --git a/tools/kvm/virtio/pci.c b/tools/kvm/virtio/pci.c
index 3051314..2643d8a 100644
--- a/tools/kvm/virtio/pci.c
+++ b/tools/kvm/virtio/pci.c
@@ -284,11 +284,16 @@ int virtio_pci__init(struct kvm *kvm, struct virtio_trans *vtrans, void *dev,
 {
 	struct virtio_pci *vpci = vtrans->virtio;
 	u8 pin, line, ndev;
+	int r;
 
 	vpci->dev = dev;
 	vpci->msix_io_block = pci_get_io_space_block(PCI_IO_SIZE * 2);
 
-	vpci->base_addr = ioport__register(IOPORT_EMPTY, &virtio_pci__io_ops, IOPORT_SIZE, vtrans);
+	r = ioport__register(IOPORT_EMPTY, &virtio_pci__io_ops, IOPORT_SIZE, vtrans);
+	if (r < 0)
+		return r;
+
+	vpci->base_addr = (u16)r;
 	kvm__register_mmio(kvm, vpci->msix_io_block, PCI_IO_SIZE, false, callback_mmio_table, vpci);
 
 	vpci->pci_hdr = (struct pci_device_header) {
-- 
1.7.8