Support for new CMOSIS 20MPix camera

8 files changed, 227 insertions, 83 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 610baa1..788d9f6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,9 +30,9 @@ link_directories(
     ${PCILIB_LIBRARY_DIRS}
 )
 
-set(HEADERS ${HEADERS} model.h cmosis.h base.h reader.h events.h data.h private.h ipecamera.h version.h)
+set(HEADERS ${HEADERS} model.h cmosis.h base.h reader.h events.h data.h env.h private.h ipecamera.h version.h)
 
-add_library(ipecamera SHARED model.c cmosis.c base.c reader.c events.c data.c)
+add_library(ipecamera SHARED model.c cmosis.c base.c reader.c events.c data.c env.c)
 
 target_link_libraries(ipecamera ${PCILIB_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${UFODECODE_LIBRARIES} )
 
diff --git a/base.c b/base.c
index f288be0..00f38d1 100644
--- a/base.c
+++ b/base.c
@@ -107,16 +107,16 @@ pcilib_context_t *ipecamera_init(pcilib_t *pcilib) {
 
 	GET_REG(firmware_version_reg, value);
 	switch (value) {
-	 case 5:
+	 case IPECAMERA_FIRMWARE_UFO5:
 	    ctx->firmware = value;
 	    err = pcilib_add_registers(pcilib, 0, cmosis_registers);
 	    break;
-	 case 6:
+	 case IPECAMERA_FIRMWARE_CMOSIS20:
 	    ctx->firmware = value;
 	    err = pcilib_add_registers(pcilib, 0, cmosis20000_registers);
 	    break;
 	 default:
-	    ctx->firmware = 5;
+	    ctx->firmware = IPECAMERA_FIRMWARE_UNKNOWN;
     	    pcilib_warning("Unsupported version of firmware (%lu)", value);
 	}
 
@@ -294,25 +294,39 @@ int ipecamera_start(pcilib_context_t *vctx, pcilib_event_t event_mask, pcilib_ev
     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;
-    ctx->dim.height = IPECAMERA_MAX_LINES;
-//    GET_REG(n_lines_reg, ctx->dim.height);
-    
-    GET_REG(output_mode_reg, value);
-    switch (value) {
-     case IPECAMERA_MODE_16_CHAN_IO:
-        ctx->cmosis_outputs = 16;
-        break;
-     case IPECAMERA_MODE_4_CHAN_IO:
-        ctx->cmosis_outputs = 4;
-        break;
+
+    switch (ctx->firmware) {
+     case IPECAMERA_FIRMWARE_UFO5:
+	ctx->dim.width = CMOSIS_WIDTH;
+	ctx->dim.height = CMOSIS_MAX_LINES;
+	break;
+     case IPECAMERA_FIRMWARE_CMOSIS20:
+	ctx->dim.width = CMOSIS20_WIDTH;
+	ctx->dim.height = CMOSIS20_MAX_LINES;
+	ctx->cmosis_outputs = CMOSIS20_MAX_CHANNELS;
+	break;
      default:
-        pcilib_error("IPECamera reporting invalid output_mode 0x%lx", value);
-        return PCILIB_ERROR_INVALID_STATE;
+	pcilib_error("Can't start undefined version (%lu) of IPECamera", ctx->firmware);
+	return PCILIB_ERROR_INVALID_REQUEST;
     }
-    
-    ipecamera_compute_buffer_size(ctx, ctx->dim.height);
+
+    if (ctx->firmware == IPECAMERA_FIRMWARE_UFO5) {
+	GET_REG(output_mode_reg, value);
+	switch (value) {
+	 case IPECAMERA_MODE_16_CHAN_IO:
+	    ctx->cmosis_outputs = 16;
+	    break;
+         case IPECAMERA_MODE_4_CHAN_IO:
+	    ctx->cmosis_outputs = 4;
+	    break;
+	 default:
+	    pcilib_error("IPECamera reporting invalid output_mode 0x%lx", value);
+	    return PCILIB_ERROR_INVALID_STATE;
+	}
+    }
+
+	// We should be careful here (currently firmware matches format, but this may not be the case in future)
+    ipecamera_compute_buffer_size(ctx, ctx->firmware, CMOSIS_FRAME_HEADER_SIZE, ctx->dim.height);
 
     ctx->raw_size = ctx->roi_raw_size;
     ctx->padded_size = ctx->roi_padded_size;
@@ -628,7 +642,7 @@ int ipecamera_trigger(pcilib_context_t *vctx, pcilib_event_t event, size_t trigg
 	    return PCILIB_ERROR_BUSY;
     }
 
-    GET_REG(control_reg, value); 
+    GET_REG(control_reg, value);
     SET_REG(control_reg, value|IPECAMERA_FRAME_REQUEST);
     usleep(IPECAMERA_TRIGGER_DELAY);
     SET_REG(control_reg, value);
diff --git a/env.c b/env.c
new file mode 100644
index 0000000..fa75408
--- /dev/null
+++ b/env.c
@@ -0,0 +1,17 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include "env.h"
+
+
+static const char *env_cache[IPECAMERA_MAX_ENV] = {0};
+
+const char *ipecamera_getenv(ipecamera_env_t env, const char *var) {
+    if (!env_cache[env]) {
+	const char *var_env = getenv(var);
+	env_cache[env] = var_env?var_env:(void*)-1;
+	return var_env;
+    }
+
+    return (env_cache[env] == (void*)-1)?NULL:env_cache[env];
+}
+
diff --git a/env.h b/env.h
new file mode 100644
index 0000000..46e4459
--- /dev/null
+++ b/env.h
@@ -0,0 +1,23 @@
+#ifndef _IPECAMERA_ENV_H
+#define _IPECAMERA_ENV_H
+
+typedef enum {
+    IPECAMERA_DEBUG_RAW_FRAMES_ENV,
+    IPECAMERA_DEBUG_BROKEN_FRAMES_ENV,
+    IPECAMERA_DEBUG_RAW_PACKETS_ENV,
+    IPECAMERA_DEBUG_HARDWARE_ENV,
+    IPECAMERA_DEBUG_FRAME_HEADERS_ENV,
+    IPECAMERA_MAX_ENV
+} ipecamera_env_t;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+const char *ipecamera_getenv(ipecamera_env_t env, const char *var);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _IPECAMERA_ENV_H */
diff --git a/private.h b/private.h
index b492a08..4bbd937 100644
--- a/private.h
+++ b/private.h
@@ -5,6 +5,7 @@
 #include <pcilib/model.h>
 #include <pcilib/debug.h>
 #include "ipecamera.h"
+#include "env.h"
 
 #define IPECAMERA_DEBUG
 #ifdef IPECAMERA_DEBUG
@@ -12,9 +13,10 @@
 # define IPECAMERA_DEBUG_BROKEN_FRAMES		//**< Store broken frames in the specified directory */
 # define IPECAMERA_DEBUG_RAW_PACKETS		//**< Store all raw packets read from DMA grouped in frames */
 # define IPECAMERA_DEBUG_HARDWARE		//**< Produce various debugging information about ipecamera operation */
+# define IPECAMERA_DEBUG_FRAME_HEADERS		//**< Print frame headers & footers */
 #endif /* IPECAMERA_DEBUG */
 
-//#define IPECAMERA_BUG_MISSING_PAYLOAD		//**< CMOSIS fails to provide a first payload for each frame, therefore the frame is 32 bit shorter */
+#define IPECAMERA_BUG_MISSING_PAYLOAD		//**< CMOSIS fails to provide a first payload for each frame, therefore the frame is 32 bit shorter */
 #define IPECAMERA_BUG_MULTIFRAME_PACKETS	//**< This is by design, start of packet comes directly after the end of last one in streaming mode */
 //#define IPECAMERA_BUG_INCOMPLETE_PACKETS	//**< Support incomplete packets, i.e. check for frame magic even if full frame size is not reached yet (slow) */
 //#define IPECAMERA_ANNOUNCE_READY		//**< Announce new event only after the reconstruction is done */
@@ -35,16 +37,24 @@
 #define IPECAMERA_NOFRAME_SLEEP 100		//**< Sleep while polling for a new frame in reader */
 #define IPECAMERA_NOFRAME_PREPROC_SLEEP 100	//**< Sleep while polling for a new frame in pre-processor */
 
-//#define IPECAMERA_MAX_LINES 1088
-#define IPECAMERA_MAX_LINES 2048
 #define IPECAMERA_EXPECTED_STATUS_4 0x08409FFFF
 #define IPECAMERA_EXPECTED_STATUS 0x08449FFFF
 
 #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 CMOSIS_FRAME_HEADER_SIZE	8 * sizeof(ipecamera_payload_t)
+#define CMOSIS_FRAME_TAIL_SIZE		8 * sizeof(ipecamera_payload_t)
+
+#define CMOSIS_MAX_CHANNELS 16
+#define CMOSIS_PIXELS_PER_CHANNEL 128
+#define CMOSIS_WIDTH (CMOSIS_MAX_CHANNELS * CMOSIS_PIXELS_PER_CHANNEL)
+//#define IPECAMERA_MAX_LINES 1088
+#define CMOSIS_MAX_LINES 2048
+#define CMOSIS20_MAX_CHANNELS 8
+#define CMOSIS20_PIXELS_PER_CHANNEL 640
+#define CMOSIS20_WIDTH (CMOSIS20_MAX_CHANNELS * CMOSIS20_PIXELS_PER_CHANNEL)
+#define CMOSIS20_MAX_LINES 3840
 
 #define IPECAMERA_FRAME_REQUEST 		0x80000209 // 0x1E9
 #define IPECAMERA_IDLE 				0x80000201 // 0x1E1
@@ -57,38 +67,47 @@
 #define IPECAMERA_MODE_11_BIT_ADC		1
 #define IPECAMERA_MODE_10_BIT_ADC		0
 
+
 #ifdef IPECAMERA_DEBUG_RAW_FRAMES
-# define IPECAMERA_DEBUG_RAW_FRAMES_MESSAGE(function, ...) pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__) 
-# define IPECAMERA_DEBUG_RAW_FRAMES_BUFFER(function, ...) pcilib_debug_data_buffer (#function, __VA_ARGS__) 
+# define IPECAMERA_DEBUG_RAW_FRAMES_MESSAGE(function, ...)  if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__); }
+# define IPECAMERA_DEBUG_RAW_FRAMES_BUFFER(function, ...)  if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_data_buffer (#function, __VA_ARGS__); }
 #else /* IPECAMERA_DEBUG_RAW_FRAMES */
 # define IPECAMERA_DEBUG_RAW_FRAMES_MESSAGE(function, ...)
 # define IPECAMERA_DEBUG_RAW_FRAMES_BUFFER(function, ...)
 #endif /* IPECAMERA_DEBUG_RAW_FRAMES */
 
 #ifdef IPECAMERA_DEBUG_BROKEN_FRAMES
-# define IPECAMERA_DEBUG_BROKEN_FRAMES_MESSAGE(function, ...) pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__) 
-# define IPECAMERA_DEBUG_BROKEN_FRAMES_BUFFER(function, ...) pcilib_debug_data_buffer (#function, __VA_ARGS__) 
+# define IPECAMERA_DEBUG_BROKEN_FRAMES_MESSAGE(function, ...) if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__); }
+# define IPECAMERA_DEBUG_BROKEN_FRAMES_BUFFER(function, ...) if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_data_buffer (#function, __VA_ARGS__); }
 #else /* IPECAMERA_DEBUG_BROKEN_FRAMES */
 # define IPECAMERA_DEBUG_BROKEN_FRAMES_MESSAGE(function, ...)
 # define IPECAMERA_DEBUG_BROKEN_FRAMES_BUFFER(function, ...)
 #endif /* IPECAMERA_DEBUG_BROKEN_FRAMES */
 
 #ifdef IPECAMERA_DEBUG_RAW_PACKETS
-# define IPECAMERA_DEBUG_RAW_PACKETS_MESSAGE(function, ...) pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__) 
-# define IPECAMERA_DEBUG_RAW_PACKETS_BUFFER(function, ...) pcilib_debug_data_buffer (#function, __VA_ARGS__) 
+# define IPECAMERA_DEBUG_RAW_PACKETS_MESSAGE(function, ...) if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__); }
+# define IPECAMERA_DEBUG_RAW_PACKETS_BUFFER(function, ...) if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_data_buffer (#function, __VA_ARGS__); }
 #else /* IPECAMERA_DEBUG_RAW_PACKETS */
 # define IPECAMERA_DEBUG_RAW_PACKETS_MESSAGE(function, ...)
 # define IPECAMERA_DEBUG_RAW_PACKETS_BUFFER(function, ...)
 #endif /* IPECAMERA_DEBUG_RAW_PACKETS */
 
 #ifdef IPECAMERA_DEBUG_HARDWARE
-# define IPECAMERA_DEBUG_HARDWARE_MESSAGE(function, ...) pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__) 
-# define IPECAMERA_DEBUG_HARDWARE_BUFFER(function, ...) pcilib_debug_data_buffer (#function, __VA_ARGS__) 
+# define IPECAMERA_DEBUG_HARDWARE_MESSAGE(function, ...) if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__); }
+# define IPECAMERA_DEBUG_HARDWARE_BUFFER(function, ...) if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_data_buffer (#function, __VA_ARGS__); }
 #else /* IPECAMERA_DEBUG_HARDWARE */
 # define IPECAMERA_DEBUG_HARDWARE_MESSAGE(function, ...)
 # define IPECAMERA_DEBUG_HARDWARE_BUFFER(function, ...)
 #endif /* IPECAMERA_DEBUG_HARDWARE */
 
+#ifdef IPECAMERA_DEBUG_FRAME_HEADERS
+# define IPECAMERA_DEBUG_FRAME_HEADERS_MESSAGE(function, ...)  if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_message (#function, __FILE__, __LINE__, __VA_ARGS__); }
+# define IPECAMERA_DEBUG_FRAME_HEADERS_BUFFER(function, ...)  if (ipecamera_getenv(function##_ENV, #function)) { pcilib_debug_data_buffer (#function, __VA_ARGS__); }
+#else /* IPECAMERA_DEBUG_RAW_FRAMES */
+# define IPECAMERA_DEBUG_FRAME_HEADERS_MESSAGE(function, ...)
+# define IPECAMERA_DEBUG_FRAME_HEADERS_BUFFER(function, ...)
+#endif /* IPECAMERA_DEBUG_RAW_FRAMES */
+
 
 #define ipecamera_debug(function, ...) \
     IPECAMERA_DEBUG_##function##_MESSAGE(IPECAMERA_DEBUG_##function, PCILIB_LOG_DEFAULT, __VA_ARGS__)
@@ -99,6 +118,18 @@
 
 typedef uint32_t ipecamera_payload_t;
 
+typedef enum {
+    IPECAMERA_FIRMWARE_UNKNOWN = 0,
+    IPECAMERA_FIRMWARE_UFO5 = 5,
+    IPECAMERA_FIRMWARE_CMOSIS20 = 6
+} ipecamera_firmware_t;
+
+typedef enum {
+    IPECAMERA_FORMAT_CMOSIS = 5,
+    IPECAMERA_FORMAT_CMOSIS20 = 6,
+    IPECAMERA_FORMAT_POLARIS = 7
+} ipecamera_format_t;
+
 typedef struct {
     pcilib_event_id_t evid;
     struct timeval timestamp;
@@ -146,9 +177,9 @@ struct ipecamera_s {
     pcilib_register_t max_frames_reg;
     pcilib_register_t num_frames_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 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 */
 
     volatile int run_reader;		/**< Instructs the reader thread to stop processing */
     volatile int run_streamer;		/**< Indicates request to stop streaming events and can be set by reader_thread upon exit or by user request */
@@ -159,19 +190,19 @@ struct ipecamera_s {
     struct timeval autostop_time;
     struct timeval next_trigger;	/**< The minimal delay between trigger signals is mandatory, this indicates time when next trigger is possible */
 
-    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;		/**< Expected maximum size of raw data in bytes */
-    size_t padded_size;		/**< Expected maximum size of buffer for raw data, including additional padding */
-    size_t roi_raw_size;	/**< Expected size (for currently configured ROI) of raw data in bytes */
-    size_t roi_padded_size;	/**< Expected size (for currently configured ROI) of buffer for raw data, including additional padding */
+    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;			/**< Expected maximum size of raw data in bytes */
+    size_t padded_size;			/**< Expected maximum size of buffer for raw data, including additional padding */
+    size_t roi_raw_size;		/**< Expected size (for currently configured ROI) of raw data in bytes */
+    size_t roi_padded_size;		/**< Expected size (for currently configured ROI) of buffer for raw data, including additional padding */
     
-    size_t image_size;		/**< Size of a single image in bytes */
+    size_t image_size;			/**< Size of a single image in bytes */
     
-    size_t max_frames;		/**< Maximal number of frames what may be buffered in camera DDR memory */
-    int firmware;		/**< Firmware version */
-    int cmosis_outputs;		/**< Number of active cmosis outputs: 4 or 16 */
+    size_t max_frames;			/**< Maximal number of frames what may be buffered in camera DDR memory */
+    ipecamera_firmware_t firmware;	/**< Firmware type */
+    int cmosis_outputs;			/**< Number of active cmosis outputs: 4 or 16 */
     int width, height;
 
     
diff --git a/reader.c b/reader.c
index ba568d4..2c95b13 100644
--- a/reader.c
+++ b/reader.c
@@ -21,35 +21,100 @@
 #include "private.h"
 #include "reader.h"
 
+//#define CHECK_FRAME_MAGIC(buf) \
+//	memcmp(buf, ((void*)frame_magic) + 1, sizeof(frame_magic) - 1)
 
-int ipecamera_compute_buffer_size(ipecamera_t *ctx, size_t lines) {
-    const size_t header_size = 8 * sizeof(ipecamera_payload_t);
-    const size_t footer_size = 8 * sizeof(ipecamera_payload_t);
+#define CHECK_FRAME_MAGIC(buf) \
+	memcmp(((ipecamera_payload_t*)(buf)) + 1, &frame_magic[1], sizeof(frame_magic) - sizeof(ipecamera_payload_t))
 
+static ipecamera_payload_t frame_magic[3] = { 0x51111111, 0x52222222, 0x53333333 };
+
+
+
+int ipecamera_compute_buffer_size(ipecamera_t *ctx, ipecamera_format_t format, size_t header_size, size_t lines) {
+//    const size_t header_size = 8 * sizeof(ipecamera_payload_t);
+    const size_t footer_size = CMOSIS_FRAME_TAIL_SIZE;
+
+    size_t max_channels;
     size_t line_size, raw_size, padded_blocks;
 
-    switch (ctx->firmware) {
+    switch (format) {
+     case IPECAMERA_FORMAT_CMOSIS:
+	max_channels = CMOSIS_MAX_CHANNELS;
+	line_size = (1 + CMOSIS_PIXELS_PER_CHANNEL) * 32; 
+	break;
+     case IPECAMERA_FORMAT_CMOSIS20:
+	max_channels = CMOSIS20_MAX_CHANNELS;
+	line_size = (1 + CMOSIS20_PIXELS_PER_CHANNEL) * 32 / 2;
+	break;
      default:
-	line_size = (1 + IPECAMERA_PIXELS_PER_CHANNEL) * 32; 
-	raw_size = lines * line_size;
-	raw_size *= 16 / ctx->cmosis_outputs;
-	raw_size += header_size  + footer_size;
+	pcilib_warning("Unsupported version (%u) of frame format...", format);
+	return PCILIB_ERROR_NOTSUPPORTED;
+    }
+
+    raw_size = lines * line_size;
+    raw_size *= max_channels / ctx->cmosis_outputs;
+    raw_size += header_size + footer_size;
 
 #ifdef IPECAMERA_BUG_MISSING_PAYLOAD
-	    // As I understand, the first 32-byte packet is missing, so we need to substract 32
-	raw_size -= 32;
+        // As I understand, the first 32-byte packet is missing, so we need to substract 32 (both CMOSIS and CMOSIS20)
+    raw_size -= 32;
 #endif /* IPECAMERA_BUG_MISSING_PAYLOAD */
-    }
 
     padded_blocks = raw_size / IPECAMERA_DMA_PACKET_LENGTH + ((raw_size % IPECAMERA_DMA_PACKET_LENGTH)?1:0);
-    
+
     ctx->roi_raw_size = raw_size;
     ctx->roi_padded_size = padded_blocks * IPECAMERA_DMA_PACKET_LENGTH;
-//    printf("%lu %lu\n", ctx->roi_raw_size, ctx->roi_padded_size);
 
     return 0;
 }
 
+
+static int ipecamera_parse_header(ipecamera_t *ctx, ipecamera_payload_t *buf, size_t buf_size) {
+    int last = buf[0] & 1;
+    int version = (buf[0] >> 1) & 7;
+    size_t size = 0, n_lines;
+    ipecamera_format_t format = IPECAMERA_FORMAT_CMOSIS;
+
+    switch (version) {
+     case 0:
+	n_lines = ((uint32_t*)buf)[5] & 0x7FF;
+	ctx->frame[ctx->buffer_pos].event.info.seqnum = buf[6] & 0xFFFFFF;
+	ctx->frame[ctx->buffer_pos].event.info.offset = (buf[7] & 0xFFFFFF) * 80;
+	break;
+     case 1:
+	n_lines = ((uint32_t*)buf)[5] & 0xFFFF;
+	if (!n_lines) {
+	    pcilib_error("The frame header claims 0 lines in the data");
+	    return 0;
+	}
+
+	ctx->frame[ctx->buffer_pos].event.info.seqnum = buf[6] & 0xFFFFFF;
+	ctx->frame[ctx->buffer_pos].event.info.offset = (buf[7] & 0xFFFFFF) * 80;
+	format = (buf[6] >> 24)&0x0F;
+        break;
+     default:
+	ipecamera_debug(HARDWARE, "Incorrect version of the frame header, ignoring broken data...");
+	return 0;
+    }
+    gettimeofday(&ctx->frame[ctx->buffer_pos].event.info.timestamp, NULL);
+
+    ipecamera_debug(FRAME_HEADERS, "frame %lu: %x %x %x %x", ctx->frame[ctx->buffer_pos].event.info.seqnum, buf[0], buf[1], buf[2], buf[3]);
+    ipecamera_debug(FRAME_HEADERS, "frame %lu: %x %x %x %x", ctx->frame[ctx->buffer_pos].event.info.seqnum, buf[4], buf[5], buf[6], buf[7]);
+
+    while ((!last)&&((size + CMOSIS_FRAME_HEADER_SIZE) <= buf_size)) {
+	size += CMOSIS_FRAME_HEADER_SIZE;
+	last = buf[size] & 1;
+    }
+
+    size += CMOSIS_FRAME_HEADER_SIZE;
+    ipecamera_compute_buffer_size(ctx, format, size, n_lines);
+
+	// Returns total size of found headers or 0 on the error
+    return size;
+}
+
+
 static inline int ipecamera_new_frame(ipecamera_t *ctx) {
     ctx->frame[ctx->buffer_pos].event.raw_size = ctx->cur_size;
 
@@ -77,8 +142,6 @@ static inline int ipecamera_new_frame(ipecamera_t *ctx) {
     return 0;
 }
 
-static uint32_t frame_magic[4] = { 0x51111111, 0x52222222, 0x53333333, 0x54444444 };
-
 static int ipecamera_data_callback(void *user, pcilib_dma_flags_t flags, size_t bufsize, void *buf) {
     int res;
     int eof = 0;
@@ -102,11 +165,11 @@ static int ipecamera_data_callback(void *user, pcilib_dma_flags_t flags, size_t
     if (!ctx->cur_size) {
 #if defined(IPECAMERA_BUG_INCOMPLETE_PACKETS)||defined(IPECAMERA_BUG_MULTIFRAME_PACKETS)
 	size_t startpos;
-	for (startpos = 0; (startpos + sizeof(frame_magic)) <= bufsize; startpos += sizeof(uint32_t)) {
-	    if (!memcmp(buf + startpos, frame_magic, sizeof(frame_magic))) break;
+	for (startpos = 0; (startpos + CMOSIS_FRAME_HEADER_SIZE) <= bufsize; startpos += sizeof(ipecamera_payload_t)) {
+	    if (!CHECK_FRAME_MAGIC(buf + startpos)) break;
 	}
 	
-	if ((startpos + sizeof(frame_magic)) > bufsize) {
+	if ((startpos + CMOSIS_FRAME_HEADER_SIZE) > bufsize) {
 	    ipecamera_debug_buffer(RAW_PACKETS, bufsize, NULL, 0, "frame%4lu/frame%9lu.invalid", ctx->event_id, packet_id);
 	    
 	    if (invalid_frame_id != ctx->event_id) {
@@ -133,14 +196,10 @@ static int ipecamera_data_callback(void *user, pcilib_dma_flags_t flags, size_t
 	}
 #endif /* IPECAMERA_BUG_INCOMPLETE_PACKETS */
 
-	if ((bufsize >= 8)&&(!memcmp(buf, frame_magic, sizeof(frame_magic)))) {
-	    // size_t first_line = ((uint32_t*)buf)[4] & 0x7FF;
-	    size_t n_lines = ((uint32_t*)buf)[5] & 0x7FF;
-	    ipecamera_compute_buffer_size(ctx, n_lines);
-
-	    ctx->frame[ctx->buffer_pos].event.info.seqnum = ((uint32_t*)buf)[6] & 0xFFFFFF;
-	    ctx->frame[ctx->buffer_pos].event.info.offset = (((uint32_t*)buf)[7] & 0xFFFFFF) * 80;
-	    gettimeofday(&ctx->frame[ctx->buffer_pos].event.info.timestamp, NULL);
+	if ((bufsize >= CMOSIS_FRAME_HEADER_SIZE)&&(!CHECK_FRAME_MAGIC(buf))) {
+		// We should handle the case when multi-header is split between multiple DMA packets
+	    if (!ipecamera_parse_header(ctx, buf, bufsize))
+		return PCILIB_STREAMING_CONTINUE;
 	} else {
 	    ipecamera_debug(HARDWARE, "Frame magic is not found, ignoring broken data...");
 	    return PCILIB_STREAMING_CONTINUE;
@@ -154,11 +213,11 @@ static int ipecamera_data_callback(void *user, pcilib_dma_flags_t flags, size_t
     if (ctx->cur_size + bufsize > ctx->roi_raw_size) {
         size_t need;
 	
-	for (need = ctx->roi_raw_size - ctx->cur_size; (need + sizeof(frame_magic)) < bufsize; need += sizeof(uint32_t)) {
-	    if (!memcmp(buf + need, frame_magic, sizeof(frame_magic))) break;
+	for (need = ctx->roi_raw_size - ctx->cur_size; (need + CMOSIS_FRAME_HEADER_SIZE) <= bufsize; need += sizeof(uint32_t)) {
+	    if (!CHECK_FRAME_MAGIC(buf + need)) break;
 	}
 	
-	if ((need + sizeof(frame_magic)) < bufsize) {
+	if ((need + CMOSIS_FRAME_HEADER_SIZE) <= bufsize) {
 	    extra_data = bufsize - need;
 	    eof = 1;
 	}
diff --git a/reader.h b/reader.h
index 5d631c0..ff60db8 100644
--- a/reader.h
+++ b/reader.h
@@ -1,7 +1,7 @@
 #ifndef _IPECAMERA_READER_H
 #define _IPECAMERA_READER_H
 
-int ipecamera_compute_buffer_size(ipecamera_t *ctx, size_t lines);
+int ipecamera_compute_buffer_size(ipecamera_t *ctx, ipecamera_format_t format, size_t header_size, size_t lines);
 
 void *ipecamera_reader_thread(void *user);
 
diff --git a/tests/autotrigger.sh b/tests/autotrigger.sh
index 678f854..f6bbd07 100755
--- a/tests/autotrigger.sh
+++ b/tests/autotrigger.sh
@@ -17,9 +17,9 @@ echo "Starting the grabber"
 pci -g -o /dev/null --run-time 12000000 --verbose 10 &
 pid=$!
 
-usleep 100000
+sleep 0.1
 pci -w 9040 80004a01
-usleep 10000000
+sleep 10
 pci -w 9040 80000201
 
 echo "Waiting grabber to finish"