diff options
Diffstat (limited to 'toxrtp/toxrtp_message.c')
-rw-r--r-- | toxrtp/toxrtp_message.c | 351 |
1 files changed, 351 insertions, 0 deletions
diff --git a/toxrtp/toxrtp_message.c b/toxrtp/toxrtp_message.c new file mode 100644 index 00000000..e7f1f2c0 --- /dev/null +++ b/toxrtp/toxrtp_message.c | |||
@@ -0,0 +1,351 @@ | |||
1 | /* rtp_message.c | ||
2 | * | ||
3 | * Rtp Message handler. It handles message/header parsing. | ||
4 | * Refer to RTP: A Transport Protocol for Real-Time Applications ( RFC 3550 ) for more info. !Red! | ||
5 | * | ||
6 | * | ||
7 | * Copyright (C) 2013 Tox project All Rights Reserved. | ||
8 | * | ||
9 | * This file is part of Tox. | ||
10 | * | ||
11 | * Tox is free software: you can redistribute it and/or modify | ||
12 | * it under the terms of the GNU General Public License as published by | ||
13 | * the Free Software Foundation, either version 3 of the License, or | ||
14 | * (at your option) any later version. | ||
15 | * | ||
16 | * Tox is distributed in the hope that it will be useful, | ||
17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
19 | * GNU General Public License for more details. | ||
20 | * | ||
21 | * You should have received a copy of the GNU General Public License | ||
22 | * along with Tox. If not, see <http://www.gnu.org/licenses/>. | ||
23 | * | ||
24 | */ | ||
25 | |||
26 | #ifdef HAVE_CONFIG_H | ||
27 | #include "config.h" | ||
28 | #endif /* HAVE_CONFIG_H */ | ||
29 | |||
30 | #include "toxrtp_message.h" | ||
31 | #include "toxrtp.h" | ||
32 | #include <stdio.h> | ||
33 | |||
34 | #ifdef _USE_ERRORS | ||
35 | #include "toxrtp_error_id.h" | ||
36 | #endif /* _USE_ERRORS */ | ||
37 | |||
38 | #include <assert.h> | ||
39 | |||
40 | /* Some defines */ | ||
41 | |||
42 | /* End of defines */ | ||
43 | |||
44 | void rtp_header_print (const rtp_header_t* _header) | ||
45 | { | ||
46 | printf("Header: \n" | ||
47 | "Version: %d\n" | ||
48 | "Padding: %d\n" | ||
49 | "Ext: %d\n" | ||
50 | "CC: %d\n" | ||
51 | "marker: %d\n" | ||
52 | "payload typ:%d\n\n" | ||
53 | "sequ num: %d\n" | ||
54 | "Timestamp: %d\n" | ||
55 | "SSrc: %d\n" | ||
56 | "CSrc: %d\n" | ||
57 | "Lenght: %d\n" | ||
58 | ,rtp_header_get_flag_version(_header) | ||
59 | ,rtp_header_get_flag_padding(_header) | ||
60 | ,rtp_header_get_flag_extension(_header) | ||
61 | ,rtp_header_get_flag_CSRC_count(_header) | ||
62 | ,rtp_header_get_setting_marker(_header) | ||
63 | ,rtp_header_get_setting_payload_type(_header) | ||
64 | ,_header->_sequence_number | ||
65 | ,_header->_timestamp | ||
66 | ,_header->_ssrc | ||
67 | ,_header->_csrc[0] | ||
68 | ,_header->_length | ||
69 | ); | ||
70 | } | ||
71 | |||
72 | rtp_header_t* rtp_extract_header ( const uint8_t* _payload, size_t _bytes ) | ||
73 | { | ||
74 | if ( !_payload ) { | ||
75 | t_perror ( RTP_ERROR_PAYLOAD_NULL ); | ||
76 | return NULL; | ||
77 | } | ||
78 | const uint8_t* _it = _payload; | ||
79 | |||
80 | rtp_header_t* _retu = calloc(sizeof(rtp_header_t), 1); | ||
81 | assert(_retu); | ||
82 | |||
83 | _retu->_flags = *_it; ++_it; | ||
84 | |||
85 | /* This indicates if the first 2 bytes are valid. | ||
86 | * Now it my happen that this is out of order but | ||
87 | * it cuts down chances of parsing some invalid value | ||
88 | */ | ||
89 | if ( rtp_header_get_flag_version(_retu) != RTP_VERSION ){ | ||
90 | printf("Invalid version: %d\n", rtp_header_get_flag_version(_retu)); | ||
91 | //assert(rtp_header_get_flag_version(_retu) == RTP_VERSION); | ||
92 | /* Deallocate */ | ||
93 | //DEALLOCATOR(_retu); | ||
94 | //return NULL; | ||
95 | } | ||
96 | |||
97 | /* | ||
98 | * Added a check for the size of the header little sooner so | ||
99 | * I don't need to parse the other stuff if it's bad | ||
100 | */ | ||
101 | uint8_t cc = rtp_header_get_flag_CSRC_count ( _retu ); | ||
102 | uint32_t _lenght = _MIN_HEADER_LENGTH + ( cc * 4 ); | ||
103 | |||
104 | if ( _bytes < _lenght ) { | ||
105 | t_perror ( RTP_ERROR_PAYLOAD_INVALID ); | ||
106 | return NULL; | ||
107 | } | ||
108 | |||
109 | if ( cc > 0 ) { | ||
110 | _retu->_csrc = calloc ( sizeof ( uint32_t ), cc ); | ||
111 | assert(_retu->_csrc); | ||
112 | |||
113 | } else { /* But this should not happen ever */ | ||
114 | t_perror ( RTP_ERROR_HEADER_PARSING ); | ||
115 | return NULL; | ||
116 | } | ||
117 | |||
118 | |||
119 | _retu->_marker_payload_t = *_it; ++_it; | ||
120 | _retu->_length = _lenght; | ||
121 | _retu->_sequence_number = ( ( uint16_t ) * _it << 8 ) | * ( _it + 1 ); | ||
122 | |||
123 | _it += 2; | ||
124 | |||
125 | _retu->_timestamp = ( ( uint32_t ) * _it << 24 ) | | ||
126 | ( ( uint32_t ) * ( _it + 1 ) << 16 ) | | ||
127 | ( ( uint32_t ) * ( _it + 2 ) << 8 ) | | ||
128 | ( * ( _it + 3 ) ) ; | ||
129 | |||
130 | _it += 4; | ||
131 | |||
132 | _retu->_ssrc = ( ( uint32_t ) * _it << 24 ) | | ||
133 | ( ( uint32_t ) * ( _it + 1 ) << 16 ) | | ||
134 | ( ( uint32_t ) * ( _it + 2 ) << 8 ) | | ||
135 | ( ( uint32_t ) * ( _it + 3 ) ) ; | ||
136 | |||
137 | |||
138 | size_t x; | ||
139 | for ( x = 0; x < cc; x++ ) { | ||
140 | _it += 4; | ||
141 | _retu->_csrc[x] = ( ( uint32_t ) * _it << 24 ) | | ||
142 | ( ( uint32_t ) * ( _it + 1 ) << 16 ) | | ||
143 | ( ( uint32_t ) * ( _it + 2 ) << 8 ) | | ||
144 | ( ( uint32_t ) * ( _it + 3 ) ) ; | ||
145 | } | ||
146 | |||
147 | return _retu; | ||
148 | } | ||
149 | |||
150 | rtp_ext_header_t* rtp_extract_ext_header ( const uint8_t* _payload, size_t _bytes ) | ||
151 | { | ||
152 | if ( !_payload ) { | ||
153 | t_perror ( RTP_ERROR_PAYLOAD_NULL ); | ||
154 | return NULL; | ||
155 | } | ||
156 | |||
157 | |||
158 | |||
159 | const uint8_t* _it = _payload; | ||
160 | |||
161 | rtp_ext_header_t* _retu = calloc(sizeof(rtp_ext_header_t), 1); | ||
162 | assert(_retu); | ||
163 | |||
164 | uint16_t _ext_len = ( ( uint16_t ) * _it << 8 ) | * ( _it + 1 ); _it += 2; | ||
165 | |||
166 | if ( _bytes < ( _ext_len * sizeof(uint32_t) ) ) { | ||
167 | t_perror ( RTP_ERROR_PAYLOAD_INVALID ); | ||
168 | return NULL; | ||
169 | } | ||
170 | |||
171 | _retu->_ext_len = _ext_len; | ||
172 | _retu->_ext_type = ( ( uint16_t ) * _it << 8 ) | * ( _it + 1 ); _it -= 2; | ||
173 | |||
174 | _retu->_hd_ext = calloc(sizeof(uint32_t), _ext_len); | ||
175 | assert(_retu->_hd_ext); | ||
176 | |||
177 | uint32_t* _hd_ext = _retu->_hd_ext; | ||
178 | size_t i; | ||
179 | for ( i = 0; i < _ext_len; i++ ) { | ||
180 | _it += 4; | ||
181 | _hd_ext[i] = ( ( uint32_t ) * _it << 24 ) | | ||
182 | ( ( uint32_t ) * ( _it + 1 ) << 16 ) | | ||
183 | ( ( uint32_t ) * ( _it + 2 ) << 8 ) | | ||
184 | ( ( uint32_t ) * ( _it + 3 ) ) ; | ||
185 | } | ||
186 | |||
187 | return _retu; | ||
188 | } | ||
189 | |||
190 | uint8_t* rtp_add_header ( rtp_header_t* _header, uint8_t* _payload ) | ||
191 | { | ||
192 | uint8_t cc = rtp_header_get_flag_CSRC_count ( _header ); | ||
193 | |||
194 | uint8_t* _it = _payload; | ||
195 | |||
196 | *_it = _header->_flags; ++_it; | ||
197 | *_it = _header->_marker_payload_t; ++_it; | ||
198 | |||
199 | *_it = ( _header->_sequence_number >> 8 ); ++_it; | ||
200 | *_it = ( _header->_sequence_number ); ++_it; | ||
201 | |||
202 | uint32_t _timestamp = _header->_timestamp; | ||
203 | *_it = ( _timestamp >> 24 ); ++_it; | ||
204 | *_it = ( _timestamp >> 16 ); ++_it; | ||
205 | *_it = ( _timestamp >> 8 ); ++_it; | ||
206 | *_it = ( _timestamp ); ++_it; | ||
207 | |||
208 | uint32_t _ssrc = _header->_ssrc; | ||
209 | *_it = ( _ssrc >> 24 ); ++_it; | ||
210 | *_it = ( _ssrc >> 16 ); ++_it; | ||
211 | *_it = ( _ssrc >> 8 ); ++_it; | ||
212 | *_it = ( _ssrc ); | ||
213 | |||
214 | uint32_t *_csrc = _header->_csrc; | ||
215 | size_t x; | ||
216 | for ( x = 0; x < cc; x++ ) { | ||
217 | ++_it; | ||
218 | *_it = ( _csrc[x] >> 24 ); ++_it; | ||
219 | *_it = ( _csrc[x] >> 16 ); ++_it; | ||
220 | *_it = ( _csrc[x] >> 8 ); ++_it; | ||
221 | *_it = ( _csrc[x] ); | ||
222 | } | ||
223 | |||
224 | return _it; | ||
225 | } | ||
226 | |||
227 | uint8_t* rtp_add_extention_header ( rtp_ext_header_t* _header, uint8_t* _payload ) | ||
228 | { | ||
229 | uint8_t* _it = _payload; | ||
230 | |||
231 | *_it = ( _header->_ext_len >> 8 ); _it++; | ||
232 | *_it = ( _header->_ext_len ); _it++; | ||
233 | |||
234 | *_it = ( _header->_ext_type >> 8 ); ++_it; | ||
235 | *_it = ( _header->_ext_type ); | ||
236 | |||
237 | size_t x; | ||
238 | |||
239 | uint32_t* _hd_ext = _header->_hd_ext; | ||
240 | for ( x = 0; x < _header->_ext_len; x++ ) { | ||
241 | ++_it; | ||
242 | *_it = ( _hd_ext[x] >> 24 ); ++_it; | ||
243 | *_it = ( _hd_ext[x] >> 16 ); ++_it; | ||
244 | *_it = ( _hd_ext[x] >> 8 ); ++_it; | ||
245 | *_it = ( _hd_ext[x] ); | ||
246 | } | ||
247 | |||
248 | return _it; | ||
249 | } | ||
250 | |||
251 | size_t rtp_header_get_size ( const rtp_header_t* _header ) | ||
252 | { | ||
253 | return ( 8 + ( rtp_header_get_flag_CSRC_count ( _header ) * 4 ) ); | ||
254 | } | ||
255 | /* Setting flags */ | ||
256 | |||
257 | void rtp_header_add_flag_version ( rtp_header_t* _header, uint32_t value ) | ||
258 | { | ||
259 | ( _header->_flags ) &= 0x3F; | ||
260 | ( _header->_flags ) |= ( ( ( value ) << 6 ) & 0xC0 ); | ||
261 | } | ||
262 | |||
263 | void rtp_header_add_flag_padding ( rtp_header_t* _header, uint32_t value ) | ||
264 | { | ||
265 | if ( value > 0 ) { | ||
266 | value = 1; /* It can only be 1 */ | ||
267 | } | ||
268 | |||
269 | ( _header->_flags ) &= 0xDF; | ||
270 | ( _header->_flags ) |= ( ( ( value ) << 5 ) & 0x20 ); | ||
271 | } | ||
272 | |||
273 | void rtp_header_add_flag_extension ( rtp_header_t* _header, uint32_t value ) | ||
274 | { | ||
275 | if ( value > 0 ) { | ||
276 | value = 1; /* It can only be 1 */ | ||
277 | } | ||
278 | |||
279 | ( _header->_flags ) &= 0xEF; | ||
280 | ( _header->_flags ) |= ( ( ( value ) << 4 ) & 0x10 ); | ||
281 | } | ||
282 | |||
283 | void rtp_header_add_flag_CSRC_count ( rtp_header_t* _header, uint32_t value ) | ||
284 | { | ||
285 | ( _header->_flags ) &= 0xF0; | ||
286 | ( _header->_flags ) |= ( ( value ) & 0x0F ); | ||
287 | } | ||
288 | |||
289 | void rtp_header_add_setting_marker ( rtp_header_t* _header, uint32_t value ) | ||
290 | { | ||
291 | if ( value > 1 ) | ||
292 | value = 1; | ||
293 | |||
294 | ( _header->_marker_payload_t ) &= 0x7F; | ||
295 | ( _header->_marker_payload_t ) |= ( ( ( value ) << 7 ) /*& 0x80 */ ); | ||
296 | } | ||
297 | |||
298 | void rtp_header_add_setting_payload ( rtp_header_t* _header, uint32_t value ) | ||
299 | { | ||
300 | if ( value > 127 ) | ||
301 | value = 127; /* Well set to maximum */ | ||
302 | |||
303 | ( _header->_marker_payload_t ) &= 0x80; | ||
304 | ( _header->_marker_payload_t ) |= ( ( value ) /* & 0x7F */ ); | ||
305 | } | ||
306 | |||
307 | /* Getting values from flags */ | ||
308 | uint8_t rtp_header_get_flag_version ( const rtp_header_t* _header ) | ||
309 | { | ||
310 | return ( _header->_flags & 0xd0 ) >> 6; | ||
311 | } | ||
312 | |||
313 | uint8_t rtp_header_get_flag_padding ( const rtp_header_t* _header ) | ||
314 | { | ||
315 | return ( _header->_flags & 0x20 ) >> 5; | ||
316 | } | ||
317 | |||
318 | uint8_t rtp_header_get_flag_extension ( const rtp_header_t* _header ) | ||
319 | { | ||
320 | return ( _header->_flags & 0x10 ) >> 4; | ||
321 | } | ||
322 | |||
323 | uint8_t rtp_header_get_flag_CSRC_count ( const rtp_header_t* _header ) | ||
324 | { | ||
325 | return ( _header->_flags & 0x0f ); | ||
326 | } | ||
327 | uint8_t rtp_header_get_setting_marker ( const rtp_header_t* _header ) | ||
328 | { | ||
329 | return ( _header->_marker_payload_t ) >> 7; | ||
330 | } | ||
331 | uint8_t rtp_header_get_setting_payload_type ( const rtp_header_t* _header ) | ||
332 | { | ||
333 | /* | ||
334 | uint8_t _retu; | ||
335 | |||
336 | if ( _header->_marker_payload_t >> 7 == 1 ) { | ||
337 | _header->_marker_payload_t ^= 0x80; | ||
338 | _retu = _header->_marker_payload_t; | ||
339 | _header->_marker_payload_t ^= 0x80; | ||
340 | } else { | ||
341 | _retu = _header->_marker_payload_t; | ||
342 | } | ||
343 | */ | ||
344 | /* return to start value | ||
345 | return _retu; */ | ||
346 | return _header->_marker_payload_t & 0x7f; | ||
347 | } | ||
348 | |||
349 | /* */ | ||
350 | |||
351 | |||