Add forward error correction code to the udp sender/receiver.
[paraslash.git] / fecdec_filter.c
1 /*
2 * Copyright (C) 2009 Andre Noll <maan@systemlinux.org>
3 *
4 * Licensed under the GPL v2. For licencing details see COPYING.
5 */
6
7 /** \file fecdev_filter.c A filter fec-decodes an audio stream. */
8
9 #include <dirent.h>
10 #include "para.h"
11 #include "error.h"
12 #include "list.h"
13 #include "sched.h"
14 #include "ggo.h"
15 #include "filter.h"
16 #include "string.h"
17 #include "portable_io.h"
18 #include "fec.h"
19 #include "fd.h"
20
21 #define NUM_FEC_GROUPS 3
22 #define INPUT_BUFFER_SIZE 16384
23
24 /** size of the output buffer */
25 #define FECDEC_OUTBUF_SIZE 81920
26
27 struct fec_header {
28 uint8_t slices_per_group;
29 uint8_t data_slices_per_group;
30 uint32_t audio_header_size;
31
32 uint32_t group_num;
33 uint32_t group_bytes;
34
35 uint8_t slice_num;
36 uint16_t slice_bytes;
37 };
38
39 struct fec_group {
40 struct fec_header h;
41 int num_received_slices;
42 int num_slices;
43 int *idx;
44 unsigned char **data;
45 };
46
47 struct private_fecdec_data {
48 struct fec_parms *fec;
49 struct fec_group groups[NUM_FEC_GROUPS];
50 };
51
52 #define FOR_EACH_FEC_GROUP(g, d) for (g = (d)->groups; \
53 (g) - (d)->groups < NUM_FEC_GROUPS; (g)++)
54
55 #define UNUSED_GROUP_NUM 0xffffffff
56
57 static int group_complete(struct fec_group *fg)
58 {
59 if (fg->h.group_num == UNUSED_GROUP_NUM)
60 return 0;
61 //PARA_INFO_LOG("received slices: %u, slices per group: %u\n", fg->num_received_slices, fg->h.data_slices_per_group);
62 return fg->num_received_slices >= fg->h.data_slices_per_group;
63 }
64
65 static int group_empty(struct fec_group *fg)
66 {
67 return fg->num_received_slices == 0;
68 }
69
70 static void clear_group(struct fec_group *fg)
71 {
72 int i;
73
74 if (!group_complete(fg) && !group_empty(fg))
75 PARA_WARNING_LOG("Clearing incomplete group %d "
76 "(contains %d slices)\n", fg->h.group_num,
77 fg->num_received_slices);
78 for (i = 0; i < fg->num_slices; i++) {
79 free(fg->data[i]);
80 fg->data[i] = NULL;
81 fg->idx[i] = -1;
82 }
83 free(fg->data);
84 free(fg->idx);
85 fg->num_slices = 0;
86 memset(&fg->h, 0, sizeof(struct fec_header));
87 fg->num_received_slices = 0;
88 fg->h.group_num = UNUSED_GROUP_NUM;
89 }
90
91 static int find_group(struct fec_header *h,
92 struct private_fecdec_data *pfd, struct fec_group **result)
93 {
94 struct fec_group *fg;
95
96 FOR_EACH_FEC_GROUP(fg, pfd) {
97 if (fg->h.group_num != h->group_num)
98 continue;
99 *result = fg;
100 return 1;
101 }
102 return 0;
103 }
104
105 static struct fec_group *find_unused_group(struct private_fecdec_data *pfd)
106 {
107 struct fec_group *fg;
108
109 FOR_EACH_FEC_GROUP(fg, pfd) {
110 if (fg->num_received_slices == 0)
111 return fg;
112 }
113 return NULL;
114 }
115
116 static struct fec_group *try_to_free_group(struct private_fecdec_data *pfd)
117 {
118 struct fec_group *fg;
119
120 FOR_EACH_FEC_GROUP(fg, pfd) {
121 if (!group_complete(fg))
122 continue;
123 clear_group(fg);
124 return fg;
125 }
126 return NULL;
127 }
128
129 static struct fec_group *free_oldest_group(struct private_fecdec_data *pfd)
130 {
131 struct fec_group *fg, *oldest = NULL;
132
133 FOR_EACH_FEC_GROUP(fg, pfd) {
134 if (!oldest || oldest->h.group_num > fg->h.group_num)
135 oldest = fg;
136 }
137 clear_group(oldest);
138 return oldest;
139 }
140
141 static int get_group(struct fec_header *h, struct private_fecdec_data *pfd,
142 struct fec_group **result)
143 {
144 struct fec_group *fg;
145 int ret = find_group(h, pfd, &fg);
146
147 if (ret < 0)
148 return ret;
149 if (ret > 0) /* found group */
150 goto success;
151 /* group not found */
152 fg = find_unused_group(pfd);
153 if (fg)
154 goto update_header;
155 fg = try_to_free_group(pfd);
156 if (fg)
157 goto update_header;
158 fg = free_oldest_group(pfd);
159 update_header:
160 fg->h = *h;
161 success:
162 *result = fg;
163 return 1;
164 }
165
166 static int add_slice(char *buf, struct fec_group *fg)
167 {
168 int r, slice_num;
169
170 if (group_complete(fg))
171 return 0;
172 slice_num = fg->h.slice_num;
173 if (fg->num_slices == 0) {
174 fg->num_slices = fg->h.slices_per_group;
175 fg->idx = malloc(fg->num_slices * sizeof(int));
176 fg->data = malloc(fg->num_slices * sizeof(unsigned char *));
177 memset(fg->data, 0, fg->num_slices * sizeof(unsigned char *));
178 }
179 r = fg->num_received_slices;
180 fg->idx[r] = slice_num;
181 fg->data[r] = malloc(fg->h.slice_bytes);
182 memcpy(fg->data[r], buf, fg->h.slice_bytes);
183 fg->num_received_slices++;
184 return 1;
185 }
186
187 static int decode_group(struct fec_group *fg, struct filter_node *fn)
188 {
189 int i, ret, sb = fg->h.slice_bytes;
190 size_t written = 0;
191 struct private_fecdec_data *pfd = fn->private_data;
192
193 ret = fec_decode(pfd->fec, fg->data, fg->idx, sb);
194 if (ret < 0)
195 return ret;
196 PARA_DEBUG_LOG("writing group %d (%d/%d decoded data bytes)\n",
197 fg->h.group_num, fg->h.group_bytes,
198 fg->h.data_slices_per_group * sb);
199 for (i = 0; i < fg->h.data_slices_per_group; i++) {
200 size_t n = sb;
201 if (n + written > fg->h.group_bytes)
202 n = fg->h.group_bytes - written;
203 if (fn->loaded + n > fn->bufsize)
204 return -E_FECDEC_OVERRUN;
205 memcpy(fn->buf + fn->loaded, fg->data[i], n);
206 fn->loaded += n;
207 written += n;
208 }
209 return 0;
210 }
211
212 /**
213 * Read a fec header from a buffer.
214 *
215 * \param buf The buffer to write to.
216 * \param h The fec header to write.
217 */
218 static int read_fec_header(char *buf, size_t len, struct fec_header *h)
219 {
220 uint32_t magic;
221
222 if (len < FEC_HEADER_SIZE)
223 return 0;
224 magic = read_u32(buf);
225 if (magic != FEC_MAGIC)
226 return -E_BAD_FEC_HEADER;
227 h->slices_per_group = read_u8(buf + 4);
228 h->data_slices_per_group = read_u8(buf + 5);
229 h->audio_header_size = read_u32(buf + 6);
230
231 h->group_num = read_u32(buf + 10);
232 h->group_bytes = read_u32(buf + 14);
233
234 h->slice_num = read_u8(buf + 18);
235 h->slice_bytes = read_u16(buf + 20);
236 if (!h->group_bytes && & h->slice_bytes)
237 return -E_FECDEC_EOF;
238 // PARA_DEBUG_LOG("group %u, slize %u, slices per group: %u\n",
239 // h->group_num, h->slice_num, h->slices_per_group);
240 return 1;
241 }
242
243 static int dispatch_slice(char *buf, size_t len, struct fec_header *h,
244 struct filter_node *fn)
245 {
246 struct fec_group *fg;
247 int ret;
248 struct private_fecdec_data *pfd = fn->private_data;
249
250 if (h->slice_bytes > len) /* can not use the thing, try to read more */
251 return 0;
252 ret = get_group(h, pfd, &fg);
253 if (ret < 0)
254 return ret;
255 if (group_complete(fg)) {
256 PARA_DEBUG_LOG("group complete, ignoring slice %d\n",
257 h->slice_num);
258 return 1;
259 }
260 fg->h = *h;
261 ret = add_slice(buf, fg);
262 if (ret < 0)
263 return ret;
264 if (group_complete(fg)) {
265 if (!pfd->fec) {
266 int k = h->data_slices_per_group, n = h->slices_per_group;
267 PARA_NOTICE_LOG("init fec (%d, %d)\n", k, n);
268 ret = fec_new(k, n, &pfd->fec);
269 if (ret < 0)
270 return ret;
271 }
272 ret = decode_group(fg, fn);
273 if (ret < 0)
274 return ret;
275 }
276 return 1;
277 }
278
279 static int fecdec(char *buf, size_t len, struct filter_node *fn)
280 {
281 int ret;
282 struct fec_header h;
283
284 ret = read_fec_header(buf, len, &h);
285 if (ret <= 0)
286 return ret;
287 if (h.slice_bytes > INPUT_BUFFER_SIZE)
288 return -E_BAD_SLICE_SIZE;
289 if (h.slice_num > h.slices_per_group)
290 return -E_BAD_SLICE_NUM;
291 ret = dispatch_slice(buf + FEC_HEADER_SIZE, len - FEC_HEADER_SIZE,
292 &h, fn);
293 //PARA_INFO_LOG("ret: %d, len: %d, slice_bytes: %d\n", ret, len, h.slice_bytes);
294 if (ret <= 0)
295 return ret;
296 return FEC_HEADER_SIZE + h.slice_bytes;
297 }
298
299 static void fecdec_close(struct filter_node *fn)
300 {
301 struct private_fecdec_data *pfd = fn->private_data;
302 struct fec_group *fg;
303
304 FOR_EACH_FEC_GROUP(fg, pfd)
305 clear_group(fg);
306 free(fn->buf);
307 fn->buf = NULL;
308 free(fn->private_data);
309 fn->private_data = NULL;
310 }
311
312 static void fecdec_open(struct filter_node *fn)
313 {
314 fn->bufsize = FECDEC_OUTBUF_SIZE;
315 fn->buf = para_malloc(fn->bufsize);
316 fn->private_data = para_calloc(sizeof(struct private_fecdec_data));
317 fn->loaded = 0;
318 }
319
320 /**
321 * The init function of the fecdec filter.
322 *
323 * \param f struct to initialize.
324 */
325 void fecdec_filter_init(struct filter *f)
326 {
327 f->convert = fecdec;
328 f->close = fecdec_close;
329 f->open = fecdec_open;
330 }