diff --git a/src/fpga-bench.c b/src/fpga-bench.c
index d86b80b0a..82b5429a4 100644
--- a/src/fpga-bench.c
+++ b/src/fpga-bench.c
@@ -6,13 +6,20 @@
  *   Unauthorized copying of this file, via any medium is strictly prohibited.
  **********************************************************************************/
 
-#include <stdbool.h>
+#include <stdio.h>
 #include <stdint.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/utsname.h>
 
 #include <villas/utils.h>
 #include <villas/hist.h>
 #include <villas/log.h>
-#include <villas/nodes/vfpga.h>
+#include <villas/nodes/fpga.h>
+#include <villas/fpga/intc.h>
+#include <villas/fpga/ip.h>
+
+#include <xilinx/xtmrctr_l.h>
 
 #include "config.h"
 #include "config-fpga.h"
@@ -23,32 +30,222 @@
 #define BENCH_ROUNDS	10000
 #define BENCH_WARMUP	2000
 
-/* List of available benchmarks */
-enum benchmarks {
-	BENCH_FIFO,
-	BENCH_DMA_SIMPLE,
-	BENCH_DMA_SG,
-	MAX_BENCHS
-};
+int fpga_benchmark_datamover(struct fpga *f);
+int fpga_benchmark_jitter(struct fpga *f);
+int fpga_benchmark_memcpy(struct fpga *f);
+int fpga_benchmark_overflows(struct fpga *f);
+int fpga_benchmark_latency(struct fpga *f);
 
-int fpga_bench(struct vfpga *f, int polling, int bench)
+enum { POLLING, IRQ } mode;
+enum { READ, WRITE } rw = READ;
+
+struct utsname uts;
+
+int fpga_benchmarks(int argc, char *argv[], struct fpga *f)
 {
-	struct hist hist;
-	uint64_t start, stop;
+	int ret;
+	struct bench {
+		const char *name;
+		int (*func)(struct fpga *f);
+	} benchmarks[] = {
+		{ "datamover",	fpga_benchmark_datamover },
+		{ "jitter",	fpga_benchmark_jitter },
+		{ "memcpy",	fpga_benchmark_memcpy },
+		{ "overflows",	fpga_benchmark_overflows },
+		{ "latency",	fpga_benchmark_latency }
+	};
 	
-	hist_create(&hist, 0, 0, 1);
+	if (argc < 2)
+		error("Usage: fpga benchmark (bench)");
+	
+	struct bench *bench = NULL;
+	for (int i = 0; i < ARRAY_LEN(benchmarks); i++) {
+		if (strcmp(benchmarks[i].name, argv[1]) == 0) {
+			bench = &benchmarks[i];
+			break;
+		}
+	}
 
-	int irq_fifo     = polling ? -1 : f->vd.msi_efds[MSI_FIFO_MM];
-	int irq_dma_mm2s = polling ? -1 : f->vd.msi_efds[MSI_DMA_MM2S];
-	int irq_dma_s2mm = polling ? -1 : f->vd.msi_efds[MSI_DMA_S2MM];
+	if (bench == NULL)
+		error("There is no benchmark named: %s", argv[1]);
+
+	ret = uname(&uts);
+	if (ret)
+		return -1;
+
+	for (int j = 0; j < 2; j++) {
+		mode = j;
+
+		ret = bench->func(f);
+		if (ret)
+			error("Benchmark %s failed", bench->name);
+	}
+
+	return -1;
+}
+
+int fpga_benchmark_overflows(struct fpga *f)
+{
+	struct ip *tmr;
+
+	tmr = list_lookup(&f->ips, "timer_0");
+	if (!tmr || !f->intc)
+		return -1;
+
+	XTmrCtr *xtmr = &tmr->timer.inst;
+
+	if (mode == IRQ)
+		intc_enable(f->intc, (1 << tmr->irq), 0);
+
+	float period = 50e-6;
+	int times = period * 1.5 * 1e6; // in uS
+	int runs = 1.0 / period;
+	int overflows[times];
+
+	XTmrCtr_SetOptions(xtmr, 0, XTC_INT_MODE_OPTION | XTC_EXT_COMPARE_OPTION | XTC_DOWN_COUNT_OPTION | XTC_AUTO_RELOAD_OPTION);
+	XTmrCtr_SetResetValue(xtmr, 0, period * AXI_HZ);
+	XTmrCtr_Start(xtmr, 0);
+
+	for (int p = 0; p < times; p++) {
+		overflows[p] = 0;
+
+		for (int i = 0; i < runs; i++) {
+			uint32_t tcsr;
+			if ((tcsr = XTmrCtr_ReadReg((uintptr_t) f->map + tmr->baseaddr, 0, XTC_TCSR_OFFSET)) & XTC_CSR_INT_OCCURED_MASK)
+				overflows[p]++;
+
+			if (mode == IRQ)
+				intc_wait(f->intc, tmr->irq, 0);
+			else {
+				while (!((tcsr = XTmrCtr_ReadReg((uintptr_t) f->map + tmr->baseaddr, 0, XTC_TCSR_OFFSET)) & XTC_CSR_INT_OCCURED_MASK)) __asm__("nop");
+			}
+			tcsr = XTmrCtr_ReadReg((uintptr_t) f->map + tmr->baseaddr, 0, XTC_TCSR_OFFSET);
+			XTmrCtr_WriteReg((uintptr_t) f->map + tmr->baseaddr, 0, XTC_TCSR_OFFSET, tcsr);
+
+			/* Processing */
+			rdtsc_sleep(p * 1000, 0);
+		}
+	}
+
+	XTmrCtr_Stop(xtmr, 0);
+
+	/* Dump results */
+	char fn[256];
+	snprintf(fn, sizeof(fn), "results/overflows_%s_%s.dat", mode == IRQ ? "irq" : "polling", uts.release);
+	FILE *g = fopen(fn, "w");
+	fprintf(g, "# period = %f\n", period);
+	fprintf(g, "# runs = %u\n", runs);
+	for (int i = 0; i < times; i++)
+		fprintf(g, "%u\n", overflows[i]);
+	fclose(g);
+
+	if (mode == IRQ)
+		intc_disable(f->intc, (1 << tmr->irq));
 	
+	return 0;
+}
+
+int fpga_benchmark_jitter(struct fpga *f)
+{
+	struct ip *tmr;
+
+	tmr = list_lookup(&f->ips, "timer_0");
+	if (!tmr || !f->intc)
+		return -1;
+
+	XTmrCtr *xtmr = &tmr->timer.inst;
+
+	if (mode == IRQ)
+		intc_enable(f->intc, (1 << tmr->irq), 0);
+	
+	float period = 50e-6;
+	int runs = 300.0 / period;
+	
+	int *hist = (int *) f->dma;
+
+	XTmrCtr_SetOptions(xtmr, 0, XTC_INT_MODE_OPTION | XTC_EXT_COMPARE_OPTION | XTC_DOWN_COUNT_OPTION | XTC_AUTO_RELOAD_OPTION);
+	XTmrCtr_SetResetValue(xtmr, 0, period * AXI_HZ);
+	XTmrCtr_Start(xtmr, 0);
+
+	uint64_t end, start = rdtscp();
+	for (int i = 0; i < runs; i++) {
+		if (mode == IRQ) {
+			uint64_t cnt = intc_wait(f->intc, tmr->irq, 0);
+			if (cnt != 1)
+				warn("fail");
+		}
+		else {
+			uint32_t tcsr;
+			while (!((tcsr = XTmrCtr_ReadReg((uintptr_t) f->map + tmr->baseaddr, 0, XTC_TCSR_OFFSET)) & XTC_CSR_INT_OCCURED_MASK)) __asm__("nop");
+			XTmrCtr_WriteReg((uintptr_t) f->map + tmr->baseaddr, 0, XTC_TCSR_OFFSET, tcsr | XTC_CSR_INT_OCCURED_MASK);
+		}
+
+		end = rdtscp();
+		hist[i] = end - start;
+		start = end;
+	}
+
+	XTmrCtr_Stop(xtmr, 0);
+
+	char fn[256];
+	snprintf(fn, sizeof(fn), "results/jitter_%s_%s.dat", mode == IRQ ? "irq" : "polling", uts.release);
+	FILE *g = fopen(fn, "w");
+	for (int i = 0; i < runs; i++)
+		fprintf(g, "%u\n", hist[i]);
+	fclose(g);
+
+	free(hist);
+	
+	if (mode == IRQ)
+		intc_disable(f->intc, (1 << tmr->irq));
+	return 0;
+}
+
+int fpga_benchmark_latency(struct fpga *f)
+{
+	uint64_t start, end;
+
+	if (!f->intc)
+		return -1;
+
+	int runs = 1000000;
+	int hist[runs];
+
+	intc_enable(f->intc, 0x100, mode == POLLING ? 1 : 0);
+
+	for (int i = 0; i < runs; i++) {
+		start = rdtscp();
+		XIntc_Out32((uintptr_t) f->map + f->intc->baseaddr + XIN_ISR_OFFSET, 0x100);
+
+		intc_wait(f->intc, 8, mode == POLLING ? 1 : 0);
+		end = rdtscp();
+		
+		hist[i] = end - start;
+	}
+
+	char fn[256];
+	snprintf(fn, sizeof(fn), "results/latency_%s_%s.dat", mode == IRQ ? "irq" : "polling", uts.release);
+	FILE *g = fopen(fn, "w");
+	for (int i = 0; i < runs; i++)
+		fprintf(g, "%u\n", hist[i]);
+	fclose(g);
+
+	intc_disable(f->intc, 0x100);
+
+	return 0;
+}
+
+int fpga_benchmark_datamover(struct fpga *f)
+{
+#if 0
+	uint64_t start, stop;
+
 	char *src, *dst;
 	size_t len;
 
 	for (int exp = BENCH_EXP_MIN; exp < BENCH_EXP_MAX; exp++) {
 		size_t sz = 1 << exp;
-		
-		hist_reset(&hist);
+	
 		read_random(src, len);
 		memset(dst, 0, len);
 
@@ -80,70 +277,40 @@ int fpga_bench(struct vfpga *f, int polling, int bench)
 			if (i > BENCH_WARMUP)
 				hist_put(&hist, stop - start);
 		}
-
-		hist_dump(&hist);
 	}
-
-	hist_destroy(&hist);
-}
-
-int fpga_bench_dm(struct vfpga *f)
-{
-	int irq_fifo, irq_dma_mm2s, irq_dma_s2mm;
-	int polling = true;
-	int ret;
-
-	/* Loopback all datamovers */
-	switch_connect(&f->sw, AXIS_SWITCH_DMA_SIMPLE, AXIS_SWITCH_DMA_SIMPLE);
-	switch_connect(&f->sw, AXIS_SWITCH_DMA_SG, AXIS_SWITCH_DMA_SG);
-	switch_connect(&f->sw, AXIS_SWITCH_FIFO_MM, AXIS_SWITCH_FIFO_MM);
-
-	for (polling = 0; polling <= 1; polling++) {
-		for (int i = 0; i < MAX_BENCHS; i++)
-			bench_run(f, polling, i);
-	}
-
+#endif
 	return 0;
 }
 
-int fpga_bench_memcpy(struct vfpga *f)
+int fpga_benchmark_memcpy(struct fpga *f)
 {
-	uint64_t start, end, total = 0;
-
 #if 1
 	char *src = f->map;
-	char *dst = alloc(1 << 13);
+	char *dst = alloc(1 << 16);
 #else
 	char *dst = f->map;
-	char *src = alloc(1 << 13);
-	read_random(src, 1 << 13);
+	char *src = alloc(1 << 16);
 #endif
 
-	char *srcb = src;
-	for (int i = 0; i < 1 << 13; i++)
-		srcb[i] = i;
+	int runs = 10000;
+	
+	char fn[256];
+	snprintf(fn, sizeof(fn), "results/bar0_%s_%s.dat", mode == IRQ ? "irq" : "polling", uts.release);
+	FILE *g = fopen(fn, "w");
+	fprintf(g, "# runs = %u\n", runs);
+	fprintf(g, "# bytes cycles\n");
 
-	unsigned long long runs = 10000;
-	
-	fgetc(stdin);
-	
-	//memcpy(dst, src, 0xFF);
-	
-	for (int i = 0; i < 64; i++)
-		dst[i] = src[i];
-	
-	fgetc(stdin);
-
-	for (int exp = 0; exp < 14; exp++) {
-		size_t len = 1 << exp;
+	size_t len = 1;
+	for (int i = 0; i < 16; i++) {
+		len = 1 << i;
 
+		uint64_t start, end, total = 0;
 		for (int i = 0; i < runs; i++) {
 			/* Clear cache */
-			for (char *p = src; p < src + len; p += 64)
-				__builtin_ia32_clflush(p);
-
-	//		for (char *p = f->dma; p < src + len; p += 64)
-	//			__builtin_ia32_clflush(p);
+//			for (char *p = src; p < src + len; p += 64)
+//				__builtin_ia32_clflush(p);
+//			for (char *p = dst; p < dst + len; p += 64)
+//				__builtin_ia32_clflush(p);
 
 			/* Start measurement */
 			start = rdtscp();
@@ -153,7 +320,11 @@ int fpga_bench_memcpy(struct vfpga *f)
 			total += end - start;
 		}
 		
-		info("avg. clk cycles for %u bytes = %u", len, total / runs);
+		fprintf(g, "%zu %lu\n", len, total / runs);
 		usleep(100000);
 	}
+	
+	fclose(g);
+
+	return 0;
 }
\ No newline at end of file
diff --git a/src/fpga-main.c b/src/fpga-main.c
index c7f4277a7..e563f7174 100644
--- a/src/fpga-main.c
+++ b/src/fpga-main.c
@@ -15,6 +15,7 @@
 #include <sched.h>
 
 #include <villas/log.h>
+#include <villas/cfg.h>
 #include <villas/timing.h>
 #include <villas/utils.h>
 #include <villas/nodes/fpga.h>
@@ -26,15 +27,17 @@
 #include "config-fpga.h"
 
 /* Declarations */
-int fpga_bench(struct fpga *f);
-int fpga_tests(struct fpga *f);
+int fpga_benchmarks(int argc, char *argv[], struct fpga *f);
+int fpga_tests(int argc, char *argv[], struct fpga *f);
+
+struct settings settings;
 
 void usage(char *name)
 {
 	printf("Usage: %s CONFIGFILE CMD [OPTIONS]\n", name);
 	printf("   Commands:\n");
-	printf("      tests    Test functionality of VILLASfpga card\n");
-	printf("      bench    Do benchmarks\n\n");
+	printf("      tests      Test functionality of VILLASfpga card\n");
+	printf("      benchmarks Do benchmarks\n\n");
 	printf("   Options:\n");
 	printf("      -d    Set log level\n\n");
 
@@ -47,36 +50,25 @@ int main(int argc, char *argv[])
 {
 	int ret;
 	struct fpga *fpga;
+	config_t config;
+
 	enum {
 		FPGA_TESTS,
 		FPGA_BENCH
 	} subcommand;
-	config_t config;
-	
+
 	if (argc < 3)
 		usage(argv[0]);
-
 	if      (strcmp(argv[2], "tests") == 0)
 		subcommand = FPGA_TESTS;
-	else if (strcmp(argv[2], "bench") == 0)
+	else if (strcmp(argv[2], "benchmarks") == 0)
 		subcommand = FPGA_BENCH;
 	else
 		usage(argv[0]);
 
-	/* Setup libconfig */
-	config_init(&config);
-	ret = config_read_file(&config, argv[1]);
-	if (ret != CONFIG_TRUE) {
-		error("Failed to parse configuration: %s in %s:%d",
-			config_error_text(&config),
-			config_error_file(&config) ? config_error_file(&config) : argv[1],
-			config_error_line(&config)
-		);
-	}
-
 	/* Parse arguments */
 	char c, *endptr;
-	while ((c = getopt (argc-1, argv+1, "d:")) != -1) {
+	while ((c = getopt(argc-1, argv+1, "d:")) != -1) {
 		switch (c) {
 			case 'd':
 				log_setlevel(strtoul(optarg, &endptr, 10), ~0);
@@ -88,6 +80,12 @@ int main(int argc, char *argv[])
 		}
 	}
 
+	info("Parsing configuration");
+	{ INDENT
+		config_init(&config);
+		config_parse(argv[1], &config, &settings, NULL, NULL);
+	}
+	
 	info("Initialize real-time system");
 	rt_init(settings.affinity, settings.priority);
 
@@ -95,33 +93,15 @@ int main(int argc, char *argv[])
 	config_setting_t *cfg_root = config_root_setting(&config);
 	ret = fpga_init(argc, argv, cfg_root);
 	if (ret)
-		error("Failed to initialize fpga card");
+		error("Failed to initialize FPGA card");
 	
 	fpga = fpga_get();
-
 	fpga_dump(fpga);
 
-	/* Setup scheduler */
-	cpu_set_t set = integer_to_cpuset(AFFINITY);
-
-	ret = sched_setaffinity(0, sizeof(set), &set);
-	if (ret)
-		serror("Failed to pin thread");
-
-	ret = sched_setscheduler(0, SCHED_FIFO, &(struct sched_param) { .sched_priority = PRIORITY });
-	if (ret)
-		serror("Failed to change scheduler");
-
-	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");
-	}
-
 	/* Start subcommand */
 	switch (subcommand) {
-		case FPGA_TESTS: fpga_tests(fpga); break;
-		case FPGA_BENCH: /*fpga_bench(fpga);*/ break;
+		case FPGA_TESTS: fpga_tests(argc-optind-1, argv+optind+1, fpga);      break;
+		case FPGA_BENCH: fpga_benchmarks(argc-optind-1, argv+optind+1, fpga); break;
 	}
 
 	/* Shutdown */
diff --git a/src/fpga-tests.c b/src/fpga-tests.c
index 6df0a5f1d..a6a06f53d 100644
--- a/src/fpga-tests.c
+++ b/src/fpga-tests.c
@@ -12,6 +12,7 @@
 #include <fcntl.h>
 #include <poll.h>
 #include <time.h>
+#include <unistd.h>
 
 #include <xilinx/xtmrctr.h>
 #include <xilinx/xintc.h>
@@ -25,13 +26,9 @@
 #include <villas/fpga/ip.h>
 #include <villas/fpga/intc.h>
 
-//#include <villas/fpga/cbmodel.h>
-
 #include "config.h"
 #include "config-fpga.h"
 
-//#include "cbuilder/cbmodel.h"
-
 #define TEST_LEN 0x1000
 
 #define CPU_HZ 3392389000
@@ -42,9 +39,10 @@ int fpga_test_timer(struct fpga *f);
 int fpga_test_fifo(struct fpga *f);
 int fpga_test_dma(struct fpga *f);
 int fpga_test_xsg(struct fpga *f);
+int fpga_test_hls_dft(struct fpga *f);
 int fpga_test_rtds_rtt(struct fpga *f);
 
-int fpga_tests(struct fpga *f)
+int fpga_tests(int argc, char *argv[], struct fpga *f)
 {
 	int ret;
 	
@@ -57,9 +55,16 @@ int fpga_tests(struct fpga *f)
 		{ "FIFO",			fpga_test_fifo },
 		{ "DMA",			fpga_test_dma },
 		{ "XSG: multiply_add",		fpga_test_xsg },
-		{ "RTDS: RTT Test", 		fpga_test_rtds_rtt }
+		{ "HLS: hls_dft",		fpga_test_hls_dft },
+		{ "RTDS: tight rtt",		fpga_test_rtds_rtt }
 	};
 
+	/* We need to overwrite the AXI4-Stream switch configuration from the
+	 * config file. Therefore we first disable everything here. */
+	XAxisScr_RegUpdateDisable(&f->sw->sw.inst);
+	XAxisScr_MiPortDisableAll(&f->sw->sw.inst);
+	XAxisScr_RegUpdateEnable(&f->sw->sw.inst);
+
 	for (int i = 0; i < ARRAY_LEN(tests); i++) {
 		ret = tests[i].func(f);
 
@@ -71,29 +76,29 @@ int fpga_tests(struct fpga *f)
 
 int fpga_test_intc(struct fpga *f)
 {
+	int ret;
 	uint32_t isr;
 
 	if (!f->intc)
 		return -1;
 
-	intc_enable(f->intc, 0xFF00);
-
-	info("ISR %#x", XIntc_In32((uintptr_t) f->map + f->intc->baseaddr + XIN_ISR_OFFSET));
-	info("IER %#x", XIntc_In32((uintptr_t) f->map + f->intc->baseaddr + XIN_IER_OFFSET));
-	info("IMR %#x", XIntc_In32((uintptr_t) f->map + f->intc->baseaddr + XIN_IMR_OFFSET));
-	info("MER %#x", XIntc_In32((uintptr_t) f->map + f->intc->baseaddr + XIN_MER_OFFSET));
+	ret = intc_enable(f->intc, 0xFF00, 0);
+	if (ret)
+		error("Failed to enable interrupt");
 
 	/* Fake IRQs in software by writing to ISR */
 	XIntc_Out32((uintptr_t) f->map + f->intc->baseaddr + XIN_ISR_OFFSET, 0xFF00);
 
 	/* Wait for 8 SW triggered IRQs */
 	for (int i = 0; i < 8; i++)
-		intc_wait(f, i+8);
+		intc_wait(f->intc, i+8, 0);
 
 	/* Check ISR if all SW IRQs have been deliverd */
 	isr = XIntc_In32((uintptr_t) f->map + f->intc->baseaddr + XIN_ISR_OFFSET);
 
-	intc_disable(f->intc, 0xFF00);
+	ret = intc_disable(f->intc, 0xFF00);
+	if (ret)
+		error("Failed to disable interrupt");
 
 	return (isr & 0xFF00) ? -1 : 0; /* ISR should get cleared by MSI_Grant_signal */
 }
@@ -101,6 +106,7 @@ int fpga_test_intc(struct fpga *f)
 int fpga_test_xsg(struct fpga *f)
 {
 	struct ip *xsg, *dma;
+	struct model_param *p;
 	int ret;
 	double factor, err = 0;
 
@@ -111,28 +117,6 @@ int fpga_test_xsg(struct fpga *f)
 	if (!dma || !xsg || !dma)
 		return -1;
 
-	ip_dump(xsg);
-
-	config_setting_t *cfg_params, *cfg_param;
-	struct model_param *p;
-	
-	cfg_params = config_setting_get_member(xsg->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);
-			
-			const char *name = config_setting_name(cfg_param);
-			double value = config_setting_get_float(cfg_param);
-			
-			p = list_lookup(&xsg->model.parameters, name);
-			if (!p)
-				cerror(cfg_param, "Model %s has no parameter named %s", xsg->name, name);
-
-			model_param_write(p, value);
-			info("Param '%s' updated to: %f", p->name, value);
-		}
-	}
-
 	p = list_lookup(&xsg->model.parameters, "factor");
 	if (!p)
 		error("Missing parameter 'factor' for model '%s'", xsg->name);
@@ -143,8 +127,12 @@ int fpga_test_xsg(struct fpga *f)
 	
 	info("Model param: factor = %f", factor);
 
-	switch_connect(f->sw, dma, xsg);
-	switch_connect(f->sw, xsg, dma);
+	ret = switch_connect(f->sw, dma, xsg);
+	if (ret)
+		error("Failed to configure switch");
+	ret = switch_connect(f->sw, xsg, dma);
+	if (ret)
+		error("Failed to configure switch");
 
 	float *src = (float *) f->dma;
 	float *dst = (float *) f->dma + TEST_LEN;
@@ -158,25 +146,70 @@ int fpga_test_xsg(struct fpga *f)
 
 	for (int i = 0; i < 6; i++)
 		err += abs(factor * src[i] - dst[i]);
-	
+
 	info("Error after FPGA operation: err = %f", err);
 
+	ret = switch_disconnect(f->sw, dma, xsg);
+	if (ret)
+		error("Failed to configure switch");
+	ret = switch_disconnect(f->sw, xsg, dma);
+	if (ret)
+		error("Failed to configure switch");
+
 	return err > 1e-3;
 }
 
 int fpga_test_hls_dft(struct fpga *f)
 {
-#if 0
-	struct ip *hls, *dma;
-
-	hls = ip_vlnv_lookup(&f->ips, NULL, "hls", NULL, NULL);
-	dma = ip_vlnv_lookup(&f->ips, "xilinx.com", "ip", "axi_dma", NULL);
+	int ret;
+	struct ip *hls, *rtds;
+	
+	rtds = ip_vlnv_lookup(&f->ips, "acs.eonerc.rwth-aachen.de", "user", "rtds_axis", NULL);
+	hls = ip_vlnv_lookup(&f->ips, NULL, "hls", "hls_dft", NULL);
 
 	/* Check if required IP is available on FPGA */
-	if (!dma || !hls || !dma)
+	if (!hls || !rtds)
 		return -1;
+
+	ret = intc_enable(f->intc, (1 << rtds->irq), 0);
+	if (ret)
+		error("Failed to enable interrupt");
+	
+	ret = switch_connect(f->sw, rtds, hls);
+	if (ret)
+		error("Failed to configure switch");
+	ret = switch_connect(f->sw, hls, rtds);
+	if (ret)
+		error("Failed to configure switch");
+
+	while(1) {
+		/* Dump RTDS AXI Stream state */
+		rtds_axis_dump(rtds);
+		sleep(1);
+	}
+#if 0
+	int len = 2000;
+	int NSAMPLES = 400;
+	float src[len], dst[len];
+	
+	for (int i = 0; i < len; i++) {
+		src[i] = 4 + 5.0 * sin(2.0 * M_PI * 1 * i / NSAMPLES) + 
+		             2.0 * sin(2.0 * M_PI * 2 * i / NSAMPLES) + 
+			     1.0 * sin(2.0 * M_PI * 5 * i / NSAMPLES) + 
+			     0.5 * sin(2.0 * M_PI * 9 * i / NSAMPLES) + 
+			     0.2 * sin(2.0 * M_PI * 15 * i / NSAMPLES);
+	
+		fifo_write()
+	}
 #endif
 
+	ret = switch_disconnect(f->sw, rtds, hls);
+	if (ret)
+		error("Failed to configure switch");
+	ret = switch_disconnect(f->sw, hls, rtds);
+	if (ret)
+		error("Failed to configure switch");
+
 	return 0;
 }
 
@@ -191,7 +224,13 @@ int fpga_test_fifo(struct fpga *f)
 	if (!fifo)
 		return -1;
 
-	intc_enable(f->intc, (1 << fifo->irq));
+	ret = intc_enable(f->intc, (1 << fifo->irq), 0);
+	if (ret)
+		error("Failed to enable interrupt");
+
+	ret = switch_connect(f->sw, fifo, fifo);
+	if (ret)
+		error("Failed to configure switch");
 
 	/* Get some random data to compare */
 	memset(dst, 0, sizeof(dst));
@@ -199,10 +238,6 @@ int fpga_test_fifo(struct fpga *f)
 	if (ret)
 		error("Failed to get random data");
 
-	ret = switch_connect(f->sw, fifo, fifo);
-	if (ret)
-		error("Failed to configure switch");
-
 	len = fifo_write(fifo, (char *) src, sizeof(src));
 	if (len != sizeof(src))
 		error("Failed to send to FIFO");
@@ -211,7 +246,13 @@ int fpga_test_fifo(struct fpga *f)
 	if (len != sizeof(dst))
 		error("Failed to read from FIFO");
 
-	intc_disable(f->intc, (1 << fifo->irq));
+	ret = intc_disable(f->intc, (1 << fifo->irq));
+	if (ret)
+		error("Failed to disable interrupt");
+
+	ret = switch_disconnect(f->sw, fifo, fifo);
+	if (ret)
+		error("Failed to configure switch");
 
 	/* Compare data */
 	return memcmp(src, dst, sizeof(src));
@@ -223,28 +264,32 @@ int fpga_test_dma(struct fpga *f)
 	ssize_t len = 0x100;
 	int ret;
 
-	/* Get some random data to compare */
 	char *src = (char *) f->dma;
 	char *dst = (char *) f->dma + len;
 
-	ret = read_random(src, len);
-	if (ret)
-		error("Failed to get random data");
-
 	list_foreach(struct ip *dma, &f->ips) { INDENT
 		if (!ip_vlnv_match(dma, "xilinx.com", "ip", "axi_dma", NULL))
 			continue; /* skip non DMA IP cores */
 
+		/* Get new random data */
+		ret = read_random(src, len);
+		if (ret)
+			error("Failed to get random data");
+
 		int irq_mm2s = dma->irq;
 		int irq_s2mm = dma->irq + 1;
 		
-		intc_enable(f->intc, (1 << irq_mm2s) | (1 << irq_s2mm));
+		ret = intc_enable(f->intc, (1 << irq_mm2s) | (1 << irq_s2mm), 0);
+		if (ret)
+			error("Failed to enable interrupt");
 
 		ret = switch_connect(f->sw, dma, dma);
 		if (ret)
 			error("Failed to configure switch");
 		
-		if (dma->dma.HasSg) {
+		XAxiDma_Reset(&dma->dma.inst);
+
+		if (dma->dma.inst.HasSg) {
 			/* Init BD rings */
 			bram = ip_vlnv_lookup(&f->ips, "xilinx.com", "ip", "axi_bram_ctrl", NULL);
 			if (!bram)
@@ -270,8 +315,9 @@ int fpga_test_dma(struct fpga *f)
 		ret = dma_write(dma, src, len);
 		if (ret)
 			error("Failed to start DMA write: %d", ret);
-		
-		ret = dma_read_complete(dma, NULL, NULL);
+	
+		size_t recvlen;
+		ret = dma_read_complete(dma, NULL, &recvlen);
 		if (ret)
 			error("Failed to complete DMA read");
 
@@ -279,11 +325,19 @@ int fpga_test_dma(struct fpga *f)
 		if (ret)
 			error("Failed to complete DMA write");
 
+		info("recvlen = %#zx", recvlen);
+		
 		ret = memcmp(src, dst, len);
 
-		info("DMA %s (%s): %s", dma->name, dma->dma.HasSg ? "sg" : "simple", ret ? RED("failed") : GRN("passed"));
+		info("DMA %s (%s): %s", dma->name, dma->dma.inst.HasSg ? "scatter-gather" : "simple", ret ? RED("failed") : GRN("passed"));
 
-		intc_disable(f->intc, (1 << irq_mm2s) | (1 << irq_s2mm));
+		ret = switch_disconnect(f->sw, dma, dma);
+		if (ret)
+			error("Failed to configure switch");
+
+		ret = intc_disable(f->intc, (1 << irq_mm2s) | (1 << irq_s2mm));
+		if (ret)
+			error("Failed to disable interrupt");
 	}
 
 	return ret;
@@ -292,26 +346,30 @@ int fpga_test_dma(struct fpga *f)
 int fpga_test_timer(struct fpga *f)
 {
 	int ret;
-	struct ip *timer;
+	struct ip *tmr;
 
-	timer = ip_vlnv_lookup(&f->ips, "xilinx.com", "ip", "axi_timer", NULL);
-	if (!timer)
+	tmr = ip_vlnv_lookup(&f->ips, "xilinx.com", "ip", "axi_timer", NULL);
+	if (!tmr)
 		return -1;
+	
+	XTmrCtr *xtmr = &tmr->timer.inst;
 
-	intc_enable(f->intc, (1 << timer->irq));
+	ret = intc_enable(f->intc, (1 << tmr->irq), 0);
+	if (ret)
+		error("Failed to enable interrupt");
 
-	XTmrCtr_SetOptions(&timer->timer, 0, XTC_EXT_COMPARE_OPTION | XTC_DOWN_COUNT_OPTION);
-	XTmrCtr_SetResetValue(&timer->timer, 0, AXI_HZ / 125);
-	XTmrCtr_Start(&timer->timer, 0);
+	XTmrCtr_SetOptions(xtmr, 0, XTC_EXT_COMPARE_OPTION | XTC_DOWN_COUNT_OPTION);
+	XTmrCtr_SetResetValue(xtmr, 0, AXI_HZ / 125);
+	XTmrCtr_Start(xtmr, 0);
 
 	struct pollfd pfd = {
-		.fd = f->vd.msi_efds[timer->irq],
+		.fd = f->vd.msi_efds[tmr->irq],
 		.events = POLLIN
 	};
 
 	ret = poll(&pfd, 1, 1000);
 	if (ret == 1) {
-		uint64_t counter = intc_wait(f, timer->irq);
+		uint64_t counter = intc_wait(f->intc, tmr->irq, 0);
 
 		info("Got IRQ: counter = %ju", counter);
 
@@ -321,91 +379,61 @@ int fpga_test_timer(struct fpga *f)
 			warn("Counter was not 1");
 	}
 
-	intc_disable(f->intc, (1 << timer->irq));
+	intc_disable(f->intc, (1 << tmr->irq));
+	if (ret)
+		error("Failed to disable interrupt");
 
 	return -1;
 }
 
 int fpga_test_rtds_rtt(struct fpga *f)
 {
-#if 0
-	uint32_t tsc = 0;
-	ssize_t len;
+	int ret;
+	struct ip *dma, *rtds;
 
-	intc_enable(f->intc, (1 << MSI_DMA_MM2S) | (1 << MSI_DMA_S2MM));
+	rtds = ip_vlnv_lookup(&f->ips, "acs.eonerc.rwth-aachen.de", "user", "rtds_axis", NULL);
+	dma = list_lookup(&f->ips, "dma_1");
 
-	/* Dump RTDS AXI Stream state */
-	rtds_axis_dump(f->map + BASEADDR_RTDS);
+	/* Check if required IP is available on FPGA */
+	if (!dma || !rtds)
+		return -1;
 
-	/* Get some random data to compare */
-	uint32_t *data_rx = (uint32_t *) (f->dma);
-	uint32_t *data_tx = (uint32_t *) (f->dma + 0x1000);
+	ret = switch_connect(f->sw, rtds, dma);
+	if (ret)
+		error("Failed to configure switch");
+	ret = switch_connect(f->sw, dma, rtds);
+	if (ret)
+		error("Failed to configure switch");
 
-	/* Setup crossbar switch */
-	switch_connect(sw, AXIS_SWITCH_RTDS, switch_port);
-	switch_connect(sw, switch_port, AXIS_SWITCH_RTDS);
-
-	/* Disable blocking Overflow status */
-	int flags = fcntl(f->vd.msi_efds[MSI_RTDS_OVF], F_GETFL, 0);
-	fcntl(f->vd.msi_efds[MSI_RTDS_OVF], F_SETFL, flags | O_NONBLOCK);
-
-	int irq_fifo = f->vd.msi_efds[MSI_FIFO_MM];
-	int rtt2_prev, rtt2;
+	size_t len = 0x100;
+	size_t recvlen;
+	char *buf = f->dma;
 
 	while (1) {
-#if 0		/* Wait for TS */
-		tsc += intc_wait(f->intc, MSI_RTDS_TS);
-#else
-		tsc++;
-#endif
-
-#if 0		/* Check for overflow */
-		ovfl += intc_wait(f->intc, MSI_RTDS_OVF);
-		if (tsc % 20000 == 0)
-			warn("Missed %u timesteps", ovfl);
-#endif
-
-#if DATAMOVER == RTDS_DM_FIFO
-retry:		len = fifo_read(&f->fifo, (char *) data_rx, 0x1000, irq_fifo);
-		if (XLlFifo_RxOccupancy(&f->fifo) > 0 )
-			goto retry;
-#elif DATAMOVER == RTDS_DM_DMA_SIMPLE
-		len = dma_read(&f->dma_simple, virt_to_dma(data_rx, f->dma), 0x1000, -1);//f->vd.msi_efds[MSI_DMA_S2MM]);
-		uint64_t start = rdtscp();
-		XAxiDma_IntrAckIrq(&f->dma_simple, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DEVICE_TO_DMA);
-#elif DATAMOVER == RTDS_DM_DMA_SG
-		len = dma_read(&f->dma_sg, virt_to_dma(data_rx, f->dma), 0x1000, f->vd.msi_efds[MSI_DMA_S2MM]);
-#endif
-		values_rx = len / sizeof(uint32_t);
-
-		for (int i = 0; i < values_tx; i++)
-			data_tx[i] = data_rx[i];
 		
-		data_tx[3] = tsc;
+		ret = dma_read(dma, buf, len);
+		if (ret)
+			error("Failed to start DMA read: %d", ret);
 
-		if (data_rx[3] == 0)
-			tsc = 0;
+		ret = dma_read_complete(dma, NULL, &recvlen);
+		if (ret)
+			error("Failed to complete DMA read: %d", ret);
+
+		ret = dma_write(dma, buf, recvlen);
+		if (ret)
+			error("Failed to start DMA write: %d", ret);
 		
-		int rtt = tsc - data_rx[3];
-		
-		rtt2_prev = rtt2;
-		rtt2 = tsc - data_rx[0];
-
-		rdtsc_sleep(30000, start);
-
-#if DATAMOVER == RTDS_DM_FIFO
-		len = fifo_write(&f->fifo, (char *) data_tx, sizeof(uint32_t) * values_tx, irq_fifo);
-		if (len != sizeof(uint32_t) * values_tx)
-			error("Failed to send to FIFO");
-#elif DATAMOVER == RTDS_DM_DMA_SIMPLE
-		len = dma_write(&f->dma_simple, virt_to_dma(data_tx, f->dma), sizeof(uint32_t) * values_tx, -1);//f->vd.msi_efds[MSI_DMA_MM2S]);
-		XAxiDma_IntrAckIrq(&f->dma_simple, XAXIDMA_IRQ_IOC_MASK, XAXIDMA_DMA_TO_DEVICE);
-#elif DATAMOVER == RTDS_DM_DMA_SG
-		len = dma_write(&f->dma_sg, virt_to_dma(data_tx, f->dma), sizeof(uint32_t) * values_tx, f->vd.msi_efds[MSI_DMA_MM2S]);
-#endif
+		ret = dma_write_complete(dma, NULL, NULL);
+		if (ret)
+			error("Failed to complete DMA write: %d", ret);
 	}
 
-	intc_disable(f->intc, (1 << MSI_DMA_MM2S) | (1 << MSI_DMA_S2MM));
-#endif
+	ret = switch_disconnect(f->sw, rtds, dma);
+	if (ret)
+		error("Failed to configure switch");
+	ret = switch_disconnect(f->sw, dma, rtds);
+	if (ret)
+		error("Failed to configure switch");
+
 	return 0;
-}
+}
\ No newline at end of file