Black magick from Michele

1 files changed, 153 insertions, 102 deletions

diff --git a/src/ufodecode.c b/src/ufodecode.c
index 68b0da7..cc25a8c 100644
--- a/src/ufodecode.c
+++ b/src/ufodecode.c
@@ -17,6 +17,13 @@
 #define IPECAMERA_PIXELS_PER_CHANNEL    128     /**< Number of pixels per channel */
 #define IPECAMERA_WIDTH (IPECAMERA_NUM_CHANNELS * IPECAMERA_PIXELS_PER_CHANNEL) /**< Total pixel width of row */
 
+#define IPECAMERA_MODE_16_CHAN_IO	0
+#define IPECAMERA_MODE_4_CHAN_IO	2
+
+#define IPECAMERA_MODE_12_BIT_ADC	2
+#define IPECAMERA_MODE_11_BIT_ADC	1
+#define IPECAMERA_MODE_10_BIT_ADC	0
+
 typedef struct {
     unsigned int pixel_number : 8;
     unsigned int row_number : 12;
@@ -113,10 +120,10 @@ ufo_decoder_set_raw_data(UfoDecoder *decoder, uint32_t *raw, size_t num_bytes)
 }
 
 static int
-ufo_decode_frame_channels_v0(UfoDecoder     *decoder, 
-                             uint16_t       *pixel_buffer, 
-                             uint32_t       *raw, 
-                             size_t          num_words, 
+ufo_decode_frame_channels_v0(UfoDecoder     *decoder,
+                             uint16_t       *pixel_buffer,
+                             uint32_t       *raw,
+                             size_t          num_words,
                              size_t         *offset)
 {
     static int channel_order[IPECAMERA_NUM_CHANNELS] = { 15, 13, 14, 12, 10, 8, 11, 7, 9, 6, 5, 2, 4, 3, 0, 1 };
@@ -171,7 +178,7 @@ ufo_decode_frame_channels_v0(UfoDecoder     *decoder,
             /* base++; */
         }
 #ifdef CHECKS
-        else 
+        else
             CHECK_FLAG("number of pixels, %i is expected", pixels == IPECAMERA_PIXELS_PER_CHANNEL, pixels, IPECAMERA_PIXELS_PER_CHANNEL);
 #endif
 
@@ -218,9 +225,9 @@ ufo_decode_frame_channels_v0(UfoDecoder     *decoder,
         for (int i = 1 ; i < bytes; i++) {
             data = raw[i];
 #ifdef DEBUG
-            header = (data >> 30) & 0x03;   
+            header = (data >> 30) & 0x03;
             CHECK_FLAG("raw data magic", header == 3, header);
-            if (err) 
+            if (err)
                 return err;
 #endif
             pixel_buffer[base++] = (data >> 20) & 0x3FF;
@@ -234,7 +241,7 @@ ufo_decode_frame_channels_v0(UfoDecoder     *decoder,
         header = (data >> 30) & 0x03;
         CHECK_FLAG("raw data magic", header == 3, header);
         CHECK_FLAG("raw footer magic", (data & 0x3FF) == 0x55, (data & 0x3FF));
-        if (err) 
+        if (err)
             return err;
 #endif
 
@@ -252,10 +259,10 @@ ufo_decode_frame_channels_v0(UfoDecoder     *decoder,
 
 static int
 ufo_decode_frame_channels_v4(UfoDecoder     *decoder,
-                             uint16_t       *pixel_buffer, 
-                             uint32_t       *raw, 
-                             size_t          num_words, 
-                             size_t          num_rows, 
+                             uint16_t       *pixel_buffer,
+                             uint32_t       *raw,
+                             size_t          num_words,
+                             size_t          num_rows,
                              size_t         *offset)
 {
     static const int channel_order[IPECAMERA_NUM_CHANNELS] = { 15, 13, 14, 12, 10, 8, 11, 7, 9, 6, 5, 2, 4, 3, 0, 1 };
@@ -355,9 +362,9 @@ ufo_decode_frame_channels_v4(UfoDecoder     *decoder,
         for (int i = 1 ; i < bytes; i++) {
             data = raw[i];
 #ifdef DEBUG
-            header = (data >> 30) & 0x03;   
+            header = (data >> 30) & 0x03;
             CHECK_FLAG("raw data magic", header == 3, header);
-            if (err) 
+            if (err)
                 return err;
 #endif
             pixel_buffer[base++] = (data >> 20) & 0x3FF;
@@ -371,7 +378,7 @@ ufo_decode_frame_channels_v4(UfoDecoder     *decoder,
         header = (data >> 30) & 0x03;
         CHECK_FLAG("raw data magic", header == 3, header);
         CHECK_FLAG("raw footer magic", (data & 0x3FF) == 0x55, (data & 0x3FF));
-        if (err) 
+        if (err)
             return err;
 #endif
 
@@ -388,12 +395,13 @@ ufo_decode_frame_channels_v4(UfoDecoder     *decoder,
 }
 
 static int
-ufo_decode_frame_channels_v5(UfoDecoder     *decoder, 
-                             uint16_t       *pixel_buffer, 
-                             uint32_t       *raw, 
-                             size_t          num_words, 
-                             size_t          num_rows, 
-                             size_t         *offset)
+ufo_decode_frame_channels_v5(UfoDecoder     *decoder,
+                             uint16_t       *pixel_buffer,
+                             uint32_t       *raw,
+                             size_t          num_words,
+                             size_t          num_rows,
+                             size_t         *offset,
+                             uint8_t         output_mode)
 {
     payload_header_v5 *header;
     size_t base = 0, index = 0;
@@ -401,7 +409,7 @@ ufo_decode_frame_channels_v5(UfoDecoder     *decoder,
 
     header = (payload_header_v5 *) &raw[base];
 
-    if (header->pixel_size == 12) {
+    if (output_mode == IPECAMERA_MODE_4_CHAN_IO) {
         while (raw[base] != 0xAAAAAAA) {
             header = (payload_header_v5 *) &raw[base];
             index = header->row_number * IPECAMERA_WIDTH + header->pixel_number;
@@ -416,7 +424,7 @@ ufo_decode_frame_channels_v5(UfoDecoder     *decoder,
                 pixel_buffer[index + (12+off)*IPECAMERA_PIXELS_PER_CHANNEL] = 0xfff & (raw[base+1] >> 16);
             }
             else {
-                off++; 
+                off++;
 
                 if (header->magic == 0xc0)
                     off = 0;
@@ -425,34 +433,66 @@ ufo_decode_frame_channels_v5(UfoDecoder     *decoder,
             base += 6;
         }
     }
-    else if (header->pixel_size == 10) {
+    else { /*if (output_mode == IPECAMERA_MODE_16_CHAN_IO)*/
         while (raw[base] != 0xAAAAAAA) {
             header = (payload_header_v5 *) &raw[base];
             index = header->row_number * IPECAMERA_WIDTH + header->pixel_number;
 
             /* Skip header + two zero-filled words */
-            base += 3;
-
-            pixel_buffer[index + 15*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 22);
-            pixel_buffer[index + 13*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 12);
-            pixel_buffer[index + 14*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 2);
-            pixel_buffer[index + 12*IPECAMERA_PIXELS_PER_CHANNEL] = ((0x3 & raw[base]) << 8) | (0x3ff & (raw[base+1] >> 24));
-            pixel_buffer[index + 10*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 14);
-            pixel_buffer[index +  8*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 4);
-            pixel_buffer[index + 11*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+1]) << 6) | (0x3ff & (raw[base+2] >> 26));
-            pixel_buffer[index +  7*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 16);
-            pixel_buffer[index +  9*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 6);
-            pixel_buffer[index +  6*IPECAMERA_PIXELS_PER_CHANNEL] = ((0x3f & raw[base+2]) << 4) | (0x3ff & (raw[base+3] >> 28));
-            pixel_buffer[index +  5*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 18);
-            pixel_buffer[index +  2*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 8);
-            pixel_buffer[index +  4*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xff & raw[base+3]) << 2) | (0x3ff & (raw[base+4] >> 30));
-            pixel_buffer[index +  3*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 20);
-            pixel_buffer[index +  0*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 10);
-            pixel_buffer[index +  1*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & raw[base+4];
-            base += 5;
+            /*
+                        base += 3;
+                        pixel_buffer[index + 15*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 22);
+                        pixel_buffer[index + 13*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 12);
+                        pixel_buffer[index + 14*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 2);
+                        pixel_buffer[index + 12*IPECAMERA_PIXELS_PER_CHANNEL] = ((0x3 & raw[base]) << 8) | (0x3ff & (raw[base+1] >> 24));
+                        pixel_buffer[index + 10*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 14);
+                        pixel_buffer[index +  8*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 4);
+                        pixel_buffer[index + 11*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+1]) << 6) | (0x3ff & (raw[base+2] >> 26));
+                        pixel_buffer[index +  7*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 16);
+                        pixel_buffer[index +  9*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 6);
+                        pixel_buffer[index +  6*IPECAMERA_PIXELS_PER_CHANNEL] = ((0x3f & raw[base+2]) << 4) | (0x3ff & (raw[base+3] >> 28));
+                        pixel_buffer[index +  5*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 18);
+                        pixel_buffer[index +  2*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 8);
+                        pixel_buffer[index +  4*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xff & raw[base+3]) << 2) | (0x3ff & (raw[base+4] >> 30));
+                        pixel_buffer[index +  3*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 20);
+                        pixel_buffer[index +  0*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 10);
+                        pixel_buffer[index +  1*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & raw[base+4];
+                        base += 5;
+            */
+
+            base += 2;
+
+            if (header->magic != 0xc0) {
+                pixel_buffer[index + 15*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 20);
+                pixel_buffer[index + 13*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 8);
+                pixel_buffer[index + 14*IPECAMERA_PIXELS_PER_CHANNEL] = (0xff & raw[base]) << 4 | (raw[base+1] >> 28);
+                pixel_buffer[index + 12*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 16);
+                pixel_buffer[index + 10*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 4);
+                pixel_buffer[index +  8*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+1]) << 8) | (raw[base+2] >> 24);
+                pixel_buffer[index + 11*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 12);
+                pixel_buffer[index +  7*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & raw[base+2];
+                pixel_buffer[index +  9*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 20);
+                pixel_buffer[index +  6*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 8);
+                pixel_buffer[index +  5*IPECAMERA_PIXELS_PER_CHANNEL] = (0xff & raw[base+3]) << 4 | (raw[base+4] >> 28);
+                pixel_buffer[index +  2*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 16);
+                pixel_buffer[index +  4*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 4);
+                pixel_buffer[index +  3*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+4]) << 8) | (raw[base+5] >> 24);
+                pixel_buffer[index +  0*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+5] >> 12);
+                pixel_buffer[index +  1*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & raw[base+5];
+            }
+            else {
+                off++;
+
+                if (header->magic == 0xc0)
+                    off = 0;
+            }
+
+            base += 6;
+
         }
     }
 
+
     *offset = base;
     return 0;
 }
@@ -473,7 +513,7 @@ void ufo_deinterlace_interpolate(const uint16_t *in, uint16_t *out, int width, i
         memcpy(out, in + row*width, row_size_bytes);
         out += width;
 
-        /* Interpolate between source row and row+1 */ 
+        /* Interpolate between source row and row+1 */
         for (int x = 0; x < width; x++)
             out[x] = (int) (0.5 * in[row*width + x] + 0.5 * in[(row+1)*width + x]);
 
@@ -497,9 +537,9 @@ void ufo_deinterlace_weave(const uint16_t *in1, const uint16_t *in2, uint16_t *o
 {
     const size_t row_size_bytes = width * sizeof(uint16_t);
     for (int row = 0; row < height; row++) {
-        memcpy(out, in1 + row*width, row_size_bytes); 
+        memcpy(out, in1 + row*width, row_size_bytes);
         out += width;
-        memcpy(out, in2 + row*width, row_size_bytes); 
+        memcpy(out, in2 + row*width, row_size_bytes);
         out += width;
     }
 }
@@ -520,9 +560,9 @@ void ufo_deinterlace_weave(const uint16_t *in1, const uint16_t *in2, uint16_t *o
  * \return number of decoded bytes or 0 in case of error
  */
 size_t ufo_decoder_decode_frame(UfoDecoder      *decoder,
-                                uint32_t        *raw, 
-                                size_t           num_bytes, 
-                                uint16_t        *pixels, 
+                                uint32_t        *raw,
+                                size_t           num_bytes,
+                                uint16_t        *pixels,
                                 UfoDecoderMeta  *meta)
 {
     int err = 0;
@@ -544,67 +584,78 @@ size_t ufo_decoder_decode_frame(UfoDecoder      *decoder,
     CHECK_VALUE(raw[pos++], 0x55555555);
 
     switch (version) {
-        case 0:
-            CHECK_VALUE(raw[pos++], 0x56666666);
-            CHECK_VALUE(raw[pos] >> 28, 0x5);
-            meta->frame_number = raw[pos++] & 0xFFFFFFF;
-            CHECK_VALUE(raw[pos] >> 28, 0x5);
-            meta->time_stamp = raw[pos++] & 0xFFFFFFF;
-            break;
-
-        case 4:
-        case 5:
-            CHECK_VALUE(raw[pos] >> 28, 0x5);
-            meta->cmosis_start_address = (raw[pos] >> 21) & 0x1FF;
-            meta->n_skipped_rows = (raw[pos] >> 15) & 0x3F;
-            meta->n_rows = rows_per_frame = raw[pos] & 0x7FF;
-            pos++;
+    case 0:
+        CHECK_VALUE(raw[pos++], 0x56666666);
+        CHECK_VALUE(raw[pos] >> 28, 0x5);
+        meta->frame_number = raw[pos++] & 0xFFFFFFF;
+        CHECK_VALUE(raw[pos] >> 28, 0x5);
+        meta->time_stamp = raw[pos++] & 0xFFFFFFF;
+        break;
+
+    case 4:
+    case 5:
+        CHECK_VALUE(raw[pos] >> 28, 0x5);
+        meta->cmosis_start_address = (raw[pos] >> 21) & 0x1FF;
+        meta->n_skipped_rows = (raw[pos] >> 15) & 0x3F;
+        meta->n_rows = rows_per_frame = raw[pos] & 0x7FF;
+        pos++;
 
-            meta->frame_number = raw[pos++] & 0x1FFFFFF;
-            CHECK_VALUE(raw[pos] >> 28, 0x5);
-            meta->time_stamp = raw[pos++] & 0xFFFFFF;
-            meta->output_mode = (raw[pos] >> 24) & 0x3;
-            meta->adc_resolution = (raw[pos] >> 26) & 0x3
-            break;
+        meta->frame_number = raw[pos++] & 0x1FFFFFF;
+        CHECK_VALUE(raw[pos] >> 28, 0x5);
+        meta->time_stamp = raw[pos] & 0xFFFFFF;
+        meta->output_mode = (raw[pos] >> 24) & 0x3;
+        meta->adc_resolution = (raw[pos] >> 26) & 0x3;
+        pos++;
+
+        if ((meta->output_mode != IPECAMERA_MODE_4_CHAN_IO)&&(meta->output_mode != IPECAMERA_MODE_4_CHAN_IO)) {
+#ifdef DEBUG
+            fprintf(stderr, "Output mode 0x%lx is not supported\n", meta->output_mode);
+#endif
+            return EILSEQ;
+        }
+        break;
 
-        default:
-            fprintf(stderr, "Unsupported data format detected\n");
-            return 0;
+    default:
+        fprintf(stderr, "Unsupported data format detected\n");
+        return 0;
     }
 
-    if (err) 
+    if (err)
         return 0;
 #else
     switch (version) {
-        case 0:
-            meta->frame_number = raw[pos + 6] & 0xFFFFFFF;
-            meta->time_stamp = raw[pos + 7] & 0xFFFFFFF;
-            break;
-        case 4:
-        case 5:
-            meta->frame_number = raw[pos + 6] & 0x1FFFFFF;
-            meta->time_stamp = raw[pos + 7] & 0xFFFFFF;
-            break;
-        default:
-            fprintf(stderr, "Unsupported data format detected\n");
-            return 0;
+    case 0:
+        meta->frame_number = raw[pos + 6] & 0xFFFFFFF;
+        meta->time_stamp = raw[pos + 7] & 0xFFFFFFF;
+        break;
+    case 4:
+    case 5:
+        meta->frame_number = raw[pos + 6] & 0x1FFFFFF;
+        meta->time_stamp = raw[pos + 7] & 0xFFFFFF;
+        meta->output_mode = (raw[pos + 7] >> 24) & 0x3;
+        meta->adc_resolution = (raw[pos + 7] >> 26) & 0x3;
+
+        break;
+    default:
+        fprintf(stderr, "Unsupported data format detected\n");
+        return 0;
     }
 
     pos += 8;
 #endif
 
     switch (version) {
-        case 0:
-            err = ufo_decode_frame_channels_v0(decoder, pixels, raw + pos, num_words - pos - 8, &advance);
-            break;
-        case 4:
-            err = ufo_decode_frame_channels_v4(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance);
-            break;
-        case 5:
-            err = ufo_decode_frame_channels_v5(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance);
-            break;
-        default:
-            break;
+    case 0:
+        err = ufo_decode_frame_channels_v0(decoder, pixels, raw + pos, num_words - pos - 8, &advance);
+        break;
+    case 4:
+        err = ufo_decode_frame_channels_v4(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance);
+        break;
+    case 5:
+        err = ufo_decode_frame_channels_v5(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance, meta->output_mode);
+        break;
+    default:
+        break;
     }
 
     if (err)
@@ -623,7 +674,7 @@ size_t ufo_decoder_decode_frame(UfoDecoder      *decoder,
     CHECK_VALUE(raw[pos++], 0x00000000);
     CHECK_VALUE(raw[pos++], 0x01111111);
 
-    if (err) 
+    if (err)
         return 0;
 #else
     pos += 8;
@@ -636,7 +687,7 @@ size_t ufo_decoder_decode_frame(UfoDecoder      *decoder,
  * \brief Iterate and decode next frame
  *
  * This function tries to decode the next frame in the currently set raw data
- * stream. 
+ * stream.
  *
  * \param decoder An UfoDecoder instance
  * \param pixels If pointer with NULL content is passed, a new buffer is
@@ -649,8 +700,8 @@ size_t ufo_decoder_decode_frame(UfoDecoder      *decoder,
  * NULL was passed but no memory could be allocated, EILSEQ if data stream is
  * corrupt and EFAULT if pixels is a NULL-pointer.
  */
-int ufo_decoder_get_next_frame(UfoDecoder     *decoder, 
-                               uint16_t      **pixels, 
+int ufo_decoder_get_next_frame(UfoDecoder     *decoder,
+                               uint16_t      **pixels,
                                UfoDecoderMeta *meta)
 {
     uint32_t *raw = decoder->raw;
@@ -662,7 +713,7 @@ int ufo_decoder_get_next_frame(UfoDecoder     *decoder,
         return 0;
 
     if (pos >= num_words)
-        return EIO; 
+        return EIO;
 
     if (num_words < 16)
         return EILSEQ;