/alps/ufodecode

#include <stdio.h>

#include <stdlib.h>

#include <string.h>

#include <errno.h>

#include "ufodecode.h"

#include "ufodecode-private.h"

#include "config.h"

#ifdef HAVE_SSE

#include <xmmintrin.h>

#endif

#define CHECKS

#ifdef DEBUG

# define SLOW_CHECKS

# define ALL_CHECKS

# define VERBOSE

#endif /* DEBUG */

#ifdef ALL_CHECKS

# undef HAVE_SSE

#endif /* ALL_CHECKS */

#define IPECAMERA_NUM_ROWS              1088

#define IPECAMERA_NUM_CHANNELS          16      /**< Number of channels per row */

#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;

    unsigned int pixel_size : 4;

    unsigned int magic : 8;

} payload_header_v5;

/**

 * Check if value matches expected input.

 */

#ifdef VERBOSE

# define CHECK_VALUE(value, expected) \

    if (value != expected) { \

        fprintf(stderr, "<%s:%i> 0x%x != 0x%x\n", __FILE__, __LINE__, value, expected); \

        err = 1; \

    }

#else

# define CHECK_VALUE(value, expected) \

    if (value != expected) { \

        err = 1; \

    }

#endif

/**

 * Check that flag evaluates to non-zero.

 */

#ifdef VERBOSE

# define CHECK_FLAG(flag, check, ...) \

    if (!(check)) { \

        fprintf(stderr, "<%s:%i> Unexpected value 0x%x of " flag "\n", __FILE__, __LINE__,  __VA_ARGS__); \

        err = 1; \

    }

#else

# define CHECK_FLAG(flag, check, ...) \

    if (!(check)) { \

        err = 1; \

    }

#endif

/**

 * \brief Setup a new decoder instance

 *

 * \param height Number of rows that are expected in the data stream. Set this

 *      smaller 0 to let the decoder figure out the number of rows.

 * \param raw The data stream from the camera or NULL if set later with

 * ufo_decoder_set_raw_data.

 * \param num_bytes Size of the data stream buffer in bytes

 *

 * \return A new decoder instance that can be used to iterate over the frames

 * using ufo_decoder_get_next_frame.

 */

UfoDecoder *

ufo_decoder_new (int32_t height, uint32_t width, uint32_t *raw, size_t num_bytes)

{

    if (width % IPECAMERA_PIXELS_PER_CHANNEL)

        return NULL;

    UfoDecoder *decoder = malloc(sizeof(UfoDecoder));

    if (decoder == NULL)

        return NULL;

    decoder->width = width;

    decoder->height = height;

    ufo_decoder_set_raw_data(decoder, raw, num_bytes);

    return decoder;

}

/**

 * \brief Release decoder instance

 *

 * \param decoder An UfoDecoder instance

 */

void

ufo_decoder_free(UfoDecoder *decoder)

{

    free(decoder);

}

/**

 * \brief Set raw data stream

 *

 * \param decoder An UfoDecoder instance

 * \param raw Raw data stream

 * \param num_bytes Size of data stream buffer in bytes

 */

void

ufo_decoder_set_raw_data(UfoDecoder *decoder, uint32_t *raw, size_t num_bytes)

{

    decoder->raw = raw;

    decoder->num_bytes = num_bytes;

    decoder->current_pos = 0;

}

static int

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 };

    static int channel_size = (2 + IPECAMERA_PIXELS_PER_CHANNEL / 3);

    const int bytes = channel_size - 1;

    const int num_rows = decoder->height;

    const size_t cpl = (decoder->width / IPECAMERA_PIXELS_PER_CHANNEL);

    const size_t cpi = num_rows * cpl;

    int pos = 0;

    uint32_t data;

#ifdef HAVE_SSE

    __m128i mask = _mm_set_epi32(0x3FF, 0x3FF, 0x3FF, 0x3FF);

    __m128i packed;

    __m128i tmp1, tmp2;

    uint32_t result[4] __attribute__ ((aligned (16))) = {0};

#endif

    if (cpi * channel_size > num_words) {

#ifdef VERBOSE

        fprintf(stderr, "Not enough data to decode frame, expected %lu bytes, but received %lu\n", cpi * channel_size * sizeof(uint32_t), num_words * sizeof(uint32_t));

#endif

        return EILSEQ;

    }

    for (size_t c = 0; c < cpi; c++) {

        const int info = raw[0];

        int row = (info >> 4) & 0x7FF;

        int channel = info & 0x0F;

        int pixels = (info >> 20) & 0xFF;

#ifdef SLOW_CHECKS

        int header = (info >> 30) & 0x03;

        const int bpp = (info >> 16) & 0x0F;

        int err;

        CHECK_FLAG("raw header magick", header == 2, header);

        CHECK_FLAG("row number, only %i rows requested", row < num_rows, row, num_rows);

        CHECK_FLAG("pixel size, only 10 bits are supported", bpp == 10, bpp);

        CHECK_FLAG("channel, limited by %zu output channels", channel < cpl, channel, cpl);

#endif

        if ((row > num_rows) || (channel > cpl) || (pixels > IPECAMERA_PIXELS_PER_CHANNEL))

            return EILSEQ;

        channel = channel_order[channel];

        int base = row * IPECAMERA_WIDTH + channel * IPECAMERA_PIXELS_PER_CHANNEL;

        /* "Correct" missing pixel */

        if ((row < 2) && (pixels == (IPECAMERA_PIXELS_PER_CHANNEL - 1))) {

            pixel_buffer[base] = 0;

            /* base++; */

        }

#ifdef SLOW_CHECKS

        else

            CHECK_FLAG("number of pixels, %i is expected", pixels == IPECAMERA_PIXELS_PER_CHANNEL, pixels, IPECAMERA_PIXELS_PER_CHANNEL);

#endif

#ifdef HAVE_SSE

        for (int i = 1 ; i < bytes-4; i += 4, base += 12) {

            packed = _mm_set_epi32(raw[i], raw[i+1], raw[i+2], raw[i+3]);

            tmp1 = _mm_srli_epi32(packed, 20);

            tmp2 = _mm_and_si128(tmp1, mask);

            _mm_storeu_si128((__m128i*) result, tmp2);

            pixel_buffer[base+0] = result[3];

            pixel_buffer[base+3] = result[2];

            pixel_buffer[base+6] = result[1];

            pixel_buffer[base+9] = result[0];

            tmp1 = _mm_srli_epi32(packed, 10);

            tmp2 = _mm_and_si128(tmp1, mask);

            _mm_storeu_si128((__m128i*) result, tmp2);

            pixel_buffer[base+1] = result[3];

            pixel_buffer[base+4] = result[2];

            pixel_buffer[base+7] = result[1];

            pixel_buffer[base+10] = result[0];

            tmp1 = _mm_and_si128(packed, mask);

            _mm_storeu_si128((__m128i*) result, tmp1);

            pixel_buffer[base+2] = result[3];

            pixel_buffer[base+5] = result[2];

            pixel_buffer[base+8] = result[1];

            pixel_buffer[base+11] = result[0];

        }

        /* Compute last pixels by hand */

        data = raw[41];

        pixel_buffer[base++] = (data >> 20) & 0x3FF;

        pixel_buffer[base++] = (data >> 10) & 0x3FF;

        pixel_buffer[base++] = data & 0x3FF;

        data = raw[42];

        pixel_buffer[base++] = (data >> 20) & 0x3FF;

        pixel_buffer[base++] = (data >> 10) & 0x3FF;

        pixel_buffer[base++] = data & 0x3FF;

#else

        for (int i = 1 ; i < bytes; i++) {

            data = raw[i];

# ifdef SLOW_CHECKS

            header = (data >> 30) & 0x03;

            CHECK_FLAG("raw data magic", header == 3, header);

            if (err)

                return err;

# endif

            pixel_buffer[base++] = (data >> 20) & 0x3FF;

            pixel_buffer[base++] = (data >> 10) & 0x3FF;

            pixel_buffer[base++] = data & 0x3FF;

        }

#endif

        data = raw[bytes];

#ifdef SLOW_CHECKS

        header = (data >> 30) & 0x03;

        CHECK_FLAG("raw data magic", header == 3, header);

        CHECK_FLAG("raw footer magic", (data & 0x3FF) == 0x55, (data & 0x3FF));

        if (err)

            return err;

#endif

        int ppw = pixels >> 6;

        for (int j = 0; j < ppw; j++)

            pixel_buffer[base++] = (data >> (10 * (ppw - j))) & 0x3FF;

        pos += channel_size;

        raw += channel_size;

    }

    *offset += pos;

    return 0;

}

static int

ufo_decode_frame_channels_v4(UfoDecoder     *decoder,

                             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 };

    static const int channel_size = (2 + IPECAMERA_PIXELS_PER_CHANNEL / 3);

    const int bytes = channel_size - 1;

    const size_t channels_per_row = (decoder->width / IPECAMERA_PIXELS_PER_CHANNEL);

    const size_t cpi = num_rows * channels_per_row;

    int pos = 0;

    uint32_t data;

#ifdef HAVE_SSE

    __m128i mask = _mm_set_epi32(0x3FF, 0x3FF, 0x3FF, 0x3FF);

    __m128i packed;

    __m128i tmp1, tmp2;

    uint32_t result[4] __attribute__ ((aligned (16))) = {0};

#endif

    if (cpi * channel_size > num_words) {

#ifdef VERBOSE

        fprintf(stderr, "Not enough data to decode frame, expected %lu bytes, but received %lu\n", cpi * channel_size * sizeof(uint32_t), num_words * sizeof(uint32_t));

#endif

        return EILSEQ;

    }

    for (size_t c = 0; c < cpi; c++) {

        int err = 0;

        const int info = raw[0];

        int row = (info >> 4) & 0x7FF;

        int channel = info & 0x0F;

        int pixels = (info >> 20) & 0xFF;

#ifdef SLOW_CHECKS

        int header = (info >> 30) & 0x03;

        const int bpp = (info >> 16) & 0x0F;

        CHECK_FLAG("raw header magick", header == 2, header);

        CHECK_FLAG("pixel size, only 10 bits are supported", bpp == 10, bpp);

#endif

        CHECK_FLAG("pixels, only %u pixels per channel (offset: %zx)", pixels <= IPECAMERA_PIXELS_PER_CHANNEL, pixels, IPECAMERA_PIXELS_PER_CHANNEL, (*offset + pos));

        CHECK_FLAG("channel, limited by %zu output channels (offset: %zx)", channel <= channels_per_row, channel, channels_per_row, (*offset + pos));

        if (err)

            return EILSEQ;

        channel = channel_order[channel];

        int base = row * IPECAMERA_WIDTH + channel * IPECAMERA_PIXELS_PER_CHANNEL;

        /* "Correct" missing pixel */

        if ((row < 2) && (pixels == (IPECAMERA_PIXELS_PER_CHANNEL - 1))) {

            pixel_buffer[base] = 0;

            /* base++; */

        }

#ifdef HAVE_SSE

        for (int i = 1 ; i < bytes-4; i += 4, base += 12) {

            packed = _mm_set_epi32(raw[i], raw[i+1], raw[i+2], raw[i+3]);

            tmp1 = _mm_srli_epi32(packed, 20);

            tmp2 = _mm_and_si128(tmp1, mask);

            _mm_storeu_si128((__m128i*) result, tmp2);

            pixel_buffer[base+0] = result[3];

            pixel_buffer[base+3] = result[2];

            pixel_buffer[base+6] = result[1];

            pixel_buffer[base+9] = result[0];

            tmp1 = _mm_srli_epi32(packed, 10);

            tmp2 = _mm_and_si128(tmp1, mask);

            _mm_storeu_si128((__m128i*) result, tmp2);

            pixel_buffer[base+1] = result[3];

            pixel_buffer[base+4] = result[2];

            pixel_buffer[base+7] = result[1];

            pixel_buffer[base+10] = result[0];

            tmp1 = _mm_and_si128(packed, mask);

            _mm_storeu_si128((__m128i*) result, tmp1);

            pixel_buffer[base+2] = result[3];

            pixel_buffer[base+5] = result[2];

            pixel_buffer[base+8] = result[1];

            pixel_buffer[base+11] = result[0];

        }

        /* Compute last pixels by hand */

        data = raw[41];

        pixel_buffer[base++] = (data >> 20) & 0x3FF;

        pixel_buffer[base++] = (data >> 10) & 0x3FF;

        pixel_buffer[base++] = data & 0x3FF;

        data = raw[42];

        pixel_buffer[base++] = (data >> 20) & 0x3FF;

        pixel_buffer[base++] = (data >> 10) & 0x3FF;

        pixel_buffer[base++] = data & 0x3FF;

#else

        for (int i = 1 ; i < bytes; i++) {

            data = raw[i];

# ifdef SLOW_CHECKS

            header = (data >> 30) & 0x03;

            CHECK_FLAG("raw data magic (offset: 0x%zx, expected: 0x02)", header == 3, header, (*offset + pos + i)*sizeof(uint32_t));

            if (err)

                return err;

# endif

            pixel_buffer[base++] = (data >> 20) & 0x3FF;

            pixel_buffer[base++] = (data >> 10) & 0x3FF;

            pixel_buffer[base++] = data & 0x3FF;

        }

#endif

        data = raw[bytes];

#ifdef SLOW_CHECKS

        header = (data >> 30) & 0x03;

        CHECK_FLAG("raw data magic (offset: 0x%zx, expected: 0x03)", header == 3, header, (*offset + pos + bytes)*sizeof(uint32_t));

        CHECK_FLAG("raw footer magic (offset: 0x%zx, expected: 0x55)", (data & 0x3FF) == 0x55, (data & 0x3FF), (*offset + pos + bytes)*sizeof(uint32_t));

        if (err)

            return err;

#endif

        int ppw = pixels >> 6;

        for (int j = 0; j < ppw; j++)

            pixel_buffer[base++] = (data >> (10 * (ppw - j))) & 0x3FF;

        pos += channel_size;

        raw += channel_size;

    }

    *offset += pos;

    return 0;

}

static int

ufo_decode_frame_channels_v5(UfoDecoder     *decoder,

                             uint16_t       *pixel_buffer,

                             uint32_t       *raw,

                             size_t          num_rows,

                             size_t         *offset,

                             uint8_t         output_mode)

{

    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;

        while (raw[base] != 0xAAAAAAA) {

            header = (payload_header_v5 *) &raw[base];

            index = header->row_number * IPECAMERA_WIDTH + header->pixel_number;

            /* Skip header + one zero-filled words */

            base += 2;

            if ((header->magic != 0xe0) && (header->magic != 0xc0)) {

                pixel_buffer[index +  (0+off)*IPECAMERA_PIXELS_PER_CHANNEL] = 0xfff & (raw[base+5] >> 12);

                pixel_buffer[index +  (4+off)*IPECAMERA_PIXELS_PER_CHANNEL] = 0xfff & (raw[base+4] >> 4);

                pixel_buffer[index +  (8+off)*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+1]) << 8) | (raw[base+2] >> 24);

                pixel_buffer[index + (12+off)*IPECAMERA_PIXELS_PER_CHANNEL] = 0xfff & (raw[base+1] >> 16);

            }

            else {

                off++;

                if (header->magic == 0xc0)

                    off = 0;

            }

            base += 6;

        }

    }

    else {

        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 += 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] = 0x3ff & (((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] = ((0x3 & 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] = 0x3ff & (((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] = ((0x3 & 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];

            }

            base += 6;

        }

    }

    *offset += base;

    return 0;

}

/**

 * \brief Deinterlace by interpolating between two rows

 *

 * \param in Input frame

 * \param out Destination of interpolated frame

 * \param width Width of frame in pixels

 * \param height Height of frame in pixels

 */

void ufo_deinterlace_interpolate(const uint16_t *in, uint16_t *out, int width, int height)

{

    const size_t row_size_bytes = width * sizeof(uint16_t);

    for (int row = 0; row < height; row++) {

        /* Copy one line */

        memcpy(out, in + row*width, row_size_bytes);

        out += width;

        /* 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]);

        out += width;

    }

    /* Copy last row */

    memcpy(out, in + width * (height - 1), row_size_bytes);

}

/**

 * \brief Deinterlace by "weaving" the rows of two frames

 *

 * \param in1 First frame

 * \param in2 Second frame

 * \param out Destination of weaved frame

 * \param width Width of frame in pixels

 * \param height Height of frame in pixels

 */

void ufo_deinterlace_weave(const uint16_t *in1, const uint16_t *in2, uint16_t *out, int width, int height)

{

    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);

        out += width;

        memcpy(out, in2 + row*width, row_size_bytes);

        out += width;

    }

}

/**

 * \brief Decodes frame

 *

 * This function tries to decode the supplied data

 *

 * \param decoder An UfoDecoder instance

 * \param raw Raw data stream

 * \param num_bytes Size of data stream buffer in bytes

 * \param pixels If pointer with NULL content is passed, a new buffer is

 * allocated otherwise, this user-supplied buffer is used.

 * \param frame_number Frame number as reported in the header

 * \param time_stamp Time stamp of the frame as reported in the header

 *

 * \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,

                                UfoDecoderMeta  *meta)

{

    int err = 0;

    size_t pos = 0;

    const size_t num_words = num_bytes / 4;

    if ((pixels == NULL) || (num_words < 16)) {

#ifdef VERBOSE

	if (num_words < 16) 

	    fprintf(stderr, "Not enough frame data, only %zu bytes supplied \n", num_bytes);

#endif

        return 0;

    }

    size_t rows_per_frame = decoder->height;

    const int version = (raw[pos+6] >> 24) & 0xF;

#ifdef CHECKS

    CHECK_VALUE(raw[pos++], 0x51111111);

    CHECK_VALUE(raw[pos++], 0x52222222);

    CHECK_VALUE(raw[pos++], 0x53333333);

    CHECK_VALUE(raw[pos++], 0x54444444);

    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;