Implement v6 data format

2 files changed, 131 insertions, 45 deletions

diff --git a/src/ufodecode.c b/src/ufodecode.c
index 9519489..f86abfc 100644
--- a/src/ufodecode.c
+++ b/src/ufodecode.c
@@ -15,6 +15,8 @@
 #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_WIDTH_20MP            5120
+
 #define IPECAMERA_MODE_16_CHAN_IO	0
 #define IPECAMERA_MODE_4_CHAN_IO	2
 
@@ -49,11 +51,12 @@ typedef struct {
 } header_v5_t;
 
 typedef struct {
+    uint32_t magic_2;
     uint32_t magic_3;
     uint32_t magic_4;
     unsigned cmosis_start_address : 16;
-    unsigned adc_resolution : 4;
     unsigned output_mode : 4;
+    unsigned adc_resolution : 4;
     unsigned five_1 : 4;
     unsigned n_rows : 16;
     unsigned n_skipped_rows : 12;
@@ -66,13 +69,12 @@ typedef struct {
 } header_v6_t;
 
 typedef struct {
-    unsigned int pixel_number : 8;
-    unsigned int row_number : 12;
-    unsigned int pixel_size : 4;
-    unsigned int magic : 8;
+    unsigned pixel_number : 8;
+    unsigned row_number : 12;
+    unsigned pixel_size : 4;
+    unsigned magic : 8;
 } payload_header_v5;
 
-
 /**
  * Check if value matches expected input.
  */
@@ -166,8 +168,6 @@ ufo_decode_frame_channels_v5 (UfoDecoder *decoder, uint16_t *pixel_buffer, uint3
     payload_header_v5 *header;
     size_t base = 0, index = 0;
 
-    header = (payload_header_v5 *) &raw[base];
-
     if (output_mode == IPECAMERA_MODE_4_CHAN_IO) {
         size_t off = 0;
 
@@ -229,6 +229,47 @@ ufo_decode_frame_channels_v5 (UfoDecoder *decoder, uint16_t *pixel_buffer, uint3
     return 0;
 }
 
+static int
+ufo_decode_frame_channels_v6 (UfoDecoder *decoder, uint16_t *pixel_buffer, uint32_t *raw, size_t num_rows, size_t *offset, uint8_t output_mode)
+{
+    size_t base = 0;
+    size_t index = 0;
+    const size_t space = 640;
+
+    while (raw[base] != 0xAAAAAAA) {
+        const size_t row_number = raw[base] & 0xfff;
+        const size_t pixel_number = (raw[base + 1] >> 16) & 0xfff;
+
+        base += 2;
+
+        index = row_number * IPECAMERA_WIDTH_20MP + pixel_number;
+
+        pixel_buffer[index + 0 * space] = (raw[base] >> 20);
+        pixel_buffer[index + 1 * space] = (raw[base] >> 8) & 0xfff;
+        pixel_buffer[index + 2 * space] = ((raw[base] << 4) & 0xff) | ((raw[base + 1] >> 28) & 0xff);
+        pixel_buffer[index + 3 * space] = (raw[base + 1] >> 16) & 0xfff;
+        pixel_buffer[index + 4 * space] = (raw[base + 1] >> 4) & 0xfff;
+        pixel_buffer[index + 5 * space] = ((raw[base + 1] << 8) & 0xfff) | (raw[base + 2] >> 24);
+        pixel_buffer[index + 6 * space] = (raw[base + 2] >> 12) & 0xfff;
+        pixel_buffer[index + 7 * space] = raw[base + 2] & 0xfff;
+
+        index += IPECAMERA_WIDTH_20MP;
+        pixel_buffer[index + 0 * space] = (raw[base + 3] >> 20);
+        pixel_buffer[index + 1 * space] = (raw[base + 3] >> 8) & 0xfff;
+        pixel_buffer[index + 2 * space] = ((raw[base + 3] << 4) & 0xfff) | (raw[base + 4] >> 28);
+        pixel_buffer[index + 3 * space] = (raw[base + 4] >> 16) & 0xfff;
+        pixel_buffer[index + 4 * space] = (raw[base + 4] >> 4) & 0xfff;
+        pixel_buffer[index + 5 * space] = ((raw[base + 4] << 8) & 0xfff) | (raw[base + 5] >> 24);
+        pixel_buffer[index + 6 * space] = (raw[base + 5] >> 12) & 0xfff;
+        pixel_buffer[index + 7 * space] = (raw[base + 5] & 0xfff);
+
+        base += 6;
+    }
+
+    *offset = base;
+    return 0;
+}
+
 /**
  * \brief Deinterlace by interpolating between two rows
  *
@@ -303,7 +344,6 @@ ufo_decoder_decode_frame (UfoDecoder *decoder, uint32_t *raw, size_t num_bytes,
     const size_t num_words = num_bytes / 4;
     size_t rows_per_frame = decoder->height;
     const pre_header_t *pre_header;
-    const header_v5_t *v5_header;
 
     if ((pixels == NULL) || (num_words < 16))
         return 0;
@@ -313,23 +353,54 @@ ufo_decoder_decode_frame (UfoDecoder *decoder, uint32_t *raw, size_t num_bytes,
     CHECK_VALUE (pre_header->five, 0x5);
     CHECK_VALUE (pre_header->ones, 0x111111);
 
-    const int version = pre_header->version + 5;    /* it starts with 0 */
+    const int header_version = pre_header->version + 5;    /* it starts with 0 */
+    int dataformat_version = 5;      /* will overwrite for header_version >= 6 */
 
-    v5_header = (header_v5_t *) &raw[pos + 1];
+    switch (header_version) {
+        case 5:
+            {
+                const header_v5_t *header = (header_v5_t *) &raw[pos + 1];
 
-    CHECK_VALUE (v5_header->magic_2, 0x52222222);
-    CHECK_VALUE (v5_header->magic_3, 0x53333333);
-    CHECK_VALUE (v5_header->magic_4, 0x54444444);
-    CHECK_VALUE (v5_header->magic_5, 0x55555555);
+                CHECK_VALUE (header->magic_2, 0x52222222);
+                CHECK_VALUE (header->magic_3, 0x53333333);
+                CHECK_VALUE (header->magic_4, 0x54444444);
+                CHECK_VALUE (header->magic_5, 0x55555555);
 
-    CHECK_VALUE (v5_header->five_1, 0x5);
-    CHECK_VALUE (v5_header->five_2, 0x5);
+                CHECK_VALUE (header->five_1, 0x5);
+                CHECK_VALUE (header->five_2, 0x5);
+
+                meta->time_stamp = header->timestamp;
+                meta->cmosis_start_address = header->cmosis_start_address;
+                meta->frame_number = header->frame_number;
+                meta->n_rows = header->n_rows;
+                meta->n_skipped_rows = header->n_skipped_rows;
+                break;
+            }
+
+        case 6:
+            {
+                const header_v6_t *header = (header_v6_t *) &raw[pos + 1];
+                CHECK_VALUE (header->magic_2, 0x52222222);
+                CHECK_VALUE (header->magic_3, 0x53333333);
+                CHECK_VALUE (header->magic_4, 0x54444444);
+
+                dataformat_version = header->dataformat_version;
+
+                meta->output_mode = header->output_mode;
+                meta->adc_resolution = header->adc_resolution;
+                meta->time_stamp = header->timestamp;
+                meta->cmosis_start_address = header->cmosis_start_address;
+                meta->frame_number = header->frame_number;
+                meta->n_rows = header->n_rows;
+                meta->n_skipped_rows = header->n_skipped_rows;
+
+                break;
+            }
+
+        default:
+            fprintf (stderr, "Unsupported header version %i\n", header_version);
+    }
 
-    meta->time_stamp = v5_header->timestamp;
-    meta->cmosis_start_address = v5_header->cmosis_start_address;
-    meta->frame_number = v5_header->frame_number;
-    meta->n_rows = v5_header->n_rows;
-    meta->n_skipped_rows = v5_header->n_skipped_rows;
 
 #ifdef DEBUG
     if ((meta->output_mode != IPECAMERA_MODE_4_CHAN_IO) && (meta->output_mode != IPECAMERA_MODE_16_CHAN_IO)) {
@@ -354,11 +425,17 @@ ufo_decoder_decode_frame (UfoDecoder *decoder, uint32_t *raw, size_t num_bytes,
 
     pos += 8;
 
-    if (version == 5) {
-        err = ufo_decode_frame_channels_v5 (decoder, pixels, raw + pos, rows_per_frame, &advance, meta->output_mode);
-    }
-    else {
-        fprintf (stderr, "Data format version %i unsupported\n", version);
+    switch (dataformat_version) {
+        case 5:
+            err = ufo_decode_frame_channels_v5 (decoder, pixels, raw + pos, rows_per_frame, &advance, meta->output_mode);
+            break;
+
+        case 6:
+            err = ufo_decode_frame_channels_v6 (decoder, pixels, raw + pos, rows_per_frame, &advance, meta->output_mode);
+            break;
+
+        default:
+            fprintf (stderr, "Data format version %i unsupported\n", dataformat_version);
     }
 
     if (err)
@@ -427,7 +504,8 @@ ufo_decoder_get_next_frame (UfoDecoder *decoder, uint16_t **pixels, UfoDecoderMe
             return ENOMEM;
     }
 
-    while ((pos < num_words) && (raw[pos] != 0x51111111))
+    while ((pos < num_words) &&
+           ((raw[pos] & 0xFFFFFFF0) != 0x51111110)) /* we can only match the first part */
         pos++;
 
     advance = ufo_decoder_decode_frame (decoder, raw + pos, decoder->num_bytes - pos, *pixels, meta);
diff --git a/test/ipedec.c b/test/ipedec.c
index 135c7c2..7baa522 100644
--- a/test/ipedec.c
+++ b/test/ipedec.c
@@ -8,13 +8,14 @@
 #include <ufodecode.h>
 #include "timer.h"
 
-static const int MAX_ROWS = 2048;
+static const int MAX_ROWS = 3840;
 
 typedef struct {
     int clear_frame;
     int dry_run;
     int verbose;
-    int rows;
+    int num_rows;
+    int num_columns;
     int print_frame_rate;
     int print_num_rows;
     int cont;
@@ -56,7 +57,8 @@ usage(void)
 Options:\n\
   -h, --help                Show this help message and exit\n\
   -v, --verbose             Print additional information on STDOUT\n\
-  -r, --num-rows=N          N rows that are contained in the file\n\
+  -r, --num-rows=N          N rows contained in the file\n\
+      --num-columns=N       N columns contained in the file\n\
   -c, --clear-frame         Clear the frame for each iteration\n\
   -d, --dry-run             Do not save the frames\n\
   -f, --print-frame-rate    Print frame rate on STDOUT\n\
@@ -113,14 +115,14 @@ write_raw_file (UfoDecoderMeta *meta,
     size_t n_rows = meta->n_rows;
 
     if (opts->convert_bayer) {
-        uint8_t *rgb_pixels = malloc (2048 * n_rows * 3);
+        uint8_t *rgb_pixels = malloc (opts->num_columns * n_rows * 3);
 
-        ufo_convert_bayer_to_rgb (pixels, rgb_pixels, 2048, n_rows);
-        fwrite (rgb_pixels, sizeof(uint8_t), 2048 * n_rows * 3, fp);
+        ufo_convert_bayer_to_rgb (pixels, rgb_pixels, opts->num_columns, n_rows);
+        fwrite (rgb_pixels, sizeof(uint8_t), opts->num_columns * n_rows * 3, fp);
         free (rgb_pixels);
     }
     else {
-        fwrite(pixels, sizeof(uint16_t), 2048 * n_rows, fp);
+        fwrite(pixels, sizeof(uint16_t), opts->num_columns * n_rows, fp);
     }
 }
 
@@ -140,14 +142,14 @@ process_file(const char *filename, Options *opts)
     char             output_name[256];
     float            mtime;
 
-    error = read_raw_file(filename, &buffer, &num_bytes);
+    error = read_raw_file (filename, &buffer, &num_bytes);
 
     if (error) {
         fprintf(stderr, "Error reading %s: %s\n", filename, strerror(error));
         return error;
     }
 
-    decoder = ufo_decoder_new(opts->rows, 2048, (uint32_t *) buffer, num_bytes);
+    decoder = ufo_decoder_new (opts->num_rows, opts->num_columns, (uint32_t *) buffer, num_bytes);
 
     if (decoder == NULL) {
         fprintf(stderr, "Failed to initialize decoder\n");
@@ -165,7 +167,7 @@ process_file(const char *filename, Options *opts)
     }
 
     timer = timer_new ();
-    pixels = (uint16_t *) malloc (2048 * MAX_ROWS * sizeof(uint16_t));
+    pixels = (uint16_t *) malloc (opts->num_columns * opts->num_rows * sizeof(uint16_t));
     n_frames = 0;
     old_time_stamp = 0;
 
@@ -174,7 +176,7 @@ process_file(const char *filename, Options *opts)
         error = ufo_decoder_get_next_frame (decoder, &pixels, &meta);
 
         if (meta.n_rows == 0)
-            meta.n_rows = opts->rows;
+            meta.n_rows = opts->num_rows;
 
         timer_stop (timer);
 
@@ -194,13 +196,13 @@ process_file(const char *filename, Options *opts)
             }
 
             if (opts->print_num_rows)
-                printf("%d", meta.n_rows); 
+                printf ("%d", meta.n_rows);
 
             if (opts->print_frame_rate || opts->print_num_rows)
-                printf("\n");
+                printf ("\n");
 
             if (opts->clear_frame)
-                memset(pixels, 0, 2048 * meta.n_rows * sizeof(uint16_t));
+                memset (pixels, 0, opts->num_columns * meta.n_rows * sizeof(uint16_t));
 
             if (!opts->dry_run)
                 write_raw_file (&meta, opts, pixels, fp);
@@ -243,15 +245,17 @@ int main(int argc, char const* argv[])
         DRY_RUN      = 'd',
         FRAME_RATE   = 'f',
         HELP         = 'h',
-        SET_NUM_ROWS = 'r', 
+        SET_NUM_ROWS = 'r',
         VERBOSE      = 'v',
         CONTINUE,
         NUM_ROWS,
+        SET_NUM_COLUMNS,
         CONVERT_BAYER,
     };
 
     static struct option long_options[] = {
         { "num-rows",           required_argument, 0, SET_NUM_ROWS },
+        { "num-columns",        required_argument, 0, SET_NUM_COLUMNS },
         { "clear-frame",        no_argument, 0, CLEAR_FRAME },
         { "verbose",            no_argument, 0, VERBOSE },
         { "help",               no_argument, 0, HELP },
@@ -264,7 +268,8 @@ int main(int argc, char const* argv[])
     };
 
     static Options opts = {
-        .rows = 1088,
+        .num_rows = 1088,
+        .num_columns = 2048,
         .verbose = 0,
         .dry_run = 0,
         .clear_frame = 0,
@@ -277,7 +282,10 @@ int main(int argc, char const* argv[])
     while ((getopt_ret = getopt_long(argc, (char *const *) argv, "r:cvhdf", long_options, &index)) != -1) {
         switch (getopt_ret) {
             case SET_NUM_ROWS:
-                opts.rows = atoi(optarg);
+                opts.num_rows = atoi(optarg);
+                break;
+            case SET_NUM_COLUMNS:
+                opts.num_columns = atoi(optarg);
                 break;
             case CLEAR_FRAME:
                 opts.clear_frame = 1;