/alps/ufodecode

To get this branch, use:
bzr branch http://suren.me/webbzr/alps/ufodecode

« back to all changes in this revision

Viewing changes to src/libipe.c

  • Committer: Matthias Vogelgesang
  • Date: 2011-12-01 08:41:56 UTC
  • Revision ID: matthias.vogelgesang@kit.edu-20111201084156-72tgvappjjy6aj4u
Make ipedec a lib and executable

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/libipe.c
 
1
 
 
2
#include <stdio.h>
 
3
#include <stdlib.h>
 
4
#include <string.h>
 
5
#include <errno.h>
 
6
#include "libipe.h"
 
7
#include "libipe-private.h"
 
8
#include "config.h"
 
9
 
 
10
#define IPECAMERA_NUM_CHANNELS 16
 
11
#define IPECAMERA_PIXELS_PER_CHANNEL 128
 
12
#define IPECAMERA_WIDTH (IPECAMERA_NUM_CHANNELS * IPECAMERA_PIXELS_PER_CHANNEL)
 
13
 
 
14
 
 
15
#define CHECK_VALUE(value, expected) \
 
16
    if (value != expected) { \
 
17
        fprintf(stderr, "<%s:%i> 0x%x != 0x%x\n", __FILE__, __LINE__, value, expected); \
 
18
        err = 1; \
 
19
    }
 
20
 
 
21
#define CHECK_FLAG(flag, check, ...) \
 
22
    if (!(check)) { \
 
23
        fprintf(stderr, "<%s:%i> Unexpected value 0x%x of " flag "\n", __FILE__, __LINE__,  __VA_ARGS__); \
 
24
        err = 1; \
 
25
    }
 
26
 
 
27
 
 
28
/**
 
29
 * \brief Setup a new decoder instance
 
30
 *
 
31
 * \param height Number of rows that are expected in the data stream
 
32
 * \param raw The data stream from the camera or NULL if set later with
 
33
 * ipe_decoder_set_raw_data.
 
34
 * \param num_bytes Size of the data stream buffer in bytes
 
35
 *
 
36
 * \return A new decoder instance that can be used to iterate over the frames
 
37
 * using ipe_decoder_get_next_frame.
 
38
 */
 
39
ipe_decoder ipe_decoder_new(uint32_t height, uint32_t *raw, size_t num_bytes)
 
40
{
 
41
    ipe_decoder decoder = malloc(sizeof(struct ipe_decoder_t));
 
42
    if (decoder == NULL)
 
43
        return NULL;
 
44
 
 
45
    decoder->height = height;
 
46
    ipe_decoder_set_raw_data(decoder, raw, num_bytes);
 
47
    return decoder;
 
48
}
 
49
 
 
50
 
 
51
/**
 
52
 * \brief Release decoder instance
 
53
 *
 
54
 * \param decoder An ipe_decoder instance
 
55
 */
 
56
void ipe_decoder_free(ipe_decoder decoder)
 
57
{
 
58
    free(decoder);
 
59
}
 
60
 
 
61
 
 
62
/**
 
63
 * \brief Set raw data stream
 
64
 *
 
65
 * \param decoder An ipe_decoder instance
 
66
 * \param raw Raw data stream
 
67
 * \param num_bytes Size of data stream buffer in bytes
 
68
 */
 
69
void ipe_decoder_set_raw_data(ipe_decoder decoder, uint32_t *raw, size_t num_bytes)
 
70
{
 
71
    decoder->raw = raw;
 
72
    decoder->num_bytes = num_bytes;
 
73
    decoder->current_pos = 0;
 
74
}
 
75
 
 
76
 
 
77
static int ipe_decode_frame(uint16_t *pixel_buffer, uint32_t *raw, int num_rows, int *offset)
 
78
{
 
79
    static int channel_order[IPECAMERA_NUM_CHANNELS] = { 15, 13, 14, 12, 10, 8, 11, 7, 9, 6, 5, 2, 4, 3, 0, 1 };
 
80
 
 
81
    int info;
 
82
    int row = 0;
 
83
    int channel = 0;
 
84
    int pos = 0;
 
85
    uint32_t data;
 
86
 
 
87
    do {
 
88
        info = raw[0];
 
89
        channel = info & 0x0F;
 
90
        row = (info >> 4) & 0x7FF;
 
91
        int pixels = (info >> 20) & 0xFF;
 
92
 
 
93
        channel = channel_order[channel];
 
94
        int base = row * IPECAMERA_WIDTH + channel * IPECAMERA_PIXELS_PER_CHANNEL;
 
95
 
 
96
#ifdef DEBUG
 
97
        int err = 0;
 
98
        int header = (info >> 30) & 0x03;   // 2 bits
 
99
        const int bpp = (info >> 16) & 0x0F;      // 4 bits
 
100
        CHECK_FLAG("raw header magick", header == 2, header);
 
101
        CHECK_FLAG("pixel size, only 10 bits are supported", bpp == 10, bpp);
 
102
        CHECK_FLAG("channel, limited by %i output channels", channel < IPECAMERA_NUM_CHANNELS, channel, IPECAMERA_NUM_CHANNELS);
 
103
#endif
 
104
 
 
105
        /* "Correct" missing pixel */
 
106
        if ((row < 2) && (pixels == (IPECAMERA_PIXELS_PER_CHANNEL - 1))) {
 
107
            pixel_buffer[base] = 0;
 
108
            base++;
 
109
        } 
 
110
#ifdef DEBUG
 
111
        else 
 
112
            CHECK_FLAG("number of pixels, %i is expected", pixels == IPECAMERA_PIXELS_PER_CHANNEL, pixels, IPECAMERA_PIXELS_PER_CHANNEL);
 
113
#endif
 
114
 
 
115
        int bytes = 43;
 
116
        /* bytes = pixels / 3; */
 
117
        /* int ppw = pixels - bytes * 3; */
 
118
        /* if (ppw) */ 
 
119
        /*     ++bytes; */
 
120
 
 
121
        for (int i = 1; i < bytes; i++) {
 
122
            data = raw[i];
 
123
 
 
124
#ifdef DEBUG
 
125
            header = (data >> 30) & 0x03;   
 
126
            CHECK_FLAG("raw data magick", header == 3, header);
 
127
            if (err) 
 
128
                return err;
 
129
#endif
 
130
            pixel_buffer[base++] = (data >> 20) & 0x3FF;
 
131
            pixel_buffer[base++] = (data >> 10) & 0x3FF;
 
132
            pixel_buffer[base++] = data & 0x3FF;
 
133
        }
 
134
 
 
135
        data = raw[bytes];
 
136
 
 
137
#ifdef DEBUG
 
138
        header = (data >> 30) & 0x03;
 
139
        CHECK_FLAG("raw data magick", header == 3, header);
 
140
        CHECK_FLAG("raw footer magick", (data & 0x3FF) == 0x55, (data & 0x3FF));
 
141
        if (err) 
 
142
            return err;
 
143
#endif
 
144
 
 
145
        int ppw = pixels >> 6;
 
146
        for (int j = 0; j < ppw; j++)
 
147
            pixel_buffer[base++] = (data >> (10 * (ppw - j))) & 0x3FF;
 
148
 
 
149
        pos += bytes + 1;
 
150
        raw += bytes + 1;
 
151
    } while ((row < (num_rows - 1)) || (channel != 1));
 
152
 
 
153
    *offset = pos;
 
154
    return 0;
 
155
}
 
156
 
 
157
 
 
158
/**
 
159
 * \brief Deinterlace by interpolating between two rows
 
160
 *
 
161
 * \param in Input frame
 
162
 * \param out Destination of interpolated frame
 
163
 * \param width Width of frame in pixels
 
164
 * \param heigh Height of frame in pixels
 
165
 */
 
166
void ipe_deinterlace_interpolate(const uint16_t *in, uint16_t *out, int width, int height)
 
167
{
 
168
    const size_t row_size_bytes = width * sizeof(uint16_t);
 
169
 
 
170
    for (int row = 0; row < height; row++) {
 
171
        /* Copy one line */
 
172
        memcpy(out, in + row*width, row_size_bytes);
 
173
        out += width;
 
174
 
 
175
        /* Interpolate between source row and row+1 */ 
 
176
        for (int x = 0; x < width; x++) {
 
177
            out[x] = (int) (0.5 * in[row*width + x] + 0.5 * in[(row+1)*width + x]);
 
178
        }
 
179
        out += width;
 
180
    }
 
181
 
 
182
    /* Copy last row */
 
183
    memcpy(out, in + width * (height - 1), row_size_bytes);
 
184
}
 
185
 
 
186
 
 
187
/**
 
188
 * \brief Deinterlace by "weaving" the rows of two frames
 
189
 *
 
190
 * \param in1 First frame
 
191
 * \param in2 Second frame
 
192
 * \param out Destination of weaved frame
 
193
 * \param width Width of frame in pixels
 
194
 * \param heigh Height of frame in pixels
 
195
 */
 
196
void ipe_deinterlace_weave(const uint16_t *in1, const uint16_t *in2, uint16_t *out, int width, int height)
 
197
{
 
198
    const size_t row_size_bytes = width * sizeof(uint16_t);
 
199
    for (int row = 0; row < height; row++) {
 
200
        memcpy(out, in1 + row*width, row_size_bytes); 
 
201
        out += width;
 
202
        memcpy(out, in2 + row*width, row_size_bytes); 
 
203
        out += width;
 
204
    }
 
205
}
 
206
 
 
207
 
 
208
/**
 
209
 * \brief Iterate and decode next frame
 
210
 *
 
211
 * This function tries to decode the next frame in the currently set raw data
 
212
 * stream. 
 
213
 *
 
214
 * \param decoder An ipe_decoder instance
 
215
 * \param pixels If pointer with NULL content is passed, a new buffer is
 
216
 * allocated otherwise, this user-supplied buffer is used.
 
217
 * \param frame_number Frame number as reported in the header
 
218
 * \param time_stamp Time stamp of the frame as reported in the header
 
219
 *
 
220
 * \return 0 in case of no error, ENOSR if end of stream was reached, ENOMEM if
 
221
 * NULL was passed but no memory could be allocated, EILSEQ if data stream is
 
222
 * corrupt and EFAULT if pixels is a NULL-pointer.
 
223
 */
 
224
int ipe_decoder_get_next_frame(ipe_decoder decoder, uint16_t **pixels, uint32_t *frame_number, uint32_t *time_stamp)
 
225
{
 
226
 
 
227
    uint32_t *raw = decoder->raw;
 
228
    int err = 0;
 
229
    int pos = decoder->current_pos;
 
230
    int advance;
 
231
    const int num_words = decoder->num_bytes / 4;
 
232
 
 
233
    if (pixels == NULL)
 
234
        return EFAULT;
 
235
 
 
236
    if (pos >= num_words)
 
237
        return ENOSR; 
 
238
 
 
239
    if (num_words < 16)
 
240
        return EILSEQ;
 
241
 
 
242
    if (*pixels == NULL) {
 
243
        *pixels = (uint16_t *) malloc(IPECAMERA_WIDTH * decoder->height * sizeof(uint16_t));
 
244
        if (*pixels == NULL)
 
245
            return ENOMEM;
 
246
    }
 
247
 
 
248
#ifdef DEBUG
 
249
    CHECK_VALUE(raw[pos++], 0x51111111);
 
250
    CHECK_VALUE(raw[pos++], 0x52222222);
 
251
    CHECK_VALUE(raw[pos++], 0x53333333);
 
252
    CHECK_VALUE(raw[pos++], 0x54444444);
 
253
    CHECK_VALUE(raw[pos++], 0x55555555);
 
254
    CHECK_VALUE(raw[pos++], 0x56666666);
 
255
    CHECK_VALUE(raw[pos] >> 28, 0x5);
 
256
    *frame_number = raw[pos++] & 0xF0000000;
 
257
    CHECK_VALUE(raw[pos] >> 28, 0x5);
 
258
    *time_stamp = raw[pos++] & 0xF0000000;
 
259
    if (err)
 
260
        return EILSEQ;
 
261
#else
 
262
    pos += 8;
 
263
#endif
 
264
 
 
265
    err = ipe_decode_frame(*pixels, raw + pos, decoder->height, &advance);
 
266
    if (err) 
 
267
        return EILSEQ;
 
268
 
 
269
    pos += advance;
 
270
 
 
271
#ifdef DEBUG
 
272
    CHECK_VALUE(raw[pos++], 0x0AAAAAAA);
 
273
    CHECK_VALUE(raw[pos++], 0x0BBBBBBB);
 
274
    CHECK_VALUE(raw[pos++], 0x0CCCCCCC);
 
275
    CHECK_VALUE(raw[pos++], 0x0DDDDDDD);
 
276
    CHECK_VALUE(raw[pos++], 0x0EEEEEEE);
 
277
    CHECK_VALUE(raw[pos++], 0x0FFFFFFF);
 
278
    CHECK_VALUE(raw[pos++], 0x00000000);
 
279
    CHECK_VALUE(raw[pos++], 0x01111111);
 
280
#else
 
281
    pos += 8;
 
282
#endif
 
283
 
 
284
    /* if bytes left and we see fill bytes, skip them */
 
285
    if ((raw[pos] == 0x0) && (raw[pos+1] == 0x1111111)) {
 
286
        pos += 2;
 
287
        while ((pos < num_words) && ((raw[pos] == 0x89abcdef) || (raw[pos] == 0x1234567)))
 
288
            pos++;
 
289
    }
 
290
 
 
291
    decoder->current_pos = pos;
 
292
    return 0;
 
293
}
 
294