multithread preprocessing of ipecamera frames and code reorganization

43 files changed, 1699 insertions, 1266 deletions

diff --git a/.bzrignore b/.bzrignore
index 8f0badc..52e1fe8 100644
--- a/.bzrignore
+++ b/.bzrignore
@@ -12,3 +12,8 @@ ipecamera.d
 pci.d
 tools.d
 *.d
+CMakeCache.txt
+CMakeFiles
+cmake_install.cmake
+Makefile
+*.so.*
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..7664481
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,38 @@
+project(pcitool)
+
+set(PCILIB_VERSION "0.0.1")
+set(PCILIB_ABI_VERSION "0")
+
+cmake_minimum_required(VERSION 2.8)
+
+find_package(PkgConfig REQUIRED)
+
+#Check in sibling directory
+pkg_check_modules(UFODECODE ufodecode REQUIRED)
+
+set(HEADERS pcilib.h pci.h register.h kmem.h irq.h dma.h event.h default.h tools.h error.h)
+add_definitions("-fPIC --std=c99 -Wall -O2 -pthread")
+
+add_subdirectory(dma)
+add_subdirectory(ipecamera)
+add_subdirectory(pcitool)
+
+add_library(pcilib SHARED pci.c register.c kmem.c irq.c dma.c event.c default.c tools.c error.c) 
+target_link_libraries(pcilib ufodecode dma ipecamera)
+add_dependencies(pcilib dma ipecamera)
+
+set_target_properties(pcilib PROPERTIES
+    VERSION ${PCILIB_VERSION}
+    SOVERSION ${PCILIB_ABI_VERSION}
+    LINK_FLAGS "-pthread"
+#    LINK_FLAGS "-pthread -Wl,--whole-archive,dma/libdma.a,ipecamera/libipecamera.a,--no-whole-archive"
+)
+
+add_executable(pci cli.c)
+add_dependencies(pci pcitool)
+target_link_libraries(pci pcilib pcitool)
+
+#set_target_properties(pci PROPERTIES
+#    LINK_FLAGS "-Wl,pcitool/libpcitool.a"
+#)
+
diff --git a/Makefile b/Makefile
deleted file mode 100644
index f510ac3..0000000
--- a/Makefile
+++ /dev/null
@@ -1,39 +0,0 @@
-BINARIES += pci
-
-INCDIR += ./
-LDINC += $(addprefix -L ,$(LIBDIR))
-LDFLAGS += -pthread -lufodecode
-CFLAGS += -pthread
-DESTDIR ?= /usr/local
-
-all: $(BINARIES)
-
-.PHONY: all depend clean
-
-include common.mk
-
-###############################################################
-# Target definitions
-
-OBJECTS = pci.o pcitool/sysinfo.o register.o kmem.o irq.o dma.o event.o default.o tools.o dma/nwl.o dma/nwl_register.o dma/nwl_irq.o dma/nwl_engine.o dma/nwl_loopback.o ipecamera/model.o ipecamera/image.o 
-
-libpcilib.so: $(OBJECTS)
-	echo -e "LD \t$@"
-	$(Q)$(CC) $(LDINC) $(LDFLAGS) $(CFLAGS) -shared -o $@ $(OBJECTS)
-
-pci: cli.o pcitool/sysinfo.o libpcilib.so
-	echo -e "LD \t$@"
-	$(Q)$(CC) $(LDINC) $(LDFLAGS) $(CFLAGS) -L. -lpcilib -o $@ $<
-
-install: pci
-	install -m 644 pcilib.h $(DESTDIR)/include
-	install -m 644 ipecamera/ipecamera.h $(DESTDIR)/include
-	if [ -d $(DESTDIR)/lib64 ]; then install -m 755 libpcilib.so $(DESTDIR)/lib64; else install -m 755 libpcilib.so $(DESTDIR)/lib; fi
-	install -m 755 pci $(DESTDIR)/bin
-	ldconfig
-
-clean:
-	@echo -e "CLEAN \t$(shell pwd)"
-	-$(Q)rm -f $(addprefix $(BINDIR)/,$(BINARIES))
-	-$(Q)rm -f $(OBJ)
-	-$(Q)rm -f $(DEPEND)
diff --git a/NOTES b/NOTES
index 36411d3..d87bbfc 100644
--- a/NOTES
+++ b/NOTES
@@ -186,16 +186,32 @@ Shall we do a special handling in case of overflow?
 Buffering
 =========
  The DMA addresses are limited to 32 bits (~4GB for everything). This means we 
- can't really use DMA pages are sole buffers. Therefore,
- 1. In streaming mode a second thread will be spawned copying the data from the
-    DMA pages into the allocated buffers. On duration expiration this thread
-    will be stopped but processing will continue until all copyied data is 
-    passed to the callbacks.
- 2. In synchronous mode, a single event will be extracted from the the DMA
-    memory.
+ can't really use DMA pages are sole buffers. Therefore, a second thread, with
+ a realtime scheduling policy if possible, will be spawned and will copy the 
+ data from the DMA pages into the allocated buffers. On expiration of duration
+ or number of events set by autostop call, this thread will be stopped but 
+ processing in streaming mode will continue until all copyied data is passed 
+ to the callbacks.
 
- - Actually, we can make another in-module buffering. But this hopefully can
- be avoided.
+ To avoid stalls, the IPECamera requires data to be read continuously read out.
+ For this reason, there is no locks in the readout thread. It will simplify
+ overwrite the old frames if data is not copied out timely. To handle this case
+ after getting the data and processing it, the calling application should use
+ return_data function and check return code. This function may return error
+ indicating that the data was overwritten meanwhile. Hence, the data is 
+ corrupted and shoud be droped by the application. The copy_data function
+ performs this check and user application can be sure it get coherent data
+ in this case.
+ 
+ There is a way to avoid this problem. For raw data, the rawdata callback
+ can be requested. This callback blocks execution of readout thread and 
+ data may be treated safely by calling application. However, this may 
+ cause problems to electronics. Therefore, only memcpy should be performed
+ on the data normally. 
+
+ The reconstructed data, however, may be safely accessed. As described above,
+ the raw data will be continuously overwritten by the reader thread. However,
+ reconstructed data, upon the get_data call, will be protected by the mutex.
 
 
 Register Access Synchronization
diff --git a/cli.c b/cli.c
index 13a71f6..93e64fe 100644
--- a/cli.c
+++ b/cli.c
@@ -345,6 +345,8 @@ void List(pcilib_t *handle, pcilib_model_description_t *model_info, const char *
 		case PCILIB_DMA_TYPE_PACKET:
 		    printf("Packet");
 		break;
+		default:
+		    printf("Unknown");
 	    }
 	    
 	    printf(", Address Width: %02lu bits\n", engine->addr_bits);
@@ -371,7 +373,7 @@ void List(pcilib_t *handle, pcilib_model_description_t *model_info, const char *
     else registers = model_info->registers;
     
     if (registers) {
-        pcilib_register_bank_addr_t bank_addr;
+        pcilib_register_bank_addr_t bank_addr = 0;
 	if (bank) {
 	    pcilib_register_bank_t bank_id = pcilib_find_bank(handle, bank);
 	    pcilib_register_bank_description_t *b = model_info->banks + bank_id;
@@ -458,7 +460,7 @@ int Benchmark(pcilib_t *handle, ACCESS_MODE mode, pcilib_dma_engine_addr_t dma,
     int err;
     int i, j, errors;
     void *data, *buf, *check;
-    void *fifo;
+    void *fifo = NULL;
     struct timeval start, end;
     unsigned long time;
     size_t size, min_size, max_size;
@@ -483,7 +485,7 @@ int Benchmark(pcilib_t *handle, ACCESS_MODE mode, pcilib_dma_engine_addr_t dma,
 	    err = pcilib_wait_irq(handle, 0, 0, &irqs);
 	    if (err) irqs = 0;
 	    
-	    printf("%8i KB - ", size / 1024);
+	    printf("%8zu KB - ", size / 1024);
 	    
 	    printf("RW: ");
 	    if (mbs < 0) printf("failed ...   ");
@@ -571,7 +573,7 @@ int Benchmark(pcilib_t *handle, ACCESS_MODE mode, pcilib_dma_engine_addr_t dma,
 	gettimeofday(&end,NULL);
 
 	time = (end.tv_sec - start.tv_sec)*1000000 + (end.tv_usec - start.tv_usec);
-	printf("%8i bytes - read: %8.2lf MB/s", size, 1000000. * size * BENCHMARK_ITERATIONS / (time * 1024. * 1024.));
+	printf("%8zu bytes - read: %8.2lf MB/s", size, 1000000. * size * BENCHMARK_ITERATIONS / (time * 1024. * 1024.));
 	
 	fflush(0);
 
@@ -611,7 +613,7 @@ int Benchmark(pcilib_t *handle, ACCESS_MODE mode, pcilib_dma_engine_addr_t dma,
 	gettimeofday(&end,NULL);
 
 	time = (end.tv_sec - start.tv_sec)*1000000 + (end.tv_usec - start.tv_usec);
-	printf("%8i bytes - read: %8.2lf MB/s", size, 1000000. * size * BENCHMARK_ITERATIONS / (time * 1024. * 1024.));
+	printf("%8zu bytes - read: %8.2lf MB/s", size, 1000000. * size * BENCHMARK_ITERATIONS / (time * 1024. * 1024.));
 	
 	fflush(0);
 
@@ -651,6 +653,8 @@ int Benchmark(pcilib_t *handle, ACCESS_MODE mode, pcilib_dma_engine_addr_t dma,
 
     free(check);
     free(buf);
+    
+    return 0;
 }
 
 #define pci2host16(endianess, value) endianess?
@@ -793,7 +797,7 @@ int ReadRegister(pcilib_t *handle, pcilib_model_description_t *model_info, const
     const char *format;
 
     pcilib_register_bank_t bank_id;
-    pcilib_register_bank_addr_t bank_addr;
+    pcilib_register_bank_addr_t bank_addr = 0;
 
     pcilib_register_value_t value;
     
@@ -871,7 +875,7 @@ int ReadRegisterRange(pcilib_t *handle, pcilib_model_description_t *model_info,
     }
 
     int access = banks[bank_id].access / 8;
-    int size = n * abs(access);
+//    int size = n * abs(access);
     int block_width, blocks_per_line;
     int numbers_per_block, numbers_per_line; 
     
@@ -918,7 +922,7 @@ int ReadRegisterRange(pcilib_t *handle, pcilib_model_description_t *model_info,
 int WriteData(pcilib_t *handle, ACCESS_MODE mode, pcilib_dma_engine_addr_t dma, pcilib_bar_t bar, uintptr_t addr, size_t n, access_t access, int endianess, char ** data) {
     int read_back = 0;
     void *buf, *check;
-    int res, i, err;
+    int res = 0, i, err;
     int size = n * abs(access);
     size_t ret;
     pcilib_dma_engine_t dmaid;
@@ -933,6 +937,7 @@ int WriteData(pcilib_t *handle, ACCESS_MODE mode, pcilib_dma_engine_addr_t dma,
 	    case 2: res = sscanf(data[i], "%hx", ((uint16_t*)buf)+i); break;
 	    case 4: res = sscanf(data[i], "%x", ((uint32_t*)buf)+i); break;
 	    case 8: res = sscanf(data[i], "%lx", ((uint64_t*)buf)+i); break;
+	    default: Error("Unexpected data size (%lu)", access);
 	}
 	if ((res != 1)||(!isxnumber(data[i]))) Error("Can't parse data value at poition %i, (%s) is not valid hex number", i, data[i]);
     }
@@ -1010,12 +1015,11 @@ int WriteRegisterRange(pcilib_t *handle, pcilib_model_description_t *model_info,
 
 int WriteRegister(pcilib_t *handle, pcilib_model_description_t *model_info, const char *bank, const char *reg, char ** data) {
     int err;
-    int i;
 
     unsigned long val;
     pcilib_register_value_t value;
 
-    const char *format;
+    const char *format = NULL;
 
     pcilib_register_t regid = pcilib_find_register(handle, bank, reg);
     if (regid == PCILIB_REGISTER_INVALID) Error("Can't find register (%s) from bank (%s)", reg, bank?bank:"autodetected");
@@ -1118,13 +1122,13 @@ int GrabCallback(pcilib_event_id_t event_id, pcilib_event_info_t *info, void *us
     
     if (info->flags&PCILIB_EVENT_INFO_FLAG_BROKEN) {
 	ctx->broken_count++;
-	return 0;
+	return PCILIB_STREAMING_CONTINUE;
     }
 
     data = pcilib_get_data(handle, event_id, ctx->data, &size);
     if (!data) {
 	ctx->broken_count++;
-	return 0;
+	return PCILIB_STREAMING_CONTINUE;
     }
     
     
@@ -1136,7 +1140,7 @@ int GrabCallback(pcilib_event_id_t event_id, pcilib_event_info_t *info, void *us
 	else Error("Write failed");
     }
 
-    pcilib_return_data(handle, event_id, data);
+    pcilib_return_data(handle, event_id, ctx->data, data);
     
 //    printf("%lu %lu\n", info->seqnum, info->offset);
 
@@ -1161,11 +1165,12 @@ int GrabCallback(pcilib_event_id_t event_id, pcilib_event_info_t *info, void *us
 */
 
 //    printf("data callback: %lu\n", event_id);    
-    return 0;
+    return PCILIB_STREAMING_CONTINUE;
 }
 
 int raw_data(pcilib_event_id_t event_id, pcilib_event_info_t *info, pcilib_event_flags_t flags, size_t size, void *data, void *user) {
 //    printf("%i\n", event_id);
+    return PCILIB_STREAMING_CONTINUE;
 }
 
 
@@ -1204,7 +1209,7 @@ void *Trigger(void *user) {
 void GrabStats(GRABContext *ctx, struct timeval *end_time) {
     pcilib_timeout_t duration, fps_duration;
     struct timeval cur;
-    double fps;
+    double fps = 0;
 
     if (!end_time) {
 	gettimeofday(&cur, NULL);
@@ -1532,11 +1537,11 @@ int ListKMEM(pcilib_t *handle, const char *device) {
     
     while ((entry = readdir(dir)) != NULL) {
 	FILE *f;
-	unsigned long use;
-	unsigned long size;
-	unsigned long refs;
-	unsigned long mode;
-	unsigned long hwref;
+	unsigned long use = 0;
+	unsigned long size = 0;
+	unsigned long refs = 0;
+	unsigned long mode = 0;
+	unsigned long hwref = 0;
 	
 	if (strncmp(entry->d_name, "kbuf", 4)) continue;
 	if (!isnumber(entry->d_name+4)) continue;
@@ -1581,9 +1586,9 @@ int ListKMEM(pcilib_t *handle, const char *device) {
 	
 	printf("%08lx  ", uses[i].use);
 	if (!i) printf("All Others         ");
-	else if ((uses[i].use >> 16) == PCILIB_KMEM_USE_DMA_RING) printf("DMA%u %s Ring      ", uses[i].use&0x7F, ((uses[i].use&0x80)?"S2C":"C2S"));
-	else if ((uses[i].use >> 16) == PCILIB_KMEM_USE_DMA_PAGES) printf("DMA%u %s Pages     ", uses[i].use&0x7F, ((uses[i].use&0x80)?"S2C":"C2S"));
-	else printf ("                   ", uses[i].use);
+	else if ((uses[i].use >> 16) == PCILIB_KMEM_USE_DMA_RING) printf("DMA%lu %s Ring      ", uses[i].use&0x7F, ((uses[i].use&0x80)?"S2C":"C2S"));
+	else if ((uses[i].use >> 16) == PCILIB_KMEM_USE_DMA_PAGES) printf("DMA%lu %s Pages     ", uses[i].use&0x7F, ((uses[i].use&0x80)?"S2C":"C2S"));
+	else printf ("                   ");
 	printf("  ");
 	printf("% 6lu", uses[i].count);
 	printf("     ");
@@ -1611,13 +1616,13 @@ int ReadKMEM(pcilib_t *handle, const char *device, pcilib_kmem_use_t use, size_t
     void *data;
     size_t size;
     pcilib_kmem_handle_t *kbuf;
-    
+
     kbuf = pcilib_alloc_kernel_memory(handle, 0, block + 1, 0, 0, use, PCILIB_KMEM_FLAG_REUSE|PCILIB_KMEM_FLAG_TRY);
     if (!kbuf) {
 	printf("The specified kernel buffer is not allocated\n");
 	return 0;
     }
-    
+
     data = pcilib_kmem_get_block_ua(handle, kbuf, block);
     if (data) {
 	size = pcilib_kmem_get_block_size(handle, kbuf, block);
@@ -1626,16 +1631,18 @@ int ReadKMEM(pcilib_t *handle, const char *device, pcilib_kmem_use_t use, size_t
     } else {
 	printf("The specified block is not existing\n");
     }
-    
+
     pcilib_free_kernel_memory(handle, kbuf, KMEM_FLAG_REUSE);
+
+    return 0;
 }
 
 int FreeKMEM(pcilib_t *handle, const char *device, const char *use, int force) {
     int err;
     int i;
-    
+
     unsigned long useid;
-    
+
     pcilib_kmem_flags_t flags = PCILIB_KMEM_FLAG_HARDWARE|PCILIB_KMEM_FLAG_PERSISTENT|PCILIB_KMEM_FLAG_EXCLUSIVE; 
     if (force) flags |= PCILIB_KMEM_FLAG_FORCE; // this will ignore mmap locks as well.
 
@@ -1653,12 +1660,12 @@ int FreeKMEM(pcilib_t *handle, const char *device, const char *use, int force) {
 	
 	return 0;
     }
-    
+
     if ((!isxnumber(use))||(sscanf(use, "%lx", &useid) != 1)) Error("Invalid use (%s) is specified", use);
-    
+
     err = pcilib_clean_kernel_memory(handle, useid, flags);
     if (err) Error("Error cleaning kernel buffers for use (0x%lx)", useid);
-    
+
     return 0;
 }
 
@@ -1689,11 +1696,11 @@ int ListDMA(pcilib_t *handle, const char *device, pcilib_model_description_t *mo
     printf("--------------------------------------------------------------------------------\n");
     while ((entry = readdir(dir)) != NULL) {
 	FILE *f;
-	unsigned long use;
-	unsigned long size;
-	unsigned long refs;
-	unsigned long mode;
-	unsigned long hwref;
+	unsigned long use = 0;
+	unsigned long size = 0;
+	unsigned long refs = 0;
+	unsigned long mode = 0;
+	unsigned long hwref = 0;
 	
 	if (strncmp(entry->d_name, "kbuf", 4)) continue;
 	if (!isnumber(entry->d_name+4)) continue;
@@ -1724,7 +1731,7 @@ int ListDMA(pcilib_t *handle, const char *device, pcilib_model_description_t *mo
 	if (dmaid == PCILIB_DMA_ENGINE_INVALID) continue;
 	
 	
-	printf("DMA%u %s         ", use&0x7F, (use&0x80)?"S2C":"C2S");
+	printf("DMA%lu %s         ", use&0x7F, (use&0x80)?"S2C":"C2S");
         err = pcilib_start_dma(handle, dmaid, 0);
         if (err) {
     	    printf("-- Wrong state, start is failed\n");
@@ -1773,47 +1780,46 @@ int ListBuffers(pcilib_t *handle, const char *device, pcilib_model_description_t
 
     dmaid = pcilib_find_dma_by_addr(handle, dma_direction, dma);
     if (dmaid == PCILIB_DMA_ENGINE_INVALID) Error("The specified DMA engine is not found");
-    
+
     err = pcilib_start_dma(handle, dmaid, 0);
     if (err) Error("Error starting the specified DMA engine");
-    
+
     err = pcilib_get_dma_status(handle, dmaid, &status, 0, NULL);
     if (err) Error("Failed to obtain status of the specified DMA engine");
-    
+
     buffer = (pcilib_dma_buffer_status_t*)malloc(status.ring_size*sizeof(pcilib_dma_buffer_status_t));
     if (!buffer) Error("Failed to allocate memory for status buffer");
-    
+
     err = pcilib_get_dma_status(handle, dmaid, &status, status.ring_size, buffer);
     if (err) Error("Failed to obtain extended status of the specified DMA engine");
-    
-    
+
+
     printf("Buffer      Status      Total Size         \n");
     printf("--------------------------------------------------------------------------------\n");
-    
+
     for (i = 0; i < status.ring_size; i++) {
 	printf("%8zu    ", i);
         printf("%c%c %c%c ", buffer[i].used?'U':' ',  buffer[i].error?'E':' ', buffer[i].first?'F':' ', buffer[i].last?'L':' ');
 	printf("% 10s", PrintSize(stmp, buffer[i].size));
 	printf("\n");
     }
-    
+
     printf("--------------------------------------------------------------------------------\n");
     printf("U - Used, E - Error, F - First block, L - Last Block\n");
-    
+
     free(buffer);
 
     pcilib_stop_dma(handle, dmaid, 0);
 
+    return 0;
 }
 
 int ReadBuffer(pcilib_t *handle, const char *device, pcilib_model_description_t *model_info, pcilib_dma_engine_addr_t dma, pcilib_dma_direction_t dma_direction, size_t block, FILE *o) {
     int err;
-    size_t i;
     pcilib_dma_engine_t dmaid;
     pcilib_dma_engine_status_t status;
     pcilib_dma_buffer_status_t *buffer;
     size_t size;
-    char stmp[256];
 
     dmaid = pcilib_find_dma_by_addr(handle, dma_direction, dma);
     if (dmaid == PCILIB_DMA_ENGINE_INVALID) Error("The specified DMA engine is not found");
@@ -1902,9 +1908,9 @@ int main(int argc, char **argv) {
     const char *data_type = NULL;
     const char *dma_channel = NULL;
     const char *use = NULL;
-    pcilib_kmem_use_t use_id;
+    pcilib_kmem_use_t use_id = 0;
     size_t block = 0;
-    pcilib_irq_hw_source_t irq_source;
+    pcilib_irq_hw_source_t irq_source =  PCILIB_IRQ_SOURCE_DEFAULT;
     pcilib_dma_direction_t dma_direction = PCILIB_DMA_BIDIRECTIONAL;
     
     pcilib_dma_engine_addr_t dma = PCILIB_DMA_ENGINE_ADDR_INVALID;
@@ -1912,7 +1918,7 @@ int main(int argc, char **argv) {
     uintptr_t start = -1;
     size_t size = 1;
     access_t access = 4;
-    int skip = 0;
+//    int skip = 0;
     int endianess = 0;
     size_t timeout = 0;
     const char *output = NULL;
@@ -1920,7 +1926,7 @@ int main(int argc, char **argv) {
     size_t iterations = BENCHMARK_ITERATIONS;
 
     pcilib_t *handle;
-    
+
     int size_set = 0;
     int timeout_set = 0;
     int run_time_set = 0;
@@ -2046,11 +2052,14 @@ int main(int argc, char **argv) {
 		mode = MODE_WAIT_IRQ;
 		if (optarg) num_offset = optarg;
 		else if ((optind < argc)&&(argv[optind][0] != '-'))  num_offset = argv[optind++];
+		else num_offset = NULL;
 		
-		if ((!isnumber(num_offset))||(sscanf(num_offset, "%li", &itmp) != 1))
-		    Usage(argc, argv, "Invalid IRQ source is specified (%s)", num_offset);
+		if (num_offset) {
+		    if ((!isnumber(num_offset))||(sscanf(num_offset, "%li", &itmp) != 1))
+			Usage(argc, argv, "Invalid IRQ source is specified (%s)", num_offset);
 
-		irq_source = itmp;
+		    irq_source = itmp;
+		}
 	    break;
 	    case OPT_LIST_KMEM:
 		if (mode != MODE_INVALID) Usage(argc, argv, "Multiple operations are not supported");
@@ -2132,11 +2141,12 @@ int main(int argc, char **argv) {
 		}
 	    break;
 	    case OPT_SIZE:
-		if ((!isnumber(optarg))||(sscanf(optarg, "%zu", &size) != 1))
+		if ((!isnumber(optarg))||(sscanf(optarg, "%zu", &size) != 1)) {
 		    if (strcasecmp(optarg, "unlimited"))
 			Usage(argc, argv, "Invalid size is specified (%s)", optarg);
 		    else
 			size = 0;//(size_t)-1;
+		}
 			
 		size_set = 1;
 	    break;
@@ -2151,11 +2161,12 @@ int main(int argc, char **argv) {
 		
 	    break;
 	    case OPT_TIMEOUT:
-		if ((!isnumber(optarg))||(sscanf(optarg, "%zu", &timeout) != 1))
+		if ((!isnumber(optarg))||(sscanf(optarg, "%zu", &timeout) != 1)) {
 		    if (strcasecmp(optarg, "unlimited"))
 			Usage(argc, argv, "Invalid timeout is specified (%s)", optarg);
 		    else
 			timeout = PCILIB_TIMEOUT_INFINITE;
+		}
 		timeout_set = 1;
 	    break;
 	    case OPT_OUTPUT:
@@ -2172,11 +2183,12 @@ int main(int argc, char **argv) {
 		data_type = optarg;
 	    break;
 	    case OPT_RUN_TIME:
-		if ((!isnumber(optarg))||(sscanf(optarg, "%zu", &run_time) != 1))
+		if ((!isnumber(optarg))||(sscanf(optarg, "%zu", &run_time) != 1)) {
 		    if (strcasecmp(optarg, "unlimited"))
 			Usage(argc, argv, "Invalid run-time is specified (%s)", optarg);
 		    else
 			run_time = 0;
+		}
 		run_time_set = 1;
 	    break;
 	    case OPT_TRIGGER_TIME:
@@ -2461,6 +2473,8 @@ int main(int argc, char **argv) {
      case MODE_FREE_KMEM:
         FreeKMEM(handle, fpga_device, use, force);
      break;
+     case MODE_INVALID:
+        break;
     }
 
     if (ofile) fclose(ofile);
diff --git a/default.c b/default.c
index 15ae076..0ea4d61 100644
--- a/default.c
+++ b/default.c
@@ -9,15 +9,13 @@
 #define default_datacpy(dst, src, access, bank)   pcilib_datacpy(dst, src, access, 1, bank->raw_endianess)
 
 int pcilib_default_read(pcilib_t *ctx, pcilib_register_bank_description_t *bank, pcilib_register_addr_t addr, pcilib_register_value_t *value) {
-    int err;
-    
     char *ptr;
     pcilib_register_value_t val = 0;
     int access = bank->access / 8;
 
     ptr =  pcilib_resolve_register_address(ctx, bank->bar, bank->read_addr + addr);
     default_datacpy(&val, ptr, access, bank);
-    
+
 //    *value = val&BIT_MASK(bits);
     *value = val;
 
@@ -26,8 +24,6 @@ int pcilib_default_read(pcilib_t *ctx, pcilib_register_bank_description_t *bank,
 
 
 int pcilib_default_write(pcilib_t *ctx, pcilib_register_bank_description_t *bank, pcilib_register_addr_t addr, pcilib_register_value_t value) {
-    int err;
-    
     char *ptr;
     int access = bank->access / 8;
 
diff --git a/dma.c b/dma.c
index 24dd89e..19ac5ab 100644
--- a/dma.c
+++ b/dma.c
@@ -55,11 +55,11 @@ pcilib_dma_engine_t pcilib_find_dma_by_addr(pcilib_t *ctx, pcilib_dma_direction_
 
 int pcilib_set_dma_engine_description(pcilib_t *ctx, pcilib_dma_engine_t engine, pcilib_dma_engine_description_t *desc) {
     ctx->dma_info.engines[engine] = desc;
+
+    return 0;
 }
 
 int pcilib_start_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, pcilib_dma_flags_t flags) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -79,8 +79,6 @@ int pcilib_start_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, pcilib_dma_flags_t
 }
 
 int pcilib_stop_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, pcilib_dma_flags_t flags) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -100,8 +98,6 @@ int pcilib_stop_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, pcilib_dma_flags_t f
 }
 
 int pcilib_enable_irq(pcilib_t *ctx, pcilib_irq_type_t irq_type, pcilib_dma_flags_t flags) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -121,8 +117,6 @@ int pcilib_enable_irq(pcilib_t *ctx, pcilib_irq_type_t irq_type, pcilib_dma_flag
 }
 
 int pcilib_disable_irq(pcilib_t *ctx, pcilib_dma_flags_t flags) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -142,8 +136,6 @@ int pcilib_disable_irq(pcilib_t *ctx, pcilib_dma_flags_t flags) {
 }
 
 int pcilib_acknowledge_irq(pcilib_t *ctx, pcilib_irq_type_t irq_type, pcilib_irq_source_t irq_source) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -208,8 +200,6 @@ static int pcilib_dma_skip_callback(void *arg, pcilib_dma_flags_t flags, size_t
 }
 
 int pcilib_stream_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, uintptr_t addr, size_t size, pcilib_dma_flags_t flags, pcilib_timeout_t timeout, pcilib_dma_callback_t cb, void *cbattr) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -231,7 +221,7 @@ int pcilib_stream_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, uintptr_t addr, si
 	return PCILIB_ERROR_NOTAVAILABLE;
     }
 
-    if (info->engines[dma]->direction&PCILIB_DMA_FROM_DEVICE == 0) {
+    if ((info->engines[dma]->direction&PCILIB_DMA_FROM_DEVICE) == 0) {
 	pcilib_error("The selected engine (%i) is S2C-only and does not support reading", dma);
 	return PCILIB_ERROR_NOTSUPPORTED;
     }
@@ -286,8 +276,6 @@ int pcilib_skip_dma(pcilib_t *ctx, pcilib_dma_engine_t dma) {
 
 
 int pcilib_push_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, uintptr_t addr, size_t size, pcilib_dma_flags_t flags, pcilib_timeout_t timeout, void *buf, size_t *written) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -309,7 +297,7 @@ int pcilib_push_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, uintptr_t addr, size
 	return PCILIB_ERROR_NOTAVAILABLE;
     }
 
-    if (info->engines[dma]->direction&PCILIB_DMA_TO_DEVICE == 0) {
+    if ((info->engines[dma]->direction&PCILIB_DMA_TO_DEVICE) == 0) {
 	pcilib_error("The selected engine (%i) is C2S-only and does not support writes", dma);
 	return PCILIB_ERROR_NOTSUPPORTED;
     }
@@ -323,8 +311,6 @@ int pcilib_write_dma(pcilib_t *ctx, pcilib_dma_engine_t dma, uintptr_t addr, siz
 }
 
 double pcilib_benchmark_dma(pcilib_t *ctx, pcilib_dma_engine_addr_t dma, uintptr_t addr, size_t size, size_t iterations, pcilib_dma_direction_t direction) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -350,8 +336,6 @@ double pcilib_benchmark_dma(pcilib_t *ctx, pcilib_dma_engine_addr_t dma, uintptr
 }
 
 int pcilib_get_dma_status(pcilib_t *ctx, pcilib_dma_engine_t dma, pcilib_dma_engine_status_t *status, size_t n_buffers, pcilib_dma_buffer_status_t *buffers) {
-    int err; 
-
     const pcilib_dma_info_t *info =  pcilib_get_dma_info(ctx);
     if (!info) {
 	pcilib_error("DMA is not supported by the device");
@@ -374,5 +358,4 @@ int pcilib_get_dma_status(pcilib_t *ctx, pcilib_dma_engine_t dma, pcilib_dma_eng
     }
 
     return ctx->model_info.dma_api->status(ctx->dma_ctx, dma, status, n_buffers, buffers);
-
 }
diff --git a/dma/CMakeLists.txt b/dma/CMakeLists.txt
new file mode 100644
index 0000000..3d4226a
--- /dev/null
+++ b/dma/CMakeLists.txt
@@ -0,0 +1,9 @@
+include_directories(
+    ${CMAKE_SOURCE_DIR}
+)
+
+
+set(HEADERS ${HEADERS} nwl.h nwl_dma.h nwl_engine.h nwl_irq.h nwl_loopback.h nwl_register.h)
+
+add_library(dma STATIC nwl.c nwl_engine.c nwl_irq.c nwl_loopback.c nwl_register.c)
+
diff --git a/dma/nwl.c b/dma/nwl.c
index cc03687..df5d0b8 100644
--- a/dma/nwl.c
+++ b/dma/nwl.c
@@ -77,7 +77,6 @@ int dma_nwl_stop(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, pcilib_dma
 pcilib_dma_context_t *dma_nwl_init(pcilib_t *pcilib, pcilib_dma_modification_t type, void *arg) {
     int i;
     int err;
-    uint32_t val;
     pcilib_dma_engine_t n_engines;
 
     pcilib_model_description_t *model_info = pcilib_get_model_description(pcilib);
@@ -128,9 +127,6 @@ pcilib_dma_context_t *dma_nwl_init(pcilib_t *pcilib, pcilib_dma_modification_t t
 }
 
 void  dma_nwl_free(pcilib_dma_context_t *vctx) {
-    int err;
-    
-    pcilib_dma_engine_t i;
     nwl_dma_t *ctx = (nwl_dma_t*)vctx;
 
     if (ctx) {
diff --git a/dma/nwl_defines.h b/dma/nwl_defines.h
index c1ff2eb..ce3b686 100644
--- a/dma/nwl_defines.h
+++ b/dma/nwl_defines.h
@@ -33,7 +33,7 @@
 
 
 
-#define DMA_BD_MINIMUM_ALIGNMENT    0x40  /**< Minimum byte alignment
+#define DMA_BD_MINIMUM_ALIGNMENT    0x40  /**< Minimum byte alignment */
 
 /* Common DMA registers */
 #define REG_DMA_CTRL_STATUS     0x4000      /**< DMA Common Ctrl & Status */
diff --git a/dma/nwl_engine.c b/dma/nwl_engine.c
index ac87f08..fc07ccd 100644
--- a/dma/nwl_engine.c
+++ b/dma/nwl_engine.c
@@ -267,14 +267,13 @@ int dma_nwl_write_fragment(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma,
 
 int dma_nwl_stream_read(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, uintptr_t addr, size_t size, pcilib_dma_flags_t flags, pcilib_timeout_t timeout, pcilib_dma_callback_t cb, void *cbattr) {
     int err, ret = PCILIB_STREAMING_REQ_PACKET;
+    pcilib_timeout_t wait = 0;
     size_t res = 0;
     size_t bufnum;
     size_t bufsize;
-    pcilib_timeout_t wait;
     
     nwl_dma_t *ctx = (nwl_dma_t*)vctx;
 
-    size_t buf_size;
     int eop;
 
     pcilib_nwl_engine_description_t *info = ctx->engines + dma;
@@ -286,7 +285,7 @@ int dma_nwl_stream_read(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, uin
 	switch (ret&PCILIB_STREAMING_TIMEOUT_MASK) {
 	    case PCILIB_STREAMING_CONTINUE: wait = PCILIB_DMA_TIMEOUT; break;
 	    case PCILIB_STREAMING_WAIT: wait = timeout; break;
-	    case PCILIB_STREAMING_CHECK: wait = 0; break;
+//	    case PCILIB_STREAMING_CHECK: wait = 0; break;
 	}
     
         bufnum = dma_nwl_wait_buffer(ctx, info, &bufsize, &eop, wait);
diff --git a/dma/nwl_engine_buffers.h b/dma/nwl_engine_buffers.h
index b97e469..191a2a6 100644
--- a/dma/nwl_engine_buffers.h
+++ b/dma/nwl_engine_buffers.h
@@ -3,7 +3,6 @@
 #define NWL_RING_UPDATE(data, offset, mask, val) *(uint32_t*)(((char*)(data)) + (offset)) = ((*(uint32_t*)(((char*)(data)) + (offset)))&(mask))|(val)
 
 static int dma_nwl_compute_read_s2c_pointers(nwl_dma_t *ctx, pcilib_nwl_engine_description_t *info, unsigned char *ring, uint32_t ring_pa) {
-    size_t pos;
     uint32_t val;
 
     char *base = info->base_addr;
@@ -42,13 +41,10 @@ static int dma_nwl_compute_read_s2c_pointers(nwl_dma_t *ctx, pcilib_nwl_engine_d
 }
 
 static int dma_nwl_compute_read_c2s_pointers(nwl_dma_t *ctx, pcilib_nwl_engine_description_t *info, unsigned char *ring, uint32_t ring_pa) {
-    size_t pos;
     uint32_t val;
-    size_t prev;
 
     char *base = info->base_addr;
 
-    
     nwl_read_register(val, ctx, base, REG_SW_NEXT_BD);
     if ((val < ring_pa)||((val - ring_pa) % PCILIB_NWL_DMA_DESCRIPTOR_SIZE)) {
 	if (val < ring_pa) pcilib_warning("Inconsistent C2S DMA Ring buffer is found (REG_SW_NEXT_BD register value (%lx) is below start of the ring [%lx,%lx])", val, ring_pa, PCILIB_NWL_DMA_DESCRIPTOR_SIZE);
@@ -112,8 +108,8 @@ static int dma_nwl_allocate_engine_buffers(nwl_dma_t *ctx, pcilib_nwl_engine_des
 	if (reuse_ring == reuse_pages) {
 	    if (reuse_ring & PCILIB_KMEM_REUSE_PARTIAL) pcilib_warning("Inconsistent DMA buffers are found (only part of required buffers is available), reinitializing...");
 	    else if (reuse_ring & PCILIB_KMEM_REUSE_REUSED) {
-		if (reuse_ring & PCILIB_KMEM_REUSE_PERSISTENT == 0) pcilib_warning("Lost DMA buffers are found (non-persistent mode), reinitializing...");
-		else if (reuse_ring & PCILIB_KMEM_REUSE_HARDWARE == 0) pcilib_warning("Lost DMA buffers are found (missing HW reference), reinitializing...");
+		if ((reuse_ring & PCILIB_KMEM_REUSE_PERSISTENT) == 0) pcilib_warning("Lost DMA buffers are found (non-persistent mode), reinitializing...");
+		else if ((reuse_ring & PCILIB_KMEM_REUSE_HARDWARE) == 0) pcilib_warning("Lost DMA buffers are found (missing HW reference), reinitializing...");
 		else {
 		    nwl_read_register(val, ctx, info->base_addr, REG_DMA_ENG_CTRL_STATUS);
 
@@ -174,7 +170,7 @@ static int dma_nwl_allocate_engine_buffers(nwl_dma_t *ctx, pcilib_nwl_engine_des
 
 static size_t dma_nwl_clean_buffers(nwl_dma_t * ctx, pcilib_nwl_engine_description_t *info) {
     size_t res = 0;
-    uint32_t status, control;
+    uint32_t status;
 
     unsigned char *ring = pcilib_kmem_get_ua(ctx->pcilib, info->ring);
     ring += info->tail * PCILIB_NWL_DMA_DESCRIPTOR_SIZE;
@@ -282,9 +278,8 @@ static int dma_nwl_push_buffer(nwl_dma_t *ctx, pcilib_nwl_engine_description_t *
 
 
 static size_t dma_nwl_wait_buffer(nwl_dma_t *ctx, pcilib_nwl_engine_description_t *info, size_t *size, int *eop, pcilib_timeout_t timeout) {
-    uint32_t val;
     struct timeval start, cur;
-    uint32_t status_size, status, control;
+    uint32_t status_size, status;
 
     unsigned char *ring = pcilib_kmem_get_ua(ctx->pcilib, info->ring);
     
@@ -325,6 +320,8 @@ static size_t dma_nwl_wait_buffer(nwl_dma_t *ctx, pcilib_nwl_engine_description_
     return (size_t)-1;
 }
 
+/*
+    // This function is not used now, but we may need it in the future
 static int dma_nwl_is_overflown(nwl_dma_t *ctx, pcilib_nwl_engine_description_t *info) {
     uint32_t status;
     unsigned char *ring = pcilib_kmem_get_ua(ctx->pcilib, info->ring);
@@ -334,6 +331,7 @@ static int dma_nwl_is_overflown(nwl_dma_t *ctx, pcilib_nwl_engine_description_t
     status = NWL_RING_GET(ring, DMA_BD_BUFL_STATUS_OFFSET);
     return status&DMA_BD_COMP_MASK?1:0;
 }
+*/
 
 static int dma_nwl_return_buffer(nwl_dma_t *ctx, pcilib_nwl_engine_description_t *info) {
     uint32_t val;
@@ -357,6 +355,8 @@ static int dma_nwl_return_buffer(nwl_dma_t *ctx, pcilib_nwl_engine_description_t
     
     info->tail++;
     if (info->tail == info->ring_size) info->tail = 0;
+    
+    return 0;
 }
 
 int dma_nwl_get_status(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, pcilib_dma_engine_status_t *status, size_t n_buffers, pcilib_dma_buffer_status_t *buffers) {
@@ -375,7 +375,7 @@ int dma_nwl_get_status(pcilib_dma_context_t *vctx, pcilib_dma_engine_t dma, pcil
     status->ring_tail = info->tail;
     
     if (info->desc.direction == PCILIB_DMA_FROM_DEVICE) {
-	size_t pos;
+	size_t pos = 0;
 	for (i = 0; i < info->ring_size; i++) {
 	    pos = status->ring_tail + i;
 	    if (pos >= info->ring_size) pos -= info->ring_size;
diff --git a/dma/nwl_loopback.c b/dma/nwl_loopback.c
index 11f7f34..ce0844f 100644
--- a/dma/nwl_loopback.c
+++ b/dma/nwl_loopback.c
@@ -66,13 +66,10 @@ int dma_nwl_stop_loopback(nwl_dma_t *ctx) {
 
 double dma_nwl_benchmark(pcilib_dma_context_t *vctx, pcilib_dma_engine_addr_t dma, uintptr_t addr, size_t size, size_t iterations, pcilib_dma_direction_t direction) {
     int iter, i;
-    int res;
     int err;
     size_t bytes, rbytes;
-    uint32_t val;
     uint32_t *buf, *cmp;
     const char *error = NULL;
-    pcilib_register_value_t regval;
     size_t packet_size, blocks;    
 
     size_t us = 0;
@@ -83,9 +80,6 @@ double dma_nwl_benchmark(pcilib_dma_context_t *vctx, pcilib_dma_engine_addr_t dm
     pcilib_dma_engine_t readid = pcilib_find_dma_by_addr(ctx->pcilib, PCILIB_DMA_FROM_DEVICE, dma);
     pcilib_dma_engine_t writeid = pcilib_find_dma_by_addr(ctx->pcilib, PCILIB_DMA_TO_DEVICE, dma);
 
-    char *read_base = ctx->engines[readid].base_addr;
-    char *write_base = ctx->engines[writeid].base_addr;
-
     if (size%sizeof(uint32_t)) size = 1 + size / sizeof(uint32_t);
     else size /= sizeof(uint32_t);
 
@@ -214,8 +208,7 @@ double dma_nwl_benchmark(pcilib_dma_context_t *vctx, pcilib_dma_engine_addr_t dm
 	
 #ifndef NWL_BUG_EXTRA_DATA
 	if (direction == PCILIB_DMA_BIDIRECTIONAL) {
-	    res = memcmp(buf, cmp, size * sizeof(uint32_t));
-	    if (res) {
+	    if (memcmp(buf, cmp, size * sizeof(uint32_t))) {
 		for (i = 0; i < size; i++)
 		    if (buf[i] != cmp[i]) break;
 		
diff --git a/dma/nwl_register.c b/dma/nwl_register.c
index 95b981a..5f94e4d 100644
--- a/dma/nwl_register.c
+++ b/dma/nwl_register.c
@@ -1,3 +1,5 @@
+#define _PCILIB_NWL_REGISTER_C 
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
diff --git a/dma/nwl_register.h b/dma/nwl_register.h
index 2f465bd..a71942b 100644
--- a/dma/nwl_register.h
+++ b/dma/nwl_register.h
@@ -1,6 +1,7 @@
 #ifndef _PCILIB_NWL_REGISTERS_H
 #define _PCILIB_NWL_REGISTERS_H 
 
+#ifdef _PCILIB_NWL_REGISTER_C 
   // DMA
 static pcilib_register_description_t nwl_dma_registers[] = {
     {0x4000, 	0, 	32, 	0, 	0x00000011,	PCILIB_REGISTER_RW  , PCILIB_REGISTER_STANDARD, PCILIB_REGISTER_BANK_DMA, "dma_control_and_status",  ""},
@@ -89,5 +90,8 @@ static pcilib_register_description_t nwl_xrawdata_registers[] = {
     {0,		0,	0,	0,	0x00000000,	0,                                            0,                        0, NULL, NULL}
 };
 
+#endif /* _PCILIB_NWL_REGISTERS_C */
+
 int nwl_add_registers(nwl_dma_t *ctx);
+
 #endif /* _PCILIB_NWL_REGISTERS_H */
diff --git a/driver/pciDriver.h b/driver/pciDriver.h
index 3ffe8b7..159f6ad 100644
--- a/driver/pciDriver.h
+++ b/driver/pciDriver.h
@@ -57,7 +57,7 @@
  */
 
 #include <linux/ioctl.h>
-#include "pcilib_types.h"
+#include "../pcilib_types.h"
 
 /* Identifies the PCI-E Xilinx ML605 */
 #define PCIE_XILINX_VENDOR_ID 0x10ee
diff --git a/error.c b/error.c
new file mode 100644
index 0000000..b4f6e2e
--- /dev/null
+++ b/error.c
@@ -0,0 +1,27 @@
+#define _PCILIB_ERROR_C
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#include "error.h"
+
+static void pcilib_print_error(const char *msg, ...) {
+    va_list va;
+    
+    va_start(va, msg);
+    vprintf(msg, va);
+    va_end(va);
+    printf("\n");
+}
+
+void (*pcilib_error)(const char *msg, ...) = pcilib_print_error;
+void (*pcilib_warning)(const char *msg, ...) = pcilib_print_error;
+
+int pcilib_set_error_handler(void (*err)(const char *msg, ...), void (*warn)(const char *msg, ...)) {
+    if (err) pcilib_error = err;
+    else pcilib_error = pcilib_print_error;
+    if (warn) pcilib_warning = warn;
+    else pcilib_warning = pcilib_print_error;
+
+    return 0;
+}
diff --git a/error.h b/error.h
index a5a9493..cbf2d98 100644
--- a/error.h
+++ b/error.h
@@ -1,25 +1,33 @@
 #ifndef _PCILIB_ERROR_H
 #define _PCILIB_ERROR_H
+
+#include <errno.h>
  
 enum {
     PCILIB_ERROR_SUCCESS = 0,
-    PCILIB_ERROR_MEMORY,
-    PCILIB_ERROR_INVALID_REQUEST,
-    PCILIB_ERROR_INVALID_ADDRESS,
-    PCILIB_ERROR_INVALID_BANK,
-    PCILIB_ERROR_INVALID_DATA,
-    PCILIB_ERROR_INVALID_STATE,
-    PCILIB_ERROR_TIMEOUT,
-    PCILIB_ERROR_FAILED,
-    PCILIB_ERROR_VERIFY,
-    PCILIB_ERROR_NOTSUPPORTED,
-    PCILIB_ERROR_NOTFOUND,
-    PCILIB_ERROR_OUTOFRANGE,
-    PCILIB_ERROR_NOTAVAILABLE,
-    PCILIB_ERROR_NOTINITIALIZED,
-    PCILIB_ERROR_TOOBIG,
-    PCILIB_ERROR_THREAD
+    PCILIB_ERROR_MEMORY = ENOMEM,
+    PCILIB_ERROR_INVALID_REQUEST = EBADR,
+    PCILIB_ERROR_INVALID_ADDRESS = EFAULT,
+    PCILIB_ERROR_INVALID_BANK = ENOENT,
+    PCILIB_ERROR_INVALID_DATA = EILSEQ,
+    PCILIB_ERROR_INVALID_STATE =  EBADFD,
+    PCILIB_ERROR_INVALID_ARGUMENT = EINVAL,
+    PCILIB_ERROR_TIMEOUT = ETIME,
+    PCILIB_ERROR_FAILED = EBADE,
+    PCILIB_ERROR_VERIFY = EREMOTEIO,
+    PCILIB_ERROR_NOTSUPPORTED = ENOTSUP,
+    PCILIB_ERROR_NOTFOUND = ESRCH,
+    PCILIB_ERROR_OUTOFRANGE = ERANGE,
+    PCILIB_ERROR_NOTAVAILABLE = ENAVAIL,
+    PCILIB_ERROR_NOTINITIALIZED = EBADFD,
+    PCILIB_ERROR_TOOBIG = EFBIG,
+    PCILIB_ERROR_OVERWRITTEN = ESTALE
 } pcilib_errot_t;
 
 
+#ifndef _PCILIB_ERROR_C
+extern void (*pcilib_error)(const char *msg, ...);
+extern void (*pcilib_warning)(const char *msg, ...);
+#endif /* _PCILIB_ERROR_C */
+
 #endif /* _PCILIB_ERROR_H */
diff --git a/event.c b/event.c
index 1711400..a277e46 100644
--- a/event.c
+++ b/event.c
@@ -29,12 +29,10 @@ struct timespec {
 
 pcilib_event_t pcilib_find_event(pcilib_t *ctx, const char *event) {
     int i;
-    pcilib_register_bank_t res;
-    unsigned long addr;
-    
+
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
     pcilib_event_description_t *events = model_info->events;
-    
+
     for (i = 0; events[i].name; i++) {
 	if (!strcasecmp(events[i].name, event)) return events[i].evid;
     }
@@ -44,8 +42,6 @@ pcilib_event_t pcilib_find_event(pcilib_t *ctx, const char *event) {
 
 pcilib_event_data_type_t pcilib_find_event_data_type(pcilib_t *ctx, pcilib_event_t event, const char *data_type) {
     int i;
-    pcilib_register_bank_t res;
-    unsigned long addr;
     
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
     pcilib_event_data_type_description_t *data_types = model_info->data_types;
@@ -193,8 +189,7 @@ static int pcilib_return_event_callback(pcilib_event_id_t event_id, pcilib_event
 }
 */
 
-int pcilib_get_next_event(pcilib_t *ctx, pcilib_timeout_t timeout, pcilib_event_id_t *evid, pcilib_event_info_t **info) {
-    int err;
+int pcilib_get_next_event(pcilib_t *ctx, pcilib_timeout_t timeout, pcilib_event_id_t *evid, size_t info_size, pcilib_event_info_t *info) {
     pcilib_event_api_description_t *api;
 //    pcilib_return_event_callback_context_t user;
     
@@ -207,7 +202,7 @@ int pcilib_get_next_event(pcilib_t *ctx, pcilib_timeout_t timeout, pcilib_event_
     }
 
     if (api->next_event) 
-	return api->next_event(ctx->event_ctx, timeout, evid, info);
+	return api->next_event(ctx->event_ctx, timeout, evid, info_size, info);
 
 /*	
     if (api->stream) {
@@ -245,22 +240,33 @@ int pcilib_trigger(pcilib_t *ctx, pcilib_event_t event, size_t trigger_size, voi
 
 
 void *pcilib_get_data_with_argument(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size) {
+    int err;
+    void *res = NULL;
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
 
     pcilib_event_api_description_t *api = model_info->event_api;
     if (!api) {
+	if (size) *size = (size_t)PCILIB_ERROR_NOTSUPPORTED;
 	pcilib_error("Event API is not supported by the selected model");
 	return NULL;
     }
 
-    if (api->get_data) 
-	return api->get_data(ctx->event_ctx, event_id, data_type, arg_size, arg, size, NULL);
-
+    if (api->get_data) {
+	err = api->get_data(ctx->event_ctx, event_id, data_type, arg_size, arg, size, &res);
+	if (err) {
+	    if (size) *size = (size_t)err;
+	    return NULL;
+	}
+	return res;
+    }
+    
+    if (size) *size = (size_t)PCILIB_ERROR_NOTSUPPORTED;
     return NULL;
 }
 
 int pcilib_copy_data_with_argument(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t size, void *buf, size_t *retsize) {
-    void *res;
+    int err;
+    void *res = buf;
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
 
     pcilib_event_api_description_t *api = model_info->event_api;
@@ -270,8 +276,10 @@ int pcilib_copy_data_with_argument(pcilib_t *ctx, pcilib_event_id_t event_id, pc
     }
 
     if (api->get_data) {
-	res = api->get_data(ctx->event_ctx, event_id, data_type, arg_size, arg, &size, buf);
-	if (!res) return PCILIB_ERROR_FAILED;
+	err = api->get_data(ctx->event_ctx, event_id, data_type, arg_size, arg, &size, &res);
+	if (err) return err;
+	
+	if (buf != res) memcpy(buf, res, size);
 	
 	if (retsize) *retsize = size;
 	return 0;
@@ -282,22 +290,33 @@ int pcilib_copy_data_with_argument(pcilib_t *ctx, pcilib_event_id_t event_id, pc
 
 
 void *pcilib_get_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t *size) {
+    int err;
+    void *res = NULL;
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
 
     pcilib_event_api_description_t *api = model_info->event_api;
     if (!api) {
 	pcilib_error("Event API is not supported by the selected model");
+	if (size) *size = (size_t)PCILIB_ERROR_NOTSUPPORTED;
 	return NULL;
     }
 
-    if (api->get_data) 
-	return api->get_data(ctx->event_ctx, event_id, data_type, 0, NULL, size, NULL);
+    if (api->get_data) {
+	err = api->get_data(ctx->event_ctx, event_id, data_type, 0, NULL, size, &res);
+	if (err) {
+	    if (size) *size = (size_t)err;
+	    return NULL;
+	}
+	return res;
+    }
 
+    if (size) *size = (size_t)PCILIB_ERROR_NOTSUPPORTED;
     return NULL;
 }
 
 int pcilib_copy_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t size, void *buf, size_t *ret_size) {
-    void *res;
+    int err;
+    void *res = buf;
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
 
     pcilib_event_api_description_t *api = model_info->event_api;
@@ -307,9 +326,11 @@ int pcilib_copy_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_dat
     }
 
     if (api->get_data) {
-	res = api->get_data(ctx->event_ctx, event_id, data_type, 0, NULL, &size, buf);
-	if (!res) return PCILIB_ERROR_FAILED;
+	err = api->get_data(ctx->event_ctx, event_id, data_type, 0, NULL, &size, &res);
+	if (err) return err;
 	
+	if (buf != res) memcpy(buf, res, size);
+
 	if (ret_size) *ret_size = size;
 	return 0;
     }
@@ -317,7 +338,7 @@ int pcilib_copy_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_dat
     return PCILIB_ERROR_NOTSUPPORTED;
 }
 
-int pcilib_return_data(pcilib_t *ctx, pcilib_event_id_t event_id, void *data) {
+int pcilib_return_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, void *data) {
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
     
     pcilib_event_api_description_t *api = model_info->event_api;
@@ -327,7 +348,7 @@ int pcilib_return_data(pcilib_t *ctx, pcilib_event_id_t event_id, void *data) {
     }
 
     if (api->return_data) 
-	return api->return_data(ctx->event_ctx, event_id, data);
+	return api->return_data(ctx->event_ctx, event_id, data_type, data);
 
     return 0;
 }
@@ -352,13 +373,13 @@ static int pcilib_grab_callback(pcilib_event_t event, pcilib_event_id_t event_id
     data = pcilib_get_data(user->ctx, event_id, PCILIB_EVENT_DATA, &size);
     if (!data) {
 	pcilib_error("Error getting event data");
-	return PCILIB_ERROR_FAILED;
+	return -(int)size;
     }
     
     if (*(user->data)) {
 	if ((user->size)&&(*(user->size) < size)) {
 	    pcilib_error("The supplied buffer does not have enough space to hold the event data. Buffer size is %z, but %z is required", user->size, size);
-	    return PCILIB_ERROR_MEMORY;
+	    return -PCILIB_ERROR_MEMORY;
 	}
 
 	*(user->size) = size;
@@ -366,7 +387,7 @@ static int pcilib_grab_callback(pcilib_event_t event, pcilib_event_id_t event_id
 	*(user->data) = malloc(size);
 	if (!*(user->data)) {
 	    pcilib_error("Memory allocation (%i bytes) for event data is failed");
-	    return PCILIB_ERROR_MEMORY;
+	    return -PCILIB_ERROR_MEMORY;
 	}
 	if (*(user->size)) *(user->size) = size;
 	allocated = 1;
@@ -374,22 +395,21 @@ static int pcilib_grab_callback(pcilib_event_t event, pcilib_event_id_t event_id
     
     memcpy(*(user->data), data, size);
     
-    err = pcilib_return_data(user->ctx, event_id, data);
+    err = pcilib_return_data(user->ctx, event_id, PCILIB_EVENT_DATA, data);
     if (err) {
 	if (allocated) {
 	    free(*(user->data));
 	    *(user->data) = NULL;
 	}
 	pcilib_error("The event data had been overwritten before it was returned, data corruption may occur");
-	return err;
+	return -err;
     }
     
-    return 0;
+    return PCILIB_STREAMING_CONTINUE;
 }
 
 int pcilib_grab(pcilib_t *ctx, pcilib_event_t event_mask, size_t *size, void **data, pcilib_timeout_t timeout) {
     int err;
-    struct timespec ts;
     pcilib_event_id_t eid;
     
     pcilib_grab_callback_user_data_t user = {ctx, size, data};
@@ -397,7 +417,7 @@ int pcilib_grab(pcilib_t *ctx, pcilib_event_t event_mask, size_t *size, void **d
     err = pcilib_start(ctx, event_mask, PCILIB_EVENT_FLAGS_DEFAULT);
     if (!err) err = pcilib_trigger(ctx, event_mask, 0, NULL);
     if (!err) {
-	err = pcilib_get_next_event(ctx, timeout, &eid, NULL);
+	err = pcilib_get_next_event(ctx, timeout, &eid, 0, NULL);
 	if (!err) pcilib_grab_callback(event_mask, eid, &user);
     }
     pcilib_stop(ctx, PCILIB_EVENT_FLAGS_DEFAULT);
diff --git a/event.h b/event.h
index 7a1810a..dfae452 100644
--- a/event.h
+++ b/event.h
@@ -3,6 +3,19 @@
 
 #include "pcilib.h"
 
+
+/*
+ * get_data: This call is used by get_data and copy_data functions of public  
+ * interface. When copy_data is the caller, the data parameter will be passed.
+ * Therefore, depending on data the parameter, the function should behave
+ * diferently. If get get_data function is used (buf == NULL), the caller is 
+ * expected to call return_data afterwards. Otherwise, if buf != NULL and 
+ * copy_data is used, the return call will not be executed.
+ * Still, the get_data function is not obliged to return the data in the
+ * passed buf, but a reference to the staticaly allocated memory may be 
+ * returned instead. The copy can be managed by the envelope function.
+ */
+
 struct pcilib_event_api_description_s {
     pcilib_context_t *(*init)(pcilib_t *ctx);
     void (*free)(pcilib_context_t *ctx);
@@ -14,10 +27,10 @@ struct pcilib_event_api_description_s {
     int (*trigger)(pcilib_context_t *ctx, pcilib_event_t event, size_t trigger_size, void *trigger_data);
     
     int (*stream)(pcilib_context_t *ctx, pcilib_event_callback_t callback, void *user);
-    pcilib_event_id_t (*next_event)(pcilib_context_t *ctx, pcilib_timeout_t timeout, pcilib_event_id_t *evid, pcilib_event_info_t **info);
+    int (*next_event)(pcilib_context_t *ctx, pcilib_timeout_t timeout, pcilib_event_id_t *evid, size_t info_size, pcilib_event_info_t *info);
 
-    void* (*get_data)(pcilib_context_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size, void *data);
-    int (*return_data)(pcilib_context_t *ctx, pcilib_event_id_t event_id, void *data);
+    int (*get_data)(pcilib_context_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size, void **data);
+    int (*return_data)(pcilib_context_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, void *data);
     
     pcilib_dma_context_t *(*init_dma)(pcilib_context_t *ctx);
 };
diff --git a/ipecamera/CMakeLists.txt b/ipecamera/CMakeLists.txt
new file mode 100644
index 0000000..029993c
--- /dev/null
+++ b/ipecamera/CMakeLists.txt
@@ -0,0 +1,8 @@
+include_directories(
+    ${CMAKE_SOURCE_DIR}
+)
+
+set(HEADERS ${HEADERS} ipecamera.h model.h reader.h events.h data.h public.h private.h)
+
+add_library(ipecamera STATIC ipecamera.c model.c reader.c events.c data.c)
+
diff --git a/ipecamera/data.c b/ipecamera/data.c
new file mode 100644
index 0000000..5de6617
--- /dev/null
+++ b/ipecamera/data.c
@@ -0,0 +1,270 @@
+#define _IPECAMERA_IMAGE_C
+#define _BSD_SOURCE
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/time.h>
+#include <pthread.h>
+#include <assert.h>
+
+#include <ufodecode.h>
+
+#include "../tools.h"
+#include "../error.h"
+
+#include "pcilib.h"
+#include "private.h"
+#include "data.h"
+
+// DS: Currently, on event_id overflow we are assuming the buffer is lost
+static int ipecamera_resolve_event_id(ipecamera_t *ctx, pcilib_event_id_t evid) {
+    pcilib_event_id_t diff;
+
+    if (evid > ctx->event_id) {
+	diff = (((pcilib_event_id_t)-1) - ctx->event_id) + evid;
+	if (diff >= ctx->buffer_size) return -1;
+    } else {
+	diff = ctx->event_id - evid;
+        if (diff >= ctx->buffer_size) return -1;
+    }
+    
+	// DS: Request buffer_size to be power of 2 and replace to shifts (just recompute in set_buffer_size)
+    return (evid - 1) % ctx->buffer_size;
+}
+
+inline static int ipecamera_decode_frame(ipecamera_t *ctx, pcilib_event_id_t event_id) {
+    int err = 0;
+    uint32_t tmp;
+    uint16_t *pixels;
+    
+    int buf_ptr = ipecamera_resolve_event_id(ctx, event_id);
+    if (buf_ptr < 0) return PCILIB_ERROR_TIMEOUT;
+    
+    if (ctx->frame[buf_ptr].event.image_ready) return 0;
+    
+    if (ctx->frame[buf_ptr].event.info.flags&PCILIB_EVENT_INFO_FLAG_BROKEN) {
+	ctx->frame[buf_ptr].event.image_broken = 1;
+	err = PCILIB_ERROR_INVALID_DATA;
+	goto ready;
+    }
+	
+		
+    pixels = ctx->image + buf_ptr * ctx->image_size;
+    memset(ctx->cmask + ctx->buffer_pos * ctx->dim.height, 0, ctx->dim.height * sizeof(ipecamera_change_mask_t));
+    err = ufo_decoder_decode_frame(ctx->ipedec, ctx->buffer + buf_ptr * ctx->padded_size, ctx->raw_size, pixels, &tmp, &tmp, ctx->cmask + ctx->buffer_pos * ctx->dim.height);
+    if (err) {
+        ctx->frame[buf_ptr].event.image_broken = 1;
+        err = PCILIB_ERROR_FAILED;
+	goto ready;
+    }
+	    
+    ctx->frame[buf_ptr].event.image_broken = 0;
+
+ready:
+    ctx->frame[buf_ptr].event.image_ready = 1;
+
+    if (ipecamera_resolve_event_id(ctx, event_id) < 0) {
+	ctx->frame[buf_ptr].event.image_ready = 0;
+	return PCILIB_ERROR_TIMEOUT;
+    }
+    
+    return err;
+}
+
+static int ipecamera_get_next_buffer_to_process(ipecamera_t *ctx, pcilib_event_id_t *evid) {
+    int res;
+
+    if (ctx->preproc_id == ctx->event_id) return -1;
+    
+    if (ctx->preproc) 
+	pthread_mutex_lock(&ctx->preproc_mutex);
+	
+    if (ctx->preproc_id == ctx->event_id) {
+	if (ctx->preproc)
+	    pthread_mutex_unlock(&ctx->preproc_mutex);
+	return -1;
+    }
+
+    if ((ctx->event_id - ctx->preproc_id) > (ctx->buffer_size - IPECAMERA_RESERVE_BUFFERS)) ctx->preproc_id = ctx->event_id - (ctx->buffer_size - 1) - IPECAMERA_RESERVE_BUFFERS - 1;
+
+    res = ctx->preproc_id%ctx->buffer_size;
+
+    if (pthread_rwlock_trywrlock(&ctx->frame[res].mutex)) {
+	pthread_mutex_unlock(&ctx->preproc_mutex);
+	return -1;
+    }
+    
+    *evid = ++ctx->preproc_id;
+
+    if (ctx->preproc)
+	pthread_mutex_unlock(&ctx->preproc_mutex);
+
+    return res;
+}
+
+
+void *ipecamera_preproc_thread(void *user) {
+    int buf_ptr;
+    pcilib_event_id_t evid;
+    
+    ipecamera_preprocessor_t *preproc = (ipecamera_preprocessor_t*)user;
+    ipecamera_t *ctx = preproc->ipecamera;
+    
+    while (ctx->run_preprocessors) {
+	buf_ptr = ipecamera_get_next_buffer_to_process(ctx, &evid);
+	if (buf_ptr < 0) {
+	    usleep(IPECAMERA_NOFRAME_PREPROC_SLEEP);
+	    continue;
+	}
+	
+	ipecamera_decode_frame(ctx, evid);
+	
+	pthread_rwlock_unlock(&ctx->frame[buf_ptr].mutex);
+    }
+    
+    return NULL;
+}
+
+static int ipecamera_get_frame(ipecamera_t *ctx, pcilib_event_id_t event_id) {
+    int err;
+    int buf_ptr = (event_id - 1) % ctx->buffer_size;
+    
+    if (!ctx->preproc) {	
+	pthread_rwlock_rdlock(&ctx->frame[buf_ptr].mutex);
+
+	err = ipecamera_decode_frame(ctx, event_id);
+
+	if (err) {
+	    pthread_rwlock_unlock(&ctx->frame[buf_ptr].mutex);
+	    return err;
+	}
+	
+	return 0;
+    }
+    
+    
+    while (!ctx->frame[buf_ptr].event.image_ready) {
+	usleep(IPECAMERA_NOFRAME_PREPROC_SLEEP);
+
+	buf_ptr = ipecamera_resolve_event_id(ctx, event_id);
+	if (buf_ptr < 0) return PCILIB_ERROR_OVERWRITTEN;
+    }	
+
+    pthread_rwlock_rdlock(&ctx->frame[buf_ptr].mutex);
+    
+    buf_ptr = ipecamera_resolve_event_id(ctx, event_id);
+    if ((buf_ptr < 0)||(!ctx->frame[buf_ptr].event.image_ready)) {
+	pthread_rwlock_unlock(&ctx->frame[buf_ptr].mutex);
+	return PCILIB_ERROR_OVERWRITTEN;
+    }
+    
+    return 0;    
+}
+
+
+/*
+ We will lock the data for non-raw data to prevent ocasional overwritting. The 
+ raw data will be overwritten by the reader thread anyway and we can't do 
+ anything to prevent it for performance reasons.
+*/
+int ipecamera_get(pcilib_context_t *vctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size, void **ret) {
+    int err;
+    int buf_ptr;
+    size_t raw_size;
+    ipecamera_t *ctx = (ipecamera_t*)vctx;
+
+    void *data = *ret;
+
+    if (!ctx) {
+	pcilib_error("IPECamera imaging is not initialized");
+	return PCILIB_ERROR_NOTINITIALIZED;
+    }
+	
+    buf_ptr = ipecamera_resolve_event_id(ctx, event_id);
+    if (buf_ptr < 0) return PCILIB_ERROR_OVERWRITTEN;
+    
+    switch ((ipecamera_data_type_t)data_type) {
+	case IPECAMERA_RAW_DATA:
+	    raw_size = ctx->frame[buf_ptr].event.raw_size;
+	    if (data) {
+		if ((!size)||(*size < raw_size)) return PCILIB_ERROR_TOOBIG;
+		memcpy(data, ctx->buffer + buf_ptr * ctx->padded_size, raw_size);
+		if (ipecamera_resolve_event_id(ctx, event_id) < 0) return PCILIB_ERROR_OVERWRITTEN;
+		*size = raw_size;
+		return 0;
+	    }
+	    if (size) *size = raw_size;
+	    *ret = ctx->buffer + buf_ptr * ctx->padded_size;
+	    return 0;
+	case IPECAMERA_IMAGE_DATA:
+	    err = ipecamera_get_frame(ctx, event_id);
+	    if (err) return err;
+
+	    if (data) {
+		if ((!size)||(*size < ctx->image_size * sizeof(ipecamera_pixel_t))) return PCILIB_ERROR_TOOBIG;
+		memcpy(data, ctx->image + buf_ptr * ctx->image_size, ctx->image_size * sizeof(ipecamera_pixel_t));
+		pthread_rwlock_unlock(&ctx->frame[buf_ptr].mutex);
+		*size =  ctx->image_size * sizeof(ipecamera_pixel_t);
+		return 0;
+	    }
+	
+	    if (size) *size = ctx->image_size * sizeof(ipecamera_pixel_t);
+	    *ret = ctx->image + buf_ptr * ctx->image_size;
+	    return 0;
+	case IPECAMERA_CHANGE_MASK:
+	    err = ipecamera_get_frame(ctx, event_id);
+	    if (err) return err;
+
+	    if (data) {
+		if ((!size)||(*size < ctx->dim.height * sizeof(ipecamera_change_mask_t))) return PCILIB_ERROR_TOOBIG;
+		memcpy(data, ctx->image + buf_ptr * ctx->dim.height, ctx->dim.height * sizeof(ipecamera_change_mask_t));
+		pthread_rwlock_unlock(&ctx->frame[buf_ptr].mutex);
+		*size =  ctx->dim.height * sizeof(ipecamera_change_mask_t);
+		return 0;
+	    }
+
+	    if (size) *size = ctx->dim.height * sizeof(ipecamera_change_mask_t);
+	    *ret = ctx->cmask + buf_ptr * ctx->dim.height;
+	    return 0;
+	case IPECAMERA_DIMENSIONS:
+	    if (size) *size = sizeof(ipecamera_image_dimensions_t);
+	    ret = (void*)&ctx->dim;
+	    return 0;
+	case IPECAMERA_IMAGE_REGION:
+	case IPECAMERA_PACKED_IMAGE:
+	    // Shall we return complete image or only changed parts?
+	case IPECAMERA_PACKED_LINE:
+	case IPECAMERA_PACKED_PAYLOAD:
+	    pcilib_error("Support for data type (%li) is not implemented yet", data_type);
+	    return PCILIB_ERROR_NOTSUPPORTED;
+	default:
+	    pcilib_error("Unknown data type (%li) is requested", data_type);
+	    return PCILIB_ERROR_INVALID_REQUEST;
+    }
+}
+
+
+/*
+ We will unlock non-raw data and check if the raw data is not overwritten yet
+*/
+int ipecamera_return(pcilib_context_t *vctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, void *data) {
+    ipecamera_t *ctx = (ipecamera_t*)vctx;
+
+    if (!ctx) {
+	pcilib_error("IPECamera imaging is not initialized");
+	return PCILIB_ERROR_NOTINITIALIZED;
+
+    }
+
+    if ((ipecamera_data_type_t)data_type == IPECAMERA_RAW_DATA) {
+	if (ipecamera_resolve_event_id(ctx, event_id) < 0) return PCILIB_ERROR_OVERWRITTEN;
+    } else {
+	int buf_ptr = (event_id - 1) % ctx->buffer_size;
+	pthread_rwlock_unlock(&ctx->frame[buf_ptr].mutex);
+    }
+
+    return 0;
+}
diff --git a/ipecamera/data.h b/ipecamera/data.h
new file mode 100644
index 0000000..846cb78
--- /dev/null
+++ b/ipecamera/data.h
@@ -0,0 +1,6 @@
+#ifndef _IPECAMERA_DATA_H
+#define _IPECAMERA_DATA_H
+
+void *ipecamera_preproc_thread(void *user);
+
+#endif /* _IPECAMERA_DATA_H */
diff --git a/ipecamera/events.c b/ipecamera/events.c
new file mode 100644
index 0000000..58d2971
--- /dev/null
+++ b/ipecamera/events.c
@@ -0,0 +1,121 @@
+#define _IPECAMERA_IMAGE_C
+#define _BSD_SOURCE
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/time.h>
+#include <pthread.h>
+#include <assert.h>
+
+#include <ufodecode.h>
+
+#include "../tools.h"
+#include "../error.h"
+
+#include "pcilib.h"
+#include "public.h"
+#include "private.h"
+#include "events.h"
+
+int ipecamera_stream(pcilib_context_t *vctx, pcilib_event_callback_t callback, void *user) {
+    int err = 0;
+    int do_stop = 0;
+    
+    ipecamera_event_info_t info;
+    ipecamera_t *ctx = (ipecamera_t*)vctx;
+
+    if (!ctx) {
+	pcilib_error("IPECamera imaging is not initialized");
+	return PCILIB_ERROR_NOTINITIALIZED;
+    }
+
+    ctx->streaming = 1;
+    ctx->run_streamer = 1;
+
+    if (!ctx->started) {
+	err = ipecamera_start(vctx, PCILIB_EVENTS_ALL, PCILIB_EVENT_FLAGS_DEFAULT);
+	if (err) {
+	    ctx->streaming = 0;
+	    return err;
+	}
+	
+	do_stop = 1;
+    }
+    
+	// This loop iterates while the generation
+    while ((ctx->run_streamer)||(ctx->reported_id != ctx->event_id)) {
+	while (ctx->reported_id != ctx->event_id) {
+	    if ((ctx->event_id - ctx->reported_id) > (ctx->buffer_size - IPECAMERA_RESERVE_BUFFERS)) ctx->reported_id = ctx->event_id - (ctx->buffer_size - 1) - IPECAMERA_RESERVE_BUFFERS;
+	    else ++ctx->reported_id;
+
+	    memcpy(&info, ctx->frame + ((ctx->reported_id-1)%ctx->buffer_size), sizeof(ipecamera_event_info_t));
+
+	    if ((ctx->event_id - ctx->reported_id) < ctx->buffer_size) {
+		err = callback(ctx->reported_id, (pcilib_event_info_t*)&info, user);
+		if (err <= 0) {
+		    if (err < 0) err = -err;
+		    break;
+		}
+	    }
+	}
+	usleep(IPECAMERA_NOFRAME_SLEEP);
+    }
+
+    ctx->streaming = 0;
+
+    if (do_stop) {
+	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+    }
+    
+
+    return err;
+}
+
+int ipecamera_next_event(pcilib_context_t *vctx, pcilib_timeout_t timeout, pcilib_event_id_t *evid, size_t info_size, pcilib_event_info_t *info) {
+    struct timeval tv;
+    ipecamera_t *ctx = (ipecamera_t*)vctx;
+
+    if (!ctx) {
+	pcilib_error("IPECamera imaging is not initialized");
+	return PCILIB_ERROR_NOTINITIALIZED;
+    }
+
+    if (!ctx->started) {
+	pcilib_error("IPECamera is not in grabbing mode");
+	return PCILIB_ERROR_INVALID_REQUEST;
+    }
+
+    if (ctx->reported_id == ctx->event_id) {
+	if (timeout) {
+	    pcilib_calc_deadline(&tv, timeout);
+	    
+	    while ((pcilib_calc_time_to_deadline(&tv) > 0)&&(ctx->reported_id == ctx->event_id))
+		usleep(IPECAMERA_NOFRAME_SLEEP);
+	}
+	
+	if (ctx->reported_id == ctx->event_id) return PCILIB_ERROR_TIMEOUT;
+    }
+
+retry:
+    if ((ctx->event_id - ctx->reported_id) > (ctx->buffer_size - IPECAMERA_RESERVE_BUFFERS)) ctx->reported_id = ctx->event_id - (ctx->buffer_size - 1) - IPECAMERA_RESERVE_BUFFERS;
+    else ++ctx->reported_id;
+
+    if (evid) *evid = ctx->reported_id;
+
+    if (info) {
+	if (info_size >= sizeof(ipecamera_event_info_t))
+	    memcpy(info, ctx->frame + ((ctx->reported_id-1)%ctx->buffer_size), sizeof(ipecamera_event_info_t));
+	else if (info_size >= sizeof(pcilib_event_info_t))
+	    memcpy(info, ctx->frame + ((ctx->reported_id-1)%ctx->buffer_size), sizeof(pcilib_event_info_t));
+	else
+	    return PCILIB_ERROR_INVALID_ARGUMENT;
+    }
+    
+    if ((ctx->event_id - ctx->reported_id) >= ctx->buffer_size) goto retry;
+    
+    return 0;
+}
+
diff --git a/ipecamera/events.h b/ipecamera/events.h
new file mode 100644
index 0000000..5268c81
--- /dev/null
+++ b/ipecamera/events.h
@@ -0,0 +1,5 @@
+#ifndef _IPECAMERA_EVENTS_H
+#define _IPECAMERA_EVENTS_H
+
+
+#endif /* _IPECAMERA_EVENTS_H */
diff --git a/ipecamera/image.c b/ipecamera/image.c
deleted file mode 100644
index e60bbf2..0000000
--- a/ipecamera/image.c
+++ /dev/null
@@ -1,971 +0,0 @@
-#define _IPECAMERA_IMAGE_C
-#define _BSD_SOURCE
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <string.h>
-#include <sys/time.h>
-#include <pthread.h>
-#include <assert.h>
-
-#include <ufodecode.h>
-
-#include "../tools.h"
-#include "../error.h"
-
-#include "pcilib.h"
-
-#include "model.h"
-#include "event.h"
-#include "image.h"
-
-#include "dma/nwl_dma.h"
-
-#ifdef IPECAMERA_DEBUG
-#include "dma/nwl.h"
-#endif /* IPECAMERA_DEBUG */
-
-
-#define IPECAMERA_BUG_EXTRA_DATA
-#define IPECAMERA_BUG_MULTIFRAME_PACKETS
-#define IPECAMERA_BUG_INCOMPLETE_PACKETS
-
-#define IPECAMERA_DEFAULT_BUFFER_SIZE 64  	//**< should be power of 2 */
-#define IPECAMERA_RESERVE_BUFFERS 2		//**< Return Frame is Lost error, if requested frame will be overwritten after specified number of frames
-#define IPECAMERA_SLEEP_TIME 250000 		//**< Michele thinks 250 should be enough, but reset failing in this case */
-#define IPECAMERA_NEXT_FRAME_DELAY 1000 	//**< Michele requires 30000 to sync between End Of Readout and next Frame Req */
-#define IPECAMERA_WAIT_FRAME_RCVD_TIME 0 	//**< by Uros ,wait 6 ms */
-#define IPECAMERA_NOFRAME_SLEEP 100
-
-#define IPECAMERA_MAX_LINES 1088
-#define IPECAMERA_EXPECTED_STATUS 0x08409FFFF
-#define IPECAMERA_END_OF_SEQUENCE 0x1F001001
-
-#define IPECAMERA_MAX_CHANNELS 16
-#define IPECAMERA_PIXELS_PER_CHANNEL 128
-#define IPECAMERA_WIDTH (IPECAMERA_MAX_CHANNELS * IPECAMERA_PIXELS_PER_CHANNEL)
-
-/*
-#define IPECAMERA_HEIGHT 1088
-#if IPECAMERA_HEIGHT < IPECAMERA_MAX_LINES
-# undef IPECAMERA_MAX_LINES
-# define IPECAMERA_MAX_LINES IPECAMERA_HEIGHT
-#endif 
-*/
-
-//#define IPECAMERA_MEMORY 
-
-#define IPECAMERA_FRAME_REQUEST 		0x1E9
-#define IPECAMERA_READOUT_FLAG			0x200
-#define IPECAMERA_READOUT			0x3E1
-#define IPECAMERA_IDLE 				0x1E1
-#define IPECAMERA_START_INTERNAL_STIMULI 	0x1F1
-
-
-typedef uint32_t ipecamera_payload_t;
-
-typedef struct {
-    pcilib_event_id_t evid;
-    struct timeval timestamp;
-} ipecamera_autostop_t;
-
-struct ipecamera_s {
-    pcilib_context_t event;
-    ufo_decoder ipedec;
-
-    char *data;
-    ipecamera_pixel_t *image;
-    size_t size;
-
-    pcilib_event_callback_t cb;
-    void *cb_user;
-
-    pcilib_event_id_t event_id;
-    pcilib_event_id_t reported_id;
-    
-    pcilib_dma_engine_t rdma, wdma;
-
-    pcilib_register_t packet_len_reg;
-    pcilib_register_t control_reg, status_reg;
-    pcilib_register_t start_reg, end_reg;
-    pcilib_register_t n_lines_reg;
-    uint16_t line_reg;
-    pcilib_register_t exposure_reg;
-    pcilib_register_t flip_reg;
-
-    int started;		/**< Camera is in grabbing mode (start function is called) */
-    int streaming;		/**< Camera is in streaming mode (we are within stream call) */
-    int parse_data;		/**< Indicates if some processing of the data is required, otherwise only rawdata_callback will be called */
-
-    int run_reader;		/**< Instructs the reader thread to stop processing */
-    int run_streamer;		/**< Indicates request to stop streaming events and can be set by reader_thread upon exit or by user request */
-    ipecamera_autostop_t autostop;
-
-    struct timeval autostop_time;
-
-    size_t buffer_size;		/**< How many images to store */
-    size_t buffer_pos;		/**< Current image offset in the buffer, due to synchronization reasons should not be used outside of reader_thread */
-    size_t cur_size;		/**< Already written part of data in bytes */
-    size_t raw_size;		/**< Size of raw data in bytes */
-    size_t full_size;		/**< Size of raw data including the padding */
-    size_t padded_size;		/**< Size of buffer for raw data, including the padding for performance */
-    
-    size_t image_size;		/**< Size of a single image in bytes */
-    
-    int width, height;
-
-    
-//    void *raw_buffer;
-    void *buffer;
-    ipecamera_change_mask_t *cmask;
-    ipecamera_event_info_t *frame_info;
-    
-
-    ipecamera_image_dimensions_t dim;
-
-    pthread_t rthread;
-};
-
-
-#define FIND_REG(var, bank, name)  \
-        ctx->var = pcilib_find_register(pcilib, bank, name); \
-	if (ctx->var ==  PCILIB_REGISTER_INVALID) { \
-	    err = PCILIB_ERROR_NOTFOUND; \
-	    pcilib_error("Unable to find a %s register", name); \
-	}
-    
-
-#define GET_REG(reg, var) \
-    if (!err) { \
-	err = pcilib_read_register_by_id(pcilib, ctx->reg, &var); \
-	if (err) { \
-	    pcilib_error("Error reading %s register", ipecamera_registers[ctx->reg].name); \
-	} \
-    }
-
-#define SET_REG(reg, val) \
-    if (!err) { \
-	err = pcilib_write_register_by_id(pcilib, ctx->reg, val); \
-	if (err) { \
-	    pcilib_error("Error writting %s register", ipecamera_registers[ctx->reg].name); \
-	} \
-    }
-
-#define CHECK_REG(reg, check) \
-    if (!err) { \
-	err = pcilib_read_register_by_id(pcilib, ctx->reg, &value); \
-	if (err) { \
-	    pcilib_error("Error reading %s register", ipecamera_registers[ctx->reg].name); \
-	} \
-	if (!(check)) { \
-	    pcilib_error("Unexpected value (%li) of register %s", value, ipecamera_registers[ctx->reg].name); \
-	    err = PCILIB_ERROR_INVALID_DATA; \
-	} \
-    }
-
-#define CHECK_VALUE(value, val) \
-    if ((!err)&&(value != val)) { \
-	pcilib_error("Unexpected value (0x%x) in data stream (0x%x is expected)", value, val); \
-	err = PCILIB_ERROR_INVALID_DATA; \
-    }
-
-#define CHECK_FLAG(flag, check, ...) \
-    if ((!err)&&(!(check))) { \
-	pcilib_error("Unexpected value (0x%x) of " flag,  __VA_ARGS__); \
-	err = PCILIB_ERROR_INVALID_DATA; \
-    }
-
-
-pcilib_context_t *ipecamera_init(pcilib_t *pcilib) {
-    int err = 0; 
-    
-    ipecamera_t *ctx = malloc(sizeof(ipecamera_t));
-
-    if (ctx) {
-	memset(ctx, 0, sizeof(ipecamera_t));
-	
-	ctx->buffer_size = IPECAMERA_DEFAULT_BUFFER_SIZE;
-	ctx->dim.bpp = sizeof(ipecamera_pixel_t) * 8;
-
-	    // We need DMA engine initialized to resolve DMA registers
-//	FIND_REG(packet_len_reg, "fpga", "xrawdata_packet_length");
-	
-	FIND_REG(status_reg, "fpga", "status");
-	FIND_REG(control_reg, "fpga", "control");
-	FIND_REG(start_reg, "fpga", "start_address");
-	FIND_REG(end_reg, "fpga", "end_address");
-
-	FIND_REG(n_lines_reg, "cmosis", "number_lines");
-	FIND_REG(line_reg, "cmosis", "start1");
-	FIND_REG(exposure_reg, "cmosis", "exp_time");
-	FIND_REG(flip_reg, "cmosis", "image_flipping");
-
-	ctx->rdma = PCILIB_DMA_ENGINE_INVALID;
-	ctx->wdma = PCILIB_DMA_ENGINE_INVALID;
-
-	if (err) {
-	    free(ctx);
-	    return NULL;
-	}
-    }
-    
-    return (pcilib_context_t*)ctx;
-}
-
-void ipecamera_free(pcilib_context_t *vctx) {
-    if (vctx) {
-	ipecamera_t *ctx = (ipecamera_t*)vctx;
-	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
-	free(ctx);
-    }
-}
-
-pcilib_dma_context_t *ipecamera_init_dma(pcilib_context_t *vctx) {
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-    
-    pcilib_model_description_t *model_info = pcilib_get_model_description(vctx->pcilib);
-    if ((!model_info->dma_api)||(!model_info->dma_api->init)) {
-	pcilib_error("The DMA engine is not configured in model");
-	return NULL;
-    }
-
-
-#ifdef IPECAMERA_DMA_R3
-    return model_info->dma_api->init(vctx->pcilib, PCILIB_NWL_MODIFICATION_IPECAMERA, NULL);
-#else
-    return model_info->dma_api->init(vctx->pcilib, PCILIB_DMA_MODIFICATION_DEFAULT, NULL);
-#endif
-}
-
-
-int ipecamera_set_buffer_size(ipecamera_t *ctx, int size) {
-    if (ctx->started) {
-	pcilib_error("Can't change buffer size while grabbing");
-	return PCILIB_ERROR_INVALID_REQUEST;
-    }
-    
-    if (size < 2) {
-	pcilib_error("The buffer size is too small");
-	return PCILIB_ERROR_INVALID_REQUEST;
-    }
-    
-    if (((size^(size-1)) < size) < size) {
-	pcilib_error("The buffer size is not power of 2");
-    }
-    
-    ctx->buffer_size = size;
-    
-    return 0;
-}
-
-int ipecamera_reset(pcilib_context_t *vctx) {
-    int err = 0;
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-    pcilib_t *pcilib = vctx->pcilib;
-
-    pcilib_register_t control, status;
-    pcilib_register_value_t value;
-
-    if (!ctx) {
-	pcilib_error("IPECamera imaging is not initialized");
-	return PCILIB_ERROR_NOTINITIALIZED;
-    }
-
-    pcilib = vctx->pcilib;
-    control = ctx->control_reg;
-    status = ctx->status_reg;
-
-	// Set Reset bit to CMOSIS
-    err = pcilib_write_register_by_id(pcilib, control, 0x1e4);
-    if (err) {
-	pcilib_error("Error setting FPGA reset bit");
-	return err;
-    }
-    usleep(IPECAMERA_SLEEP_TIME);
-
-	// Remove Reset bit to CMOSIS
-    err = pcilib_write_register_by_id(pcilib, control, 0x1e1);
-    if (err) {
-	pcilib_error("Error reseting FPGA reset bit");
-	return err;
-    }
-    usleep(IPECAMERA_SLEEP_TIME);
-
-	// Special settings for CMOSIS v.2
-    value = 01; err = pcilib_write_register_space(pcilib, "cmosis", 115, 1, &value);
-    if (err) {
-	pcilib_error("Error setting CMOSIS configuration");
-	return err;
-    }
-    usleep(IPECAMERA_SLEEP_TIME);
-
-    value = 07; err = pcilib_write_register_space(pcilib, "cmosis", 82, 1, &value);
-    if (err) {
-	pcilib_error("Error setting CMOSIS configuration");
-	return err;
-    }
-    usleep(IPECAMERA_SLEEP_TIME);
-
-	// Set default parameters
-    err = pcilib_write_register_by_id(pcilib, control, IPECAMERA_IDLE);
-    if (err) {
-	pcilib_error("Error bringing FPGA in default mode");
-	return err;
-    }
-
-    usleep(10000);
-
-        
-    err = pcilib_read_register_by_id(pcilib, status, &value);
-    if (err) {
-	pcilib_error("Error reading status register");
-	return err;
-    }
-
-    if (value != IPECAMERA_EXPECTED_STATUS) {
-	pcilib_error("Unexpected value (%lx) of status register, expected %lx", value, IPECAMERA_EXPECTED_STATUS);
-	return PCILIB_ERROR_VERIFY;
-    }
-
-    return 0;    
-}
-
-// DS: Currently, on event_id overflow we are assuming the buffer is lost
-static int ipecamera_resolve_event_id(ipecamera_t *ctx, pcilib_event_id_t evid) {
-    pcilib_event_id_t diff;
-
-    if (evid > ctx->event_id) {
-	diff = (((pcilib_event_id_t)-1) - ctx->event_id) + evid;
-	if (diff >= ctx->buffer_size) return -1;
-    } else {
-	diff = ctx->event_id - evid;
-        if (diff >= ctx->buffer_size) return -1;
-    }
-    
-	// DS: Request buffer_size to be power of 2 and replace to shifts (just recompute in set_buffer_size)
-    return (evid - 1) % ctx->buffer_size;
-}
-
-static inline int ipecamera_new_frame(ipecamera_t *ctx) {
-    ctx->frame_info[ctx->buffer_pos].raw_size = ctx->cur_size;
-
-    if (ctx->cur_size < ctx->raw_size) ctx->frame_info[ctx->buffer_pos].info.flags |= PCILIB_EVENT_INFO_FLAG_BROKEN;
-    
-    ctx->buffer_pos = (++ctx->event_id) % ctx->buffer_size;
-    ctx->cur_size = 0;
-
-    ctx->frame_info[ctx->buffer_pos].info.type = PCILIB_EVENT0;
-    ctx->frame_info[ctx->buffer_pos].info.flags = 0;
-    ctx->frame_info[ctx->buffer_pos].image_ready = 0;
-
-    if ((ctx->event_id == ctx->autostop.evid)&&(ctx->event_id)) {
-	ctx->run_reader = 0;
-	return 1;
-    }
-	
-    if (pcilib_check_deadline(&ctx->autostop.timestamp, PCILIB_DMA_TIMEOUT)) {
-	ctx->run_reader = 0;
-	return 1;
-    }
-    
-    return 0;
-}
-
-static uint32_t frame_magic[6] = { 0x51111111, 0x52222222, 0x53333333, 0x54444444, 0x55555555, 0x56666666 };
-
-static int ipecamera_data_callback(void *user, pcilib_dma_flags_t flags, size_t bufsize, void *buf) {
-    int eof = 0;
-
-#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
-    size_t extra_data = 0;
-#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
-
-    ipecamera_t *ctx = (ipecamera_t*)user;
-
-    if (!ctx->cur_size) {
-#if defined(IPECAMERA_BUG_INCOMPLETE_PACKETS)||defined(IPECAMERA_BUG_MULTIFRAME_PACKETS)
-	size_t startpos;
-	for (startpos = 0; (startpos + 8) < bufsize; startpos++) {
-	    if (!memcmp(buf + startpos, frame_magic, sizeof(frame_magic))) break;
-	}
-	
-	if (startpos) {
-	    buf += startpos;
-	    bufsize -= startpos;
-	}
-#endif /* IPECAMERA_BUG_INCOMPLETE_PACKETS */
-
-	if ((bufsize >= 8)&&(!memcmp(buf, frame_magic, sizeof(frame_magic)))) {
-/*
-		// Not implemented in hardware yet
-	    ctx->frame_info[ctx->buffer_pos].info.seqnum = ((uint32_t*)buf)[6] & 0xF0000000;
-	    ctx->frame_info[ctx->buffer_pos].info.offset = ((uint32_t*)buf)[7] & 0xF0000000;
-*/
-	    gettimeofday(&ctx->frame_info[ctx->buffer_pos].info.timestamp, NULL);
-	} else {
-//	    pcilib_warning("Frame magic is not found, ignoring broken data...");
-	    return PCILIB_STREAMING_CONTINUE;
-	}
-    }
-
-#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
-    if (ctx->cur_size + bufsize > ctx->raw_size) {
-        size_t need;
-	
-	for (need = ctx->raw_size - ctx->cur_size; need < bufsize; need += sizeof(uint32_t)) {
-	    if (*(uint32_t*)(buf + need) == frame_magic[0]) break;
-	}
-	
-	if (need < bufsize) {
-	    extra_data = bufsize - need;
-	    //bufsize = need;
-	    eof = 1;
-	}
-	
-	    // just rip of padding
-	bufsize = ctx->raw_size - ctx->cur_size;
-    }
-#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
-
-    if (ctx->parse_data) {
-	if (ctx->cur_size + bufsize > ctx->full_size) {
-    	    pcilib_error("Unexpected event data, we are expecting at maximum (%zu) bytes, but (%zu) already read", ctx->full_size, ctx->cur_size + bufsize);
-	    return -PCILIB_ERROR_TOOBIG;
-	}
-
-        memcpy(ctx->buffer + ctx->buffer_pos * ctx->padded_size +  ctx->cur_size, buf, bufsize);
-    }
-
-    ctx->cur_size += bufsize;
-//    printf("%i: %i %i\n", ctx->buffer_pos, ctx->cur_size, bufsize);
-
-    if (ctx->cur_size >= ctx->full_size) eof = 1;
-
-    if (ctx->event.params.rawdata.callback) {
-	ctx->event.params.rawdata.callback(ctx->event_id, (pcilib_event_info_t*)(ctx->frame_info + ctx->buffer_pos), (eof?PCILIB_EVENT_FLAG_EOF:PCILIB_EVENT_FLAGS_DEFAULT), bufsize, buf, ctx->event.params.rawdata.user);
-    }
-    
-    if (eof) {
-	if (ipecamera_new_frame(ctx)) {
-	    return PCILIB_STREAMING_STOP;
-	}
-	
-#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
-	if (extra_data) {
-	    return ipecamera_data_callback(user, flags, extra_data, buf + bufsize);
-	}
-#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
-    }
-
-    return PCILIB_STREAMING_REQ_FRAGMENT;
-}
-
-static void *ipecamera_reader_thread(void *user) {
-    int err;
-    ipecamera_t *ctx = (ipecamera_t*)user;
-    
-    while (ctx->run_reader) {
-	err = pcilib_stream_dma(ctx->event.pcilib, ctx->rdma, 0, 0, PCILIB_DMA_FLAG_MULTIPACKET, PCILIB_DMA_TIMEOUT, &ipecamera_data_callback, user);
-	if (err) {
-	    if (err == PCILIB_ERROR_TIMEOUT) {
-		if (ctx->cur_size > ctx->raw_size) ipecamera_new_frame(ctx);
-#ifdef IPECAMERA_BUG_INCOMPLETE_PACKETS
-		else if (ctx->cur_size > 0) ipecamera_new_frame(ctx);
-#endif /* IPECAMERA_BUG_INCOMPLETE_PACKETS */
-		if (pcilib_check_deadline(&ctx->autostop.timestamp, PCILIB_DMA_TIMEOUT)) {
-		    ctx->run_reader = 0;
-		    break;
-		}
-		usleep(IPECAMERA_NOFRAME_SLEEP);
-	    } else pcilib_error("DMA error while reading IPECamera frames, error: %i", err);
-	} else printf("no error\n");
-
-	//usleep(1000);
-    }
-    
-    ctx->run_streamer = 0;
-    
-    if (ctx->cur_size)
-	pcilib_error("partialy read frame after stop signal, %zu bytes in the buffer", ctx->cur_size);
-
-    return NULL;
-}
-
-int ipecamera_start(pcilib_context_t *vctx, pcilib_event_t event_mask, pcilib_event_flags_t flags) {
-    int err = 0;
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-    pcilib_t *pcilib = vctx->pcilib;
-    pcilib_register_value_t value;
-    
-    const size_t chan_size = (2 + IPECAMERA_PIXELS_PER_CHANNEL / 3) * sizeof(ipecamera_payload_t);
-    const size_t line_size = (IPECAMERA_MAX_CHANNELS * chan_size);
-    const size_t header_size = 8 * sizeof(ipecamera_payload_t);
-    const size_t footer_size = 8 * sizeof(ipecamera_payload_t);
-    size_t raw_size;
-    size_t padded_blocks;
-    
-    pthread_attr_t attr;
-    struct sched_param sched;
-    
-    if (!ctx) {
-	pcilib_error("IPECamera imaging is not initialized");
-	return PCILIB_ERROR_NOTINITIALIZED;
-    }
-    
-    if (ctx->started) {
-	pcilib_error("IPECamera grabbing is already started");
-	return PCILIB_ERROR_INVALID_REQUEST;
-    }
-
-
-	// Allow readout and clean the FRAME_REQUEST mode if set for some reason
-    SET_REG(control_reg, IPECAMERA_IDLE|IPECAMERA_READOUT_FLAG);
-    usleep(IPECAMERA_SLEEP_TIME);
-    CHECK_REG(status_reg, IPECAMERA_EXPECTED_STATUS);
-    if (err) return err;
-
-
-    ctx->event_id = 0;
-    ctx->reported_id = 0;
-    ctx->buffer_pos = 0;
-    ctx->parse_data = (flags&PCILIB_EVENT_FLAG_RAW_DATA_ONLY)?0:1;
-    ctx->cur_size = 0;
-
-    ctx->dim.width = IPECAMERA_WIDTH;
-    GET_REG(n_lines_reg, ctx->dim.height);
-    
-    raw_size = header_size + ctx->dim.height * line_size + footer_size;
-    padded_blocks = raw_size / IPECAMERA_DMA_PACKET_LENGTH + ((raw_size % IPECAMERA_DMA_PACKET_LENGTH)?1:0);
-    
-    ctx->image_size = ctx->dim.width * ctx->dim.height;
-    ctx->raw_size = raw_size;
-    ctx->full_size = padded_blocks * IPECAMERA_DMA_PACKET_LENGTH;
-
-#ifdef IPECAMERA_BUG_EXTRA_DATA
-    ctx->full_size += 8;
-    padded_blocks ++;
-#endif /* IPECAMERA_BUG_EXTRA_DATA */
-
-    ctx->padded_size = padded_blocks * IPECAMERA_DMA_PACKET_LENGTH;
-
-    ctx->buffer = malloc(ctx->padded_size * ctx->buffer_size);
-    if (!ctx->buffer) {
-	err = PCILIB_ERROR_MEMORY;
-	pcilib_error("Unable to allocate ring buffer (%lu bytes)", ctx->padded_size * ctx->buffer_size);
-	return err;
-    }
-
-    ctx->image = (ipecamera_pixel_t*)malloc(ctx->image_size * ctx->buffer_size * sizeof(ipecamera_pixel_t));
-    if (!ctx->image) {
-	err = PCILIB_ERROR_MEMORY;
-	pcilib_error("Unable to allocate image buffer (%lu bytes)", ctx->image_size * ctx->buffer_size * sizeof(ipecamera_pixel_t));
-	return err;
-    }
-
-    ctx->cmask = malloc(ctx->dim.height * ctx->buffer_size * sizeof(ipecamera_change_mask_t));
-    if (!ctx->cmask) {
-	err = PCILIB_ERROR_MEMORY;
-	pcilib_error("Unable to allocate change-mask buffer");
-	return err;
-    }
-
-    ctx->frame_info = malloc(ctx->buffer_size * sizeof(ipecamera_event_info_t));
-    if (!ctx->frame_info) {
-	err = PCILIB_ERROR_MEMORY;
-	pcilib_error("Unable to allocate frame-info buffer");
-	return err;
-    }
-    
-    ctx->ipedec = ufo_decoder_new(ctx->dim.height, ctx->dim.width, NULL, 0);
-    if (!ctx->ipedec) {
-	pcilib_error("Unable to initialize IPECamera decoder library");
-	return PCILIB_ERROR_FAILED;
-    }
-
-    if (!err) {
-	ctx->rdma = pcilib_find_dma_by_addr(vctx->pcilib, PCILIB_DMA_FROM_DEVICE, IPECAMERA_DMA_ADDRESS);
-	if (ctx->rdma == PCILIB_DMA_ENGINE_INVALID) {
-	    err = PCILIB_ERROR_NOTFOUND;
-	    pcilib_error("The C2S channel of IPECamera DMA Engine (%u) is not found", IPECAMERA_DMA_ADDRESS);
-	} else {
-	    err = pcilib_start_dma(vctx->pcilib, ctx->rdma, PCILIB_DMA_FLAGS_DEFAULT);
-	    if (err) {
-		ctx->rdma = PCILIB_DMA_ENGINE_INVALID;
-		pcilib_error("Failed to initialize C2S channel of IPECamera DMA Engine (%u)", IPECAMERA_DMA_ADDRESS);
-	    }
-	}
-    }
-    
-/*    
-    if (!err) {
-	ctx->wdma = pcilib_find_dma_by_addr(vctx->pcilib, PCILIB_DMA_TO_DEVICE, IPECAMERA_DMA_ADDRESS);
-	if (ctx->wdma == PCILIB_DMA_ENGINE_INVALID) {
-	    err = PCILIB_ERROR_NOTFOUND;
-	    pcilib_error("The S2C channel of IPECamera DMA Engine (%u) is not found", IPECAMERA_DMA_ADDRESS);
-	} else {
-	    err = pcilib_start_dma(vctx->pcilib, ctx->wdma, PCILIB_DMA_FLAGS_DEFAULT);
-	    if (err) {
-		ctx->wdma = PCILIB_DMA_ENGINE_INVALID;
-		pcilib_error("Failed to initialize S2C channel of IPECamera DMA Engine (%u)", IPECAMERA_DMA_ADDRESS);
-	    }
-	}
-    }
-*/    
-
-/*
-    SET_REG(packet_len_reg, IPECAMERA_DMA_PACKET_LENGTH);
-    if (err) return err;
-*/
-
-	// Clean DMA
-    err = pcilib_skip_dma(vctx->pcilib, ctx->rdma);
-    if (err) {
-	pcilib_error("Can't start grabbing, device continuously writes unexpected data using DMA engine");
-	return err;
-    }
-
-    if (err) {
-	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
-	return err;
-    }
-
-    if (vctx->params.autostop.duration) {
-	gettimeofday(&ctx->autostop.timestamp, NULL);
-	ctx->autostop.timestamp.tv_usec += vctx->params.autostop.duration % 1000000;
-	if (ctx->autostop.timestamp.tv_usec > 999999) {
-	    ctx->autostop.timestamp.tv_sec += 1 + vctx->params.autostop.duration / 1000000;
-	    ctx->autostop.timestamp.tv_usec -= 1000000;
-	} else {
-	    ctx->autostop.timestamp.tv_sec += vctx->params.autostop.duration / 1000000;
-	}
-    }
-    
-    if (vctx->params.autostop.max_events) {
-	ctx->autostop.evid = vctx->params.autostop.max_events;
-    }
-    
-    ctx->started = 1;
-    ctx->run_reader = 1;
-    
-    pthread_attr_init(&attr);
-
-    if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO)) {
-	pcilib_warning("Can't schedule a real-time thread, you may consider running as root");
-    } else {
-	sched.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;	// Let 1 priority for something really critcial
-	pthread_attr_setschedparam(&attr, &sched);
-    }
-    
-    if (pthread_create(&ctx->rthread, &attr, &ipecamera_reader_thread, (void*)ctx)) {
-	ctx->started = 0;
-	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
-	err = PCILIB_ERROR_THREAD;
-    }
-    
-    pthread_attr_destroy(&attr);    
-
-    return err;
-}
-
-
-int ipecamera_stop(pcilib_context_t *vctx, pcilib_event_flags_t flags) {
-    int err;
-    void *retcode;
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-
-    if (!ctx) {
-	pcilib_error("IPECamera imaging is not initialized");
-	return PCILIB_ERROR_NOTINITIALIZED;
-    }
-
-    if (flags&PCILIB_EVENT_FLAG_STOP_ONLY) {
-	ctx->run_reader = 0;
-	return 0;
-    }
-    
-    if (ctx->started) {
-	ctx->run_reader = 0;
-	err = pthread_join(ctx->rthread, &retcode);
-	if (err) pcilib_error("Error joining the reader thread");
-    }
-
-    if (ctx->wdma != PCILIB_DMA_ENGINE_INVALID) {
-	pcilib_stop_dma(vctx->pcilib, ctx->wdma, PCILIB_DMA_FLAGS_DEFAULT);
-	ctx->wdma = PCILIB_DMA_ENGINE_INVALID;
-    }
-
-    if (ctx->rdma != PCILIB_DMA_ENGINE_INVALID) {
-	pcilib_stop_dma(vctx->pcilib, ctx->rdma, PCILIB_DMA_FLAGS_DEFAULT);
-	ctx->rdma = PCILIB_DMA_ENGINE_INVALID;
-    }
-    
-    if (ctx->ipedec) {
-	ufo_decoder_free(ctx->ipedec);
-	ctx->ipedec = NULL;
-    }
-
-    if (ctx->frame_info) {
-	free(ctx->frame_info);
-	ctx->frame_info = NULL;
-    }
-
-    if (ctx->cmask) {
-	free(ctx->cmask);
-	ctx->cmask = NULL;
-    }
-
-    if (ctx->image) {
-	free(ctx->image);
-	ctx->image = NULL;
-    }
-
-    if (ctx->buffer) {
-	free(ctx->buffer);
-	ctx->buffer = NULL;
-    }
-    
-
-    memset(&ctx->autostop, 0, sizeof(ipecamera_autostop_t));
-
-    ctx->event_id = 0;
-    ctx->reported_id = 0;
-    ctx->buffer_pos = 0; 
-    ctx->started = 0;
-
-    return 0;
-}
-
-
-int ipecamera_trigger(pcilib_context_t *vctx, pcilib_event_t event, size_t trigger_size, void *trigger_data) {
-    int err = 0;
-    pcilib_register_value_t value;
-
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-    pcilib_t *pcilib = vctx->pcilib;
-
-    if (!ctx) {
-	pcilib_error("IPECamera imaging is not initialized");
-	return PCILIB_ERROR_NOTINITIALIZED;
-    }
-
-    if (!ctx->started) {
-	pcilib_error("Can't trigger while grabbing is not started");
-	return PCILIB_ERROR_INVALID_REQUEST;
-    }
-
-    SET_REG(control_reg, IPECAMERA_FRAME_REQUEST|IPECAMERA_READOUT_FLAG);
-    usleep(IPECAMERA_WAIT_FRAME_RCVD_TIME);
-    CHECK_REG(status_reg, IPECAMERA_EXPECTED_STATUS);
-    SET_REG(control_reg, IPECAMERA_IDLE|IPECAMERA_READOUT_FLAG);
-
-
-	// DS: Just measure when next trigger is allowed instead and wait in the beginning
-    usleep(IPECAMERA_NEXT_FRAME_DELAY);  // minimum delay between End Of Readout and next Frame Req
-
-    return 0;
-}
-
-int ipecamera_stream(pcilib_context_t *vctx, pcilib_event_callback_t callback, void *user) {
-    int err = 0;
-    int do_stop = 0;
-    pcilib_event_id_t reported;
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-
-    size_t events = 0;
-
-    struct timeval stream_stop;
-
-    if (!ctx) {
-	pcilib_error("IPECamera imaging is not initialized");
-	return PCILIB_ERROR_NOTINITIALIZED;
-    }
-
-    ctx->streaming = 1;
-    ctx->run_streamer = 1;
-
-    if (!ctx->started) {
-	err = ipecamera_start(vctx, PCILIB_EVENTS_ALL, PCILIB_EVENT_FLAGS_DEFAULT);
-	if (err) {
-	    ctx->streaming = 0;
-	    pcilib_error("IPECamera is not in grabbing state");
-	    return PCILIB_ERROR_INVALID_STATE;
-	}
-	
-	do_stop = 1;
-    }
-    
-	// This loop iterates while the generation
-    while ((ctx->run_streamer)||(ctx->reported_id != ctx->event_id)) {
-	while (ctx->reported_id != ctx->event_id) {
-	    if ((ctx->event_id - ctx->reported_id) > (ctx->buffer_size - IPECAMERA_RESERVE_BUFFERS)) ctx->reported_id = ctx->event_id - (ctx->buffer_size - 1) - IPECAMERA_RESERVE_BUFFERS;
-	    else ++ctx->reported_id;
-
-	    callback(ctx->reported_id, (pcilib_event_info_t*)(ctx->frame_info + ((ctx->reported_id-1)%ctx->buffer_size)), user);
-	}
-	usleep(IPECAMERA_NOFRAME_SLEEP);
-    }
-
-    ctx->streaming = 0;
-
-    if (do_stop) {
-	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
-    }
-    
-
-    return err;
-}
-
-int ipecamera_next_event(pcilib_context_t *vctx, pcilib_event_t event_mask, pcilib_timeout_t timeout, pcilib_event_id_t *evid, pcilib_event_info_t **info) {
-    struct timeval tv;
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-
-    if (!ctx) {
-	pcilib_error("IPECamera imaging is not initialized");
-	return PCILIB_ERROR_NOTINITIALIZED;
-    }
-
-    if (!ctx->started) {
-	pcilib_error("IPECamera is not in grabbing mode");
-	return PCILIB_ERROR_INVALID_REQUEST;
-    }
-
-    if (ctx->reported_id == ctx->event_id) {
-	if (timeout) {
-	    pcilib_calc_deadline(&tv, timeout);
-	    
-	    while ((pcilib_calc_time_to_deadline(&tv) > 0)&&(ctx->reported_id == ctx->event_id))
-		usleep(IPECAMERA_NOFRAME_SLEEP);
-	}
-	
-	if (ctx->reported_id == ctx->event_id) return PCILIB_ERROR_TIMEOUT;
-    }
-
-    if ((ctx->event_id - ctx->reported_id) > (ctx->buffer_size - IPECAMERA_RESERVE_BUFFERS)) ctx->reported_id = ctx->event_id - (ctx->buffer_size - 1) - IPECAMERA_RESERVE_BUFFERS;
-    else ++ctx->reported_id;
-
-    if (evid) *evid = ctx->reported_id;
-    if (info) *info = (pcilib_event_info_t*)(ctx->frame_info + ((ctx->reported_id-1)%ctx->buffer_size));
-    
-    return 0;
-}
-
-
-inline static int ipecamera_decode_frame(ipecamera_t *ctx, pcilib_event_id_t event_id) {
-    int err;
-    uint32_t tmp;
-    uint16_t *pixels;
-    
-    int buf_ptr = (event_id - 1) % ctx->buffer_size;
-    
-    if (!ctx->frame_info[buf_ptr].image_ready) {
-	if (ctx->frame_info[buf_ptr].info.flags&PCILIB_EVENT_INFO_FLAG_BROKEN) return PCILIB_ERROR_INVALID_DATA;
-	
-	ufo_decoder_set_raw_data(ctx->ipedec, ctx->buffer + buf_ptr * ctx->padded_size, ctx->raw_size);
-		
-	pixels = ctx->image + buf_ptr * ctx->image_size;
-	memset(ctx->cmask + ctx->buffer_pos * ctx->dim.height, 0, ctx->dim.height * sizeof(ipecamera_change_mask_t));
-	err = ufo_decoder_get_next_frame(ctx->ipedec, &pixels, &tmp, &tmp, ctx->cmask + ctx->buffer_pos * ctx->dim.height);
-	if (err) return PCILIB_ERROR_FAILED;
-	    
-	ctx->frame_info[buf_ptr].image_ready = 1;
-
-	if (ipecamera_resolve_event_id(ctx, event_id) < 0) {
-	    ctx->frame_info[buf_ptr].image_ready = 0;
-	    return PCILIB_ERROR_TIMEOUT;
-	}
-    }
-    
-    return 0;
-}
-
-void* ipecamera_get(pcilib_context_t *vctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size, void *data) {
-    int err;
-    int buf_ptr;
-    size_t raw_size;
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-    uint16_t *pixels;
-
-    if (!ctx) {
-	pcilib_error("IPECamera imaging is not initialized");
-	return NULL;
-    }
-    
-
-    buf_ptr = ipecamera_resolve_event_id(ctx, event_id);
-    if (buf_ptr < 0) return NULL;
-    
-    switch ((ipecamera_data_type_t)data_type) {
-	case IPECAMERA_RAW_DATA:
-	    raw_size = ctx->frame_info[buf_ptr].raw_size;
-	    if (data) {
-		if ((!size)||(*size < raw_size)) return NULL;
-		memcpy(data, ctx->buffer + buf_ptr * ctx->padded_size, raw_size);
-		if (ipecamera_resolve_event_id(ctx, event_id) < 0) return NULL;
-		*size = raw_size;
-		return data;
-	    }
-	    if (size) *size = raw_size;
-	    return ctx->buffer + buf_ptr * ctx->padded_size;
-	case IPECAMERA_IMAGE_DATA:
-	    err = ipecamera_decode_frame(ctx, event_id);
-	    if (err) return NULL;
-
-	    if (data) {
-		if ((!size)||(*size < ctx->image_size * sizeof(ipecamera_pixel_t))) return NULL;
-		memcpy(data, ctx->image + buf_ptr * ctx->image_size, ctx->image_size * sizeof(ipecamera_pixel_t));
-		if (ipecamera_resolve_event_id(ctx, event_id) < 0) return NULL;
-		*size =  ctx->image_size * sizeof(ipecamera_pixel_t);
-		return data;
-	    }
-	
-	    if (size) *size = ctx->image_size * sizeof(ipecamera_pixel_t);
-	    return ctx->image + buf_ptr * ctx->image_size;
-	case IPECAMERA_CHANGE_MASK:
-	    err = ipecamera_decode_frame(ctx, event_id);
-	    if (err) return NULL;
-
-	    if (data) {
-		if ((!size)||(*size < ctx->dim.height * sizeof(ipecamera_change_mask_t))) return NULL;
-		memcpy(data, ctx->image + buf_ptr * ctx->dim.height, ctx->dim.height * sizeof(ipecamera_change_mask_t));
-		if (ipecamera_resolve_event_id(ctx, event_id) < 0) return NULL;
-		*size =  ctx->dim.height * sizeof(ipecamera_change_mask_t);
-
-		return data;
-	    }
-
-	    if (size) *size = ctx->dim.height * sizeof(ipecamera_change_mask_t);
-	    return ctx->cmask + buf_ptr * ctx->dim.height;
-	case IPECAMERA_DIMENSIONS:
-	    if (size) *size = sizeof(ipecamera_image_dimensions_t);
-	    return &ctx->dim;
-	case IPECAMERA_IMAGE_REGION:
-	case IPECAMERA_PACKED_IMAGE:
-	    // Shall we return complete image or only changed parts?
-	case IPECAMERA_PACKED_LINE:
-	case IPECAMERA_PACKED_PAYLOAD:
-	    pcilib_error("Support for data type (%li) is not implemented yet", data_type);
-	    return NULL;
-	default:
-	    pcilib_error("Unknown data type (%li) is requested", data_type);
-	    return NULL;
-    }
-}
-
-
-
-int ipecamera_return(pcilib_context_t *vctx, pcilib_event_id_t event_id, void *data) {
-    ipecamera_t *ctx = (ipecamera_t*)vctx;
-
-    if (!ctx) {
-	pcilib_error("IPECamera imaging is not initialized");
-	return PCILIB_ERROR_NOTINITIALIZED;
-
-    }
-
-    if (ipecamera_resolve_event_id(ctx, event_id) < 0) {
-	return PCILIB_ERROR_NOTAVAILABLE;
-    }
-
-    return 0;
-}
diff --git a/ipecamera/ipecamera.c b/ipecamera/ipecamera.c
new file mode 100644
index 0000000..0819284
--- /dev/null
+++ b/ipecamera/ipecamera.c
@@ -0,0 +1,597 @@
+#define _IPECAMERA_IMAGE_C
+#define _BSD_SOURCE
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/time.h>
+#include <pthread.h>
+#include <assert.h>
+
+#include <ufodecode.h>
+
+#include "../tools.h"
+#include "../error.h"
+#include "../event.h"
+
+#include "pcilib.h"
+#include "private.h"
+#include "model.h"
+#include "reader.h"
+#include "events.h"
+#include "data.h"
+
+#include "dma/nwl_dma.h"
+
+#ifdef IPECAMERA_DEBUG
+#include "dma/nwl.h"
+#endif /* IPECAMERA_DEBUG */
+
+
+#define FIND_REG(var, bank, name)  \
+        ctx->var = pcilib_find_register(pcilib, bank, name); \
+	if (ctx->var ==  PCILIB_REGISTER_INVALID) { \
+	    err = PCILIB_ERROR_NOTFOUND; \
+	    pcilib_error("Unable to find a %s register", name); \
+	}
+    
+
+#define GET_REG(reg, var) \
+    if (!err) { \
+	err = pcilib_read_register_by_id(pcilib, ctx->reg, &var); \
+	if (err) { \
+	    pcilib_error("Error reading %s register", ipecamera_registers[ctx->reg].name); \
+	} \
+    }
+
+#define SET_REG(reg, val) \
+    if (!err) { \
+	err = pcilib_write_register_by_id(pcilib, ctx->reg, val); \
+	if (err) { \
+	    pcilib_error("Error writting %s register", ipecamera_registers[ctx->reg].name); \
+	} \
+    }
+
+#define CHECK_REG(reg, check) \
+    if (!err) { \
+	err = pcilib_read_register_by_id(pcilib, ctx->reg, &value); \
+	if (err) { \
+	    pcilib_error("Error reading %s register", ipecamera_registers[ctx->reg].name); \
+	} \
+	if (!(check)) { \
+	    pcilib_error("Unexpected value (%li) of register %s", value, ipecamera_registers[ctx->reg].name); \
+	    err = PCILIB_ERROR_INVALID_DATA; \
+	} \
+    }
+
+#define CHECK_VALUE(value, val) \
+    if ((!err)&&(value != val)) { \
+	pcilib_error("Unexpected value (0x%x) in data stream (0x%x is expected)", value, val); \
+	err = PCILIB_ERROR_INVALID_DATA; \
+    }
+
+#define CHECK_FLAG(flag, check, ...) \
+    if ((!err)&&(!(check))) { \
+	pcilib_error("Unexpected value (0x%x) of " flag,  __VA_ARGS__); \
+	err = PCILIB_ERROR_INVALID_DATA; \
+    }
+
+
+pcilib_context_t *ipecamera_init(pcilib_t *pcilib) {
+    int err = 0; 
+    
+    ipecamera_t *ctx = malloc(sizeof(ipecamera_t));
+
+    if (ctx) {
+	memset(ctx, 0, sizeof(ipecamera_t));
+	
+	ctx->buffer_size = IPECAMERA_DEFAULT_BUFFER_SIZE;
+	ctx->dim.bpp = sizeof(ipecamera_pixel_t) * 8;
+
+	    // We need DMA engine initialized to resolve DMA registers
+//	FIND_REG(packet_len_reg, "fpga", "xrawdata_packet_length");
+	
+	FIND_REG(status_reg, "fpga", "status");
+	FIND_REG(control_reg, "fpga", "control");
+	FIND_REG(start_reg, "fpga", "start_address");
+	FIND_REG(end_reg, "fpga", "end_address");
+
+	FIND_REG(n_lines_reg, "cmosis", "number_lines");
+	FIND_REG(line_reg, "cmosis", "start1");
+	FIND_REG(exposure_reg, "cmosis", "exp_time");
+	FIND_REG(flip_reg, "cmosis", "image_flipping");
+
+	ctx->rdma = PCILIB_DMA_ENGINE_INVALID;
+	ctx->wdma = PCILIB_DMA_ENGINE_INVALID;
+
+	if (err) {
+	    free(ctx);
+	    return NULL;
+	}
+    }
+    
+    return (pcilib_context_t*)ctx;
+}
+
+void ipecamera_free(pcilib_context_t *vctx) {
+    if (vctx) {
+	ipecamera_t *ctx = (ipecamera_t*)vctx;
+	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	free(ctx);
+    }
+}
+
+pcilib_dma_context_t *ipecamera_init_dma(pcilib_context_t *vctx) {
+//    ipecamera_t *ctx = (ipecamera_t*)vctx;
+    
+    pcilib_model_description_t *model_info = pcilib_get_model_description(vctx->pcilib);
+    if ((!model_info->dma_api)||(!model_info->dma_api->init)) {
+	pcilib_error("The DMA engine is not configured in model");
+	return NULL;
+    }
+
+
+#ifdef IPECAMERA_DMA_R3
+    return model_info->dma_api->init(vctx->pcilib, PCILIB_NWL_MODIFICATION_IPECAMERA, NULL);
+#else
+    return model_info->dma_api->init(vctx->pcilib, PCILIB_DMA_MODIFICATION_DEFAULT, NULL);
+#endif
+}
+
+
+int ipecamera_set_buffer_size(ipecamera_t *ctx, int size) {
+    if (ctx->started) {
+	pcilib_error("Can't change buffer size while grabbing");
+	return PCILIB_ERROR_INVALID_REQUEST;
+    }
+    
+    if (size < 2) {
+	pcilib_error("The buffer size is too small");
+	return PCILIB_ERROR_INVALID_REQUEST;
+    }
+    
+    if (((size^(size-1)) < size) < size) {
+	pcilib_error("The buffer size is not power of 2");
+    }
+    
+    ctx->buffer_size = size;
+    
+    return 0;
+}
+
+int ipecamera_reset(pcilib_context_t *vctx) {
+    int err = 0;
+    ipecamera_t *ctx = (ipecamera_t*)vctx;
+    pcilib_t *pcilib = vctx->pcilib;
+
+    pcilib_register_t control, status;
+    pcilib_register_value_t value;
+
+    if (!ctx) {
+	pcilib_error("IPECamera imaging is not initialized");
+	return PCILIB_ERROR_NOTINITIALIZED;
+    }
+
+    pcilib = vctx->pcilib;
+    control = ctx->control_reg;
+    status = ctx->status_reg;
+
+	// Set Reset bit to CMOSIS
+    err = pcilib_write_register_by_id(pcilib, control, 0x1e4);
+    if (err) {
+	pcilib_error("Error setting FPGA reset bit");
+	return err;
+    }
+    usleep(IPECAMERA_SLEEP_TIME);
+
+	// Remove Reset bit to CMOSIS
+    err = pcilib_write_register_by_id(pcilib, control, 0x1e1);
+    if (err) {
+	pcilib_error("Error reseting FPGA reset bit");
+	return err;
+    }
+    usleep(IPECAMERA_SLEEP_TIME);
+
+	// Special settings for CMOSIS v.2
+    value = 01; err = pcilib_write_register_space(pcilib, "cmosis", 115, 1, &value);
+    if (err) {
+	pcilib_error("Error setting CMOSIS configuration");
+	return err;
+    }
+    usleep(IPECAMERA_SLEEP_TIME);
+
+    value = 07; err = pcilib_write_register_space(pcilib, "cmosis", 82, 1, &value);
+    if (err) {
+	pcilib_error("Error setting CMOSIS configuration");
+	return err;
+    }
+    usleep(IPECAMERA_SLEEP_TIME);
+
+	// Set default parameters
+    err = pcilib_write_register_by_id(pcilib, control, IPECAMERA_IDLE);
+    if (err) {
+	pcilib_error("Error bringing FPGA in default mode");
+	return err;
+    }
+
+    usleep(10000);
+
+        
+    err = pcilib_read_register_by_id(pcilib, status, &value);
+    if (err) {
+	pcilib_error("Error reading status register");
+	return err;
+    }
+
+    if (value != IPECAMERA_EXPECTED_STATUS) {
+	pcilib_error("Unexpected value (%lx) of status register, expected %lx", value, IPECAMERA_EXPECTED_STATUS);
+	return PCILIB_ERROR_VERIFY;
+    }
+
+    return 0;    
+}
+
+
+int ipecamera_start(pcilib_context_t *vctx, pcilib_event_t event_mask, pcilib_event_flags_t flags) {
+    int i;
+    int err = 0;
+    ipecamera_t *ctx = (ipecamera_t*)vctx;
+    pcilib_t *pcilib = vctx->pcilib;
+    pcilib_register_value_t value;
+    
+    const size_t chan_size = (2 + IPECAMERA_PIXELS_PER_CHANNEL / 3) * sizeof(ipecamera_payload_t);
+    const size_t line_size = (IPECAMERA_MAX_CHANNELS * chan_size);
+    const size_t header_size = 8 * sizeof(ipecamera_payload_t);
+    const size_t footer_size = 8 * sizeof(ipecamera_payload_t);
+    size_t raw_size;
+    size_t padded_blocks;
+    
+    pthread_attr_t attr;
+    struct sched_param sched;
+    
+    if (!ctx) {
+	pcilib_error("IPECamera imaging is not initialized");
+	return PCILIB_ERROR_NOTINITIALIZED;
+    }
+    
+    if (ctx->started) {
+	pcilib_error("IPECamera grabbing is already started");
+	return PCILIB_ERROR_INVALID_REQUEST;
+    }
+
+
+	// Allow readout and clean the FRAME_REQUEST mode if set for some reason
+    SET_REG(control_reg, IPECAMERA_IDLE|IPECAMERA_READOUT_FLAG);
+    usleep(IPECAMERA_SLEEP_TIME);
+    CHECK_REG(status_reg, IPECAMERA_EXPECTED_STATUS);
+    if (err) return err;
+
+
+    ctx->event_id = 0;
+    ctx->preproc_id = 0;
+    ctx->reported_id = 0;
+    ctx->buffer_pos = 0;
+    ctx->parse_data = (flags&PCILIB_EVENT_FLAG_RAW_DATA_ONLY)?0:1;
+    ctx->cur_size = 0;
+
+    ctx->dim.width = IPECAMERA_WIDTH;
+    GET_REG(n_lines_reg, ctx->dim.height);
+    
+    raw_size = header_size + ctx->dim.height * line_size + footer_size;
+    padded_blocks = raw_size / IPECAMERA_DMA_PACKET_LENGTH + ((raw_size % IPECAMERA_DMA_PACKET_LENGTH)?1:0);
+    
+    ctx->image_size = ctx->dim.width * ctx->dim.height;
+    ctx->raw_size = raw_size;
+    ctx->full_size = padded_blocks * IPECAMERA_DMA_PACKET_LENGTH;
+
+#ifdef IPECAMERA_BUG_EXTRA_DATA
+    ctx->full_size += 8;
+    padded_blocks ++;
+#endif /* IPECAMERA_BUG_EXTRA_DATA */
+
+    ctx->padded_size = padded_blocks * IPECAMERA_DMA_PACKET_LENGTH;
+
+    ctx->buffer = malloc(ctx->padded_size * ctx->buffer_size);
+    if (!ctx->buffer) {
+	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	pcilib_error("Unable to allocate ring buffer (%lu bytes)", ctx->padded_size * ctx->buffer_size);
+	return PCILIB_ERROR_MEMORY;
+    }
+
+    ctx->image = (ipecamera_pixel_t*)malloc(ctx->image_size * ctx->buffer_size * sizeof(ipecamera_pixel_t));
+    if (!ctx->image) {
+	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	pcilib_error("Unable to allocate image buffer (%lu bytes)", ctx->image_size * ctx->buffer_size * sizeof(ipecamera_pixel_t));
+	return PCILIB_ERROR_MEMORY;
+    }
+
+    ctx->cmask = malloc(ctx->dim.height * ctx->buffer_size * sizeof(ipecamera_change_mask_t));
+    if (!ctx->cmask) {
+	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	pcilib_error("Unable to allocate change-mask buffer");
+	return PCILIB_ERROR_MEMORY;
+    }
+
+    ctx->frame = (ipecamera_frame_t*)malloc(ctx->buffer_size * sizeof(ipecamera_frame_t));
+    if (!ctx->frame) {
+	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	pcilib_error("Unable to allocate frame-info buffer");
+	return PCILIB_ERROR_MEMORY;
+    }
+    
+    memset(ctx->frame, 0, ctx->buffer_size * sizeof(ipecamera_frame_t));
+    
+    for (i = 0; i < ctx->buffer_size; i++) {
+	err = pthread_rwlock_init(&ctx->frame[i].mutex, NULL);
+	if (err) break;
+    }
+
+    ctx->frame_mutex_destroy = i;
+
+    if (err) {
+        ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	pcilib_error("Initialization of rwlock mutexes for frame synchronization has failed");
+	return PCILIB_ERROR_FAILED;
+    }
+    
+    ctx->ipedec = ufo_decoder_new(ctx->dim.height, ctx->dim.width, NULL, 0);
+    if (!ctx->ipedec) {
+        ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	pcilib_error("Unable to initialize IPECamera decoder library");
+	return PCILIB_ERROR_FAILED;
+    }
+
+    if (!err) {
+	ctx->rdma = pcilib_find_dma_by_addr(vctx->pcilib, PCILIB_DMA_FROM_DEVICE, IPECAMERA_DMA_ADDRESS);
+	if (ctx->rdma == PCILIB_DMA_ENGINE_INVALID) {
+	    err = PCILIB_ERROR_NOTFOUND;
+	    pcilib_error("The C2S channel of IPECamera DMA Engine (%u) is not found", IPECAMERA_DMA_ADDRESS);
+	} else {
+	    err = pcilib_start_dma(vctx->pcilib, ctx->rdma, PCILIB_DMA_FLAGS_DEFAULT);
+	    if (err) {
+		ctx->rdma = PCILIB_DMA_ENGINE_INVALID;
+		pcilib_error("Failed to initialize C2S channel of IPECamera DMA Engine (%u)", IPECAMERA_DMA_ADDRESS);
+	    }
+	}
+    }
+    
+/*    
+    if (!err) {
+	ctx->wdma = pcilib_find_dma_by_addr(vctx->pcilib, PCILIB_DMA_TO_DEVICE, IPECAMERA_DMA_ADDRESS);
+	if (ctx->wdma == PCILIB_DMA_ENGINE_INVALID) {
+	    err = PCILIB_ERROR_NOTFOUND;
+	    pcilib_error("The S2C channel of IPECamera DMA Engine (%u) is not found", IPECAMERA_DMA_ADDRESS);
+	} else {
+	    err = pcilib_start_dma(vctx->pcilib, ctx->wdma, PCILIB_DMA_FLAGS_DEFAULT);
+	    if (err) {
+		ctx->wdma = PCILIB_DMA_ENGINE_INVALID;
+		pcilib_error("Failed to initialize S2C channel of IPECamera DMA Engine (%u)", IPECAMERA_DMA_ADDRESS);
+	    }
+	}
+    }
+*/    
+
+/*
+    SET_REG(packet_len_reg, IPECAMERA_DMA_PACKET_LENGTH);
+*/
+
+    if (err) {
+        ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	return err;
+    }
+
+	// Clean DMA
+    err = pcilib_skip_dma(vctx->pcilib, ctx->rdma);
+    if (err) {
+        ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	pcilib_error("Can't start grabbing, device continuously writes unexpected data using DMA engine");
+	return err;
+    }
+
+    if (vctx->params.autostop.duration) {
+	gettimeofday(&ctx->autostop.timestamp, NULL);
+	ctx->autostop.timestamp.tv_usec += vctx->params.autostop.duration % 1000000;
+	if (ctx->autostop.timestamp.tv_usec > 999999) {
+	    ctx->autostop.timestamp.tv_sec += 1 + vctx->params.autostop.duration / 1000000;
+	    ctx->autostop.timestamp.tv_usec -= 1000000;
+	} else {
+	    ctx->autostop.timestamp.tv_sec += vctx->params.autostop.duration / 1000000;
+	}
+    }
+    
+    if (vctx->params.autostop.max_events) {
+	ctx->autostop.evid = vctx->params.autostop.max_events;
+    }
+    
+    if (flags&PCILIB_EVENT_FLAG_PREPROCESS) {
+	ctx->n_preproc = pcilib_get_cpu_count();
+	switch (ctx->n_preproc) {
+	    case 1: break;
+	    case 2-3: ctx->n_preproc -= 1; break;
+	    default: ctx->n_preproc -= 2; break;
+	}
+	
+	ctx->preproc = (ipecamera_preprocessor_t*)malloc(ctx->n_preproc * sizeof(ipecamera_preprocessor_t));
+	if (!ctx->preproc) {
+	    ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	    pcilib_error("Unable to allocate memory for preprocessor contexts");
+	    return PCILIB_ERROR_MEMORY;
+	}
+
+	memset(ctx->preproc, 0, ctx->n_preproc * sizeof(ipecamera_preprocessor_t));
+
+	err = pthread_mutex_init(&ctx->preproc_mutex, NULL);
+	if (err) {
+	    ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	    pcilib_error("Failed to initialize event mutex");
+	    return PCILIB_ERROR_FAILED;
+	}
+	ctx->preproc_mutex_destroy = 1;
+	
+
+	ctx->run_preprocessors = 1;
+	for (i = 0; i < ctx->n_preproc; i++) {
+	    ctx->preproc[i].i = i;
+	    ctx->preproc[i].ipecamera = ctx;
+	    err = pthread_create(&ctx->preproc[i].thread, NULL, ipecamera_preproc_thread, ctx->preproc + i);
+	    if (err) {
+		err = PCILIB_ERROR_FAILED;
+		break;
+	    } else {
+		ctx->preproc[i].started = 1;
+	    }
+	}
+	
+	if (err) {
+	    ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	    pcilib_error("Failed to schedule some of the preprocessor threads");
+	    return err;
+	}
+    } else {
+	ctx->n_preproc = 0;
+    }
+
+    ctx->started = 1;
+    ctx->run_reader = 1;
+
+    pthread_attr_init(&attr);
+
+    if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO)) {
+	pcilib_warning("Can't schedule a real-time thread, you may consider running as root");
+    } else {
+	sched.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;	// Let 1 priority for something really critcial
+	pthread_attr_setschedparam(&attr, &sched);
+    }
+    
+    if (pthread_create(&ctx->rthread, &attr, &ipecamera_reader_thread, (void*)ctx)) {
+	ctx->started = 0;
+	ipecamera_stop(vctx, PCILIB_EVENT_FLAGS_DEFAULT);
+	err = PCILIB_ERROR_FAILED;
+    }
+    
+    pthread_attr_destroy(&attr);    
+
+    return err;
+}
+
+
+int ipecamera_stop(pcilib_context_t *vctx, pcilib_event_flags_t flags) {
+    int i;
+    int err;
+    void *retcode;
+    ipecamera_t *ctx = (ipecamera_t*)vctx;
+
+    if (!ctx) {
+	pcilib_error("IPECamera imaging is not initialized");
+	return PCILIB_ERROR_NOTINITIALIZED;
+    }
+
+    if (flags&PCILIB_EVENT_FLAG_STOP_ONLY) {
+	ctx->run_reader = 0;
+	return 0;
+    }
+
+    if (ctx->started) {
+	ctx->run_reader = 0;
+	err = pthread_join(ctx->rthread, &retcode);
+	if (err) pcilib_error("Error joining the reader thread");
+    }
+    
+    if (ctx->preproc) {
+	ctx->run_preprocessors = 0;
+	
+	for (i = 0; i < ctx->n_preproc; i++) {
+	    if (ctx->preproc[i].started) {
+		pthread_join(ctx->preproc[i].thread, &retcode);
+		ctx->preproc[i].started = 0;
+	    }
+	}
+
+	if (ctx->preproc_mutex_destroy) {
+	    pthread_mutex_destroy(&ctx->preproc_mutex);
+	    ctx->preproc_mutex_destroy = 0;
+	}
+	
+	free(ctx->preproc);
+	ctx->preproc = NULL;
+    }
+    
+    if (ctx->frame_mutex_destroy) {
+	for (i = 0; i < ctx->frame_mutex_destroy; i++) {
+	    pthread_rwlock_destroy(&ctx->frame[i].mutex);
+	}
+	ctx->frame_mutex_destroy = 0;
+    }
+    
+    if (ctx->wdma != PCILIB_DMA_ENGINE_INVALID) {
+	pcilib_stop_dma(vctx->pcilib, ctx->wdma, PCILIB_DMA_FLAGS_DEFAULT);
+	ctx->wdma = PCILIB_DMA_ENGINE_INVALID;
+    }
+
+    if (ctx->rdma != PCILIB_DMA_ENGINE_INVALID) {
+	pcilib_stop_dma(vctx->pcilib, ctx->rdma, PCILIB_DMA_FLAGS_DEFAULT);
+	ctx->rdma = PCILIB_DMA_ENGINE_INVALID;
+    }
+    
+    if (ctx->ipedec) {
+	ufo_decoder_free(ctx->ipedec);
+	ctx->ipedec = NULL;
+    }
+
+    if (ctx->frame) {
+	free(ctx->frame);
+	ctx->frame = NULL;
+    }
+
+    if (ctx->cmask) {
+	free(ctx->cmask);
+	ctx->cmask = NULL;
+    }
+
+    if (ctx->image) {
+	free(ctx->image);
+	ctx->image = NULL;
+    }
+
+    if (ctx->buffer) {
+	free(ctx->buffer);
+	ctx->buffer = NULL;
+    }
+    
+
+    memset(&ctx->autostop, 0, sizeof(ipecamera_autostop_t));
+
+    ctx->event_id = 0;
+    ctx->reported_id = 0;
+    ctx->buffer_pos = 0; 
+    ctx->started = 0;
+
+    return 0;
+}
+
+
+int ipecamera_trigger(pcilib_context_t *vctx, pcilib_event_t event, size_t trigger_size, void *trigger_data) {
+    int err = 0;
+    pcilib_register_value_t value;
+
+    ipecamera_t *ctx = (ipecamera_t*)vctx;
+    pcilib_t *pcilib = vctx->pcilib;
+
+    if (!ctx) {
+	pcilib_error("IPECamera imaging is not initialized");
+	return PCILIB_ERROR_NOTINITIALIZED;
+    }
+
+    SET_REG(control_reg, IPECAMERA_FRAME_REQUEST|IPECAMERA_READOUT_FLAG);
+    usleep(IPECAMERA_WAIT_FRAME_RCVD_TIME);
+    CHECK_REG(status_reg, IPECAMERA_EXPECTED_STATUS);
+    SET_REG(control_reg, IPECAMERA_IDLE|IPECAMERA_READOUT_FLAG);
+
+
+	// DS: Just measure when next trigger is allowed instead and wait in the beginning
+    usleep(IPECAMERA_NEXT_FRAME_DELAY);  // minimum delay between End Of Readout and next Frame Req
+
+    return 0;
+}
diff --git a/ipecamera/ipecamera.h b/ipecamera/ipecamera.h
index 3abcf25..19c123b 100644
--- a/ipecamera/ipecamera.h
+++ b/ipecamera/ipecamera.h
@@ -4,9 +4,9 @@
 typedef struct ipecamera_s ipecamera_t;
 
 typedef  struct {
-    int bpp;			/*<< Bits per pixel (8, 16, or 32) as returned by IPECAMERA_IMAGE_DATA */
-    int real_bpp;		/*<< Bits per pixel as returned by camera and IPECAMERA_PACKED_IMAGE */
-    int width, height;
+    unsigned int bpp;			/*<< Bits per pixel (8, 16, or 32) as returned by IPECAMERA_IMAGE_DATA */
+    unsigned int real_bpp;		/*<< Bits per pixel as returned by camera and IPECAMERA_PACKED_IMAGE */
+    unsigned int width, height;
 } ipecamera_image_dimensions_t;
 
 typedef enum {
@@ -25,8 +25,9 @@ typedef uint16_t ipecamera_pixel_t;
 
 typedef struct {
     pcilib_event_info_t info;
-    size_t raw_size;		/**< Indicates the actual size of raw data */
     int image_ready;		/**< Indicates if image data is parsed */
+    int image_broken;		/**< Unlike the info.flags this is bound to the reconstructed image (i.e. is not updated on rawdata overwrite) */
+    size_t raw_size;		/**< Indicates the actual size of raw data */
 } ipecamera_event_info_t;
 
 int ipecamera_set_buffer_size(ipecamera_t *ctx, int size);
diff --git a/ipecamera/model.c b/ipecamera/model.c
index ddf9ee6..e60561b 100644
--- a/ipecamera/model.c
+++ b/ipecamera/model.c
@@ -26,7 +26,7 @@
 int ipecamera_read(pcilib_t *ctx, pcilib_register_bank_description_t *bank, pcilib_register_addr_t addr, pcilib_register_value_t *value) {
     uint32_t val, tmp[4];
     char *wr, *rd;
-    struct timeval start, cur;
+    struct timeval start;//, cur;
     int retries = RETRIES;
 
     assert(addr < 128);
@@ -116,7 +116,7 @@ retry:
 int ipecamera_write(pcilib_t *ctx, pcilib_register_bank_description_t *bank, pcilib_register_addr_t addr, pcilib_register_value_t value) {
     uint32_t val, tmp[4];
     char *wr, *rd;
-    struct timeval start, cur;
+    struct timeval start;//, cur;
     int retries = RETRIES;
 
     assert(addr < 128);
diff --git a/ipecamera/model.h b/ipecamera/model.h
index b9c3b32..4a0fc8c 100644
--- a/ipecamera/model.h
+++ b/ipecamera/model.h
@@ -4,7 +4,7 @@
 #include <stdio.h>
 
 #include "pcilib.h"
-#include "image.h"
+#include "public.h"
 
 //#define IPECAMERA_DEBUG
 
@@ -126,7 +126,7 @@ pcilib_event_api_description_t ipecamera_image_api = {
     ipecamera_trigger,
     
     ipecamera_stream,
-    NULL, //ipecamera_next_event,
+    ipecamera_next_event,
     ipecamera_get,
     ipecamera_return,
     ipecamera_init_dma
diff --git a/ipecamera/private.h b/ipecamera/private.h
new file mode 100644
index 0000000..b583f38
--- /dev/null
+++ b/ipecamera/private.h
@@ -0,0 +1,121 @@
+#ifndef _IPECAMERA_PRIVATE_H
+#define _IPECAMERA_PRIVATE_H
+
+#include "ipecamera.h"
+
+#define IPECAMERA_BUG_EXTRA_DATA
+#define IPECAMERA_BUG_MULTIFRAME_PACKETS
+#define IPECAMERA_BUG_INCOMPLETE_PACKETS
+
+#define IPECAMERA_DEFAULT_BUFFER_SIZE 64  	//**< should be power of 2 */
+#define IPECAMERA_RESERVE_BUFFERS 2		//**< Return Frame is Lost error, if requested frame will be overwritten after specified number of frames
+#define IPECAMERA_SLEEP_TIME 250000 		//**< Michele thinks 250 should be enough, but reset failing in this case */
+#define IPECAMERA_NEXT_FRAME_DELAY 1000 	//**< Michele requires 30000 to sync between End Of Readout and next Frame Req */
+#define IPECAMERA_WAIT_FRAME_RCVD_TIME 0 	//**< by Uros ,wait 6 ms */
+#define IPECAMERA_NOFRAME_SLEEP 100
+#define IPECAMERA_NOFRAME_PREPROC_SLEEP 100
+
+#define IPECAMERA_MAX_LINES 1088
+#define IPECAMERA_EXPECTED_STATUS 0x08409FFFF
+#define IPECAMERA_END_OF_SEQUENCE 0x1F001001
+
+#define IPECAMERA_MAX_CHANNELS 16
+#define IPECAMERA_PIXELS_PER_CHANNEL 128
+#define IPECAMERA_WIDTH (IPECAMERA_MAX_CHANNELS * IPECAMERA_PIXELS_PER_CHANNEL)
+
+#define IPECAMERA_FRAME_REQUEST 		0x1E9
+#define IPECAMERA_READOUT_FLAG			0x200
+#define IPECAMERA_READOUT			0x3E1
+#define IPECAMERA_IDLE 				0x1E1
+#define IPECAMERA_START_INTERNAL_STIMULI 	0x1F1
+
+
+typedef uint32_t ipecamera_payload_t;
+
+typedef struct {
+    pcilib_event_id_t evid;
+    struct timeval timestamp;
+} ipecamera_autostop_t;
+
+typedef struct {
+    size_t i;
+    pthread_t thread;
+    ipecamera_t *ipecamera;
+    
+    int started;			/**< flag indicating that join & cleanup is required */
+} ipecamera_preprocessor_t;
+
+
+typedef struct {
+    ipecamera_event_info_t event;	/**< this structure is overwritten by the reader thread, we need a copy */
+    pthread_rwlock_t mutex;		/**< this mutex protects reconstructed buffers only, the raw data, event_info, etc. will be overwritten by reader thread anyway */
+} ipecamera_frame_t;
+
+struct ipecamera_s {
+    pcilib_context_t event;
+    ufo_decoder ipedec;
+
+    char *data;
+    ipecamera_pixel_t *image;
+    size_t size;
+
+    pcilib_event_callback_t cb;
+    void *cb_user;
+
+    pcilib_event_id_t event_id;
+    pcilib_event_id_t preproc_id;
+    pcilib_event_id_t reported_id;
+    
+    pcilib_dma_engine_t rdma, wdma;
+
+    pcilib_register_t packet_len_reg;
+    pcilib_register_t control_reg, status_reg;
+    pcilib_register_t start_reg, end_reg;
+    pcilib_register_t n_lines_reg;
+    uint16_t line_reg;
+    pcilib_register_t exposure_reg;
+    pcilib_register_t flip_reg;
+
+    int started;		/**< Camera is in grabbing mode (start function is called) */
+    int streaming;		/**< Camera is in streaming mode (we are within stream call) */
+    int parse_data;		/**< Indicates if some processing of the data is required, otherwise only rawdata_callback will be called */
+
+    int run_reader;		/**< Instructs the reader thread to stop processing */
+    int run_streamer;		/**< Indicates request to stop streaming events and can be set by reader_thread upon exit or by user request */
+    int run_preprocessors;	/**< Instructs preprocessors to exit */
+    
+    ipecamera_autostop_t autostop;
+
+    struct timeval autostop_time;
+
+    size_t buffer_size;		/**< How many images to store */
+    size_t buffer_pos;		/**< Current image offset in the buffer, due to synchronization reasons should not be used outside of reader_thread */
+    size_t cur_size;		/**< Already written part of data in bytes */
+    size_t raw_size;		/**< Size of raw data in bytes */
+    size_t full_size;		/**< Size of raw data including the padding */
+    size_t padded_size;		/**< Size of buffer for raw data, including the padding for performance */
+    
+    size_t image_size;		/**< Size of a single image in bytes */
+    
+    int width, height;
+
+    
+//    void *raw_buffer;
+    void *buffer;
+    ipecamera_change_mask_t *cmask;
+    ipecamera_frame_t *frame;
+    
+
+    ipecamera_image_dimensions_t dim;
+
+    pthread_t rthread;
+    
+    size_t n_preproc;
+    ipecamera_preprocessor_t *preproc;
+    pthread_mutex_t preproc_mutex;
+    
+    int preproc_mutex_destroy;
+    int frame_mutex_destroy;
+};
+
+#endif /* _IPECAMERA_PRIVATE_H */
diff --git a/ipecamera/image.h b/ipecamera/public.h
index c311b4d..b745d6c 100644
--- a/ipecamera/image.h
+++ b/ipecamera/public.h
@@ -1,25 +1,25 @@
-#ifndef _IPECAMERA_IMAGE_H
-#define _IPECAMERA_IMAGE_H
+#ifndef _IPECAMERA_PUBLIC_H
+#define _IPECAMERA_PUBLIC_H
 
 #include <stdio.h>
 
 #include "ipecamera.h"
 #include "pcilib.h"
 
+
 pcilib_context_t *ipecamera_init(pcilib_t *pcilib);
 void ipecamera_free(pcilib_context_t *ctx);
 
+pcilib_dma_context_t *ipecamera_init_dma(pcilib_context_t *ctx);
+
 int ipecamera_reset(pcilib_context_t *ctx);
 int ipecamera_start(pcilib_context_t *ctx, pcilib_event_t event_mask, pcilib_event_flags_t flags);
 int ipecamera_stop(pcilib_context_t *ctx, pcilib_event_flags_t flags);
 int ipecamera_trigger(pcilib_context_t *ctx, pcilib_event_t event, size_t trigger_size, void *trigger_data);
 int ipecamera_stream(pcilib_context_t *vctx, pcilib_event_callback_t callback, void *user);
-//pcilib_event_id_t ipecamera_next_event(pcilib_context_t *ctx, pcilib_event_t event_mask, pcilib_timeout_t timeout);
-
-void* ipecamera_get(pcilib_context_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size, void *buf);
-int ipecamera_return(pcilib_context_t *ctx, pcilib_event_id_t event_id, void *data);
-
-pcilib_dma_context_t *ipecamera_init_dma(pcilib_context_t *ctx);
+int ipecamera_next_event(pcilib_context_t *vctx, pcilib_timeout_t timeout, pcilib_event_id_t *evid, size_t info_size, pcilib_event_info_t *info);
 
+int ipecamera_get(pcilib_context_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size, void **buf);
+int ipecamera_return(pcilib_context_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, void *data);
 
-#endif /* _IPECAMERA_IMAGE_H */
+#endif /* _IPECAMERA_PUBLIC_H */
diff --git a/ipecamera/reader.c b/ipecamera/reader.c
new file mode 100644
index 0000000..f02df0f
--- /dev/null
+++ b/ipecamera/reader.c
@@ -0,0 +1,170 @@
+#define _IPECAMERA_IMAGE_C
+#define _BSD_SOURCE
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/time.h>
+#include <pthread.h>
+#include <assert.h>
+
+#include <ufodecode.h>
+
+#include "../tools.h"
+#include "../error.h"
+
+#include "pcilib.h"
+#include "private.h"
+#include "reader.h"
+
+static inline int ipecamera_new_frame(ipecamera_t *ctx) {
+    ctx->frame[ctx->buffer_pos].event.raw_size = ctx->cur_size;
+
+    if (ctx->cur_size < ctx->raw_size) ctx->frame[ctx->buffer_pos].event.info.flags |= PCILIB_EVENT_INFO_FLAG_BROKEN;
+    
+    ctx->buffer_pos = (++ctx->event_id) % ctx->buffer_size;
+    ctx->cur_size = 0;
+
+    ctx->frame[ctx->buffer_pos].event.info.type = PCILIB_EVENT0;
+    ctx->frame[ctx->buffer_pos].event.info.flags = 0;
+    ctx->frame[ctx->buffer_pos].event.image_ready = 0;
+
+    if ((ctx->event_id == ctx->autostop.evid)&&(ctx->event_id)) {
+	ctx->run_reader = 0;
+	return 1;
+    }
+	
+    if (pcilib_check_deadline(&ctx->autostop.timestamp, PCILIB_DMA_TIMEOUT)) {
+	ctx->run_reader = 0;
+	return 1;
+    }
+    
+    return 0;
+}
+
+static uint32_t frame_magic[6] = { 0x51111111, 0x52222222, 0x53333333, 0x54444444, 0x55555555, 0x56666666 };
+
+static int ipecamera_data_callback(void *user, pcilib_dma_flags_t flags, size_t bufsize, void *buf) {
+    int res;
+    int eof = 0;
+
+#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
+    size_t extra_data = 0;
+#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
+
+    ipecamera_t *ctx = (ipecamera_t*)user;
+
+    if (!ctx->cur_size) {
+#if defined(IPECAMERA_BUG_INCOMPLETE_PACKETS)||defined(IPECAMERA_BUG_MULTIFRAME_PACKETS)
+	size_t startpos;
+	for (startpos = 0; (startpos + 8) < bufsize; startpos++) {
+	    if (!memcmp(buf + startpos, frame_magic, sizeof(frame_magic))) break;
+	}
+	
+	if (startpos) {
+	    buf += startpos;
+	    bufsize -= startpos;
+	}
+#endif /* IPECAMERA_BUG_INCOMPLETE_PACKETS */
+
+	if ((bufsize >= 8)&&(!memcmp(buf, frame_magic, sizeof(frame_magic)))) {
+/*
+		// Not implemented in hardware yet
+	    ctx->frame[ctx->buffer_pos].event.info.seqnum = ((uint32_t*)buf)[6] & 0xF0000000;
+	    ctx->frame[ctx->buffer_pos].event.info.offset = ((uint32_t*)buf)[7] & 0xF0000000;
+*/
+	    gettimeofday(&ctx->frame[ctx->buffer_pos].event.info.timestamp, NULL);
+	} else {
+//	    pcilib_warning("Frame magic is not found, ignoring broken data...");
+	    return PCILIB_STREAMING_CONTINUE;
+	}
+    }
+
+#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
+    if (ctx->cur_size + bufsize > ctx->raw_size) {
+        size_t need;
+	
+	for (need = ctx->raw_size - ctx->cur_size; need < bufsize; need += sizeof(uint32_t)) {
+	    if (*(uint32_t*)(buf + need) == frame_magic[0]) break;
+	}
+	
+	if (need < bufsize) {
+	    extra_data = bufsize - need;
+	    //bufsize = need;
+	    eof = 1;
+	}
+	
+	    // just rip of padding
+	bufsize = ctx->raw_size - ctx->cur_size;
+    }
+#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
+
+    if (ctx->parse_data) {
+	if (ctx->cur_size + bufsize > ctx->full_size) {
+    	    pcilib_error("Unexpected event data, we are expecting at maximum (%zu) bytes, but (%zu) already read", ctx->full_size, ctx->cur_size + bufsize);
+	    return -PCILIB_ERROR_TOOBIG;
+	}
+
+        memcpy(ctx->buffer + ctx->buffer_pos * ctx->padded_size +  ctx->cur_size, buf, bufsize);
+    }
+
+    ctx->cur_size += bufsize;
+//    printf("%i: %i %i\n", ctx->buffer_pos, ctx->cur_size, bufsize);
+
+    if (ctx->cur_size >= ctx->full_size) eof = 1;
+
+    if (ctx->event.params.rawdata.callback) {
+	res = ctx->event.params.rawdata.callback(ctx->event_id, (pcilib_event_info_t*)(ctx->frame + ctx->buffer_pos), (eof?PCILIB_EVENT_FLAG_EOF:PCILIB_EVENT_FLAGS_DEFAULT), bufsize, buf, ctx->event.params.rawdata.user);
+	if (res <= 0) {
+	    if (res < 0) return res;
+	    ctx->run_reader = 0;
+	}
+    }
+    
+    if (eof) {
+	if (ipecamera_new_frame(ctx)) {
+	    return PCILIB_STREAMING_STOP;
+	}
+	
+#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
+	if (extra_data) {
+	    return ipecamera_data_callback(user, flags, extra_data, buf + bufsize);
+	}
+#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
+    }
+
+    return PCILIB_STREAMING_REQ_FRAGMENT;
+}
+
+void *ipecamera_reader_thread(void *user) {
+    int err;
+    ipecamera_t *ctx = (ipecamera_t*)user;
+    
+    while (ctx->run_reader) {
+	err = pcilib_stream_dma(ctx->event.pcilib, ctx->rdma, 0, 0, PCILIB_DMA_FLAG_MULTIPACKET, PCILIB_DMA_TIMEOUT, &ipecamera_data_callback, user);
+	if (err) {
+	    if (err == PCILIB_ERROR_TIMEOUT) {
+		if (ctx->cur_size > ctx->raw_size) ipecamera_new_frame(ctx);
+#ifdef IPECAMERA_BUG_INCOMPLETE_PACKETS
+		else if (ctx->cur_size > 0) ipecamera_new_frame(ctx);
+#endif /* IPECAMERA_BUG_INCOMPLETE_PACKETS */
+		if (pcilib_check_deadline(&ctx->autostop.timestamp, PCILIB_DMA_TIMEOUT)) {
+		    ctx->run_reader = 0;
+		    break;
+		}
+		usleep(IPECAMERA_NOFRAME_SLEEP);
+	    } else pcilib_error("DMA error while reading IPECamera frames, error: %i", err);
+	} else printf("no error\n");
+
+	//usleep(1000);
+    }
+    
+    ctx->run_streamer = 0;
+    
+    if (ctx->cur_size)
+	pcilib_error("partialy read frame after stop signal, %zu bytes in the buffer", ctx->cur_size);
+
+    return NULL;
+}
diff --git a/ipecamera/reader.h b/ipecamera/reader.h
new file mode 100644
index 0000000..738d742
--- /dev/null
+++ b/ipecamera/reader.h
@@ -0,0 +1,6 @@
+#ifndef _IPECAMERA_READER_H
+#define _IPECAMERA_READER_H
+
+void *ipecamera_reader_thread(void *user);
+
+#endif /* _IPECAMERA_READER_H */
diff --git a/kmem.c b/kmem.c
index aecccde..2708eb1 100644
--- a/kmem.c
+++ b/kmem.c
@@ -119,24 +119,24 @@ pcilib_kmem_handle_t *pcilib_alloc_kernel_memory(pcilib_t *ctx, pcilib_kmem_type
 	
 	    if (persistent) {
 		if (persistent < 0) {
-		    if ((flags&PCILIB_KMEM_FLAG_PERSISTENT == 0)&&(kh.flags&KMEM_FLAG_REUSED_PERSISTENT)) err = PCILIB_ERROR_INVALID_STATE;
+		    if (((flags&PCILIB_KMEM_FLAG_PERSISTENT) == 0)&&(kh.flags&KMEM_FLAG_REUSED_PERSISTENT)) err = PCILIB_ERROR_INVALID_STATE;
 		    else persistent = (kh.flags&KMEM_FLAG_REUSED_PERSISTENT)?1:0;
-		} else if (kh.flags&KMEM_FLAG_REUSED_PERSISTENT == 0) err = PCILIB_ERROR_INVALID_STATE;
+		} else if ((kh.flags&KMEM_FLAG_REUSED_PERSISTENT) == 0) err = PCILIB_ERROR_INVALID_STATE;
 	    } else if (kh.flags&KMEM_FLAG_REUSED_PERSISTENT) err = PCILIB_ERROR_INVALID_STATE;
 	    
 	    if (hardware) {
 		if (hardware < 0) {
-		    if ((flags&PCILIB_KMEM_FLAG_HARDWARE == 0)&&(kh.flags&KMEM_FLAG_REUSED_HW)) err = PCILIB_ERROR_INVALID_STATE;
+		    if (((flags&PCILIB_KMEM_FLAG_HARDWARE) == 0)&&(kh.flags&KMEM_FLAG_REUSED_HW)) err = PCILIB_ERROR_INVALID_STATE;
 		    else hardware = (kh.flags&KMEM_FLAG_REUSED_HW)?1:0;
-		} else if (kh.flags&KMEM_FLAG_REUSED_HW == 0) err = PCILIB_ERROR_INVALID_STATE;
+		} else if ((kh.flags&KMEM_FLAG_REUSED_HW) == 0) err = PCILIB_ERROR_INVALID_STATE;
 	    } else if (kh.flags&KMEM_FLAG_REUSED_HW) err = PCILIB_ERROR_INVALID_STATE;
 	    
 	} else {
 	    if (!i) reused = PCILIB_TRISTATE_NO;
 	    else if (reused) reused = PCILIB_TRISTATE_PARTIAL;
 	    
-	    if ((persistent > 0)&&(flags&PCILIB_KMEM_FLAG_PERSISTENT == 0)) err = PCILIB_ERROR_INVALID_STATE;
-	    if ((hardware > 0)&&(flags&PCILIB_KMEM_FLAG_HARDWARE == 0)) err = PCILIB_ERROR_INVALID_STATE;
+	    if ((persistent > 0)&&((flags&PCILIB_KMEM_FLAG_PERSISTENT) == 0)) err = PCILIB_ERROR_INVALID_STATE;
+	    if ((hardware > 0)&&((flags&PCILIB_KMEM_FLAG_HARDWARE) == 0)) err = PCILIB_ERROR_INVALID_STATE;
 	}
 	
 	if (err) {
@@ -205,7 +205,6 @@ pcilib_kmem_handle_t *pcilib_alloc_kernel_memory(pcilib_t *ctx, pcilib_kmem_type
 void pcilib_free_kernel_memory(pcilib_t *ctx, pcilib_kmem_handle_t *k, pcilib_kmem_flags_t flags) {
     int ret, err = 0; 
     int i;
-    kmem_handle_t kh = {0};
     pcilib_kmem_list_t *kbuf = (pcilib_kmem_list_t*)k;
 
 	// if linked in to the list
diff --git a/pci.c b/pci.c
index 59b354e..b1286d5 100644
--- a/pci.c
+++ b/pci.c
@@ -7,7 +7,6 @@
 #include <strings.h>
 #include <stdlib.h>
 #include <stdint.h>
-#include <stdarg.h>
 #include <fcntl.h>
 #include <unistd.h>
 #include <sys/ioctl.h>
@@ -16,8 +15,8 @@
 #include <errno.h>
 #include <assert.h>
 
+#include "pcilib.h"
 #include "pci.h"
-
 #include "kernel.h"
 #include "tools.h"
 #include "error.h"
@@ -25,25 +24,6 @@
 #include "ipecamera/model.h"
 
 
-static void pcilib_print_error(const char *msg, ...) {
-    va_list va;
-    
-    va_start(va, msg);
-    vprintf(msg, va);
-    va_end(va);
-    printf("\n");
-}
-
-void (*pcilib_error)(const char *msg, ...) = pcilib_print_error;
-void (*pcilib_warning)(const char *msg, ...) = pcilib_print_error;
-
-int pcilib_set_error_handler(void (*err)(const char *msg, ...), void (*warn)(const char *msg, ...)) {
-    if (err) pcilib_error = err;
-    else pcilib_error = pcilib_print_error;
-    if (warn) pcilib_warning = warn;
-    else pcilib_warning = pcilib_print_error;
-}
-
 pcilib_t *pcilib_open(const char *device, pcilib_model_t model) {
     pcilib_t *ctx = malloc(sizeof(pcilib_t));
 
@@ -98,8 +78,8 @@ pcilib_context_t *pcilib_get_implementation_context(pcilib_t *ctx) {
 
 pcilib_model_t pcilib_get_model(pcilib_t *ctx) {
     if (ctx->model == PCILIB_MODEL_DETECT) {
-	unsigned short vendor_id;
-	unsigned short device_id;
+//	unsigned short vendor_id;
+//	unsigned short device_id;
 
 	//return PCILIB_MODEL_PCI;
 	
@@ -421,42 +401,46 @@ void pcilib_close(pcilib_t *ctx) {
 }
 
 int pcilib_read(pcilib_t *ctx, pcilib_bar_t bar, uintptr_t addr, size_t size, void *buf) {
-    int i;
     void *data;
 
     pcilib_detect_address(ctx, &bar, &addr, size);
     data = pcilib_map_bar(ctx, bar);
 
     pcilib_memcpy(buf, data + addr, size);
-    
-    pcilib_unmap_bar(ctx, bar, data);    
+
+    pcilib_unmap_bar(ctx, bar, data);
+
+    return 0;
 }
 
 int pcilib_write(pcilib_t *ctx, pcilib_bar_t bar, uintptr_t addr, size_t size, void *buf) {
-    int i;
     void *data;
 
     pcilib_detect_address(ctx, &bar, &addr, size);
     data = pcilib_map_bar(ctx, bar);
 
     pcilib_memcpy(data + addr, buf, size);
-    
-    pcilib_unmap_bar(ctx, bar, data);    
+
+    pcilib_unmap_bar(ctx, bar, data);
+
+    return 0;
 }
 
 
 int pcilib_read_fifo(pcilib_t *ctx, pcilib_bar_t bar, uintptr_t addr, uint8_t fifo_size, size_t n, void *buf) {
     int i;
     void *data;
-    
+
     pcilib_detect_address(ctx, &bar, &addr, fifo_size);
     data = pcilib_map_bar(ctx, bar);
 
     for (i = 0; i < n; i++) {
 	pcilib_memcpy(buf + i * fifo_size, data + addr, fifo_size);
     }
-    
-    pcilib_unmap_bar(ctx, bar, data);    
+
+    pcilib_unmap_bar(ctx, bar, data);
+
+    return 0;
 }
 
 int pcilib_write_fifo(pcilib_t *ctx, pcilib_bar_t bar, uintptr_t addr, uint8_t fifo_size, size_t n, void *buf) {
@@ -470,6 +454,7 @@ int pcilib_write_fifo(pcilib_t *ctx, pcilib_bar_t bar, uintptr_t addr, uint8_t f
 	pcilib_memcpy(data + addr, buf + i * fifo_size, fifo_size);
     }
 
-    pcilib_unmap_bar(ctx, bar, data);    
-}
+    pcilib_unmap_bar(ctx, bar, data);
 
+    return 0;
+}
diff --git a/pci.h b/pci.h
index f3ced74..bdd34a7 100644
--- a/pci.h
+++ b/pci.h
@@ -1,6 +1,7 @@
 #ifndef _PCITOOL_PCI_H
 #define _PCITOOL_PCI_H
 
+#define PCILIB_DEFAULT_CPU_COUNT 2
 #define PCILIB_EVENT_TIMEOUT 1000000		/**< us */
 #define PCILIB_TRIGGER_TIMEOUT 100000		/**< us */
 #define PCILIB_DMA_TIMEOUT 10000		/**< us */
@@ -67,10 +68,6 @@ pcilib_protocol_description_t pcilib_protocol[3] = {
 };
 #else
 extern pcilib_model_description_t pcilib_model[];
-
-extern void (*pcilib_error)(const char *msg, ...);
-extern void (*pcilib_warning)(const char *msg, ...);
-
 extern pcilib_protocol_description_t pcilib_protocol[];
 #endif /* _PCILIB_PCI_C */
 
diff --git a/pcilib.h b/pcilib.h
index 2eaf63c..e9cd4ec 100644
--- a/pcilib.h
+++ b/pcilib.h
@@ -82,14 +82,15 @@ typedef enum {
     PCILIB_STREAMING_REQ_FRAGMENT = 5,	/**< only fragment of a packet is read, wait for next fragment and fail if no data during DMA timeout */
     PCILIB_STREAMING_REQ_PACKET = 6,	/**< wait for next packet and fail if no data during the specified timeout */
     PCILIB_STREAMING_TIMEOUT_MASK = 3	/**< mask specifying all timeout modes */
-} pcilib_streaming_action;
+} pcilib_streaming_action_t;
 
 
 typedef enum {
     PCILIB_EVENT_FLAGS_DEFAULT = 0,
     PCILIB_EVENT_FLAG_RAW_DATA_ONLY = 1,	/**< Do not parse data, just read raw and pass it to rawdata callback */
     PCILIB_EVENT_FLAG_STOP_ONLY = 1,		/**< Do not cleanup, just stop acquiring new frames, the cleanup should be requested afterwards */
-    PCILIB_EVENT_FLAG_EOF = 2			/**< Indicates that it is the last part of the frame (not required) */
+    PCILIB_EVENT_FLAG_EOF = 2,			/**< Indicates that it is the last part of the frame (not required) */
+    PCILIB_EVENT_FLAG_PREPROCESS = 4		/**< Enables preprocessing of the raw data (decoding frames, etc.) */
 } pcilib_event_flags_t;
 
 typedef enum {
@@ -333,19 +334,20 @@ int pcilib_configure_rawdata_callback(pcilib_t *ctx, pcilib_event_rawdata_callba
 
 int pcilib_start(pcilib_t *ctx, pcilib_event_t event_mask, pcilib_event_flags_t flags);
 int pcilib_stop(pcilib_t *ctx, pcilib_event_flags_t flags);
+
 int pcilib_stream(pcilib_t *ctx, pcilib_event_callback_t callback, void *user);
+int pcilib_get_next_event(pcilib_t *ctx, pcilib_timeout_t timeout, pcilib_event_id_t *evid, size_t info_size, pcilib_event_info_t *info);
 
-int pcilib_get_next_event(pcilib_t *ctx, pcilib_timeout_t timeout, pcilib_event_id_t *evid, pcilib_event_info_t **info);
 int pcilib_copy_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t size, void *buf, size_t *retsize);
 int pcilib_copy_data_with_argument(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t size, void *buf, size_t *retsize);
-void *pcilib_get_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t *size);
-void *pcilib_get_data_with_argument(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size);
+void *pcilib_get_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t *size_or_err);
+void *pcilib_get_data_with_argument(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, size_t arg_size, void *arg, size_t *size_or_err);
 
 /*
  * This function is provided to find potentially corrupted data. If the data is overwritten by 
  * the time return_data is called it will return error. 
  */
-int pcilib_return_data(pcilib_t *ctx, pcilib_event_id_t event_id, void *data);
+int pcilib_return_data(pcilib_t *ctx, pcilib_event_id_t event_id, pcilib_event_data_type_t data_type, void *data);
 
 
 
diff --git a/pcitool/CMakeLists.txt b/pcitool/CMakeLists.txt
new file mode 100644
index 0000000..ad2cb4e
--- /dev/null
+++ b/pcitool/CMakeLists.txt
@@ -0,0 +1,8 @@
+include_directories(
+    ${CMAKE_SOURCE_DIR}
+)
+
+set(HEADERS ${HEADERS} sysinfo.h)
+
+add_library(pcitool STATIC sysinfo.c)
+
diff --git a/pcitool/sysinfo.c b/pcitool/sysinfo.c
index 7b25972..d6f57fe 100644
--- a/pcitool/sysinfo.c
+++ b/pcitool/sysinfo.c
@@ -104,7 +104,7 @@ int get_file_fs(const char *fname, size_t size, char *fs) {
   
   if ((!fname)||(!fs)||(size < 3)) return -1;
   
-  if (*fn == '/') {
+  if (*fname == '/') {
     fn = (char*)fname;
   } else {
     if (!getcwd(buf, 4095)) return -1;
diff --git a/register.c b/register.c
index 661c9b0..9a10d41 100644
--- a/register.c
+++ b/register.c
@@ -19,15 +19,13 @@
 
 int pcilib_add_registers(pcilib_t *ctx, size_t n, pcilib_register_description_t *registers) {
     pcilib_register_description_t *regs;
-    size_t size, n_present, n_new;
+    size_t size, n_present = 0;
 
     if (!n) {
 	for (n = 0; registers[n].bits; n++);
     }
 
 
-    if (pcilib_model[ctx->model].registers) 
-    
     if (ctx->model_info.registers == pcilib_model[ctx->model].registers) {
         for (n_present = 0; ctx->model_info.registers[n_present].bits; n_present++);
 	for (size = 1024; size < 2 * (n + n_present + 1); size<<=1);
@@ -46,17 +44,17 @@ int pcilib_add_registers(pcilib_t *ctx, size_t n, pcilib_register_description_t
 
 	    regs = (pcilib_register_description_t*)realloc(ctx->model_info.registers, size * sizeof(pcilib_register_description_t));
 	    if (!regs) return PCILIB_ERROR_MEMORY;
-	    
-    	    ctx->model_info.registers = regs;
+
+	    ctx->model_info.registers = regs;
 	    ctx->alloc_reg = size;
 	}
 	ctx->num_reg += n;
     }
-    
+
     memcpy(ctx->model_info.registers + ctx->num_reg, ctx->model_info.registers + n_present, sizeof(pcilib_register_description_t));
     memcpy(ctx->model_info.registers + n_present, registers, n * sizeof(pcilib_register_description_t));
-    
-    return 0;    
+
+    return 0;
 }
 
 pcilib_register_bank_t pcilib_find_bank_by_addr(pcilib_t *ctx, pcilib_register_bank_addr_t bank) {
@@ -66,7 +64,7 @@ pcilib_register_bank_t pcilib_find_bank_by_addr(pcilib_t *ctx, pcilib_register_b
 
     for (i = 0; banks[i].access; i++)
 	if (banks[i].addr == bank) return i;
-	
+
     return -1;
 }
 
@@ -77,7 +75,7 @@ pcilib_register_bank_t pcilib_find_bank_by_name(pcilib_t *ctx, const char *bankn
 
     for (i = 0; banks[i].access; i++)
 	if (!strcasecmp(banks[i].name, bankname)) return i;
-	
+
     return -1;
 }
 
@@ -104,7 +102,7 @@ pcilib_register_bank_t pcilib_find_bank(pcilib_t *ctx, const char *bank) {
 pcilib_register_t pcilib_find_register(pcilib_t *ctx, const char *bank, const char *reg) {
     pcilib_register_t i;
     pcilib_register_bank_t bank_id;
-    pcilib_register_bank_addr_t bank_addr;
+    pcilib_register_bank_addr_t bank_addr = 0;
 
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
     pcilib_register_description_t *registers =  model_info->registers;
@@ -136,7 +134,6 @@ pcilib_register_t pcilib_find_register(pcilib_t *ctx, const char *bank, const ch
 
 static int pcilib_read_register_space_internal(pcilib_t *ctx, pcilib_register_bank_t bank, pcilib_register_addr_t addr, size_t n, pcilib_register_size_t offset, pcilib_register_size_t bits, pcilib_register_value_t *buf) {
     int err;
-    int rest;
     size_t i;
     
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
@@ -231,7 +228,6 @@ int pcilib_read_register_by_id(pcilib_t *ctx, pcilib_register_t reg, pcilib_regi
 
 
 int pcilib_read_register(pcilib_t *ctx, const char *bank, const char *regname, pcilib_register_value_t *value) {
-    int err;
     int reg;
     
     reg = pcilib_find_register(ctx, bank, regname);
@@ -246,7 +242,6 @@ int pcilib_read_register(pcilib_t *ctx, const char *bank, const char *regname, p
 
 static int pcilib_write_register_space_internal(pcilib_t *ctx, pcilib_register_bank_t bank, pcilib_register_addr_t addr, size_t n, pcilib_register_size_t offset, pcilib_register_size_t bits, pcilib_register_value_t rwmask, pcilib_register_value_t *buf) {
     int err;
-    int rest;
     size_t i;
     
     pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
@@ -356,7 +351,6 @@ int pcilib_write_register_by_id(pcilib_t *ctx, pcilib_register_t reg, pcilib_reg
 }
 
 int pcilib_write_register(pcilib_t *ctx, const char *bank, const char *regname, pcilib_register_value_t value) {
-    int err;
     int reg;
     
     reg = pcilib_find_register(ctx, bank, regname);
diff --git a/tools.c b/tools.c
index c673258..8fce755 100644
--- a/tools.c
+++ b/tools.c
@@ -1,4 +1,5 @@
 #define _POSIX_C_SOURCE 200112L
+#define _GNU_SOURCE
 
 #include <stdio.h>
 #include <string.h>
@@ -7,10 +8,12 @@
 #include <assert.h>
 #include <ctype.h>
 #include <time.h>
+#include <sched.h>
 #include <arpa/inet.h>
 #include <sys/time.h>
 
 #include "tools.h"
+#include "error.h"
 
 int pcilib_isnumber(const char *str) {
     int i = 0;
@@ -240,6 +243,8 @@ void *pcilib_datacpy32(void * dst, void const * src, uint8_t size, size_t n, pci
             --n;
         }
     }
+
+    return dst;
 } 
 
 int pcilib_get_page_mask() {
@@ -254,6 +259,28 @@ int pcilib_get_page_mask() {
     return pagemask;
 }
 
+int pcilib_get_cpu_count() {
+    int err;
+
+    int cpu_count;
+    cpu_set_t mask;
+
+    err = sched_getaffinity(getpid(), sizeof(mask), &mask);
+    if (err) return 1;
+
+#ifdef CPU_COUNT
+    cpu_count = CPU_COUNT(&mask);
+#else
+    for (cpu_count = 0; cpu_count < CPU_SETSIZE; cpu_count++) {
+	if (!CPU_ISSET(cpu_count, &mask)) break;
+    }
+#endif
+
+    if (!cpu_count) cpu_count = PCILIB_DEFAULT_CPU_COUNT;
+    return cpu_count;    
+}
+
+
 int pcilib_add_timeout(struct timeval *tv, pcilib_timeout_t timeout) {
     tv->tv_usec += timeout%1000000;
     if (tv->tv_usec > 999999) {
@@ -262,21 +289,23 @@ int pcilib_add_timeout(struct timeval *tv, pcilib_timeout_t timeout) {
     } else {
 	tv->tv_sec += timeout/1000000;
     }
+
+    return 0;
 }
 
 int pcilib_calc_deadline(struct timeval *tv, pcilib_timeout_t timeout) {
     gettimeofday(tv, NULL);
     pcilib_add_timeout(tv, timeout);
-        
+
     return 0;
 }
 
 int pcilib_check_deadline(struct timeval *tve, pcilib_timeout_t timeout) {
     int64_t res;
     struct timeval tvs;
-    
+
     if (!tve->tv_sec) return 0;
-    
+
     gettimeofday(&tvs, NULL);
     res = ((tve->tv_sec - tvs.tv_sec)*1000000 + (tve->tv_usec - tvs.tv_usec));
 	// Hm... Some problems comparing signed and unsigned. So, sign check first
diff --git a/tools.h b/tools.h
index 009fd68..28f8b0a 100644
--- a/tools.h
+++ b/tools.h
@@ -32,6 +32,7 @@ void * pcilib_memcpy64(void * dst, void const * src, size_t len);
 void * pcilib_datacpy32(void * dst, void const * src, uint8_t size, size_t n, pcilib_endianess_t endianess);
 
 int pcilib_get_page_mask();
+int pcilib_get_cpu_count();
 
 
 int pcilib_add_timeout(struct timeval *tv, pcilib_timeout_t timeout);