diff --git a/include/villas/fpga/dft.h b/include/villas/fpga/dft.h
new file mode 100644
index 000000000..017117c99
--- /dev/null
+++ b/include/villas/fpga/dft.h
@@ -0,0 +1,34 @@
+/** Moving window / Recursive DFT implementation based on HLS
+ *
+ * @file
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2015-2016, Steffen Vogel
+ *   This file is part of S2SS. All Rights Reserved. Proprietary and confidential.
+ *   Unauthorized copying of this file, via any medium is strictly prohibited.
+ **********************************************************************************/
+ 
+#ifndef _DFT_H_
+#define _DFT_H_
+
+#include <xilinx/xhls_dft.h>
+
+/* Forward declaration */
+struct ip;
+
+struct dft {
+	XHls_dft inst;
+	
+	int period; /* in samples */
+	int num_harmonics;
+	float *fharmonics;
+	int decimation;
+};
+
+int dft_parse(struct ip *c);
+
+int dft_init(struct ip *c);
+
+void dft_destroy(struct ip *c);
+
+
+#endif /* _DFT_H_ */
\ No newline at end of file
diff --git a/include/villas/fpga/dma.h b/include/villas/fpga/dma.h
index b3cd4defd..bf94399e9 100644
--- a/include/villas/fpga/dma.h
+++ b/include/villas/fpga/dma.h
@@ -20,6 +20,10 @@
 
 #define XAXIDMA_SR_SGINCL_MASK	0x00000008
 
+struct dma {
+	XAxiDma inst;
+};
+
 struct dma_mem {
 	uint32_t base_virt;
 	uint32_t base_phys;
diff --git a/include/villas/fpga/fifo.h b/include/villas/fpga/fifo.h
index 63f489375..597cd9a04 100644
--- a/include/villas/fpga/fifo.h
+++ b/include/villas/fpga/fifo.h
@@ -17,6 +17,12 @@
 #include <xilinx/xstatus.h>
 #include <xilinx/xllfifo.h>
 
+struct fifo {
+	XLlFifo inst;
+	
+	uint32_t baseaddr_axi4;
+};
+
 /* Forward declaration */
 struct ip;
 
diff --git a/include/villas/fpga/intc.h b/include/villas/fpga/intc.h
index 8e0a20d66..65784f081 100644
--- a/include/villas/fpga/intc.h
+++ b/include/villas/fpga/intc.h
@@ -9,12 +9,14 @@
 #ifndef _INTC_H_
 #define _INTC_H_
 
+#include <xilinx/xintc.h>
+
 int intc_init(struct ip *c);
 
-uint32_t intc_enable(struct ip *c, uint32_t mask);
+int intc_enable(struct ip *c, uint32_t mask, int poll);
 
-void intc_disable(struct ip *c, uint32_t mask);
+int intc_disable(struct ip *c, uint32_t mask);
 
-uint64_t intc_wait(struct fpga *f, int irq);
+uint64_t intc_wait(struct ip *c, int irq, int poll);
 
 #endif /* _INTC_H_ */
\ No newline at end of file
diff --git a/include/villas/fpga/ip.h b/include/villas/fpga/ip.h
index c91b845ef..3eb2d9fd4 100644
--- a/include/villas/fpga/ip.h
+++ b/include/villas/fpga/ip.h
@@ -9,45 +9,69 @@
 #include <xilinx/xaxis_switch.h>
 #include <xilinx/xaxidma.h>
 
+#include "utils.h"
 #include "fpga/dma.h"
 #include "fpga/switch.h"
 #include "fpga/fifo.h"
 #include "fpga/rtds_axis.h"
+#include "fpga/timer.h"
 #include "fpga/model.h"
+#include "fpga/dft.h"
 #include "nodes/fpga.h"
 
+#define REGISTER_IP_TYPE(ip)			\
+__attribute__((constructor)) static		\
+void UNIQUE(__register_)() {		\
+	list_push(&ip_types, ip);		\
+}
+
+extern struct list ip_types;	/**< Table of existing FPGA IP core drivers */
+
 enum ip_state {
 	IP_STATE_UNKNOWN,
 	IP_STATE_INITIALIZED
 };
 
+struct ip_vlnv {
+	char *vendor;
+	char *library;
+	char *name;
+	char *version;
+};
+
+struct ip_type {
+	struct ip_vlnv vlnv;
+
+	int (*parse)(struct ip *c);
+	int (*init)(struct ip *c);
+	void (*dump)(struct ip *c);
+	void (*destroy)(struct ip *c);
+};
+
 struct ip {
 	char *name;
-	
-	struct {
-		char *vendor;
-		char *library;
-		char *name;
-		char *version;
-	} vlnv;
+
+	struct ip_vlnv vlnv;
 
 	uintptr_t baseaddr;
 	uintptr_t baseaddr_axi4;
-	int irq;
-	int port;
+
+	int port, irq;
 
 	enum ip_state state;
 
+	struct ip_type *_vt;
+
 	union {
 		struct model model;
-		struct bram {
+		struct {
 			int size;
 		} bram;
-
-		XTmrCtr timer;
-		XLlFifo fifo_mm;
-		XAxiDma dma;
-		XAxis_Switch sw;
+		struct timer timer;
+		struct fifo fifo;
+		struct dma dma;
+		struct sw sw;
+		struct dft dft;
 	};
 	
 	struct fpga *card;
diff --git a/include/villas/fpga/model.h b/include/villas/fpga/model.h
index 610a8b0a4..f4269046c 100644
--- a/include/villas/fpga/model.h
+++ b/include/villas/fpga/model.h
@@ -54,7 +54,7 @@ union model_param_value {
 
 struct model {
 	enum model_type type;		/**< Either HLS or XSG model */
-	char *xml;			/**< An optional path to a XML file which describes this model */
+
 	struct list parameters;		/**< List of model parameters. */
 	struct list infos;		/**< A list of key / value pairs with model details */
 
@@ -91,6 +91,9 @@ struct model_param {
 /** Initialize a model */
 int model_init(struct ip *c);
 
+/** Parse model */
+int model_parse(struct ip *c);
+
 /** Destroy a model */
 void model_destroy(struct ip *c);
 
@@ -118,4 +121,6 @@ int model_param_read(struct model_param *p, double *v);
  */
 int model_param_write(struct model_param *p, double v);
 
+int model_param_update(struct model_param *p, struct model_param *u);
+
 #endif /* _MODEL_H_ */
\ No newline at end of file
diff --git a/include/villas/fpga/rtds_axis.h b/include/villas/fpga/rtds_axis.h
index ea2fea4d1..85bf10f32 100644
--- a/include/villas/fpga/rtds_axis.h
+++ b/include/villas/fpga/rtds_axis.h
@@ -10,6 +10,9 @@
 #ifndef _RTDS_AXIS_H_
 #define _RTDS_AXIS_H_
 
+/* Forward declaration */
+struct ip;
+
 #define RTDS_HZ				100000000 // 100 MHz
 
 #define RTDS_AXIS_MAX_TX		64	/**< The amount of values which is supported by the vfpga card */
@@ -23,8 +26,7 @@
 #define RTDS_AXIS_TS_PERIOD_OFFSET	0x10	/**< Period in clock cycles of previous timestep (read-only). */
 #define RTDS_AXIS_COALESC_OFFSET	0x14	/**< IRQ Coalescing register (read/write). */
 #define RTDS_AXIS_VERSION_OFFSET	0x18	/**< 16 bit version field passed back to the rack for version reporting (visible from “status” command, read/write). */
-#define RTDS_AXIS_MRATE_RTDS2AXIS	0x1C	/**< */
-#define RTDS_AXIS_MRATE_AXIS2RTDS	0x20	/**< */
+#define RTDS_AXIS_MRATE			0x1C	/**< Multi-rate register */
 
 /* Status register bits */
 #define RTDS_AXIS_SR_CARDDETECTED	(1 << 0)/**< ‘1’ when RTDS software has detected and configured card. */
@@ -36,8 +38,8 @@
 /* Control register bits */
 #define RTDS_AXIS_CR_DISABLE_LINK	0	/**< Disable SFP TX when set */
 
-void rtds_axis_dump(char *baseaddr);
+void rtds_axis_dump(struct ip *c);
 
-double rtds_axis_dt(char *baseaddr);
+double rtds_axis_dt(struct ip *c);
 
 #endif /* _RTDS_AXIS_H_ */
\ No newline at end of file
diff --git a/include/villas/fpga/switch.h b/include/villas/fpga/switch.h
index 38c0a069e..1d9326512 100644
--- a/include/villas/fpga/switch.h
+++ b/include/villas/fpga/switch.h
@@ -14,10 +14,33 @@
 
 #include <xilinx/xaxis_switch.h>
 
+#include "list.h"
+
+/* Forward declaration */
+struct ip;
+
+struct sw_path {
+	const char *in;
+	const char *out;
+};
+
+struct sw {
+	XAxis_Switch inst;
+
+	int num_ports;
+	struct list paths;
+};
+
 struct ip;
 
 int switch_init(struct ip *c);
 
+void switch_destroy(struct ip *c);
+
+int switch_parse(struct ip *c);
+
 int switch_connect(struct ip *c, struct ip *mi, struct ip *si);
 
+int switch_disconnect(struct ip *c, struct ip *mi, struct ip *si);
+
 #endif /* _SWITCH_H_ */
\ No newline at end of file
diff --git a/include/villas/fpga/timer.h b/include/villas/fpga/timer.h
index e13212102..b818c0058 100644
--- a/include/villas/fpga/timer.h
+++ b/include/villas/fpga/timer.h
@@ -12,4 +12,8 @@
 #ifndef _TIMER_H_
 #define _TIMER_H_
 
+struct timer {
+	XTmrCtr inst;
+};
+
 #endif
\ No newline at end of file
diff --git a/include/villas/nodes/fpga.h b/include/villas/nodes/fpga.h
index 1b049a6d5..3b7a88267 100644
--- a/include/villas/nodes/fpga.h
+++ b/include/villas/nodes/fpga.h
@@ -50,12 +50,15 @@ struct fpga {
 	struct ip *intc;
 	struct ip *reset;
 	struct ip *sw;
+	
+	config_setting_t *cfg;
 };
 
 struct fpga_dm {
 	struct ip *ip;
+	const char *ip_name;
 
-	bool use_irqs;
+	int use_irqs;
 
 	enum {
 		FPGA_DM_DMA,
diff --git a/lib/fpga/bram.c b/lib/fpga/bram.c
new file mode 100644
index 000000000..dc9caa30a
--- /dev/null
+++ b/lib/fpga/bram.c
@@ -0,0 +1,32 @@
+/** Block RAM
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2015-2016, Steffen Vogel
+ *   This file is part of S2SS. All Rights Reserved. Proprietary and confidential.
+ *   Unauthorized copying of this file, via any medium is strictly prohibited.
+ **********************************************************************************/
+
+#include "fpga/ip.h"
+#include "fpga/timer.h"
+
+int bram_parse(struct ip *c)
+{
+	int size;
+
+	if (config_setting_lookup_int(c->cfg, "size", &size))
+		c->bram.size = size;
+	else
+		cerror(c->cfg, "BRAM IP core requires 'size' setting");
+
+	if (size <= 0)
+		error("BRAM IP core has invalid size: %d", size);
+
+	return 0;
+}
+
+static struct ip_type ip = {
+	.vlnv = { "xilinx.com", "ip", "axi_bram_ctrl", NULL },
+	.parse = bram_parse
+};
+
+REGISTER_IP_TYPE(&ip)
\ No newline at end of file
diff --git a/lib/fpga/dft.c b/lib/fpga/dft.c
new file mode 100644
index 000000000..8cbfefa85
--- /dev/null
+++ b/lib/fpga/dft.c
@@ -0,0 +1,95 @@
+/** Moving window / Recursive DFT implementation based on HLS
+ *
+ * @file
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2015-2016, Steffen Vogel
+ *   This file is part of S2SS. All Rights Reserved. Proprietary and confidential.
+ *   Unauthorized copying of this file, via any medium is strictly prohibited.
+ **********************************************************************************/
+
+#include "log.h"
+#include "fpga/ip.h"
+#include "fpga/dft.h"
+
+int dft_parse(struct ip *c)
+{
+	struct dft *dft = &c->dft;
+
+	config_setting_t *cfg_harms;
+	
+	if (!config_setting_lookup_int(c->cfg, "period", &dft->period))
+		cerror(c->cfg, "DFT IP core requires 'period' setting");
+	
+	if (!config_setting_lookup_int(c->cfg, "decimation", &dft->decimation))
+		cerror(c->cfg, "DFT IP core requires 'decimation' setting");
+
+	cfg_harms = config_setting_get_member(c->cfg, "harmonics");
+	if (!cfg_harms)
+		cerror(c->cfg, "DFT IP core requires 'harmonics' setting!");
+	
+	if (!config_setting_is_array(cfg_harms))
+		cerror(c->cfg, "DFT IP core requires 'harmonics' to be an array of integers!");
+	
+	dft->num_harmonics = config_setting_length(cfg_harms);
+	if (dft->num_harmonics <= 0)
+		cerror(cfg_harms, "DFT IP core requires 'harmonics' to contain at least 1 integer!");
+	
+	dft->fharmonics = alloc(sizeof(float) * dft->num_harmonics);
+
+	for (int i = 0; i < dft->num_harmonics; i++)
+		dft->fharmonics[i] = (float) config_setting_get_int_elem(cfg_harms, i) / dft->period;
+
+	return 0;
+}
+
+int dft_init(struct ip *c)
+{
+	int ret;
+	struct dft *dft = &c->dft;
+	XHls_dft *xdft = &dft->inst;
+	XHls_dft_Config xdft_cfg = {
+		.Ctrl_BaseAddress = (uintptr_t) c->card->map + c->baseaddr
+	};
+
+	ret = XHls_dft_CfgInitialize(xdft, &xdft_cfg);
+	if (ret != XST_SUCCESS)
+		return ret;
+
+	int max_harmonics = XHls_dft_Get_fharmonics_TotalBytes(xdft) / sizeof(dft->fharmonics[0]);
+
+	if (dft->num_harmonics > max_harmonics)
+		error("DFT IP core supports a maximum of %u harmonics", max_harmonics);
+
+	XHls_dft_Set_num_harmonics_V(xdft, dft->num_harmonics);
+	
+	XHls_dft_Set_decimation_V(xdft, dft->decimation);
+
+	memcpy((void *) (uintptr_t) XHls_dft_Get_fharmonics_BaseAddress(xdft), dft->fharmonics, dft->num_harmonics * sizeof(dft->fharmonics[0]));
+
+	XHls_dft_EnableAutoRestart(xdft);
+	XHls_dft_Start(xdft);
+	
+	return 0;
+}
+
+void dft_destroy(struct ip *c)
+{
+	struct dft *dft = &c->dft;
+	XHls_dft *xdft = &dft->inst;
+
+	XHls_dft_DisableAutoRestart(xdft);
+
+	if (dft->fharmonics) {
+		free(dft->fharmonics);
+		dft->fharmonics = NULL;
+	}
+}
+
+static struct ip_type ip = {
+	.vlnv = { "acs.eonerc.rwth-aachen.de", "hls", "hls_dft", NULL },
+	.init = dft_init,
+	.destroy = dft_destroy,
+	.parse = dft_parse
+};
+
+REGISTER_IP_TYPE(&ip)
\ No newline at end of file
diff --git a/lib/fpga/dma.c b/lib/fpga/dma.c
index 9d1f243ba..f34baf9dd 100644
--- a/lib/fpga/dma.c
+++ b/lib/fpga/dma.c
@@ -10,6 +10,7 @@
 
 #include <sys/types.h>
 #include <unistd.h>
+#include <pthread.h>
 
 #include <villas/log.h>
 
@@ -42,7 +43,7 @@ int dma_ping_pong(struct ip *c, char *src, char *dst, size_t len)
 
 int dma_write(struct ip *c, char *buf, size_t len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 
 	return xdma->HasSg
 		? dma_sg_write(c, buf, len)
@@ -51,7 +52,7 @@ int dma_write(struct ip *c, char *buf, size_t len)
 
 int dma_read(struct ip *c, char *buf, size_t len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 
 	return xdma->HasSg
 		? dma_sg_read(c, buf, len)
@@ -60,7 +61,7 @@ int dma_read(struct ip *c, char *buf, size_t len)
 
 int dma_read_complete(struct ip *c, char **buf, size_t *len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 
 	return xdma->HasSg
 		? dma_sg_read_complete(c, buf, len)
@@ -69,7 +70,7 @@ int dma_read_complete(struct ip *c, char **buf, size_t *len)
 
 int dma_write_complete(struct ip *c, char **buf, size_t *len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 
 	return xdma->HasSg
 		? dma_sg_write_complete(c, buf, len)
@@ -78,7 +79,7 @@ int dma_write_complete(struct ip *c, char **buf, size_t *len)
 
 int dma_sg_write(struct ip *c, char *buf, size_t len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 	XAxiDma_BdRing *ring = XAxiDma_GetTxRing(xdma);
 	XAxiDma_Bd *bd;
 
@@ -129,7 +130,7 @@ int dma_sg_write(struct ip *c, char *buf, size_t len)
 
 int dma_sg_read(struct ip *c, char *buf, size_t len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 	XAxiDma_BdRing *ring = XAxiDma_GetRxRing(xdma);
 	XAxiDma_Bd *bd;
 
@@ -180,22 +181,22 @@ int dma_sg_read(struct ip *c, char *buf, size_t len)
 
 int dma_sg_write_complete(struct ip *c, char **buf, size_t *len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 	XAxiDma_BdRing *ring = XAxiDma_GetTxRing(xdma);
 	XAxiDma_Bd *bd;
 
 	int processed, ret;
 
-	
 	/* Wait until the one BD TX transaction is done */
-	while ((processed = XAxiDma_BdRingFromHw(ring, XAXIDMA_ALL_BDS, &bd)) == 0) {
-//		if (c->irq >= 0) {
-//			intc_wait(c->card, c->irq);
-//			XAxiDma_IntrAckIrq(xmda, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DMA_TO_DEVICE);
-//		}
-//		else
-			__asm__ ("nop");
-	}
+	while ((processed = XAxiDma_BdRingFromHw(ring, XAXIDMA_ALL_BDS, &bd)) == 0)
+		pthread_testcancel();
+	XAxiDma_IntrAckIrq(xdma, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DMA_TO_DEVICE);
+	
+	if (len != NULL)
+		*len = XAxiDma_BdGetActualLength(bd, XAXIDMA_MAX_TRANSFER_LEN);
+
+	if (buf != NULL)
+		*buf = (char *) (uintptr_t) XAxiDma_BdGetBufAddr(bd);
 
 	/* Free all processed TX BDs for future transmission */
 	ret = XAxiDma_BdRingFree(ring, processed, bd);
@@ -209,7 +210,7 @@ int dma_sg_write_complete(struct ip *c, char **buf, size_t *len)
 
 int dma_sg_read_complete(struct ip *c, char **buf, size_t *len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 	XAxiDma_BdRing *ring = XAxiDma_GetRxRing(xdma);
 	XAxiDma_Bd *bd;
 
@@ -218,14 +219,9 @@ int dma_sg_read_complete(struct ip *c, char **buf, size_t *len)
 	if (!xdma->HasSg)
 		return -1;
 
-	while (XAxiDma_BdRingFromHw(ring, 1, &bd) == 0) {
-//		if (irq >= 0) {
-//			intc_wait(c->card, c->irq + 1);
-//			XAxiDma_IntrAckIrq(xdma, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DEVICE_TO_DMA);
-//		}
-//		else
-			__asm__("nop");
-	}
+	while (XAxiDma_BdRingFromHw(ring, 1, &bd) == 0)
+		pthread_testcancel();
+	XAxiDma_IntrAckIrq(xdma, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DEVICE_TO_DMA);
 
 	if (len != NULL)
 		*len = XAxiDma_BdGetActualLength(bd, XAXIDMA_MAX_TRANSFER_LEN);
@@ -243,7 +239,7 @@ int dma_sg_read_complete(struct ip *c, char **buf, size_t *len)
 
 int dma_simple_read(struct ip *c, char *buf, size_t len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 	XAxiDma_BdRing *ring = XAxiDma_GetRxRing(xdma);
 
 	buf = virt_to_dma(buf, c->card->dma);
@@ -263,7 +259,7 @@ int dma_simple_read(struct ip *c, char *buf, size_t len)
 		if ((uintptr_t) buf & mask)
 			return -4;
 	}
-
+	
 	if(!(XAxiDma_ReadReg(ring->ChanBase, XAXIDMA_SR_OFFSET) & XAXIDMA_HALTED_MASK)) {
 		if (XAxiDma_Busy(xdma, XAXIDMA_DEVICE_TO_DMA))
 			return -5;
@@ -281,7 +277,7 @@ int dma_simple_read(struct ip *c, char *buf, size_t len)
 
 int dma_simple_write(struct ip *c, char *buf, size_t len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 	XAxiDma_BdRing *ring = XAxiDma_GetTxRing(xdma);
 
 	buf = virt_to_dma(buf, c->card->dma);
@@ -320,25 +316,42 @@ int dma_simple_write(struct ip *c, char *buf, size_t len)
 
 int dma_simple_read_complete(struct ip *c, char **buf, size_t *len)
 {
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 	XAxiDma_BdRing *ring = XAxiDma_GetRxRing(xdma);
 
-	while (!(XAxiDma_IntrGetIrq(xdma, XAXIDMA_DEVICE_TO_DMA) & XAXIDMA_IRQ_IOC_MASK));
-	
-//	if(!(XAxiDma_ReadReg(ring->ChanBase, XAXIDMA_SR_OFFSET) & XAXIDMA_HALTED_MASK)) {
-//		if (XAxiDma_Busy(xdma, XAXIDMA_DMA_TO_DEVICE))
-//			return -5;
-//	}
+	while (!(XAxiDma_IntrGetIrq(xdma, XAXIDMA_DEVICE_TO_DMA) & XAXIDMA_IRQ_IOC_MASK))
+		pthread_testcancel();
+	XAxiDma_IntrAckIrq(xdma, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DEVICE_TO_DMA);
+
+	if (len)
+		*len = XAxiDma_ReadReg(ring->ChanBase, XAXIDMA_BUFFLEN_OFFSET);
+
+	if (buf) {
+		*buf = (char *) (uintptr_t) XAxiDma_ReadReg(ring->ChanBase, XAXIDMA_DESTADDR_OFFSET);
+		if (xdma->AddrWidth > 32)
+			*buf += XAxiDma_ReadReg(ring->ChanBase, XAXIDMA_DESTADDR_MSB_OFFSET);
+	}
 
 	return 0;
 }
 
 int dma_simple_write_complete(struct ip *c, char **buf, size_t *len)
 {
-	XAxiDma *xdma = &c->dma;
-	XAxiDma_BdRing *ring = XAxiDma_GetRxRing(xdma);
+	XAxiDma *xdma = &c->dma.inst;
+	XAxiDma_BdRing *ring = XAxiDma_GetTxRing(xdma);
 
-	while (!(XAxiDma_IntrGetIrq(xdma, XAXIDMA_DEVICE_TO_DMA) & XAXIDMA_IRQ_IOC_MASK));
+	while (!(XAxiDma_IntrGetIrq(xdma, XAXIDMA_DMA_TO_DEVICE) & XAXIDMA_IRQ_IOC_MASK))
+		pthread_testcancel();
+	XAxiDma_IntrAckIrq(xdma, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DMA_TO_DEVICE);
+
+	if (len)
+		*len = XAxiDma_ReadReg(ring->ChanBase, XAXIDMA_BUFFLEN_OFFSET);
+
+	if (buf) {
+		*buf = (char *) (uintptr_t) XAxiDma_ReadReg(ring->ChanBase, XAXIDMA_SRCADDR_OFFSET);
+		if (xdma->AddrWidth > 32)
+			*buf += XAxiDma_ReadReg(ring->ChanBase, XAXIDMA_SRCADDR_MSB_OFFSET);
+	}
 
 	return 0;
 }
@@ -362,7 +375,7 @@ static int dma_setup_ring(XAxiDma_BdRing *ring, struct dma_mem *bdbuf)
 
 	ret = XAxiDma_BdRingCreate(ring, bdbuf->base_phys, bdbuf->base_virt, XAXIDMA_BD_MINIMUM_ALIGNMENT, cnt);
 	if (ret != XST_SUCCESS) {
-		info("RX create BD ring failed %d", ret);
+		info("RX create BD ring failed (%d)", ret);
 		return XST_FAILURE;
 	}
 
@@ -386,8 +399,7 @@ static int dma_setup_ring(XAxiDma_BdRing *ring, struct dma_mem *bdbuf)
 int dma_init_rings(struct ip *c, struct dma_mem *bd)
 {
 	int ret;
-	XAxiDma *xdma = &c->dma;
-
+	XAxiDma *xdma = &c->dma.inst;
 
 	/* Split BD memory equally between Rx and Tx rings */
 	struct dma_mem bd_rx = {
@@ -415,7 +427,7 @@ int dma_init_rings(struct ip *c, struct dma_mem *bd)
 int dma_init(struct ip *c)
 {
 	int ret, sg;
-	XAxiDma *xdma = &c->dma;
+	XAxiDma *xdma = &c->dma.inst;
 	
 	/* Guess DMA type */
 	sg = (XAxiDma_In32((uintptr_t) c->card->map + c->baseaddr + XAXIDMA_TX_OFFSET+ XAXIDMA_SR_OFFSET) &
@@ -457,4 +469,11 @@ int dma_init(struct ip *c)
 	XAxiDma_IntrEnable(xdma, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DEVICE_TO_DMA);
 
 	return 0;
-}
\ No newline at end of file
+}
+
+static struct ip_type ip = {
+	.vlnv = { "xilinx.com", "ip", "axi_dma", NULL },
+	.init = dma_init
+};
+
+REGISTER_IP_TYPE(&ip)
\ No newline at end of file
diff --git a/lib/fpga/fifo.c b/lib/fpga/fifo.c
index 448b20cf1..fcf2de659 100644
--- a/lib/fpga/fifo.c
+++ b/lib/fpga/fifo.c
@@ -19,28 +19,34 @@
 
 int fifo_init(struct ip *c)
 {
-	XLlFifo *fifo = &c->fifo_mm;
+	struct fifo *fifo = &c->fifo;
+	XLlFifo *xfifo = &fifo->inst;
 
 	int ret;
 
 	XLlFifo_Config fifo_cfg = {
 		.BaseAddress = (uintptr_t) c->card->map + c->baseaddr,
-		.Axi4BaseAddress = (uintptr_t) c->card->map + c->baseaddr_axi4,
-		.Datainterface = ((c->baseaddr_axi4) && (c->baseaddr_axi4 != -1)) ? 1 : 0 /* use AXI4 for Data, AXI4-Lite for control */
+		.Axi4BaseAddress = (uintptr_t) c->card->map + fifo->baseaddr_axi4,
+		.Datainterface = (fifo->baseaddr_axi4 != -1) ? 1 : 0 /* use AXI4 for Data, AXI4-Lite for control */
 	};
 	
-	ret = XLlFifo_CfgInitialize(fifo, &fifo_cfg, (uintptr_t) c->card->map + c->baseaddr);
+	ret = XLlFifo_CfgInitialize(xfifo, &fifo_cfg, (uintptr_t) c->card->map + c->baseaddr);
 	if (ret != XST_SUCCESS)
 		return -1;
 
-	XLlFifo_IntEnable(fifo, XLLF_INT_RC_MASK); /* Receive complete IRQ */
+	XLlFifo_IntEnable(xfifo, XLLF_INT_RC_MASK); /* Receive complete IRQ */
 
 	return 0;
 }
 
 ssize_t fifo_write(struct ip *c, char *buf, size_t len)
 {
-	XLlFifo *fifo = &c->fifo_mm;
+	XLlFifo *fifo = &c->fifo.inst;
+	uint32_t tdfv;
+
+	tdfv = XLlFifo_TxVacancy(fifo);
+	if (tdfv < len)
+		return -1;
 
 	XLlFifo_Write(fifo, buf, len);
 	XLlFifo_TxSetLen(fifo, len);
@@ -50,24 +56,43 @@ ssize_t fifo_write(struct ip *c, char *buf, size_t len)
 
 ssize_t fifo_read(struct ip *c, char *buf, size_t len)
 {
-	XLlFifo *fifo = &c->fifo_mm;
+	XLlFifo *fifo = &c->fifo.inst;
 
 	size_t nextlen = 0;
+	uint32_t rdfo, rxlen;
 
-	while (XLlFifo_RxOccupancy(fifo) == 0) {
-		if (c->irq >= 0)
-			intc_wait(c->card, c->irq);
-		else
-			__asm__("nop");
-	}
+	while (!(rdfo = XLlFifo_RxOccupancy(fifo)))
+		pthread_testcancel();
+	XLlFifo_IntClear(fifo, XLLF_INT_RC_MASK);
 
 	/* Get length of next frame */
-	nextlen = MIN(XLlFifo_RxGetLen(fifo), len);
-
+	rxlen = XLlFifo_RxGetLen(fifo);
+	nextlen = MIN(rxlen, len);
+	
 	/* Read from FIFO */
 	XLlFifo_Read(fifo, buf, nextlen);
-	
-	XLlFifo_IntClear(fifo, XLLF_INT_RC_MASK);
 
 	return nextlen;
 }
+
+int fifo_parse(struct ip *c)
+{
+	struct fifo *fifo = &c->fifo;
+
+	int baseaddr_axi4;
+	
+	if (config_setting_lookup_int(c->cfg, "baseaddr_axi4", &baseaddr_axi4))
+		fifo->baseaddr_axi4 = baseaddr_axi4;
+	else
+		fifo->baseaddr_axi4 = -1;
+
+	return 0;
+}
+
+static struct ip_type ip = {
+	.vlnv = { "xilinx.com", "ip", "axi_fifo_mm_s", NULL },
+	.init = fifo_init,
+	.parse = fifo_parse
+};
+
+REGISTER_IP_TYPE(&ip)
\ No newline at end of file
diff --git a/lib/fpga/intc.c b/lib/fpga/intc.c
index 6ba2e3780..4ef2b2d91 100644
--- a/lib/fpga/intc.c
+++ b/lib/fpga/intc.c
@@ -8,8 +8,6 @@
 
 #include <unistd.h>
  
-#include <xilinx/xintc.h>
-
 #include "log.h"
 #include "nodes/fpga.h"
 #include "kernel/vfio.h" 
@@ -21,6 +19,9 @@ int intc_init(struct ip *c)
 	struct fpga *f = c->card;
 	int ret;
 
+	if (c != f->intc)
+		error("There can be only one interrupt controller per FPGA");
+
 	uintptr_t base = (uintptr_t) f->map + c->baseaddr;
 
 	/* Setup IRQs */
@@ -40,38 +41,78 @@ int intc_init(struct ip *c)
 	XIntc_Out32(base + XIN_IER_OFFSET, 0x00000000); /* Disable all IRQs by default */
 	XIntc_Out32(base + XIN_MER_OFFSET, XIN_INT_HARDWARE_ENABLE_MASK | XIN_INT_MASTER_ENABLE_MASK);
 
+	debug(4, "FPGA: enabled interrupts");
+	
 	return 0;
 }
 
-uint32_t intc_enable(struct ip *c, uint32_t mask)
+int intc_enable(struct ip *c, uint32_t mask, int poll)
 {
 	struct fpga *f = c->card;
 
+	uint32_t ier, imr;
 	uintptr_t base = (uintptr_t) f->map + c->baseaddr;
-	uint32_t ier = XIntc_In32(base + XIN_IER_OFFSET);
 
-	XIntc_Out32(base + XIN_IER_OFFSET, ier | mask);
+	/* Current state of INTC */
+	ier = XIntc_In32(base + XIN_IER_OFFSET);
+	imr = XIntc_In32(base + XIN_IMR_OFFSET);
 
-	return ier;
+	/* 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);
+
+	debug(8, "FPGA: Interupt enabled: %#x", mask);
+
+	return 0;
 }
 
-void intc_disable(struct ip *c, uint32_t mask)
+int intc_disable(struct ip *c, uint32_t mask)
 {
 	struct fpga *f = c->card;
 
 	uintptr_t base = (uintptr_t) f->map + c->baseaddr;
 
 	XIntc_Out32(base + XIN_IER_OFFSET, XIntc_In32(base + XIN_IER_OFFSET) & ~mask);
+
+	return 0;
 }
 
-uint64_t intc_wait(struct fpga *f, int irq)
+uint64_t intc_wait(struct ip *c, int irq, int poll)
 {
-	ssize_t ret;
-	uint64_t cnt;
+	struct fpga *f = c->card;
 
-	ret = read(f->vd.msi_efds[irq], &cnt, sizeof(cnt));
-	if (ret != sizeof(cnt))
-		return 0;
+	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 */
+			isr = XIntc_In32(base + XIN_ISR_OFFSET);
+		} while (!(isr & mask));
+
+		/* Acknowlege */
+		XIntc_Out32(base + XIN_IAR_OFFSET, mask);
+		
+		cnt = 1;
+	}
+	else {
+		ssize_t ret = read(f->vd.msi_efds[irq], &cnt, sizeof(cnt));
+		if (ret != sizeof(cnt))
+			return 0;
+	}
 
 	return cnt;
-}
\ No newline at end of file
+}
+
+static struct ip_type ip = {
+	.vlnv = { "acs.eonerc.rwth-aachen.de", "user", "axi_pcie_intc", NULL },
+	.init = intc_init
+};
+
+REGISTER_IP_TYPE(&ip)
diff --git a/lib/fpga/ip.c b/lib/fpga/ip.c
index 8b5084078..45ab456ec 100644
--- a/lib/fpga/ip.c
+++ b/lib/fpga/ip.c
@@ -12,6 +12,8 @@
 
 #include "config-fpga.h"
 
+struct list ip_types;	/**< Table of existing FPGA IP core drivers */
+
 struct ip * ip_vlnv_lookup(struct list *l, const char *vendor, const char *library, const char *name, const char *version)
 {
 	list_foreach(struct ip *c, l) {
@@ -46,60 +48,24 @@ int ip_vlnv_parse(struct ip *c, const char *vlnv)
 
 int ip_init(struct ip *c)
 {
-	struct fpga *f = c->card;
 	int ret;
 
-	if      (ip_vlnv_match(c, "xilinx.com", "ip", "axis_interconnect", NULL)) {
-		if (c != f->sw)
-			error("There can be only one AXI4-Stream interconnect per FPGA");
-
-		ret = switch_init(c);
-	}
-	else if (ip_vlnv_match(c, "xilinx.com", "ip", "axi_fifo_mm_s", NULL)) {
-		ret = fifo_init(c);
-	}
-	else if (ip_vlnv_match(c, "xilinx.com", "ip", "axi_dma", NULL)) {
-		ret = dma_init(c);
-	}
-	else if (ip_vlnv_match(c, "xilinx.com", "ip", "axi_timer", NULL)) {
-		XTmrCtr_Config tmr_cfg = {
-			.BaseAddress = (uintptr_t) c->card->map + c->baseaddr,
-			.SysClockFreqHz = AXI_HZ
-		};
-
-		XTmrCtr_CfgInitialize(&c->timer, &tmr_cfg, (uintptr_t) c->card->map + c->baseaddr);
-		XTmrCtr_InitHw(&c->timer);
-	}
-	else if (ip_vlnv_match(c, "xilinx.com", "ip", "axi_gpio", NULL)) {
-		if (strstr(c->name, "reset")) {
-			if (f->reset)
-				error("There can be only one reset controller per FPGA");
-			
-			f->reset = c;
-		}
-	}
-	else if (ip_vlnv_match(c, "acs.eonerc.rwth-aachen.de", "user", "axi_pcie_intc", NULL)) {
-		if (c != f->intc)
-			error("There can be only one interrupt controller per FPGA");
-
-		ret = intc_init(c);
-	}
-	else if (ip_vlnv_match(c, NULL, "hls", NULL, NULL) ||
-		 ip_vlnv_match(c, NULL, "sysgen", NULL, NULL)) {
-		ret = model_init(c);
-	}
+	ret = c->_vt && c->_vt->init ? c->_vt->init(c) : 0;
+	if (ret)
+		error("Failed to intialize IP core: %s", c->name);
 
 	if (ret == 0)
 		c->state = IP_STATE_INITIALIZED;
+	
+	debug(8, "IP Core %s initalized (%u)", c->name, ret);
 
 	return ret;
 }
 
 void ip_destroy(struct ip *c)
 {
-	if (ip_vlnv_match(c, NULL, "hls", NULL, NULL) ||
-	    ip_vlnv_match(c, NULL, "sysgen", NULL, NULL))
-		model_destroy(c);
+	if (c->_vt && c->_vt->destroy)
+		c->_vt->destroy(c);
 
 	free(c->vlnv.vendor);
 	free(c->vlnv.library);
@@ -113,16 +79,18 @@ void ip_dump(struct ip *c)
 		c->name, c->vlnv.vendor, c->vlnv.library, c->vlnv.name, c->vlnv.version, 
 		c->baseaddr, c->irq, c->port);
 
-	if (ip_vlnv_match(c, NULL, "hls", NULL, NULL) ||
-	    ip_vlnv_match(c, NULL, "sysgen", NULL, NULL))
-		model_dump(c);
+	if (c->_vt && c->_vt->dump)
+		c->_vt->dump(c);
 }
 
 int ip_parse(struct ip *c, config_setting_t *cfg)
 {
+	int ret;
 	const char *vlnv;
 	long long baseaddr;
 
+	c->cfg = cfg;
+
 	c->name = config_setting_name(cfg);
 	if (!c->name)
 		cerror(cfg, "IP is missing a name");
@@ -130,38 +98,16 @@ int ip_parse(struct ip *c, config_setting_t *cfg)
 	if (!config_setting_lookup_string(cfg, "vlnv", &vlnv))
 		cerror(cfg, "IP %s is missing the VLNV identifier", c->name);
 
-	ip_vlnv_parse(c, vlnv);
-	
-	if      (ip_vlnv_match(c, "xilinx.com", "ip", "axis_interconnect", NULL)) {
-		int numports;
+	ret = ip_vlnv_parse(c, vlnv);
+	if (ret)
+		cerror(cfg, "Failed to parse VLNV identifier");
 
-		if (!config_setting_lookup_int(cfg, "numports", &numports))
-			cerror(cfg, "Switch IP '%s' requires 'numports' option", c->name);
-		
-		c->sw.Config.MaxNumSI = c->sw.Config.MaxNumMI = numports;
-	}
-	else if (ip_vlnv_match(c, "xilinx.com", "ip", "axi_bram_ctrl", NULL)) {
-		if (!config_setting_lookup_int(cfg, "size", &c->bram.size))
-			cerror(cfg, "Block RAM requires 'size' option");
-	}
-	else if (ip_vlnv_match(c, "xilinx.com", "ip", "axi_fifo_mm_s", NULL)) {
-		if (config_setting_lookup_int64(cfg, "baseaddr_axi4", &baseaddr))
-			c->baseaddr_axi4 = baseaddr;
-		else
-			c->baseaddr_axi4 = -1;
-	}
-	else if (ip_vlnv_match(c, NULL, "hls", NULL, NULL) ||
-		 ip_vlnv_match(c, NULL, "sysgen", NULL, NULL)) {
-
-		if      (strcmp(c->vlnv.library, "hls") == 0)
-			c->model.type = MODEL_TYPE_HLS;
-		else if (strcmp(c->vlnv.library, "sysgen") == 0)
-			c->model.type = MODEL_TYPE_XSG;
-		else
-			cerror(cfg, "Invalid model type: %s", c->vlnv.library);
-
-		if (!config_setting_lookup_string(cfg, "xml", (const char **) &c->model.xml))
-			c->model.xml = NULL;
+	/* Try to find matching IP type */
+	list_foreach(struct ip_type *t, &ip_types) {
+		if (ip_vlnv_match(c, t->vlnv.vendor, t->vlnv.library, t->vlnv.name, t->vlnv.version)) {
+			c->_vt = t;
+			break;
+		}
 	}
 
 	/* Common settings */
@@ -175,7 +121,10 @@ int ip_parse(struct ip *c, config_setting_t *cfg)
 	if (!config_setting_lookup_int(cfg, "port", &c->port))
 		c->port = -1;
 
-	c->cfg = cfg;
+	/* Type sepecific settings */
+	ret = c->_vt && c->_vt->parse ? c->_vt->parse(c) : 0;
+	if (ret)
+		error("Failed to parse settings for IP core '%s'", c->name);
 
 	return 0;
 }
diff --git a/lib/fpga/model.c b/lib/fpga/model.c
index 980b1572e..737cd6355 100644
--- a/lib/fpga/model.c
+++ b/lib/fpga/model.c
@@ -42,7 +42,7 @@ static int model_xsg_map_parse(uint32_t *map, size_t len, struct list *parameter
 {
 #define copy_string(off) strndup((char *) (data + (off)), (length - (off)) * 4);
 	int j;
-	struct model_param *p;
+	struct model_param p, *e;
 	struct model_info *i;
 
 	/* Check magic */
@@ -59,17 +59,21 @@ static int model_xsg_map_parse(uint32_t *map, size_t len, struct list *parameter
 			case XSG_BLOCK_GATEWAY_OUT:
 				if (length < 4)
 					break; /* block is to small to describe a gateway */
-			
-				p = alloc(sizeof(*p));
 
-				p->name          =  copy_string(3);
-				p->default_value.flt =  *((float *) &data[1]);
-				p->offset        = data[2];
-				p->direction     = type & 0x1;
-				p->type          = (data[0] >> 0) & 0xFF;
-				p->binpt         = (data[0] >> 8) & 0xFF;
+				memset(&p, 0, sizeof(p));
 
-				list_push(parameters, p);
+				p.name          =  copy_string(3);
+				p.default_value.flt =  *((float *) &data[1]);
+				p.offset        = data[2];
+				p.direction     = type & 0x1;
+				p.type          = (data[0] >> 0) & 0xFF;
+				p.binpt         = (data[0] >> 8) & 0xFF;
+				
+				e = list_lookup(parameters, p.name);
+				if (e)
+					model_param_update(e, &p);
+				else
+					list_push(parameters, memdup(&p, sizeof(p)));
 				break;
 
 			case XSG_BLOCK_INFO:
@@ -125,13 +129,35 @@ static int model_xsg_map_read(uint32_t *map, size_t len, void *baseaddr)
 	return i;
 }
 
-static int model_init_from_xml(struct ip *c)
+int model_parse(struct ip *c)
 {
-	//struct model *m = &c->model;
+	struct model *m = &c->model;
+	struct model_param p;
+
+	config_setting_t *cfg_params, *cfg_param;
 	
-	/** @todo Implement */
-	
-	return -1;
+	if      (strcmp(c->vlnv.library, "hls") == 0)
+		c->model.type = MODEL_TYPE_HLS;
+	else if (strcmp(c->vlnv.library, "sysgen") == 0)
+		c->model.type = MODEL_TYPE_XSG;
+	else
+		cerror(c->cfg, "Invalid model type: %s", c->vlnv.library);
+
+	cfg_params = config_setting_get_member(c->cfg, "parameters");
+	if (cfg_params) {
+		for (int i = 0; i < config_setting_length(cfg_params); i++) {
+			cfg_param = config_setting_get_elem(cfg_params, i);
+
+			memset(&p, 0, sizeof(p));
+
+			p.name = config_setting_name(cfg_param);
+			p.default_value.flt = config_setting_get_float(cfg_param);
+
+			list_push(&m->parameters, memdup(&p, sizeof(p)));
+		}
+	}
+
+	return 0;
 }
 
 static int model_init_from_xsg_map(struct model *m, void *baseaddr)
@@ -169,18 +195,15 @@ int model_init(struct ip *c)
 	list_init(&m->parameters);
 	list_init(&m->infos);
 
-	if (m->xml)
-		ret = model_init_from_xml(c);
-	else if (ip_vlnv_match(c, NULL, "sysgen", NULL, NULL))
+	if (ip_vlnv_match(c, NULL, "sysgen", NULL, NULL))
 		ret = model_init_from_xsg_map(m, c->card->map + c->baseaddr);
 	else
 		ret = 0;
-	
-	list_foreach(struct model_param *p, &m->parameters)
-		p->ip = c;
-		
+
 	/* Set default values for parameters */
 	list_foreach(struct model_param *p, &m->parameters) {
+		p->ip = c;
+
 		if (p->direction == MODEL_PARAM_IN) {
 			model_param_write(p, p->default_value.flt);
 			info("Set parameter '%s' updated to default value: %f", p->name, p->default_value.flt);
@@ -316,3 +339,35 @@ int model_param_remove(struct ip *c, const char *name)
 	
 	return 0;
 }
+
+int model_param_update(struct model_param *p, struct model_param *u)
+{
+	if (strcmp(p->name, u->name) != 0)
+		return -1;
+
+	p->direction = u->direction;
+	p->type = u->type;
+	p->binpt = u->binpt;
+	p->offset = u->offset;
+
+	return 0;
+}
+
+static struct ip_type ip_hls = {
+	.vlnv = { NULL, "hls", NULL, NULL },
+	.init = model_init,
+	.destroy = model_destroy,
+	.dump = model_dump,
+	.parse = model_parse
+};
+
+static struct ip_type ip_xsg = {
+	.vlnv = { NULL, "sysgen", NULL, NULL },
+	.init = model_init,
+	.destroy = model_destroy,
+	.dump = model_dump,
+	.parse = model_parse
+};
+
+REGISTER_IP_TYPE(&ip_hls)
+REGISTER_IP_TYPE(&ip_xsg)
\ No newline at end of file
diff --git a/lib/fpga/rtds_axis.c b/lib/fpga/rtds_axis.c
index 6aade1cc5..cb35bd2ae 100644
--- a/lib/fpga/rtds_axis.c
+++ b/lib/fpga/rtds_axis.c
@@ -11,14 +11,15 @@
 #include "log.h"
 #include "utils.h"
 
+#include "fpga/ip.h"
 #include "fpga/rtds_axis.h"
 
-void rtds_axis_dump(char *baseaddr)
+void rtds_axis_dump(struct ip *c)
 {
 	/* Check RTDS_Axis registers */
-	uint32_t *rtds = (uint32_t *) baseaddr;
+	uint32_t *regs = (uint32_t *) (c->card->map + c->baseaddr);
 
-	uint32_t sr = rtds[RTDS_AXIS_SR_OFFSET/4];
+	uint32_t sr = regs[RTDS_AXIS_SR_OFFSET/4];
 	info("RTDS AXI Stream interface details");
 	{ INDENT
 		info("RTDS status:  %#08x", sr);
@@ -30,20 +31,27 @@ void rtds_axis_dump(char *baseaddr)
 			info("Case running:   %s", sr & RTDS_AXIS_SR_CASE_RUNNING ? GRN("yes") : RED("no"));
 		}
 
-		info("RTDS control: %#08x", rtds[RTDS_AXIS_CR_OFFSET/4]);
-		info("RTDS IRQ coalesc: %u", rtds[RTDS_AXIS_COALESC_OFFSET/4]);
-		info("RTDS IRQ version: %#06x", rtds[RTDS_AXIS_VERSION_OFFSET/4]);
-		info("RTDS IRQ multi-rate RTDS2AXIS: %u", rtds[RTDS_AXIS_MRATE_RTDS2AXIS/4]);
-		info("RTDS IRQ multi-rate AXIS2RTDS: %u", rtds[RTDS_AXIS_MRATE_AXIS2RTDS/4]);
+		info("RTDS control: %#08x", regs[RTDS_AXIS_CR_OFFSET/4]);
+		info("RTDS IRQ coalesc: %u", regs[RTDS_AXIS_COALESC_OFFSET/4]);
+		info("RTDS IRQ version: %#06x", regs[RTDS_AXIS_VERSION_OFFSET/4]);
+		info("RTDS IRQ multi-rate: %u", regs[RTDS_AXIS_MRATE/4]);
 
-		info("RTDS timestep counter: %lu", (uint64_t) rtds[RTDS_AXIS_TSCNT_LOW_OFFSET/4] | (uint64_t) rtds[RTDS_AXIS_TSCNT_HIGH_OFFSET/4] << 32);
-		info("RTDS timestep period:  %.3f uS", rtds_axis_dt(baseaddr) * 1e6);
+		info("RTDS timestep counter: %lu", (uint64_t) regs[RTDS_AXIS_TSCNT_LOW_OFFSET/4] | (uint64_t) regs[RTDS_AXIS_TSCNT_HIGH_OFFSET/4] << 32);
+		info("RTDS timestep period:  %.3f uS", rtds_axis_dt(c) * 1e6);
 	}
 }
 
-double rtds_axis_dt(char *baseaddr)
+double rtds_axis_dt(struct ip *c)
 {
-	uint32_t *rtds = (uint32_t *) baseaddr;
+	uint32_t *regs = (uint32_t *) (c->card->map + c->baseaddr);
+	uint16_t dt = regs[RTDS_AXIS_TS_PERIOD_OFFSET/4];
 
-	return (double) rtds[RTDS_AXIS_TS_PERIOD_OFFSET/4] / RTDS_HZ;
-}
\ No newline at end of file
+	return (dt == 0xFFFF) ? -1.0 : (double) dt / RTDS_HZ;
+}
+
+static struct ip_type ip = {
+	.vlnv = { "acs.eonerc.rwth-aachen.de", "user", "rtds_axis", NULL },
+	.dump = rtds_axis_dump
+};
+
+REGISTER_IP_TYPE(&ip)
\ No newline at end of file
diff --git a/lib/fpga/switch.c b/lib/fpga/switch.c
index e782ffbd3..ca0ee424d 100644
--- a/lib/fpga/switch.c
+++ b/lib/fpga/switch.c
@@ -8,46 +8,134 @@
  *   Unauthorized copying of this file, via any medium is strictly prohibited.
  **********************************************************************************/
 
+#include "list.h"
+#include "log.h"
 #include "fpga/switch.h"
 #include "fpga/ip.h"
 
 int switch_init(struct ip *c)
 {
-	XAxis_Switch *sw = &c->sw;
 	int ret;
+	struct sw *sw = &c->sw;
+	struct fpga *f = c->card;
+
+	if (c != f->sw)
+		error("There can be only one AXI4-Stream interconnect per FPGA");
+
+	XAxis_Switch *xsw = &sw->inst;
 
 	/* Setup AXI-stream switch */
 	XAxis_Switch_Config sw_cfg = {
 		.BaseAddress = (uintptr_t) c->card->map + c->baseaddr,
-		.MaxNumMI = sw->Config.MaxNumMI,
-		.MaxNumSI = sw->Config.MaxNumSI
+		.MaxNumMI = sw->num_ports,
+		.MaxNumSI = sw->num_ports
 	};
 	
-	ret = XAxisScr_CfgInitialize(sw, &sw_cfg, (uintptr_t) c->card->map + c->baseaddr);
+	ret = XAxisScr_CfgInitialize(xsw, &sw_cfg, (uintptr_t) c->card->map + c->baseaddr);
 	if (ret != XST_SUCCESS)
 		return -1;
 
-#if 0
-	ret = XAxisScr_SelfTest(sw);
-	if (ret != TRUE)
-		return -1;
-#endif
-
 	/* Disable all masters */
-	XAxisScr_RegUpdateDisable(sw);
-	XAxisScr_MiPortDisableAll(sw);
-	XAxisScr_RegUpdateEnable(sw);
+	XAxisScr_RegUpdateDisable(xsw);
+	XAxisScr_MiPortDisableAll(xsw);
+	XAxisScr_RegUpdateEnable(xsw);
+	
+	debug(8, "FPGA: Switch disabled all");
+	
+	list_foreach(struct sw_path *p, &sw->paths) {
+		struct ip *mi, *si;
+		
+		mi = list_lookup(&c->card->ips, p->out);
+		si = list_lookup(&c->card->ips, p->in);
+		
+		if (!mi || !si || mi->port == -1 || si->port == -1)
+			error("Invalid path configuration for FPGA");
 
+		ret = switch_connect(c, mi, si);
+		if (ret)
+			error("Failed to configure switch");
+	}
+
+	return 0;
+}
+
+void switch_destroy(struct ip *c)
+{
+	struct sw *sw = &c->sw;
+
+	list_destroy(&sw->paths, NULL, true);
+}
+
+int switch_parse(struct ip *c)
+{
+	struct sw *sw = &c->sw;
+	
+	list_init(&sw->paths);
+
+	config_setting_t *cfg_sw, *cfg_path;
+	
+	if (!config_setting_lookup_int(c->cfg, "num_ports", &sw->num_ports))
+		cerror(c->cfg, "Switch IP '%s' requires 'num_ports' option", c->name);
+
+	cfg_sw = config_setting_get_member(c->card->cfg, "paths");
+	if (!cfg_sw)
+		return 0; /* no switch config available */
+	
+	for (int i = 0; i < config_setting_length(cfg_sw); i++) {
+		cfg_path = config_setting_get_elem(cfg_sw, i);
+
+		struct sw_path path;
+		if (!config_setting_lookup_string(cfg_path, "in", &path.in) &&
+		    !config_setting_lookup_string(cfg_path, "from", &path.in) &&
+		    !config_setting_lookup_string(cfg_path, "src", &path.in) &&
+		    !config_setting_lookup_string(cfg_path, "source", &path.in))
+			cerror(cfg_path, "Path is missing 'in' setting");
+
+		if (!config_setting_lookup_string(cfg_path, "out", &path.out) &&
+		    !config_setting_lookup_string(cfg_path, "to", &path.out) &&
+		    !config_setting_lookup_string(cfg_path, "dst", &path.out) &&
+		    !config_setting_lookup_string(cfg_path, "dest", &path.out) &&
+		    !config_setting_lookup_string(cfg_path, "sink", &path.out))
+			cerror(cfg_path, "Path is missing 'out' setting");
+
+		list_push(&sw->paths, memdup(&path, sizeof(path)));
+	}
+	
 	return 0;
 }
 
 int switch_connect(struct ip *c, struct ip *mi, struct ip *si)
 {
-	XAxis_Switch *sw = &c->sw;
-
-	XAxisScr_RegUpdateDisable(sw);
-	XAxisScr_MiPortEnable(sw, mi->port, si->port);
-	XAxisScr_RegUpdateEnable(sw);
+	struct sw *sw = &c->sw;
+	XAxis_Switch *xsw = &sw->inst;
+	
+	XAxisScr_RegUpdateDisable(xsw);
+	XAxisScr_MiPortEnable(xsw, mi->port, si->port);
+	XAxisScr_RegUpdateEnable(xsw);
+	
+	debug(8, "FPGA: Switch connected %s (%u) to %s (%u)", mi->name, mi->port, si->name, si->port);
 
 	return 0;
 }
+
+int switch_disconnect(struct ip *c, struct ip *mi, struct ip *si)
+{
+	struct sw *sw = &c->sw;
+	XAxis_Switch *xsw = &sw->inst;
+
+	if (!XAxisScr_IsMiPortEnabled(xsw, mi->port, si->port))
+		return -1;
+
+	XAxisScr_MiPortDisable(xsw, mi->port);
+
+	return 0;
+}
+
+static struct ip_type ip = {
+	.vlnv = { "xilinx.com", "ip", "axis_interconnect", NULL },
+	.init = switch_init,
+	.destroy = switch_destroy,
+	.parse = switch_parse
+};
+
+REGISTER_IP_TYPE(&ip)
\ No newline at end of file
diff --git a/lib/fpga/timer.c b/lib/fpga/timer.c
new file mode 100644
index 000000000..55fe48a73
--- /dev/null
+++ b/lib/fpga/timer.c
@@ -0,0 +1,39 @@
+/** Timer related helper functions
+ *
+ * These functions present a simpler interface to Xilinx' Timer Counter driver (XTmrCtr_*)
+ *
+ * @file
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2015-2016, Steffen Vogel
+ *   This file is part of S2SS. All Rights Reserved. Proprietary and confidential.
+ *   Unauthorized copying of this file, via any medium is strictly prohibited.
+ **********************************************************************************/
+
+#include "config-fpga.h"
+
+#include "fpga/ip.h"
+#include "fpga/timer.h"
+
+int timer_init(struct ip *c)
+{
+	struct timer *tmr = &c->timer;
+	struct fpga *f = c->card;
+
+	XTmrCtr *xtmr = &tmr->inst;
+	XTmrCtr_Config xtmr_cfg = {
+		.BaseAddress = (uintptr_t) f->map + c->baseaddr,
+		.SysClockFreqHz = AXI_HZ
+	};
+
+	XTmrCtr_CfgInitialize(xtmr, &xtmr_cfg, (uintptr_t) f->map + c->baseaddr);
+	XTmrCtr_InitHw(xtmr);
+	
+	return 0;
+}
+
+static struct ip_type ip = {
+	.vlnv = { "xilinx.com", "ip", "axi_timer", NULL },
+	.init = timer_init
+};
+
+REGISTER_IP_TYPE(&ip)
\ No newline at end of file
diff --git a/lib/nodes/fpga.c b/lib/nodes/fpga.c
index 63da81694..8125c61f7 100644
--- a/lib/nodes/fpga.c
+++ b/lib/nodes/fpga.c
@@ -18,6 +18,7 @@
 #include "nodes/fpga.h"
 
 #include "fpga/ip.h"
+#include "fpga/intc.h"
 
 #include "config-fpga.h"
 #include "utils.h"
@@ -38,7 +39,7 @@ int fpga_reset(struct fpga *f)
 
 	uint32_t *rst_reg = (uint32_t *) (f->map + f->reset->baseaddr);
 
-	info("Reset fpga");
+	debug(3, "FPGA: reset");
 	rst_reg[0] = 1;
 
 	usleep(100000);
@@ -48,9 +49,7 @@ int fpga_reset(struct fpga *f)
 	if (ret != sizeof(state))
 		return -1;
 
-	info("Reset status = %#x", *rst_reg);
-
-	return 0;
+	return  rst_reg[0];
 }
 
 void fpga_dump(struct fpga *f)
@@ -88,27 +87,27 @@ struct fpga * fpga_get()
 	return &fpga;
 }
 
-int fpga_parse_card(struct fpga *v, int argc, char * argv[], config_setting_t *cfg)
+int fpga_parse_card(struct fpga *f, int argc, char * argv[], config_setting_t *cfg)
 {
 	int ret;
 	const char *slot, *id, *err;
-	config_setting_t *cfg_fpga, *cfg_ips, *cfg_slot, *cfg_id;
+	config_setting_t *cfg_ips, *cfg_slot, *cfg_id;
 
 	/* Default values */
-	v->filter.vendor = PCI_VID_XILINX;
-	v->filter.device = PCI_PID_VFPGA;
+	f->filter.vendor = PCI_VID_XILINX;
+	f->filter.device = PCI_PID_VFPGA;
 
-	cfg_fpga = config_setting_get_member(cfg, "fpga");
-	if (!cfg_fpga)
+	f->cfg = config_setting_get_member(cfg, "fpga");
+	if (!f->cfg)
 		cerror(cfg, "Config file is missing VILLASfpga configuration");
 
-	config_setting_lookup_bool(cfg_fpga, "do_reset", &v->do_reset);
+	config_setting_lookup_bool(f->cfg, "do_reset", &f->do_reset);
 
-	cfg_slot = config_setting_get_member(cfg_fpga, "slot");
+	cfg_slot = config_setting_get_member(f->cfg, "slot");
 	if (cfg_slot) {
 		slot = config_setting_get_string(cfg_slot);
 		if (slot) {
-			err = pci_filter_parse_slot(&v->filter, (char*) slot);
+			err = pci_filter_parse_slot(&f->filter, (char*) slot);
 			if (err)
 				cerror(cfg_slot, "%s", err);
 		}
@@ -116,11 +115,11 @@ int fpga_parse_card(struct fpga *v, int argc, char * argv[], config_setting_t *c
 			cerror(cfg_slot, "Invalid slot format");
 	}
 
-	cfg_id = config_setting_get_member(cfg_fpga, "id");
+	cfg_id = config_setting_get_member(f->cfg, "id");
 	if (cfg_id) {
 		id = config_setting_get_string(cfg_id);
 		if (id) {
-			err = pci_filter_parse_id(&v->filter, (char*) id);
+			err = pci_filter_parse_id(&f->filter, (char*) id);
 			if (err)
 				cerror(cfg_id, "%s", err);
 		}
@@ -128,22 +127,22 @@ int fpga_parse_card(struct fpga *v, int argc, char * argv[], config_setting_t *c
 			cerror(cfg_slot, "Invalid id format");
 	}
 	
-	cfg_ips = config_setting_get_member(cfg_fpga, "ips");
+	cfg_ips = config_setting_get_member(f->cfg, "ips");
 	if (!cfg_ips)
-		cerror(cfg_fpga, "FPGA configuration is missing ips section");
+		cerror(f->cfg, "FPGA configuration is missing ips section");
 
 	for (int i = 0; i < config_setting_length(cfg_ips); i++) {
-		struct ip *ip;
 		config_setting_t *cfg_ip = config_setting_get_elem(cfg_ips, i);
 
-		ip = alloc(sizeof(struct ip));
-		ret = ip_parse(ip, cfg_ip);
-		if (ret) {
-			free(ip);
-			cerror(cfg_ip, "Failed to parse VILLASfpga ip");
-		}
+		struct ip ip = {
+			.card = f
+		};
+	
+		ret = ip_parse(&ip, cfg_ip);
+		if (ret)
+			cerror(cfg_ip, "Failed to parse VILLASfpga IP core");
 
-		list_push(&v->ips, ip);
+		list_push(&f->ips, memdup(&ip, sizeof(ip)));
 	}
 
 	return 0;
@@ -158,13 +157,22 @@ int fpga_init(int argc, char * argv[], config_setting_t *cfg)
 
 	/* For now we only support a single VILALSfpga card */
 	f = fpga_get();
+	list_init(&f->ips);
+
+	pacc = pci_get_handle();
+	pci_filter_init(pacc, &f->filter);
+
+	/* Parse FPGA configuration */
+	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 (!reset)
+	if (!f->reset)
 		error("FPGA is missing a reset controller");
 
-	f->intc = ip_vlnv_lookup(&f->ips, "xilinx.com", "ip", "axi_pcie_intc", NULL);
+	f->intc = ip_vlnv_lookup(&f->ips, "acs.eonerc.rwth-aachen.de", "user", "axi_pcie_intc", NULL);
 	if (!f->intc)
 		error("FPGA is missing a interrupt controller");
 
@@ -172,22 +180,11 @@ int fpga_init(int argc, char * argv[], config_setting_t *cfg)
 	if (!f->sw)
 		warn("FPGA is missing an AXI4-Stream switch");
 
-	pacc = pci_get_handle();
-	pci_filter_init(pacc, &f->filter);
-	list_init(&f->ips);
-
-	ret = fpga_parse_card(f, argc, argv, cfg);
-	if (ret)
-		cerror(cfg, "Failed to parse VILLASfpga config");
-
-	/* Search for fpga card */
+	/* Search for FPGA card */
 	pdev = pci_find_device(pacc, &f->filter);
 	if (!pdev)
 		error("Failed to find PCI device");
 
-	if (pdev->vendor_id != PCI_VID_XILINX)
-		error("This is not a Xilinx FPGA board!");
-
 	/* Get VFIO handles and details */
 	ret = vfio_init(&vc);
 	if (ret)
@@ -204,7 +201,7 @@ int fpga_init(int argc, char * argv[], config_setting_t *cfg)
 		serror("Failed to mmap() BAR0");
 	
 	/* Map DMA accessible memory */
-	f->dma = vfio_map_dma(f->vd.group->container, 0x10000, 0x1000, 0x0);
+	f->dma = vfio_map_dma(f->vd.group->container, 16 << 20, 0x1000, 0x0);
 	if (f->dma == MAP_FAILED)
 		serror("Failed to mmap() DMA");
 
@@ -217,7 +214,7 @@ int fpga_init(int argc, char * argv[], config_setting_t *cfg)
 	if (f->do_reset) {
 		ret = fpga_reset(f);
 		if (ret)
-			serror("Failed to reset fpga card");
+			error("Failed to reset FGPA card");
 		
 		/* Reset / detect PCI device */
 		ret = vfio_pci_reset(&f->vd);
@@ -227,8 +224,9 @@ int fpga_init(int argc, char * argv[], config_setting_t *cfg)
 
 	/* Initialize IP cores */
 	list_foreach(struct ip *c, &f->ips) {
-		c->card = f;
-		ip_init(c);
+		ret = ip_init(c);
+		if (ret)
+			error("Failed to initalize IP core: %s (%u)", c->name, ret);
 	}
 
 	return 0;
@@ -256,21 +254,11 @@ int fpga_parse(struct node *n, config_setting_t *cfg)
 	/* There is currently only support for a single FPGA card */
 	d->card = fpga_get();
 
-	const char *dm;
-
-	if (!config_setting_lookup_string(cfg, "datamover", &dm))
+	if (!config_setting_lookup_string(cfg, "datamover", &d->ip_name))
 		cerror(cfg, "Node '%s' is missing the 'datamover' setting", node_name(n));
 
-	d->ip = list_lookup(&d->card->ips, dm);
-	if (!d->ip)
-		cerror(cfg, "Datamover '%s' is unknown. Please specify it in the fpga.ips section", dm);
-
-	if      (ip_vlnv_match(d->ip, "xilinx.com", "ip", "axi_dma", NULL))
-		d->type = FPGA_DM_DMA;
-	else if (ip_vlnv_match(d->ip, "xilinx.com", "ip", "axi_fifo_mm", NULL))
-		d->type = FPGA_DM_FIFO;
-	else
-		cerror(cfg, "IP '%s' is not a supported datamover", dm);
+	if (!config_setting_lookup_bool(cfg, "use_irqs", &d->use_irqs))
+		d->use_irqs = false;
 
 	return 0;
 }
@@ -279,27 +267,92 @@ char * fpga_print(struct node *n)
 {
 	struct fpga_dm *d = n->_vd;
 	struct fpga *f = d->card;
+	
+	if (d->ip)
+		return strf("dm=%s (%s:%s:%s:%s) baseaddr=%#jx port=%u slot=%02"PRIx8":%02"PRIx8".%"PRIx8" id=%04"PRIx16":%04"PRIx16,
+			d->ip->name, d->ip->vlnv.vendor, d->ip->vlnv.library, d->ip->vlnv.name, d->ip->vlnv.version, d->ip->baseaddr, d->ip->port,
+			f->filter.bus, f->filter.device, f->filter.func,
+			f->filter.vendor, f->filter.device);
+	else
+		return strf("dm=%s", d->ip_name);
+}
 
-	return strf("dm=%s (%s:%s:%s:%s) baseaddr=%#jx port=%u slot=%02"PRIx8":%02"PRIx8".%"PRIx8" id=%04"PRIx16":%04"PRIx16,
-		d->ip->name, d->ip->vlnv.vendor, d->ip->vlnv.library, d->ip->vlnv.name, d->ip->vlnv.version, d->ip->baseaddr, d->ip->port,
-		f->filter.bus, f->filter.device, f->filter.func,
-		f->filter.vendor, f->filter.device);
+int fpga_get_type(struct ip *c)
+{
+	if      (ip_vlnv_match(c, "xilinx.com", "ip", "axi_dma", NULL))
+		return FPGA_DM_DMA;
+	else if (ip_vlnv_match(c, "xilinx.com", "ip", "axi_fifo_mm_s", NULL))
+		return FPGA_DM_FIFO;
+	else
+		return -1;
 }
 
 int fpga_open(struct node *n)
 {
-//	struct fpga *v = n->_vd;
+	int ret;
+	struct fpga_dm *d = n->_vd;
+	struct fpga *f = d->card;
 
-	/** @todo Implement */
+	d->ip = list_lookup(&d->card->ips, d->ip_name);
+	if (!d->ip)
+		cerror(n->cfg, "Datamover '%s' is unknown. Please specify it in the fpga.ips section", d->ip_name);
+
+	d->type = fpga_get_type(d->ip);
+	if (d->type < 0)
+		cerror(n->cfg, "IP '%s' is not a supported datamover", d->ip->name);
+
+	switch (d->type) {
+		case FPGA_DM_DMA:
+			XAxiDma_Reset(&d->ip->dma.inst);
+
+			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 */
+			break;
+
+		case FPGA_DM_FIFO:
+			XLlFifo_Reset(&d->ip->fifo.inst);
+
+			intc_enable(f->intc, (1 << d->ip->irq),      !d->use_irqs);	/* MM2S & S2MM */
+			break;
+	}
+	
 
 	return 0;
 }
 
 int fpga_close(struct node *n)
 {
-//	struct fpga *v = n->_vd;
-
-	/** @todo Implement */
+	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 */
+			}
+		
+		case FPGA_DM_FIFO:
+			if (d->use_irqs)
+				intc_disable(f->intc, d->ip->irq);	/* MM2S & S2MM */
+	}
 
 	return 0;
 }
@@ -307,23 +360,39 @@ int fpga_close(struct node *n)
 int fpga_read(struct node *n, struct sample *smps[], unsigned cnt)
 {
 	struct fpga_dm *d = n->_vd;
+	struct fpga *f = d->card;
 	struct sample *smp = smps[0];
 
-	char *addr = (char *) smp->values;
-	size_t len = smp->length * sizeof(smp->values[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]);
+	
+	/* We dont get a sequence no from the FPGA. Lets fake it */
+	smp->sequence = n->received;
+	smp->ts.origin = time_now();
 
 	/* Read data from RTDS */
 	switch (d->type) {
 		case FPGA_DM_DMA:
-			if (d->ip->dma.HasSg)
-				return -1; /* Currently not supported */
-			else {
-				len = dma_read(d->ip, addr, len);
-				XAxiDma_IntrAckIrq(&d->ip->dma, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DEVICE_TO_DMA);
-				return 1;
-			}
+			ret = dma_read(d->ip, tmp, len);
+			if (ret)
+				return ret;
+			
+			ret = dma_read_complete(d->ip, NULL, &recvlen);
+			if (ret)
+				return ret;
+
+			memcpy(addr, tmp, recvlen);
+
+			smp->length = recvlen / 4;
+			return 1;
 		case FPGA_DM_FIFO:
-			break;
+			recvlen = fifo_read(d->ip, addr, len);
+			
+			smp->length = recvlen / 4;
+			return 1;
 	}
 
 	return -1;
@@ -331,18 +400,41 @@ int fpga_read(struct node *n, struct sample *smps[], unsigned cnt)
 
 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]);
 
+	//intc_wait(f->intc, 5, 1);
+	
+	//if (n->received % 40000 == 0) {
+	//	struct timespec now = time_now();
+	//	info("proc time = %f", time_delta(&smp->ts.origin, &now));
+	//}
+
 	/* Send data to RTDS */
 	switch (d->type) {
 		case FPGA_DM_DMA:
-			len = dma_write(d->ip, addr, len);
+			memcpy(tmp, addr, len);
+
+			ret = dma_write(d->ip, tmp, len);
+			if (ret)
+				return ret;
+			
+			ret = dma_write_complete(d->ip, NULL, &sentlen);
+			if (ret)
+				return ret;
+
+			//info("Sent %u bytes to FPGA", sentlen);
+
 			return 1;
 		case FPGA_DM_FIFO:
+			sentlen = fifo_write(d->ip, addr, len);
 			break;
 	}
 	
@@ -352,6 +444,7 @@ int fpga_write(struct node *n, struct sample *smps[], unsigned cnt)
 static struct node_type vt = {
 	.name		= "fpga",
 	.description	= "VILLASfpga PCIe card (libpci)",
+	.size		= sizeof(struct fpga_dm),
 	.vectorize	= 1,
 	.parse		= fpga_parse,
 	.print		= fpga_print,
@@ -363,4 +456,4 @@ static struct node_type vt = {
 	.deinit		= fpga_deinit
 };
 
-REGISTER_NODE_TYPE(&vt)
\ No newline at end of file
+REGISTER_NODE_TYPE(&vt)