diff --git a/lib/fpga/intc.c b/lib/fpga/intc.c
index 4ef2b2d91..bab5304ac 100644
--- a/lib/fpga/intc.c
+++ b/lib/fpga/intc.c
@@ -7,32 +7,44 @@
  **********************************************************************************/
 
 #include <unistd.h>
- 
+
+#include "config.h" 
 #include "log.h"
 #include "nodes/fpga.h"
 #include "kernel/vfio.h" 
+#include "kernel/kernel.h" 
 #include "fpga/ip.h"
 #include "fpga/intc.h"
 
 int intc_init(struct ip *c)
 {
-	struct fpga *f = c->card;
 	int ret;
 
+	struct fpga *f = c->card;
+	struct intc *intc = &c->intc;
+
+	uintptr_t base = (uintptr_t) f->map + c->baseaddr;
+
 	if (c != f->intc)
 		error("There can be only one interrupt controller per FPGA");
 
-	uintptr_t base = (uintptr_t) f->map + c->baseaddr;
+	intc->num_irqs = vfio_pci_msi_init(&f->vd, intc->efds);
+	if (intc->num_irqs < 0)
+		return -1;
+
+	ret = vfio_pci_msi_find(&f->vd, intc->nos);
+	if (ret)
+		return -2;
+
+	/* For each IRQ */
+	for (int i = 0; i < intc->num_irqs; i++) {
+		/* Pin to core */
+		ret = kernel_irq_setaffinity(intc->nos[i], DEFAULT_AFFINITY, NULL);
+		if (ret)
+			serror("Failed to change affinity of VFIO-MSI interrupt");
 
-	/* Setup IRQs */
-	for (int i = 0; i < f->vd.irqs[VFIO_PCI_MSI_IRQ_INDEX].count; i++) {
 		/* Setup vector */
 		XIntc_Out32(base + XIN_IVAR_OFFSET + i * 4, i);
-		
-		/* Register eventfd with VFIO */
-		ret = vfio_pci_msi_fd(&f->vd, (1 << i));
-		if (ret < 0)
-			serror("Failed to create eventfd for IRQ: ret=%d", f->vd.msi_efds[i]);
 	}
 
 	XIntc_Out32(base + XIN_IMR_OFFSET, 0);		/* Use manual acknowlegement for all IRQs */
@@ -46,9 +58,18 @@ int intc_init(struct ip *c)
 	return 0;
 }
 
-int intc_enable(struct ip *c, uint32_t mask, int poll)
+void intc_destroy(struct ip *c)
 {
 	struct fpga *f = c->card;
+	struct intc *intc = &c->intc;
+
+	vfio_pci_msi_deinit(&f->vd, intc->efds);
+}
+
+int intc_enable(struct ip *c, uint32_t mask, int flags)
+{
+	struct intc *intc = &c->intc;
+	struct fpga *f = c->card;
 
 	uint32_t ier, imr;
 	uintptr_t base = (uintptr_t) f->map + c->baseaddr;
@@ -60,12 +81,25 @@ int intc_enable(struct ip *c, uint32_t mask, int poll)
 	/* Clear pending IRQs */
 	XIntc_Out32(base + XIN_IAR_OFFSET, mask);
 	
-	if (poll)
-		XIntc_Out32(base + XIN_IMR_OFFSET, imr & ~mask);
-	else
-		XIntc_Out32(base + XIN_IER_OFFSET, ier | mask);
+	for (int i = 0; i < intc->num_irqs; i++) {
+		if (mask & (1 << i))
+			intc->flags[i] = flags;
+	}
 
-	debug(8, "FPGA: Interupt enabled: %#x", mask);
+	if (flags & INTC_POLLING) {
+		XIntc_Out32(base + XIN_IMR_OFFSET, imr & ~mask);
+		XIntc_Out32(base + XIN_IER_OFFSET, ier & ~mask);
+	}
+	else {
+		XIntc_Out32(base + XIN_IER_OFFSET, ier | mask);
+		XIntc_Out32(base + XIN_IMR_OFFSET, imr | mask);
+	}
+
+	debug(3, "New ier = %#x", XIntc_In32(base + XIN_IER_OFFSET));
+	debug(3, "New imr = %#x", XIntc_In32(base + XIN_IMR_OFFSET));
+	debug(3, "New isr = %#x", XIntc_In32(base + XIN_ISR_OFFSET));
+
+	debug(8, "FPGA: Interupt enabled: mask=%#x flags=%#x", mask, flags);
 
 	return 0;
 }
@@ -75,44 +109,46 @@ int intc_disable(struct ip *c, uint32_t mask)
 	struct fpga *f = c->card;
 
 	uintptr_t base = (uintptr_t) f->map + c->baseaddr;
+	uint32_t ier = XIntc_In32(base + XIN_IER_OFFSET);
 
-	XIntc_Out32(base + XIN_IER_OFFSET, XIntc_In32(base + XIN_IER_OFFSET) & ~mask);
+	XIntc_Out32(base + XIN_IER_OFFSET, ier & ~mask);
 
 	return 0;
 }
 
-uint64_t intc_wait(struct ip *c, int irq, int poll)
+uint64_t intc_wait(struct ip *c, int irq)
 {
+	struct intc *intc = &c->intc;
 	struct fpga *f = c->card;
 
 	uintptr_t base = (uintptr_t) f->map + c->baseaddr;
-	uint64_t cnt;
-	
-	if (poll) {
-		uint32_t mask = 1 << irq;
-		uint32_t isr;
 
-		do { /* Wait for IRQ to occur */
+	if (intc->flags[irq] & INTC_POLLING) {
+		uint32_t isr, mask = 1 << irq;
+
+		do {
 			isr = XIntc_In32(base + XIN_ISR_OFFSET);
-		} while (!(isr & mask));
+			pthread_testcancel();
+		} while ((isr & mask) != mask);
 
-		/* Acknowlege */
 		XIntc_Out32(base + XIN_IAR_OFFSET, mask);
-		
-		cnt = 1;
+
+		return 1;
 	}
 	else {
-		ssize_t ret = read(f->vd.msi_efds[irq], &cnt, sizeof(cnt));
+		uint64_t cnt;
+		ssize_t ret = read(intc->efds[irq], &cnt, sizeof(cnt));
 		if (ret != sizeof(cnt))
 			return 0;
+		
+		return cnt;
 	}
-
-	return cnt;
 }
 
 static struct ip_type ip = {
 	.vlnv = { "acs.eonerc.rwth-aachen.de", "user", "axi_pcie_intc", NULL },
-	.init = intc_init
+	.init = intc_init,
+	.destroy = intc_destroy
 };
 
 REGISTER_IP_TYPE(&ip)
diff --git a/lib/kernel/rt.c b/lib/kernel/rt.c
index e4b80757a..1343d36b7 100644
--- a/lib/kernel/rt.c
+++ b/lib/kernel/rt.c
@@ -17,14 +17,6 @@ int rt_init(int affinity, int priority)
 	char isolcpus[255];
 	int is_isol, is_rt, ret;
 
-	/* Pin interrupts to our core */
-/*	for (int i = 0; i < fpga->vd.irqs[VFIO_PCI_MSI_IRQ_INDEX].count; i++) {
-		ret = kernel_irq_setaffinity(fpga->vd.msi_irqs[i], AFFINITY, NULL);
-		if (ret)
-			serror("Failed to change affinity of VFIO-MSI interrupt");
-	}
-*/
-
 	/* Use FIFO scheduler with real time priority */
 	is_rt = kernel_is_rt();
 	if (is_rt)
diff --git a/lib/kernel/vfio.c b/lib/kernel/vfio.c
index be6c85a7d..2d2481afd 100644
--- a/lib/kernel/vfio.c
+++ b/lib/kernel/vfio.c
@@ -16,10 +16,8 @@
 #include <string.h>
 
 #include <sys/ioctl.h>
-#include <sys/mman.h>
 #include <sys/eventfd.h>
 
-
 #include "utils.h"
 #include "log.h"
 
@@ -29,6 +27,8 @@
 #include "kernel/vfio.h"
 #include "kernel/pci.h"
 
+#include "fpga/dma.h"
+
 static const char *vfio_pci_region_names[] = {
 	"PCI_BAR0",		// VFIO_PCI_BAR0_REGION_INDEX,
 	"PCI_BAR1",		// VFIO_PCI_BAR1_REGION_INDEX,
@@ -117,14 +117,6 @@ void vfio_dev_destroy(struct vfio_dev *d)
 	for (int i = 0; i < d->info.num_regions; i++)
 		vfio_unmap_region(d, i);
 
-	/* Check if this is really a vfio-pci device */
-	if (d->info.flags & VFIO_DEVICE_FLAGS_PCI) {
-		for (int i = 0; i < d->irqs[VFIO_PCI_MSI_IRQ_INDEX].count; i++)
-			close(d->msi_efds[i]);
-
-		free(d->msi_efds);
-	}
-
 	ret = close(d->fd);
 	if (ret)
 		return;
@@ -247,14 +239,6 @@ int vfio_pci_attach(struct vfio_dev *d, struct vfio_container *c, struct pci_dev
 
 	d->pdev = pdev;
 
-	/* Initialize MSI irqs */
-	d->msi_efds = alloc(sizeof(int) * d->irqs[VFIO_PCI_MSI_IRQ_INDEX].count);
-	d->msi_irqs = alloc(sizeof(int) * d->irqs[VFIO_PCI_MSI_IRQ_INDEX].count);
-	for (int i = 0; i < d->irqs[VFIO_PCI_MSI_IRQ_INDEX].count; i++) {
-		d->msi_efds[i] = -1;
-		d->msi_irqs[i] = -1;
-	}
-
 	return 0;
 }
 
@@ -368,24 +352,25 @@ int vfio_pci_reset(struct vfio_dev *d)
 	return ret;
 }
 
-int vfio_pci_msi_findirqs(struct vfio_dev *d)
+int vfio_pci_msi_find(struct vfio_dev *d, int nos[32])
 {
+	int ret, idx, irq;
+	char *end, *col, *last, line[1024], name[13];
 	FILE *f;
-	char *end, *col, *last, line[1024];
-	int ret, vector;
-	
+
 	f = fopen("/proc/interrupts", "r");
 	if (!f)
 		return -1;
 
-	/* Ignore header line */
-	fgets(line, sizeof(line), f);
+	for (int i = 0; i < 32; i++)
+		nos[i] = -1;
 
+	/* For each line in /proc/interruipts */
 	while (fgets(line, sizeof(line), f)) {
 		col = strtok(line, " ");
 
 		/* IRQ number is in first column */
-		int irq = strtol(col, &end, 10);
+		irq = strtol(col, &end, 10);
 		if (col == end)
 			continue;
 
@@ -393,79 +378,85 @@ int vfio_pci_msi_findirqs(struct vfio_dev *d)
 		while ((col = strtok(NULL, " ")))
 			last = col;
 
-		char name[13];
-		ret = sscanf(last, "vfio-msi[%u](%12[0-9:])", &vector, name);
-		if (ret == 2) { /* match was successful */
-			if (strstr(d->name, name) == d->name) /* device matches */
-				d->msi_irqs[vector] = irq;
+		ret = sscanf(last, "vfio-msi[%u](%12[0-9:])", &idx, name);
+		if (ret == 2) {
+			if (strstr(d->name, name) == d->name)
+				nos[idx] = irq;
 		}
 	}
-	
+
 	fclose(f);
 
 	return 0;
 }
 
-int vfio_pci_msi_fd(struct vfio_dev *d, uint32_t mask)
+int vfio_pci_msi_deinit(struct vfio_dev *d, int efds[32])
 {
-	int efd, ret;
-	struct vfio_irq_info *msi_irqs = &d->irqs[VFIO_PCI_MSI_IRQ_INDEX];
+	int ret, irq_setlen, irq_count = d->irqs[VFIO_PCI_MSI_IRQ_INDEX].count; 
 	struct vfio_irq_set *irq_set;
-	size_t irq_setlen;
-	
+
 	/* Check if this is really a vfio-pci device */
 	if (!(d->info.flags & VFIO_DEVICE_FLAGS_PCI))
 		return -1;
 
-	/* Check if already assigned */
-	for (int i = 0; i < MIN(sizeof(mask) * 8, msi_irqs->count); i++) {
-		if ((mask & (1 << i)) && (d->msi_efds[i] >= 0))
-			return -1;
-	}
-
-	/* Create new eventfd */
-	efd = eventfd(0, 0);
-	if (efd < 0)
-		return -3;
-
-	/* Assign new efd */
-	for (int i = 0; i < MIN(sizeof(mask) * 8, msi_irqs->count); i++) {
-		if (mask & (1 << i))
-			d->msi_efds[i] = efd;
-	}
-
-	irq_setlen = sizeof(struct vfio_irq_set) + msi_irqs->count * (sizeof(d->msi_efds[0]));
+	irq_setlen = sizeof(struct vfio_irq_set) + sizeof(int) * irq_count;
 	irq_set = alloc(irq_setlen);
 
 	irq_set->argsz = irq_setlen;
 	irq_set->flags = VFIO_IRQ_SET_DATA_EVENTFD | VFIO_IRQ_SET_ACTION_TRIGGER;
 	irq_set->index = VFIO_PCI_MSI_IRQ_INDEX;
-	irq_set->count = msi_irqs->count;
+	irq_set->count = irq_count;
 	irq_set->start = 0;
-	
-	/* We need to disable the complete MSI index first before adding new ones */
-	if (msi_irqs->flags & VFIO_IRQ_INFO_NORESIZE) {
-		memset(&irq_set->data[0], 0xFF, msi_irqs->count * sizeof(d->msi_efds[0]));
 
-		ret = ioctl(d->fd, VFIO_DEVICE_SET_IRQS, irq_set);
-		if (ret)
-			return -4;
+	for (int i = 0; i < irq_count; i++) {
+		close(efds[i]);
+		efds[i] = -1;
 	}
 
-	memcpy(&irq_set->data[0], d->msi_efds, msi_irqs->count * sizeof(d->msi_efds[0]));
+	memcpy(irq_set->data, efds, sizeof(int) * irq_count);
 
 	ret = ioctl(d->fd, VFIO_DEVICE_SET_IRQS, irq_set);
 	if (ret)
 		return -4;
 
-	/* Find corresponding Linux IRQ numbers */
-	ret = vfio_pci_msi_findirqs(d);
+	free(irq_set);
+
+	return irq_count;
+}
+
+int vfio_pci_msi_init(struct vfio_dev *d, int efds[32])
+{
+	int ret, irq_setlen, irq_count = d->irqs[VFIO_PCI_MSI_IRQ_INDEX].count; 
+	struct vfio_irq_set *irq_set;
+	
+	/* Check if this is really a vfio-pci device */
+	if (!(d->info.flags & VFIO_DEVICE_FLAGS_PCI))
+		return -1;
+
+	irq_setlen = sizeof(struct vfio_irq_set) + sizeof(int) * irq_count;
+	irq_set = alloc(irq_setlen);
+
+	irq_set->argsz = irq_setlen;
+	irq_set->flags = VFIO_IRQ_SET_DATA_EVENTFD | VFIO_IRQ_SET_ACTION_TRIGGER;
+	irq_set->index = VFIO_PCI_MSI_IRQ_INDEX;
+	irq_set->start = 0;
+	irq_set->count = irq_count;
+
+	/* Now set the new eventfds */
+	for (int i = 0; i < irq_count; i++) {
+		efds[i] = eventfd(0, 0);
+		if (efds[i] < 0)
+			return -3;
+	}
+	memcpy(irq_set->data, efds, sizeof(int) * irq_count);
+
+	ret = ioctl(d->fd, VFIO_DEVICE_SET_IRQS, irq_set);
 	if (ret)
-		return -5;
+		return -4;
 
 	free(irq_set);
 
-	return efd;
+	return irq_count;
 }
 
 int vfio_pci_enable(struct vfio_dev *d)
@@ -556,33 +547,44 @@ void * vfio_map_region(struct vfio_dev *d, int idx)
 	return d->mappings[idx];
 }
 
-void * vfio_map_dma(struct vfio_container *c, size_t size, size_t pgsize, uint64_t phyaddr)
+int vfio_unmap_region(struct vfio_dev *d, int idx)
 {
-	void *vaddr;
-	int ret, pgbits, flags;
-	size_t defpgsize;
-
-	defpgsize = sysconf(_SC_PAGESIZE);
-	if (!pgsize)
-		pgsize = defpgsize;
-
-	pgbits = 8 * sizeof(unsigned long long) - __builtin_clzll((unsigned long long) pgsize) - 1;
-
-	flags = MAP_PRIVATE | MAP_ANONYMOUS | MAP_32BIT;
-	if (pgsize != defpgsize) /* Map as Hugepages */
-		flags |= MAP_HUGETLB | (pgbits << MAP_HUGE_SHIFT);
-
-	vaddr = mmap(0, size, PROT_READ | PROT_WRITE, flags, 0, 0);
-	if (vaddr == MAP_FAILED)
-		return MAP_FAILED;
-
-	debug(3, "Allocated VM for DMA mapping at: %p", vaddr);
+	int ret;
+	struct vfio_region_info *r = &d->regions[idx];
 	
+	if (!d->mappings[idx])
+		return -1; /* was not mapped */
+	
+	debug(3, "VFIO: unmap region %u from device", idx);
+	
+	ret = munmap(d->mappings[idx], r->size);
+	if (ret)
+		return -2;
+	
+	d->mappings[idx] = NULL;
+
+	return 0;
+}
+
+int vfio_map_dma(struct vfio_container *c, struct dma_mem *mem)
+{
+	int ret;
+	
+	if (mem->len & 0xFFF) {
+		mem->len += 0x1000;
+		mem->len &= ~0xFFF;
+	}
+	
+	if (mem->base_phys == -1) {
+		mem->base_phys = c->iova_next;
+		c->iova_next += mem->len;
+	}
+
 	struct vfio_iommu_type1_dma_map dma_map = {
 		.argsz = sizeof(struct vfio_iommu_type1_dma_map),
-		.vaddr = (uint64_t) vaddr,
-		.size = size,
-		.iova = phyaddr,
+		.vaddr = (uint64_t) mem->base_virt,
+		.iova  = (uint64_t) mem->base_phys,
+		.size  = mem->len,
 		.flags = VFIO_DMA_MAP_FLAG_READ | VFIO_DMA_MAP_FLAG_WRITE
 	};
 
@@ -592,31 +594,23 @@ void * vfio_map_dma(struct vfio_container *c, size_t size, size_t pgsize, uint64
 
 	info("DMA map size=%#llx, iova=%#llx, vaddr=%#llx", dma_map.size, dma_map.iova, dma_map.vaddr);
 
-	return vaddr;
+	return 0;
 }
 
-int vfio_unmap_region(struct vfio_dev *d, int idx)
+int vfio_unmap_dma(struct vfio_container *c, struct dma_mem *mem)
 {
-	struct vfio_region_info *r = &d->regions[idx];
+	int ret;
 	
-	if (!d->mappings[idx])
-		return -1; /* was not mapped */
-	
-	debug(3, "VFIO: unmap region %u from device", idx);
-	
-	return munmap(d->mappings[idx], r->size);
-}
-
-int vfio_unmap_dma(struct vfio_container *c)
-{
 	struct vfio_iommu_type1_dma_unmap dma_unmap = {
 		.argsz = sizeof(struct vfio_iommu_type1_dma_unmap),
 		.flags = 0,
-		.iova = 0, /* IO virtual address */
-		.size = 0  /* Size of mapping (bytes) */
+		.iova  = (uint64_t) mem->base_phys,
+		.size  = mem->len,
 	};
-
-	debug(3, "VFIO: unmap DMA region from device");
 	
-	return ioctl(c->fd, VFIO_IOMMU_UNMAP_DMA, &dma_unmap);
+	ret = ioctl(c->fd, VFIO_IOMMU_UNMAP_DMA, &dma_unmap);
+	if (ret)
+		serror("Failed to unmap DMA mapping");
+	
+	return 0;
 }
diff --git a/lib/nodes/fpga.c b/lib/nodes/fpga.c
index 8125c61f7..59dac729e 100644
--- a/lib/nodes/fpga.c
+++ b/lib/nodes/fpga.c
@@ -49,7 +49,11 @@ int fpga_reset(struct fpga *f)
 	if (ret != sizeof(state))
 		return -1;
 
-	return  rst_reg[0];
+	/* After reset the value should be zero again */
+	if (rst_reg[0])
+		return -2;
+
+	return 0;
 }
 
 void fpga_dump(struct fpga *f)
@@ -71,7 +75,6 @@ void fpga_dump(struct fpga *f)
 		info("Class  ID: %04x", fpga.vd.pdev->device_class);
 
 		info("BAR0 mapped at %p", fpga.map);
-		info("DMA mapped at %p", fpga.dma);
 
 		info("IP blocks:");
 		list_foreach(struct ip *i, &f->ips) { INDENT
@@ -166,7 +169,7 @@ int fpga_init(int argc, char * argv[], config_setting_t *cfg)
 	ret = fpga_parse_card(f, argc, argv, cfg);
 	if (ret)
 		cerror(cfg, "Failed to parse VILLASfpga config");
-	
+
 	/* Check FPGA configuration */
 	f->reset = ip_vlnv_lookup(&f->ips, "xilinx.com", "ip", "axi_gpio", NULL);
 	if (!f->reset)
@@ -199,27 +202,22 @@ int fpga_init(int argc, char * argv[], config_setting_t *cfg)
 	f->map = vfio_map_region(&f->vd, VFIO_PCI_BAR0_REGION_INDEX);
 	if (f->map == MAP_FAILED)
 		serror("Failed to mmap() BAR0");
-	
-	/* Map DMA accessible memory */
-	f->dma = vfio_map_dma(f->vd.group->container, 16 << 20, 0x1000, 0x0);
-	if (f->dma == MAP_FAILED)
-		serror("Failed to mmap() DMA");
 
 	/* Enable memory access and PCI bus mastering for DMA */
 	ret = vfio_pci_enable(&f->vd);
 	if (ret)
 		serror("Failed to enable PCI device");
 	
-	/* Reset system ? */
+	/* Reset system? */
 	if (f->do_reset) {
-		ret = fpga_reset(f);
-		if (ret)
-			error("Failed to reset FGPA card");
-		
 		/* Reset / detect PCI device */
 		ret = vfio_pci_reset(&f->vd);
 		if (ret)
 			serror("Failed to reset PCI device");
+
+		ret = fpga_reset(f);
+		if (ret)
+			error("Failed to reset FGPA card");
 	}
 
 	/* Initialize IP cores */
@@ -290,6 +288,7 @@ int fpga_get_type(struct ip *c)
 int fpga_open(struct node *n)
 {
 	int ret;
+
 	struct fpga_dm *d = n->_vd;
 	struct fpga *f = d->card;
 
@@ -301,35 +300,25 @@ int fpga_open(struct node *n)
 	if (d->type < 0)
 		cerror(n->cfg, "IP '%s' is not a supported datamover", d->ip->name);
 
+	switch_init_paths(f->sw);
+
+	int flags = 0;
+	if (!d->use_irqs)
+		flags |= INTC_POLLING;
+
 	switch (d->type) {
 		case FPGA_DM_DMA:
-			XAxiDma_Reset(&d->ip->dma.inst);
+			/* Map DMA accessible memory */
+			ret = dma_alloc(d->ip, &d->dma, 0x1000, 0);
+			if (ret)
+				return ret;
 
-			if (d->ip->dma.inst.HasSg) {
-				struct ip *bram = ip_vlnv_lookup(&f->ips, "xilinx.com", "ip", "axi_bram_ctrl", NULL);
-				if (!bram)
-					return -3;
-
-				/* Memory for buffer descriptors */
-				struct dma_mem bd = {
-					.base_virt = (uintptr_t) f->map + bram->baseaddr,
-					.base_phys = bram->baseaddr,
-					.len = bram->bram.size
-				};
-
-				ret = dma_init_rings(d->ip, &bd);
-				if (ret)
-					return -4;
-			}
-			
-			intc_enable(f->intc, (1 <<  d->ip->irq),      !d->use_irqs); /* MM2S */
-			intc_enable(f->intc, (1 << (d->ip->irq + 1)), !d->use_irqs); /* S2MM */
+			intc_enable(f->intc, (1 << (d->ip->irq    )), flags); /* MM2S */
+			intc_enable(f->intc, (1 << (d->ip->irq + 1)), flags); /* S2MM */
 			break;
 
 		case FPGA_DM_FIFO:
-			XLlFifo_Reset(&d->ip->fifo.inst);
-
-			intc_enable(f->intc, (1 << d->ip->irq),      !d->use_irqs);	/* MM2S & S2MM */
+			intc_enable(f->intc, (1 << d->ip->irq),      flags);	/* MM2S & S2MM */
 			break;
 	}
 	
@@ -339,16 +328,20 @@ int fpga_open(struct node *n)
 
 int fpga_close(struct node *n)
 {
+	int ret;
+
 	struct fpga_dm *d = n->_vd;
 	struct fpga *f = d->card;
-	
+
 	switch (d->type) {
 		case FPGA_DM_DMA:
-			if (d->use_irqs) {
-				intc_disable(f->intc, d->ip->irq);	/* MM2S */
-				intc_disable(f->intc, d->ip->irq + 1);	/* S2MM */
-			}
-		
+			intc_disable(f->intc, d->ip->irq);	/* MM2S */
+			intc_disable(f->intc, d->ip->irq + 1);	/* S2MM */
+
+			ret = dma_free(d->ip, &d->dma);
+			if (ret)
+				return ret;
+
 		case FPGA_DM_FIFO:
 			if (d->use_irqs)
 				intc_disable(f->intc, d->ip->irq);	/* MM2S & S2MM */
@@ -359,13 +352,13 @@ int fpga_close(struct node *n)
 
 int fpga_read(struct node *n, struct sample *smps[], unsigned cnt)
 {
+	int ret;
+
 	struct fpga_dm *d = n->_vd;
-	struct fpga *f = d->card;
 	struct sample *smp = smps[0];
 
-	int ret;
-	char *tmp = f->dma, *addr = (char *) smp->values;
 	size_t recvlen;
+
 	//size_t len = smp->length * sizeof(smp->values[0]);
 	size_t len = 64 * sizeof(smp->values[0]);
 	
@@ -376,7 +369,7 @@ int fpga_read(struct node *n, struct sample *smps[], unsigned cnt)
 	/* Read data from RTDS */
 	switch (d->type) {
 		case FPGA_DM_DMA:
-			ret = dma_read(d->ip, tmp, len);
+			ret = dma_read(d->ip, d->dma.base_phys + 0x800, len);
 			if (ret)
 				return ret;
 			
@@ -384,12 +377,12 @@ int fpga_read(struct node *n, struct sample *smps[], unsigned cnt)
 			if (ret)
 				return ret;
 
-			memcpy(addr, tmp, recvlen);
+			memcpy(smp->values, d->dma.base_virt + 0x800, recvlen);
 
 			smp->length = recvlen / 4;
 			return 1;
 		case FPGA_DM_FIFO:
-			recvlen = fifo_read(d->ip, addr, len);
+			recvlen = fifo_read(d->ip, (char *) smp->values, len);
 			
 			smp->length = recvlen / 4;
 			return 1;
@@ -402,11 +395,8 @@ int fpga_write(struct node *n, struct sample *smps[], unsigned cnt)
 {
 	int ret;
 	struct fpga_dm *d = n->_vd;
-	struct fpga *f = d->card;
 	struct sample *smp = smps[0];
 
-	char *tmp = f->dma + 0x1000;
-	char *addr = (char *) smp->values;
 	size_t sentlen;
 	size_t len = smp->length * sizeof(smp->values[0]);
 
@@ -420,9 +410,9 @@ int fpga_write(struct node *n, struct sample *smps[], unsigned cnt)
 	/* Send data to RTDS */
 	switch (d->type) {
 		case FPGA_DM_DMA:
-			memcpy(tmp, addr, len);
+			memcpy(d->dma.base_virt, smp->values, len);
 
-			ret = dma_write(d->ip, tmp, len);
+			ret = dma_write(d->ip, d->dma.base_phys, len);
 			if (ret)
 				return ret;
 			
@@ -434,7 +424,8 @@ int fpga_write(struct node *n, struct sample *smps[], unsigned cnt)
 
 			return 1;
 		case FPGA_DM_FIFO:
-			sentlen = fifo_write(d->ip, addr, len);
+			sentlen = fifo_write(d->ip, (char *) smp->values, len);
+			return sentlen / sizeof(smp->values[0]);
 			break;
 	}