1 /*
2 * The oRTP library is an RTP (Realtime Transport Protocol - rfc3550) implementation with additional features.
3 * Copyright (C) 2017 Belledonne Communications SARL
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18 */
19
20 #ifdef HAVE_CONFIG_H
21 #include "ortp-config.h"
22 #endif
23
24 #include "ortp/str_utils.h"
25 #include "utils.h"
26
27
qinit(queue_t * q)28 void qinit(queue_t *q){
29 mblk_init(&q->_q_stopper);
30 q->_q_stopper.b_next=&q->_q_stopper;
31 q->_q_stopper.b_prev=&q->_q_stopper;
32 q->q_mcount=0;
33 }
34
mblk_init(mblk_t * mp)35 void mblk_init(mblk_t *mp)
36 {
37 memset(mp, 0, sizeof(mblk_t));
38 }
39
mblk_meta_copy(const mblk_t * source,mblk_t * dest)40 void mblk_meta_copy(const mblk_t *source, mblk_t *dest) {
41 dest->reserved1 = source->reserved1;
42 dest->reserved2 = source->reserved2;
43 memcpy(&dest->net_addr,&source->net_addr,source->net_addrlen);
44 dest->net_addrlen = source->net_addrlen;
45 #if defined(ORTP_TIMESTAMP)
46 dest->timestamp = source->timestamp;
47 #endif
48 dest->ttl_or_hl = source->ttl_or_hl;
49 }
50
datab_alloc(size_t size)51 dblk_t *datab_alloc(size_t size){
52 dblk_t *db;
53 size_t total_size=sizeof(dblk_t)+size;
54 db=(dblk_t *) ortp_malloc(total_size);
55 db->db_base=(uint8_t*)db+sizeof(dblk_t);
56 db->db_lim=db->db_base+size;
57 db->db_ref=1;
58 db->db_freefn=NULL; /* the buffer pointed by db_base must never be freed !*/
59 return db;
60 }
61
dblk_ref(dblk_t * d)62 void dblk_ref(dblk_t *d){
63 #if HAVE_STDATOMIC_H
64 atomic_fetch_add_explicit(&d->db_ref, 1, memory_order_relaxed);
65 #else
66 d->db_ref++;
67 #endif
68 }
69
dblk_unref(dblk_t * d)70 void dblk_unref(dblk_t *d){
71 #if HAVE_STDATOMIC_H
72 atomic_int previous_ref = atomic_fetch_sub_explicit(&d->db_ref, 1, memory_order_release);
73 #else
74 int previous_ref = d->db_ref--;
75 #endif
76 if (previous_ref == 1){
77 if (d->db_freefn!=NULL)
78 d->db_freefn(d->db_base);
79 ortp_free(d);
80 }
81 }
82
dblk_base(dblk_t * db)83 unsigned char * dblk_base(dblk_t *db) {
84 return db->db_base;
85 }
86
dblk_lim(dblk_t * db)87 unsigned char * dblk_lim(dblk_t *db) {
88 return db->db_lim;
89 }
90
dblk_ref_value(dblk_t * db)91 int dblk_ref_value(dblk_t *db) {
92 return (int)db->db_ref;
93 }
94
allocb(size_t size,int pri)95 mblk_t *allocb(size_t size, int pri)
96 {
97 mblk_t *mp;
98 dblk_t *datab;
99
100 mp=(mblk_t *) ortp_malloc0(sizeof(mblk_t));
101 datab=datab_alloc(size);
102
103 mp->b_datap=datab;
104 mp->b_rptr=mp->b_wptr=datab->db_base;
105 mp->b_next=mp->b_prev=mp->b_cont=NULL;
106 return mp;
107 }
108
esballoc(uint8_t * buf,size_t size,int pri,void (* freefn)(void *))109 mblk_t *esballoc(uint8_t *buf, size_t size, int pri, void (*freefn)(void*) )
110 {
111 mblk_t *mp;
112 dblk_t *datab;
113
114 mp=(mblk_t *) ortp_malloc0(sizeof(mblk_t));
115 datab=(dblk_t *) ortp_malloc(sizeof(dblk_t));
116
117 datab->db_base=buf;
118 datab->db_lim=buf+size;
119 datab->db_ref=1;
120 datab->db_freefn=freefn;
121
122 mp->b_datap=datab;
123 mp->b_rptr=mp->b_wptr=buf;
124 mp->b_next=mp->b_prev=mp->b_cont=NULL;
125 return mp;
126 }
127
128
freeb(mblk_t * mp)129 void freeb(mblk_t *mp)
130 {
131 return_if_fail(mp->b_datap!=NULL);
132 return_if_fail(mp->b_datap->db_base!=NULL);
133
134 dblk_unref(mp->b_datap);
135 ortp_free(mp);
136 }
137
freemsg(mblk_t * mp)138 void freemsg(mblk_t *mp)
139 {
140 mblk_t *tmp1,*tmp2;
141 tmp1=mp;
142 while(tmp1!=NULL)
143 {
144 tmp2=tmp1->b_cont;
145 freeb(tmp1);
146 tmp1=tmp2;
147 }
148 }
149
dupb(mblk_t * mp)150 mblk_t *dupb(mblk_t *mp)
151 {
152 mblk_t *newm;
153 return_val_if_fail(mp->b_datap!=NULL,NULL);
154 return_val_if_fail(mp->b_datap->db_base!=NULL,NULL);
155
156 dblk_ref(mp->b_datap);
157 newm=(mblk_t *) ortp_malloc0(sizeof(mblk_t));
158 mblk_meta_copy(mp, newm);
159 newm->b_datap=mp->b_datap;
160 newm->b_rptr=mp->b_rptr;
161 newm->b_wptr=mp->b_wptr;
162 return newm;
163 }
164
165 /* duplicates a complex mblk_t */
dupmsg(mblk_t * m)166 mblk_t *dupmsg(mblk_t* m)
167 {
168 mblk_t *newm=NULL,*mp,*prev;
169 prev=newm=dupb(m);
170 m=m->b_cont;
171 while (m!=NULL){
172 mp=dupb(m);
173 prev->b_cont=mp;
174 prev=mp;
175 m=m->b_cont;
176 }
177 return newm;
178 }
179
putq(queue_t * q,mblk_t * mp)180 void putq(queue_t *q,mblk_t *mp)
181 {
182 q->_q_stopper.b_prev->b_next=mp;
183 mp->b_prev=q->_q_stopper.b_prev;
184 mp->b_next=&q->_q_stopper;
185 q->_q_stopper.b_prev=mp;
186 q->q_mcount++;
187 }
188
getq(queue_t * q)189 mblk_t *getq(queue_t *q)
190 {
191 mblk_t *tmp;
192 tmp=q->_q_stopper.b_next;
193 if (tmp==&q->_q_stopper) return NULL;
194 q->_q_stopper.b_next=tmp->b_next;
195 tmp->b_next->b_prev=&q->_q_stopper;
196 tmp->b_prev=NULL;
197 tmp->b_next=NULL;
198 q->q_mcount--;
199 return tmp;
200 }
201
peekq(queue_t * q)202 mblk_t * peekq(queue_t *q){
203 mblk_t *tmp;
204 tmp=q->_q_stopper.b_next;
205 if (tmp==&q->_q_stopper) return NULL;
206 return tmp;
207 }
208
209 /* insert mp in q just before emp */
insq(queue_t * q,mblk_t * emp,mblk_t * mp)210 void insq(queue_t *q,mblk_t *emp, mblk_t *mp)
211 {
212 if (emp==NULL){
213 putq(q,mp);
214 return;
215 }
216 q->q_mcount++;
217 emp->b_prev->b_next=mp;
218 mp->b_prev=emp->b_prev;
219 emp->b_prev=mp;
220 mp->b_next=emp;
221 }
222
remq(queue_t * q,mblk_t * mp)223 void remq(queue_t *q, mblk_t *mp){
224 q->q_mcount--;
225 mp->b_prev->b_next=mp->b_next;
226 mp->b_next->b_prev=mp->b_prev;
227 mp->b_next=NULL;
228 mp->b_prev=NULL;
229 }
230
231 /* remove and free all messages in the q */
flushq(queue_t * q,int how)232 void flushq(queue_t *q, int how)
233 {
234 mblk_t *mp;
235
236 while ((mp=getq(q))!=NULL)
237 {
238 freemsg(mp);
239 }
240 }
241
msgdsize(const mblk_t * mp)242 size_t msgdsize(const mblk_t *mp)
243 {
244 size_t msgsize=0;
245 while(mp!=NULL){
246 msgsize+=(size_t) (mp->b_wptr-mp->b_rptr);
247 mp=mp->b_cont;
248 }
249 return msgsize;
250 }
251
msgpullup(mblk_t * mp,size_t len)252 void msgpullup(mblk_t *mp,size_t len)
253 {
254 mblk_t *firstm=mp;
255 dblk_t *db;
256 size_t wlen=0;
257
258 if (mp->b_cont==NULL && len==(size_t)-1) return; /*nothing to do, message is not fragmented */
259
260 if (len==(size_t)-1) len=msgdsize(mp);
261 db=datab_alloc(len);
262 while(wlen<len && mp!=NULL){
263 int remain=(int)(len-wlen);
264 int mlen=(int)(mp->b_wptr-mp->b_rptr);
265 if (mlen<=remain){
266 memcpy(&db->db_base[wlen],mp->b_rptr,mlen);
267 wlen+=mlen;
268 mp=mp->b_cont;
269 }else{
270 memcpy(&db->db_base[wlen],mp->b_rptr,remain);
271 wlen+=remain;
272 }
273 }
274 /*set firstm to point to the new datab */
275 freemsg(firstm->b_cont);
276 firstm->b_cont=NULL;
277 dblk_unref(firstm->b_datap);
278 firstm->b_datap=db;
279 firstm->b_rptr=db->db_base;
280 firstm->b_wptr=firstm->b_rptr+wlen;
281 }
282
283
copyb(const mblk_t * mp)284 mblk_t *copyb(const mblk_t *mp)
285 {
286 mblk_t *newm;
287 int len=(int) (mp->b_wptr-mp->b_rptr);
288 newm=allocb(len,BPRI_MED);
289 memcpy(newm->b_wptr,mp->b_rptr,len);
290 newm->b_wptr+=len;
291 memcpy(&newm->recv_addr,&mp->recv_addr,sizeof(newm->recv_addr));
292 return newm;
293 }
294
copymsg(const mblk_t * mp)295 mblk_t *copymsg(const mblk_t *mp)
296 {
297 mblk_t *newm=0,*m;
298 m=newm=copyb(mp);
299 mp=mp->b_cont;
300 while(mp!=NULL){
301 m->b_cont=copyb(mp);
302 m=m->b_cont;
303 mp=mp->b_cont;
304 }
305 return newm;
306 }
307
appendb(mblk_t * mp,const char * data,size_t size,bool_t pad)308 mblk_t * appendb(mblk_t *mp, const char *data, size_t size, bool_t pad){
309 size_t padcnt=0;
310 size_t i;
311 if (pad){
312 padcnt = (size_t)(4 - ((((intptr_t)mp->b_wptr) + size) % 4)) % 4;
313 }
314 if ((mp->b_wptr + size +padcnt) > mp->b_datap->db_lim){
315 /* buffer is not large enough: append a new block (with the same size ?)*/
316 size_t plen=(size_t)((char*)mp->b_datap->db_lim - (char*) mp->b_datap->db_base);
317 mp->b_cont=allocb(MAX(plen,size),0);
318 mp=mp->b_cont;
319 }
320 if (size) memcpy(mp->b_wptr,data,size);
321 mp->b_wptr+=size;
322 for (i=0;i<padcnt;i++){
323 mp->b_wptr[0]=0;
324 mp->b_wptr++;
325 }
326 return mp;
327 }
328
msgappend(mblk_t * mp,const char * data,size_t size,bool_t pad)329 void msgappend(mblk_t *mp, const char *data, size_t size, bool_t pad){
330 while(mp->b_cont!=NULL) mp=mp->b_cont;
331 appendb(mp,data,size,pad);
332 }
333
concatb(mblk_t * mp,mblk_t * newm)334 mblk_t *concatb(mblk_t *mp, mblk_t *newm){
335 while (mp->b_cont!=NULL) mp=mp->b_cont;
336 mp->b_cont=newm;
337 while(newm->b_cont!=NULL) newm=newm->b_cont;
338 return newm;
339 }
340
msgb_allocator_init(msgb_allocator_t * a)341 void msgb_allocator_init(msgb_allocator_t *a){
342 qinit(&a->q);
343 }
344
msgb_allocator_alloc(msgb_allocator_t * a,size_t size)345 mblk_t *msgb_allocator_alloc(msgb_allocator_t *a, size_t size){
346 queue_t *q=&a->q;
347 mblk_t *m,*found=NULL;
348
349 /*lookup for an unused msgb (data block with ref count ==1)*/
350 for(m=qbegin(q);!qend(q,m);m=qnext(q,m)){
351 if ((m->b_datap->db_ref == 1) && ((size_t)(m->b_datap->db_lim - m->b_datap->db_base) >= size)){
352 found=m;
353 break;
354 }
355 }
356 if (found==NULL){
357 found=allocb(size,0);
358 putq(q,found);
359 }
360 return dupb(found);
361 }
362
msgb_allocator_uninit(msgb_allocator_t * a)363 void msgb_allocator_uninit(msgb_allocator_t *a){
364 flushq(&a->q,-1);
365 }
366
ortp_recvaddr_to_sockaddr(ortp_recv_addr_t * recvaddr,struct sockaddr * addr,socklen_t * socklen)367 void ortp_recvaddr_to_sockaddr(ortp_recv_addr_t *recvaddr, struct sockaddr *addr, socklen_t *socklen) {
368 if (recvaddr->family == AF_INET) {
369 struct sockaddr_in *addr_in = (struct sockaddr_in *)addr;
370 addr_in->sin_family = AF_INET;
371 addr_in->sin_addr = recvaddr->addr.ipi_addr;
372 addr_in->sin_port = recvaddr->port;
373 *socklen = sizeof(struct sockaddr_in);
374 } else if (recvaddr->family == AF_INET6) {
375 struct sockaddr_in6 *addr_in6 = (struct sockaddr_in6 *)addr;
376 addr_in6->sin6_family = AF_INET6;
377 addr_in6->sin6_addr = recvaddr->addr.ipi6_addr;
378 addr_in6->sin6_port = recvaddr->port;
379 *socklen = sizeof(struct sockaddr_in6);
380 }
381 }
382