Support both UFO4 and UFO5 frame formats

3 files changed, 34 insertions, 9 deletions

diff --git a/ipecamera/ipecamera.c b/ipecamera/ipecamera.c
index 1ea54f4..f8136d4 100644
--- a/ipecamera/ipecamera.c
+++ b/ipecamera/ipecamera.c
@@ -66,6 +66,9 @@
 	} \
     }
 
+#define IPECAMERA_GET_EXPECTED_STATUS(ctx) ((ctx->firmware == 4)?IPECAMERA_EXPECTED_STATUS_4:IPECAMERA_EXPECTED_STATUS)
+#define CHECK_STATUS_REG() CHECK_REG(status_reg, IPECAMERA_GET_EXPECTED_STATUS(ctx))
+
 #define CHECK_VALUE(value, val) \
     if ((!err)&&(value != val)) { \
 	pcilib_error("Unexpected value (0x%x) in data stream (0x%x is expected)", value, val); \
@@ -103,6 +106,7 @@ pcilib_context_t *ipecamera_init(pcilib_t *pcilib) {
 	FIND_REG(exposure_reg, "cmosis", "cmosis_exp_time");
 	FIND_REG(flip_reg, "cmosis", "cmosis_image_flipping");
 	
+	FIND_REG(firmware_version_reg, "fpga", "firmware_version");
 	FIND_REG(adc_resolution_reg, "fpga", "adc_resolution");
 	FIND_REG(output_mode_reg, "fpga", "output_mode");
 	
@@ -224,15 +228,14 @@ int ipecamera_reset(pcilib_context_t *vctx) {
 
     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);
+    if (value != IPECAMERA_GET_EXPECTED_STATUS(ctx)) {
+	pcilib_error("Unexpected value (%lx) of status register, expected %lx", value, IPECAMERA_GET_EXPECTED_STATUS(ctx));
 	return PCILIB_ERROR_VERIFY;
     }
 
@@ -261,10 +264,20 @@ int ipecamera_start(pcilib_context_t *vctx, pcilib_event_t event_mask, pcilib_ev
     }
 
 
+    GET_REG(firmware_version_reg, value);
+    switch (value) {
+     case 4:
+     case 5:
+	ctx->firmware = value;
+	break;
+     default:
+        pcilib_error("Unsupported version of firmware (%lu)", value);
+    }
+
 	// 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);
+    CHECK_STATUS_REG();
     if (err) return err;
 
     ctx->event_id = 0;
diff --git a/ipecamera/private.h b/ipecamera/private.h
index a200b75..f5300a2 100644
--- a/ipecamera/private.h
+++ b/ipecamera/private.h
@@ -18,7 +18,9 @@
 #define IPECAMERA_NOFRAME_PREPROC_SLEEP 100
 
 #define IPECAMERA_MAX_LINES 1088
+#define IPECAMERA_EXPECTED_STATUS_4 0x08409FFFF
 #define IPECAMERA_EXPECTED_STATUS 0x08449FFFF
+
 #define IPECAMERA_END_OF_SEQUENCE 0x1F001001
 
 #define IPECAMERA_MAX_CHANNELS 16
@@ -84,6 +86,7 @@ struct ipecamera_s {
     pcilib_register_t exposure_reg;
     pcilib_register_t flip_reg;
 
+    pcilib_register_t firmware_version_reg;
     pcilib_register_t adc_resolution_reg;
     pcilib_register_t output_mode_reg;
     
@@ -116,6 +119,7 @@ struct ipecamera_s {
     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 */
     int width, height;
 
diff --git a/ipecamera/reader.c b/ipecamera/reader.c
index cf5fc5c..048f536 100644
--- a/ipecamera/reader.c
+++ b/ipecamera/reader.c
@@ -21,14 +21,22 @@
 
 
 int ipecamera_compute_buffer_size(ipecamera_t *ctx, size_t lines) {
-    const size_t line_size = (1 + IPECAMERA_PIXELS_PER_CHANNEL) * 32;
     const size_t header_size = 8 * sizeof(ipecamera_payload_t);
     const size_t footer_size = 8 * sizeof(ipecamera_payload_t);
 
-    size_t raw_size, padded_blocks;
-    
-    raw_size = header_size + lines * line_size - 32 + footer_size;
-    raw_size *= 16 / ctx->cmosis_outputs;
+    size_t line_size, raw_size, padded_blocks;
+
+
+    switch (ctx->firmware) {
+     case 4:
+	line_size = IPECAMERA_MAX_CHANNELS * (2 + IPECAMERA_PIXELS_PER_CHANNEL / 3) * sizeof(ipecamera_payload_t);
+	raw_size = header_size + lines * line_size + footer_size;
+	break;
+     default:
+	line_size = (1 + IPECAMERA_PIXELS_PER_CHANNEL) * 32; 
+	raw_size = header_size + lines * line_size - 32 + footer_size;
+	raw_size *= 16 / ctx->cmosis_outputs;
+    }
 
     padded_blocks = raw_size / IPECAMERA_DMA_PACKET_LENGTH + ((raw_size % IPECAMERA_DMA_PACKET_LENGTH)?1:0);