1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
|
/*
* frame_rifo.c
*
* This is for the IP -> E1 direction, where IP packets may arrive with
* re-ordering. So this "Random [order] In, First Out" is reconstructing
* the original order.
*
* (C) 2022 by Harald Welte <laforge@osmocom.org>
*
* All Rights Reserved
*
* SPDX-License-Identifier: GPL-2.0+
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#include <errno.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <osmocom/core/utils.h>
#include "frame_rifo.h"
/***********************************************************************
* Frame RIFO
***********************************************************************/
/* return the absolute bucket number (0.. FRAMES_PER_FIFO-1) for given fn */
static inline uint32_t bucket_for_fn(const struct frame_rifo *rifo, uint32_t fn)
{
uint32_t next_out_bucket = (rifo->next_out - rifo->buf) / BYTES_PER_FRAME;
/* offset in frames compared to next_out */
uint32_t offset = (fn - rifo->next_out_fn) % FRAMES_PER_FIFO;
return (next_out_bucket + offset) % FRAMES_PER_FIFO;
}
/* set the bucket bit for given bucket number */
static void bucket_bit_set(struct frame_rifo *rifo, uint32_t bucket_nr)
{
uint8_t byte = bucket_nr/8;
uint8_t bit = bucket_nr%8;
OSMO_ASSERT(byte < sizeof(rifo->bitvec));
rifo->bitvec[byte] |= (1 << bit);
}
/* clear the bucket bit for given bucket number */
static void bucket_bit_clear(struct frame_rifo *rifo, uint32_t bucket_nr)
{
uint8_t byte = bucket_nr/8;
uint8_t bit = bucket_nr%8;
OSMO_ASSERT(byte < sizeof(rifo->bitvec));
rifo->bitvec[byte] &= ~(1 << bit);
}
/* is the given bucket bit number set? */
static bool bucket_bit_get(struct frame_rifo *rifo, uint32_t bucket_nr)
{
uint8_t byte = bucket_nr/8;
uint8_t bit = bucket_nr%8;
OSMO_ASSERT(byte < sizeof(rifo->bitvec));
return rifo->bitvec[byte] & (1 << bit);
}
/*! Initialize a frame RIFO.
* \param rifo Caller-allocated memory for RIFO data structure */
void frame_rifo_init(struct frame_rifo *rifo)
{
memset(rifo->buf, 0xff, sizeof(rifo->buf));
rifo->next_out = rifo->buf;
rifo->next_out_fn = 0;
rifo->last_in_fn = -1;
memset(rifo->bitvec, 0, sizeof(rifo->bitvec));
}
#define RIFO_BUF_END(f) ((f)->buf + sizeof((f)->buf))
/*! put one received frame into the RIFO at a given specified frame number.
* \param rifo The RIFO to which we want to put (append) multiple frames
* \param frame Pointer to memory containing the frame data
* \param fn Absolute frame number at which to insert the frame.
* \returns 0 on success; -1 on error (overflow) */
int frame_rifo_in(struct frame_rifo *rifo, const uint8_t *frame, uint32_t fn)
{
uint32_t bucket;
uint8_t *dst;
if (!frame_rifo_fn_in_range(rifo, fn))
{
return -ERANGE;
}
bucket = bucket_for_fn(rifo, fn);
dst = rifo->buf + bucket * BYTES_PER_FRAME;
OSMO_ASSERT(dst + BYTES_PER_FRAME <= RIFO_BUF_END(rifo));
memcpy(dst, frame, BYTES_PER_FRAME);
bucket_bit_set(rifo, bucket);
rifo->last_in_fn = fn;
return 0;
}
/*! pull one frames out of the RIFO.
* \param rifo The RIFO from which we want to pull frames
* \param out Caller-allocated output buffer
* \returns 0 on success; -1 if no frame available; -2 if RIFO depth == 0 */
int frame_rifo_out(struct frame_rifo *rifo, uint8_t *out)
{
uint32_t next_out_bucket = (rifo->next_out - rifo->buf) / BYTES_PER_FRAME;
bool bucket_bit = bucket_bit_get(rifo, next_out_bucket);
int rc = 0;
if (frame_rifo_depth(rifo) == 0) {
/* if we don't have any RIFO depth at all, our jitter buffer has
* run empty and most likely there is some fundamental clock sync problem
* somewhere. */
rc = -2;
} else if (!bucket_bit) {
/* caller is supposed to copy/duplicate previous frame */
rc = -1;
} else {
memcpy(out, rifo->next_out, BYTES_PER_FRAME);
bucket_bit_clear(rifo, next_out_bucket);
}
/* advance by one frame */
rifo->next_out += BYTES_PER_FRAME;
if (rifo->next_out >= RIFO_BUF_END(rifo))
rifo->next_out -= sizeof(rifo->buf);
/* if we're empty we 'drag' last_in along to avoid overflows */
if (frame_rifo_depth(rifo) == 0)
rifo->last_in_fn += 1;
rifo->next_out_fn += 1;
return rc;
}
|
