diff options
Diffstat (limited to 'src/libipe.c')
| -rw-r--r-- | src/libipe.c | 294 |
1 files changed, 294 insertions, 0 deletions
diff --git a/src/libipe.c b/src/libipe.c new file mode 100644 index 0000000..7b919d5 --- /dev/null +++ b/src/libipe.c @@ -0,0 +1,294 @@ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <errno.h> +#include "libipe.h" +#include "libipe-private.h" +#include "config.h" + +#define IPECAMERA_NUM_CHANNELS 16 +#define IPECAMERA_PIXELS_PER_CHANNEL 128 +#define IPECAMERA_WIDTH (IPECAMERA_NUM_CHANNELS * IPECAMERA_PIXELS_PER_CHANNEL) + + +#define CHECK_VALUE(value, expected) \ + if (value != expected) { \ + fprintf(stderr, "<%s:%i> 0x%x != 0x%x\n", __FILE__, __LINE__, value, expected); \ + err = 1; \ + } + +#define CHECK_FLAG(flag, check, ...) \ + if (!(check)) { \ + fprintf(stderr, "<%s:%i> Unexpected value 0x%x of " flag "\n", __FILE__, __LINE__, __VA_ARGS__); \ + err = 1; \ + } + + +/** + * \brief Setup a new decoder instance + * + * \param height Number of rows that are expected in the data stream + * \param raw The data stream from the camera or NULL if set later with + * ipe_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 ipe_decoder_get_next_frame. + */ +ipe_decoder ipe_decoder_new(uint32_t height, uint32_t *raw, size_t num_bytes) +{ + ipe_decoder decoder = malloc(sizeof(struct ipe_decoder_t)); + if (decoder == NULL) + return NULL; + + decoder->height = height; + ipe_decoder_set_raw_data(decoder, raw, num_bytes); + return decoder; +} + + +/** + * \brief Release decoder instance + * + * \param decoder An ipe_decoder instance + */ +void ipe_decoder_free(ipe_decoder decoder) +{ + free(decoder); +} + + +/** + * \brief Set raw data stream + * + * \param decoder An ipe_decoder instance + * \param raw Raw data stream + * \param num_bytes Size of data stream buffer in bytes + */ +void ipe_decoder_set_raw_data(ipe_decoder decoder, uint32_t *raw, size_t num_bytes) +{ + decoder->raw = raw; + decoder->num_bytes = num_bytes; + decoder->current_pos = 0; +} + + +static int ipe_decode_frame(uint16_t *pixel_buffer, uint32_t *raw, int num_rows, int *offset) +{ + static int channel_order[IPECAMERA_NUM_CHANNELS] = { 15, 13, 14, 12, 10, 8, 11, 7, 9, 6, 5, 2, 4, 3, 0, 1 }; + + int info; + int row = 0; + int channel = 0; + int pos = 0; + uint32_t data; + + do { + info = raw[0]; + channel = info & 0x0F; + row = (info >> 4) & 0x7FF; + int pixels = (info >> 20) & 0xFF; + + channel = channel_order[channel]; + int base = row * IPECAMERA_WIDTH + channel * IPECAMERA_PIXELS_PER_CHANNEL; + +#ifdef DEBUG + int err = 0; + int header = (info >> 30) & 0x03; // 2 bits + const int bpp = (info >> 16) & 0x0F; // 4 bits + CHECK_FLAG("raw header magick", header == 2, header); + CHECK_FLAG("pixel size, only 10 bits are supported", bpp == 10, bpp); + CHECK_FLAG("channel, limited by %i output channels", channel < IPECAMERA_NUM_CHANNELS, channel, IPECAMERA_NUM_CHANNELS); +#endif + + /* "Correct" missing pixel */ + if ((row < 2) && (pixels == (IPECAMERA_PIXELS_PER_CHANNEL - 1))) { + pixel_buffer[base] = 0; + base++; + } +#ifdef DEBUG + else + CHECK_FLAG("number of pixels, %i is expected", pixels == IPECAMERA_PIXELS_PER_CHANNEL, pixels, IPECAMERA_PIXELS_PER_CHANNEL); +#endif + + int bytes = 43; + /* bytes = pixels / 3; */ + /* int ppw = pixels - bytes * 3; */ + /* if (ppw) */ + /* ++bytes; */ + + for (int i = 1; i < bytes; i++) { + data = raw[i]; + +#ifdef DEBUG + header = (data >> 30) & 0x03; + CHECK_FLAG("raw data magick", 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; + } + + data = raw[bytes]; + +#ifdef DEBUG + header = (data >> 30) & 0x03; + CHECK_FLAG("raw data magick", header == 3, header); + CHECK_FLAG("raw footer magick", (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 += bytes + 1; + raw += bytes + 1; + } while ((row < (num_rows - 1)) || (channel != 1)); + + *offset = pos; + 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 heigh Height of frame in pixels + */ +void ipe_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 heigh Height of frame in pixels + */ +void ipe_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 Iterate and decode next frame + * + * This function tries to decode the next frame in the currently set raw data + * stream. + * + * \param decoder An ipe_decoder instance + * \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 0 in case of no error, ENOSR if end of stream was reached, ENOMEM if + * NULL was passed but no memory could be allocated, EILSEQ if data stream is + * corrupt and EFAULT if pixels is a NULL-pointer. + */ +int ipe_decoder_get_next_frame(ipe_decoder decoder, uint16_t **pixels, uint32_t *frame_number, uint32_t *time_stamp) +{ + + uint32_t *raw = decoder->raw; + int err = 0; + int pos = decoder->current_pos; + int advance; + const int num_words = decoder->num_bytes / 4; + + if (pixels == NULL) + return EFAULT; + + if (pos >= num_words) + return ENOSR; + + if (num_words < 16) + return EILSEQ; + + if (*pixels == NULL) { + *pixels = (uint16_t *) malloc(IPECAMERA_WIDTH * decoder->height * sizeof(uint16_t)); + if (*pixels == NULL) + return ENOMEM; + } + +#ifdef DEBUG + 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); + CHECK_VALUE(raw[pos++], 0x56666666); + CHECK_VALUE(raw[pos] >> 28, 0x5); + *frame_number = raw[pos++] & 0xF0000000; + CHECK_VALUE(raw[pos] >> 28, 0x5); + *time_stamp = raw[pos++] & 0xF0000000; + if (err) + return EILSEQ; +#else + pos += 8; +#endif + + err = ipe_decode_frame(*pixels, raw + pos, decoder->height, &advance); + if (err) + return EILSEQ; + + pos += advance; + +#ifdef DEBUG + CHECK_VALUE(raw[pos++], 0x0AAAAAAA); + CHECK_VALUE(raw[pos++], 0x0BBBBBBB); + CHECK_VALUE(raw[pos++], 0x0CCCCCCC); + CHECK_VALUE(raw[pos++], 0x0DDDDDDD); + CHECK_VALUE(raw[pos++], 0x0EEEEEEE); + CHECK_VALUE(raw[pos++], 0x0FFFFFFF); + CHECK_VALUE(raw[pos++], 0x00000000); + CHECK_VALUE(raw[pos++], 0x01111111); +#else + pos += 8; +#endif + + /* if bytes left and we see fill bytes, skip them */ + if ((raw[pos] == 0x0) && (raw[pos+1] == 0x1111111)) { + pos += 2; + while ((pos < num_words) && ((raw[pos] == 0x89abcdef) || (raw[pos] == 0x1234567))) + pos++; + } + + decoder->current_pos = pos; + return 0; +} + |