/alps/pcitool

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

« back to all changes in this revision

Viewing changes to ipecamera/reader.c

  • Committer: Suren A. Chilingaryan
  • Date: 2015-04-20 20:01:04 UTC
  • Revision ID: csa@suren.me-20150420200104-b5xny65io8lvoz3w
Big redign of model structures

Show diffs side-by-side

added added

removed removed

Lines of Context:
ipecamera/reader.c
1
 
#define _BSD_SOURCE
2
 
#define _GNU_SOURCE
3
 
 
4
 
#include <stdio.h>
5
 
#include <stdlib.h>
6
 
#include <unistd.h>
7
 
#include <string.h>
8
 
#include <sys/time.h>
9
 
#include <sys/stat.h>
10
 
#include <sys/types.h>
11
 
#include <pthread.h>
12
 
#include <assert.h>
13
 
 
14
 
#include <ufodecode.h>
15
 
 
16
 
#include "../tools.h"
17
 
#include "../error.h"
18
 
 
19
 
#include "pcilib.h"
20
 
#include "model.h"
21
 
#include "private.h"
22
 
#include "reader.h"
23
 
 
24
 
 
25
 
int ipecamera_compute_buffer_size(ipecamera_t *ctx, size_t lines) {
26
 
    const size_t header_size = 8 * sizeof(ipecamera_payload_t);
27
 
    const size_t footer_size = 8 * sizeof(ipecamera_payload_t);
28
 
 
29
 
    size_t line_size, raw_size, padded_blocks;
30
 
 
31
 
 
32
 
    switch (ctx->firmware) {
33
 
     case 4:
34
 
        line_size = IPECAMERA_MAX_CHANNELS * (2 + IPECAMERA_PIXELS_PER_CHANNEL / 3) * sizeof(ipecamera_payload_t);
35
 
        raw_size = header_size + lines * line_size + footer_size;
36
 
        break;
37
 
     default:
38
 
        line_size = (1 + IPECAMERA_PIXELS_PER_CHANNEL) * 32; 
39
 
        raw_size = header_size + lines * line_size - 32 + footer_size;
40
 
        raw_size *= 16 / ctx->cmosis_outputs;
41
 
    }
42
 
 
43
 
    padded_blocks = raw_size / IPECAMERA_DMA_PACKET_LENGTH + ((raw_size % IPECAMERA_DMA_PACKET_LENGTH)?1:0);
44
 
    
45
 
    ctx->cur_raw_size = raw_size;
46
 
    ctx->cur_full_size = padded_blocks * IPECAMERA_DMA_PACKET_LENGTH;
47
 
 
48
 
#ifdef IPECAMERA_BUG_EXTRA_DATA
49
 
    ctx->cur_full_size += 8;
50
 
    padded_blocks ++;
51
 
#endif /* IPECAMERA_BUG_EXTRA_DATA */
52
 
 
53
 
    ctx->cur_padded_size = padded_blocks * IPECAMERA_DMA_PACKET_LENGTH;
54
 
 
55
 
    return 0;
56
 
}
57
 
 
58
 
static inline int ipecamera_new_frame(ipecamera_t *ctx) {
59
 
    ctx->frame[ctx->buffer_pos].event.raw_size = ctx->cur_size;
60
 
 
61
 
    if (ctx->cur_size < ctx->cur_raw_size) {
62
 
        ctx->frame[ctx->buffer_pos].event.info.flags |= PCILIB_EVENT_INFO_FLAG_BROKEN;
63
 
    }
64
 
    
65
 
    ctx->buffer_pos = (++ctx->event_id) % ctx->buffer_size;
66
 
    ctx->cur_size = 0;
67
 
 
68
 
    ctx->frame[ctx->buffer_pos].event.info.type = PCILIB_EVENT0;
69
 
    ctx->frame[ctx->buffer_pos].event.info.flags = 0;
70
 
    ctx->frame[ctx->buffer_pos].event.image_ready = 0;
71
 
 
72
 
    if ((ctx->event_id == ctx->autostop.evid)&&(ctx->event_id)) {
73
 
        ctx->run_reader = 0;
74
 
        return 1;
75
 
    }
76
 
        
77
 
    if (pcilib_check_deadline(&ctx->autostop.timestamp, PCILIB_DMA_TIMEOUT)) {
78
 
        ctx->run_reader = 0;
79
 
        return 1;
80
 
    }
81
 
    
82
 
    return 0;
83
 
}
84
 
 
85
 
static uint32_t frame_magic[5] = { 0x51111111, 0x52222222, 0x53333333, 0x54444444, 0x55555555 };
86
 
 
87
 
static int ipecamera_data_callback(void *user, pcilib_dma_flags_t flags, size_t bufsize, void *buf) {
88
 
    int res;
89
 
    int eof = 0;
90
 
 
91
 
#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
92
 
    size_t real_size;
93
 
    size_t extra_data = 0;
94
 
#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
95
 
 
96
 
    ipecamera_t *ctx = (ipecamera_t*)user;
97
 
 
98
 
#if defined(IPECAMERA_BUG_INCOMPLETE_PACKETS)||defined(IPECAMERA_BUG_MULTIFRAME_PACKETS)
99
 
    static  pcilib_event_id_t invalid_frame_id = (pcilib_event_id_t)-1;
100
 
#endif
101
 
 
102
 
#ifdef IPECAMERA_DEBUG_RAW_PACKETS
103
 
    char fname[128];
104
 
    { 
105
 
        static unsigned long packet_id = 0;
106
 
        sprintf(fname,"%s/frame%4lu", IPECAMERA_DEBUG_RAW_PACKETS, ctx->event_id);
107
 
        mkdir(fname, 0755);
108
 
        sprintf(fname,"%s/frame%4lu/frame%9lu", IPECAMERA_DEBUG_RAW_PACKETS, ctx->event_id, packet_id);
109
 
        FILE *f = fopen(fname, "w");
110
 
        if (f) {
111
 
            fwrite(buf, 1, bufsize, f);
112
 
            fclose(f);
113
 
        }
114
 
        sprintf(fname,"%s/frame%4lu/frame%9lu.invalid", IPECAMERA_DEBUG_RAW_PACKETS, ctx->event_id, packet_id++);
115
 
    }
116
 
#endif /* IPECAMERA_DEBUG_RAW_PACKETS */
117
 
    
118
 
    if (!ctx->cur_size) {
119
 
#if defined(IPECAMERA_BUG_INCOMPLETE_PACKETS)||defined(IPECAMERA_BUG_MULTIFRAME_PACKETS)
120
 
        size_t startpos;
121
 
        for (startpos = 0; (startpos + sizeof(frame_magic)) < bufsize; startpos += sizeof(uint32_t)) {
122
 
            if (!memcmp(buf + startpos, frame_magic, sizeof(frame_magic))) break;
123
 
        }
124
 
        
125
 
        if ((startpos + sizeof(frame_magic)) >= bufsize) {
126
 
#ifdef IPECAMERA_DEBUG_RAW_PACKETS
127
 
            FILE *f = fopen(fname, "w");
128
 
            if (f) fclose(f);
129
 
#endif /* IPECAMERA_DEBUG_RAW_PACKETS */
130
 
            
131
 
            if (invalid_frame_id != ctx->event_id) {
132
 
                pcilib_warning("No frame magic in DMA packet of %u bytes, current event %lu", bufsize, ctx->event_id);
133
 
                invalid_frame_id = ctx->event_id;
134
 
            }
135
 
 
136
 
            return PCILIB_STREAMING_CONTINUE;
137
 
        }
138
 
        
139
 
        if (startpos) {
140
 
                // pass padding to rawdata callback
141
 
            if (ctx->event.params.rawdata.callback) {
142
 
                res = ctx->event.params.rawdata.callback(0, NULL, PCILIB_EVENT_FLAG_RAW_DATA_ONLY, startpos, buf, ctx->event.params.rawdata.user);
143
 
                if (res <= 0) {
144
 
                    if (res < 0) return res;
145
 
                    ctx->run_reader = 0;
146
 
                }
147
 
            }
148
 
 
149
 
 
150
 
            buf += startpos;
151
 
            bufsize -= startpos;
152
 
        }
153
 
#endif /* IPECAMERA_BUG_INCOMPLETE_PACKETS */
154
 
 
155
 
        if ((bufsize >= 8)&&(!memcmp(buf, frame_magic, sizeof(frame_magic)))) {
156
 
            size_t n_lines = ((uint32_t*)buf)[5] & 0x7FF;
157
 
            ipecamera_compute_buffer_size(ctx, n_lines);
158
 
 
159
 
            ctx->frame[ctx->buffer_pos].event.info.seqnum = ((uint32_t*)buf)[6] & 0x1FFFFFF;
160
 
            ctx->frame[ctx->buffer_pos].event.info.offset = (((uint32_t*)buf)[7] & 0xFFFFFF) * 80;
161
 
 
162
 
//          ctx->frame[ctx->buffer_pos].event.info.seqnum = ctx->event_id + 1;
163
 
 
164
 
            gettimeofday(&ctx->frame[ctx->buffer_pos].event.info.timestamp, NULL);
165
 
        } else {
166
 
//          pcilib_warning("Frame magic is not found, ignoring broken data...");
167
 
            return PCILIB_STREAMING_CONTINUE;
168
 
        }
169
 
    }
170
 
 
171
 
#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
172
 
        // for rawdata_callback with complete padding
173
 
    real_size = bufsize;
174
 
    
175
 
    if (ctx->cur_size + bufsize > ctx->cur_raw_size) {
176
 
        size_t need;
177
 
        
178
 
        for (need = ctx->cur_raw_size - ctx->cur_size; (need + sizeof(frame_magic)) < bufsize; need += sizeof(uint32_t)) {
179
 
            if (!memcmp(buf + need, frame_magic, sizeof(frame_magic))) break;
180
 
        }
181
 
        
182
 
        if ((need + sizeof(frame_magic)) < bufsize) {
183
 
            extra_data = bufsize - need;
184
 
            //bufsize = need;
185
 
            eof = 1;
186
 
        }
187
 
        
188
 
            // just rip of padding
189
 
        bufsize = ctx->cur_raw_size - ctx->cur_size;
190
 
 
191
 
#ifdef IPECAMERA_DEBUG_RAW_PACKETS
192
 
        sprintf(fname + strlen(fname) - 8, ".partial");
193
 
        FILE *f = fopen(fname, "w");
194
 
        if (f) {
195
 
            fwrite(buf, 1, bufsize, f);
196
 
            fclose(f);
197
 
        }
198
 
#endif /* IPECAMERA_DEBUG_RAW_PACKETS */
199
 
    }
200
 
#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
201
 
 
202
 
    if (ctx->parse_data) {
203
 
        if (ctx->cur_size + bufsize > ctx->full_size) {
204
 
            pcilib_error("Unexpected event data, we are expecting at maximum (%zu) bytes, but (%zu) already read", ctx->full_size, ctx->cur_size + bufsize);
205
 
            return -PCILIB_ERROR_TOOBIG;
206
 
        }
207
 
 
208
 
        memcpy(ctx->buffer + ctx->buffer_pos * ctx->padded_size +  ctx->cur_size, buf, bufsize);
209
 
    }
210
 
 
211
 
    ctx->cur_size += bufsize;
212
 
//    printf("%i: %i %i\n", ctx->buffer_pos, ctx->cur_size, bufsize);
213
 
 
214
 
    if (ctx->cur_size >= ctx->full_size) eof = 1;
215
 
 
216
 
    if (ctx->event.params.rawdata.callback) {
217
 
        res = ctx->event.params.rawdata.callback(ctx->event_id, (pcilib_event_info_t*)(ctx->frame + ctx->buffer_pos), (eof?PCILIB_EVENT_FLAG_EOF:PCILIB_EVENT_FLAGS_DEFAULT), bufsize, buf, ctx->event.params.rawdata.user);
218
 
        if (res <= 0) {
219
 
            if (res < 0) return res;
220
 
            ctx->run_reader = 0;
221
 
        }
222
 
    }
223
 
    
224
 
    if (eof) {
225
 
        if ((ipecamera_new_frame(ctx))||(!ctx->run_reader)) {
226
 
            return PCILIB_STREAMING_STOP;
227
 
        }
228
 
        
229
 
#ifdef IPECAMERA_BUG_MULTIFRAME_PACKETS
230
 
        if (extra_data) {
231
 
            return ipecamera_data_callback(user, flags, extra_data, buf + (real_size - extra_data));
232
 
        }
233
 
#endif /* IPECAMERA_BUG_MULTIFRAME_PACKETS */
234
 
    }
235
 
 
236
 
    return PCILIB_STREAMING_REQ_FRAGMENT;
237
 
}
238
 
 
239
 
void *ipecamera_reader_thread(void *user) {
240
 
    int err;
241
 
    ipecamera_t *ctx = (ipecamera_t*)user;
242
 
    
243
 
    while (ctx->run_reader) {
244
 
        err = pcilib_stream_dma(ctx->event.pcilib, ctx->rdma, 0, 0, PCILIB_DMA_FLAG_MULTIPACKET, PCILIB_DMA_TIMEOUT, &ipecamera_data_callback, user);
245
 
        if (err) {
246
 
            if (err == PCILIB_ERROR_TIMEOUT) {
247
 
                if (ctx->cur_size >= ctx->cur_raw_size) ipecamera_new_frame(ctx);
248
 
#ifdef IPECAMERA_BUG_INCOMPLETE_PACKETS
249
 
                else if (ctx->cur_size > 0) ipecamera_new_frame(ctx);
250
 
#endif /* IPECAMERA_BUG_INCOMPLETE_PACKETS */
251
 
                if (pcilib_check_deadline(&ctx->autostop.timestamp, PCILIB_DMA_TIMEOUT)) {
252
 
                    ctx->run_reader = 0;
253
 
                    break;
254
 
                }
255
 
                usleep(IPECAMERA_NOFRAME_SLEEP);
256
 
            } else pcilib_error("DMA error while reading IPECamera frames, error: %i", err);
257
 
        } //else printf("no error\n");
258
 
 
259
 
        //usleep(1000);
260
 
    }
261
 
    
262
 
    ctx->run_streamer = 0;
263
 
    
264
 
    if (ctx->cur_size)
265
 
        pcilib_error("partialy read frame after stop signal, %zu bytes in the buffer", ctx->cur_size);
266
 
 
267
 
    return NULL;
268
 
}