1 |
pcg |
1.1 |
/* |
2 |
|
|
ROHC Project 2003 at Lulea University of Technology, Sweden. |
3 |
|
|
Authors: Andreas Vernersson <andver-8@student.luth.se> |
4 |
|
|
Daniel Pettersson <danpet-7@student.luth.se> |
5 |
|
|
Erik Soderstrom <soderstrom@yahoo.com> |
6 |
|
|
Fredrik Lindstrom <frelin-9@student.luth.se> |
7 |
|
|
Johan Stenmark <johste-8@student.luth.se> |
8 |
|
|
Martin Juhlin <juhlin@users.sourceforge.net> |
9 |
|
|
Mikael Larsson <larmik-9@student.luth.se> |
10 |
|
|
Robert Maxe <robmax-1@student.luth.se> |
11 |
|
|
|
12 |
|
|
Copyright (C) 2003 Andreas Vernersson, Daniel Pettersson, |
13 |
|
|
Erik Soderström, Fredrik Lindström, Johan Stenmark, |
14 |
|
|
Martin Juhlin, Mikael Larsson, Robert Maxe. |
15 |
|
|
|
16 |
|
|
This program is free software; you can redistribute it and/or modify |
17 |
|
|
it under the terms of the GNU General Public License as published by |
18 |
|
|
the Free Software Foundation; either version 2 of the License, or |
19 |
|
|
(at your option) any later version. |
20 |
|
|
|
21 |
|
|
This program is distributed in the hope that it will be useful, |
22 |
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
23 |
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
24 |
|
|
GNU General Public License for more details. |
25 |
|
|
|
26 |
|
|
You should have received a copy of the GNU General Public License |
27 |
|
|
along with this program; if not, write to the Free Software |
28 |
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
29 |
|
|
*/ |
30 |
|
|
|
31 |
|
|
// The UDP-lite profile in the decompressor |
32 |
|
|
|
33 |
|
|
#include <asm/byteorder.h> |
34 |
|
|
|
35 |
|
|
#include "rohc.h" |
36 |
|
|
#include "decomp.h" |
37 |
|
|
|
38 |
|
|
#include "d_udp_lite.h" |
39 |
|
|
#include "d_util.h" |
40 |
|
|
#include "comp.h" |
41 |
|
|
#include "c_util.h" |
42 |
|
|
|
43 |
|
|
|
44 |
|
|
//--------------------------------------------------------- Helpfunctions |
45 |
|
|
|
46 |
|
|
/* Decode a UO-0 package */ |
47 |
|
|
static int udp_lite_decode_uo0( |
48 |
|
|
struct sd_rohc * state, |
49 |
|
|
struct sd_context * context, |
50 |
|
|
unsigned char * head, |
51 |
|
|
unsigned char * src, |
52 |
|
|
unsigned char * dest, |
53 |
|
|
int payload_size |
54 |
|
|
); |
55 |
|
|
|
56 |
|
|
/* Decode a UO-1 package */ |
57 |
|
|
static int udp_lite_decode_uo1( |
58 |
|
|
struct sd_rohc * state, |
59 |
|
|
struct sd_context * context, |
60 |
|
|
unsigned char * head, |
61 |
|
|
unsigned char * src2, |
62 |
|
|
unsigned char * dest, |
63 |
|
|
int payload_size |
64 |
|
|
); |
65 |
|
|
|
66 |
|
|
/* Decode a UO-2 package */ |
67 |
|
|
static int udp_lite_decode_uor2( |
68 |
|
|
struct sd_rohc * state, |
69 |
|
|
struct sd_context * context, |
70 |
|
|
unsigned char * head, |
71 |
|
|
unsigned char * src2, |
72 |
|
|
unsigned char * dest, |
73 |
|
|
int payload_size |
74 |
|
|
); |
75 |
|
|
|
76 |
|
|
/* Decode a IR-Dyn package */ |
77 |
|
|
static int udp_lite_decode_irdyn( |
78 |
|
|
struct sd_rohc * state, |
79 |
|
|
struct sd_context * context, |
80 |
|
|
unsigned char * head, |
81 |
|
|
unsigned char * src2, |
82 |
|
|
unsigned char * dest, |
83 |
|
|
int payload_size |
84 |
|
|
); |
85 |
|
|
|
86 |
|
|
/* Decode extention 0 */ |
87 |
|
|
static int udp_lite_decode_extention0(unsigned char * src, int * sn, int * ip_id); |
88 |
|
|
|
89 |
|
|
/* Decode extention 1 */ |
90 |
|
|
static int udp_lite_decode_extention1(unsigned char * src, int * sn, int * ip_id); |
91 |
|
|
|
92 |
|
|
/* Decode extention 2 */ |
93 |
|
|
static int udp_lite_decode_extention2(unsigned char * src, int * sn, int * ip_id, int * ip_id2); |
94 |
|
|
|
95 |
|
|
/* Decode extention 3 |
96 |
|
|
* - Updates random fields in the s_udp_change |
97 |
|
|
* - May update the SN value with 8 lower bits. sn_size is then changed |
98 |
|
|
*/ |
99 |
|
|
static int udp_lite_decode_extention3( |
100 |
|
|
unsigned char * src, |
101 |
|
|
struct sd_rohc * state, |
102 |
|
|
struct sd_context * context, |
103 |
|
|
int * sn, |
104 |
|
|
int * sn_size, |
105 |
|
|
int * ip_id_changed, |
106 |
|
|
int * id2_updated |
107 |
|
|
); |
108 |
|
|
|
109 |
|
|
// Make a copy of the "active"-struct to the "last"-struct. |
110 |
|
|
static void udp_lite_syncronize(struct s_udp_lite_profile_data *); |
111 |
|
|
|
112 |
|
|
// Decode package type |
113 |
|
|
static int udp_lite_package_type(const unsigned char * p); |
114 |
|
|
|
115 |
|
|
/* Deceide the extention type */ |
116 |
|
|
static int udp_lite_extention_type(const unsigned char * p); |
117 |
|
|
|
118 |
|
|
/* Write a uncompressed IP v4 header */ |
119 |
|
|
static void udp_lite_write_uncompressed_ip4( |
120 |
|
|
struct s_udp_lite_change * active1, |
121 |
|
|
int ip_id, |
122 |
|
|
unsigned char * dest, |
123 |
|
|
int payload_size |
124 |
|
|
); |
125 |
|
|
|
126 |
|
|
/* Write a uncompressed UDP header */ |
127 |
|
|
static void udp_lite_write_uncompressed_udp( |
128 |
|
|
struct s_udp_lite_profile_data *pro, |
129 |
|
|
int checksum, |
130 |
|
|
int coverage, |
131 |
|
|
unsigned char * dest, |
132 |
|
|
int payload_size |
133 |
|
|
); |
134 |
|
|
|
135 |
|
|
/* Copy the last-struct to active-struct. */ |
136 |
|
|
static void udp_lite_sync_on_failure(struct s_udp_lite_profile_data * pro); |
137 |
|
|
|
138 |
|
|
/* Decode inner IP flags and fields. Storage the values |
139 |
|
|
* in a IP-head struct. |
140 |
|
|
*/ |
141 |
|
|
static int udp_lite_decode_inner_header_flags( |
142 |
|
|
unsigned char * flags, |
143 |
|
|
unsigned char * fields, |
144 |
|
|
struct iphdr * ip, |
145 |
|
|
int * rnd, int * nbo); |
146 |
|
|
|
147 |
|
|
/* Decode outer IP flags and fields. Storage the values |
148 |
|
|
* in a IP-head struct. |
149 |
|
|
*/ |
150 |
|
|
static int udp_lite_decode_outer_header_flags( |
151 |
|
|
unsigned char * flags, |
152 |
|
|
unsigned char * fields, |
153 |
|
|
struct iphdr * ip, |
154 |
|
|
int * rnd, int * nbo, |
155 |
|
|
int * updated_ip); |
156 |
|
|
|
157 |
|
|
/* A functions that decode uo0 and uo1 packets |
158 |
|
|
* |
159 |
|
|
*/ |
160 |
|
|
static int udp_lite_do_decode_uo0_and_uo1( |
161 |
|
|
struct sd_context * context, |
162 |
|
|
unsigned char * src, |
163 |
|
|
unsigned char * dest, |
164 |
|
|
int * payload_size, |
165 |
|
|
int sn_bits, |
166 |
|
|
int number_of_sn_bits, |
167 |
|
|
int * id, |
168 |
|
|
int number_of_id_bits, |
169 |
|
|
int * id2, |
170 |
|
|
int * sn, |
171 |
|
|
int * calc_crc |
172 |
|
|
); |
173 |
|
|
|
174 |
|
|
/* A functions that decode uor2 packets |
175 |
|
|
* |
176 |
|
|
*/ |
177 |
|
|
static int udp_lite_do_decode_uor2( |
178 |
|
|
struct sd_rohc * state, |
179 |
|
|
struct sd_context * context, |
180 |
|
|
unsigned char * src2, |
181 |
|
|
unsigned char * dest, |
182 |
|
|
int * payload_size, |
183 |
|
|
int * id, |
184 |
|
|
int * id2, |
185 |
|
|
int * sn, |
186 |
|
|
int * sn_size, |
187 |
|
|
int sn_bits, |
188 |
|
|
int ext, |
189 |
|
|
int * calc_crc |
190 |
|
|
); |
191 |
|
|
|
192 |
|
|
/* Try to repair the packet with an other SN value |
193 |
|
|
* |
194 |
|
|
*/ |
195 |
|
|
static int udp_lite_crc_failure_action( |
196 |
|
|
struct sd_rohc * state, |
197 |
|
|
struct sd_context * context, |
198 |
|
|
unsigned char * src, |
199 |
|
|
unsigned char * dest, |
200 |
|
|
int sn_size, |
201 |
|
|
int * sn_curr1, |
202 |
|
|
int sn_bits, |
203 |
|
|
int * payload_size, |
204 |
|
|
int * id, |
205 |
|
|
int id_size, |
206 |
|
|
int * id2, |
207 |
|
|
int * calc_crc, |
208 |
|
|
int * real_crc, |
209 |
|
|
int ext |
210 |
|
|
); |
211 |
|
|
|
212 |
|
|
|
213 |
|
|
static void udp_lite_update_packet_time(struct s_udp_lite_profile_data * pro); |
214 |
|
|
//---------------------------------------------------------- Code |
215 |
|
|
|
216 |
|
|
// Allocate the profile data |
217 |
|
|
void * udp_lite_allocate_decode_data(void) |
218 |
|
|
{ |
219 |
|
|
struct s_udp_lite_profile_data * data; |
220 |
|
|
struct s_udp_lite_change * ch; |
221 |
|
|
void * p = kmalloc(sizeof(struct s_udp_lite_profile_data) + 4 * sizeof(struct s_udp_lite_change), |
222 |
|
|
GFP_ATOMIC); |
223 |
|
|
|
224 |
|
|
if (p==NULL) { |
225 |
|
|
rohc_debugf(0, "udp_lite_allocate_decode_data(): no mem for udp-lite profile data\n"); |
226 |
|
|
return NULL; |
227 |
|
|
} |
228 |
|
|
|
229 |
|
|
data = (struct s_udp_lite_profile_data *)p; |
230 |
|
|
ch = (struct s_udp_lite_change *)(data + 1); |
231 |
|
|
|
232 |
|
|
memset(p, 0, sizeof(struct s_udp_lite_profile_data) + |
233 |
|
|
4 * sizeof(struct s_udp_lite_change) ); |
234 |
|
|
|
235 |
|
|
data->last1 = ch; ch++; |
236 |
|
|
data->active1 = ch; ch++; |
237 |
|
|
data->last2 = ch; ch++; |
238 |
|
|
data->active2 = ch; |
239 |
|
|
|
240 |
|
|
return p; |
241 |
|
|
} |
242 |
|
|
|
243 |
|
|
// Deallocate the profile data |
244 |
|
|
void udp_lite_free_decode_data(void * p) |
245 |
|
|
{ |
246 |
|
|
kfree(p); |
247 |
|
|
} |
248 |
|
|
|
249 |
|
|
// Decode an IR-package and initalize context |
250 |
|
|
int udp_lite_decode_ir( |
251 |
|
|
struct sd_rohc * state, |
252 |
|
|
struct sd_context * context, |
253 |
|
|
unsigned char * src, |
254 |
|
|
int copy_size, |
255 |
|
|
int dynamic_present, |
256 |
|
|
unsigned char * dest |
257 |
|
|
) |
258 |
|
|
{ |
259 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
260 |
|
|
struct s_udp_lite_change * active1 = pro->active1; |
261 |
|
|
struct s_udp_lite_change * active2 = pro->active2; |
262 |
|
|
unsigned char * s = src; |
263 |
|
|
unsigned char * d = dest; |
264 |
|
|
int size, sn; |
265 |
|
|
int udp_length = 0; |
266 |
|
|
pro->current_packet_time = get_microseconds(); |
267 |
|
|
|
268 |
|
|
// Ipv4 static |
269 |
|
|
size = d_decode_static_ip4(s, &active1->ip); |
270 |
|
|
|
271 |
|
|
if (size == -1) |
272 |
|
|
return ROHC_ERROR; |
273 |
|
|
|
274 |
|
|
s += size; copy_size -= size; |
275 |
|
|
|
276 |
|
|
// multiple ip-headers |
277 |
|
|
if (active1->ip.protocol == PROTOCOL_IP_IN_IP){ |
278 |
|
|
pro->multiple_ip = 1; |
279 |
|
|
rohc_debugf(1, "Multiple IP header\n"); |
280 |
|
|
}else{ |
281 |
|
|
pro->multiple_ip = 0; |
282 |
|
|
} |
283 |
|
|
|
284 |
|
|
// if multiple ip-header |
285 |
|
|
if(pro->multiple_ip){ |
286 |
|
|
size = d_decode_static_ip4(s, &active2->ip); |
287 |
|
|
s += size; copy_size -= size; |
288 |
|
|
if (size == -1) |
289 |
|
|
return ROHC_ERROR; |
290 |
|
|
} |
291 |
|
|
|
292 |
|
|
// Static udp |
293 |
|
|
size = d_decode_static_udp(s, &active1->udp); |
294 |
|
|
s += size; copy_size -= size; |
295 |
|
|
|
296 |
|
|
// Dynamic field |
297 |
|
|
if (dynamic_present) { |
298 |
|
|
// Reset the correction-counter |
299 |
|
|
pro->counter = 0; |
300 |
|
|
|
301 |
|
|
size = d_decode_dynamic_ip4(s, &active1->ip, &active1->rnd, &active1->nbo); |
302 |
|
|
s += size; copy_size -= (size + 2); |
303 |
|
|
|
304 |
|
|
// if multiple ip-header |
305 |
|
|
if(pro->multiple_ip){ |
306 |
|
|
size = d_decode_dynamic_ip4(s, &active2->ip, &active2->rnd, &active2->nbo); |
307 |
|
|
s += size; copy_size -= size; |
308 |
|
|
} |
309 |
|
|
|
310 |
|
|
size = d_decode_dynamic_udp_lite(s, &active1->udp); |
311 |
|
|
copy_size -= size; s += size; |
312 |
|
|
|
313 |
|
|
udp_length = copy_size + sizeof(struct udphdr); |
314 |
|
|
|
315 |
|
|
// set the checksum coverage flags |
316 |
|
|
pro->coverage_present = (udp_length != ntohs(active1->udp.len)); |
317 |
|
|
pro->coverage_inferred = (udp_length == ntohs(active1->udp.len)); // |
318 |
|
|
|
319 |
|
|
//debug |
320 |
|
|
rohc_debugf(2,"(decomp) coverage present %d\n",pro->coverage_present); |
321 |
|
|
rohc_debugf(2,"(decomp) coverage inferred %d\n",pro->coverage_inferred); |
322 |
|
|
|
323 |
|
|
// Get and set SN |
324 |
|
|
sn = ntohs(* ((__u16 *)s)); |
325 |
|
|
d_lsb_init(&pro->sn, sn, -1); |
326 |
|
|
d_ip_id_init(&pro->ip_id1, ntohs(active1->ip.id), sn); |
327 |
|
|
s += 2; |
328 |
|
|
|
329 |
|
|
// If multiple-ip-header |
330 |
|
|
if(pro->multiple_ip){ |
331 |
|
|
d_ip_id_init(&pro->ip_id2, ntohs(active2->ip.id), sn); |
332 |
|
|
} |
333 |
|
|
|
334 |
|
|
context->state = ROHC_FULL_CONTEXT; |
335 |
|
|
|
336 |
|
|
} else if (context->state != ROHC_FULL_CONTEXT) { |
337 |
|
|
// in static/no context and not get dynamic part |
338 |
|
|
return ROHC_ERROR; |
339 |
|
|
} |
340 |
|
|
|
341 |
|
|
// Header write |
342 |
|
|
if (pro->multiple_ip){ |
343 |
|
|
udp_lite_write_uncompressed_ip4(active1, ntohs(active1->ip.id), d, copy_size+sizeof(struct iphdr)+sizeof(struct udphdr)); |
344 |
|
|
d += sizeof(struct iphdr); |
345 |
|
|
udp_lite_write_uncompressed_ip4(active2, ntohs(active2->ip.id), d, copy_size + sizeof(struct udphdr)); |
346 |
|
|
|
347 |
|
|
}else{ |
348 |
|
|
udp_lite_write_uncompressed_ip4(active1, ntohs(active1->ip.id), d, copy_size + sizeof(struct udphdr)); |
349 |
|
|
} |
350 |
|
|
d += sizeof(struct iphdr); |
351 |
|
|
|
352 |
|
|
// write udp-header |
353 |
|
|
memcpy(d, &active1->udp, sizeof(struct udphdr)); |
354 |
|
|
|
355 |
|
|
d += sizeof(struct udphdr); |
356 |
|
|
|
357 |
|
|
// Syncronize the change structs |
358 |
|
|
udp_lite_syncronize(pro); |
359 |
|
|
|
360 |
|
|
// Update the inter-packet variable |
361 |
|
|
udp_lite_update_packet_time(pro); |
362 |
|
|
|
363 |
|
|
// Copy payload |
364 |
|
|
if (copy_size == 0) return ROHC_OK_NO_DATA; |
365 |
|
|
|
366 |
|
|
memcpy(d, s, copy_size); |
367 |
|
|
|
368 |
|
|
// Statistics: |
369 |
|
|
context->header_compressed_size += s - src; |
370 |
|
|
c_add_wlsb(context->header_16_compressed, 0,0, s - src); |
371 |
|
|
context->header_uncompressed_size += (pro->multiple_ip+1)*sizeof(struct iphdr) + sizeof(struct udphdr); |
372 |
|
|
c_add_wlsb(context->header_16_uncompressed, 0, 0, (pro->multiple_ip+1)*sizeof(struct iphdr) + sizeof(struct udphdr)); |
373 |
|
|
|
374 |
|
|
return copy_size + (pro->multiple_ip + 1) * sizeof(struct iphdr) + sizeof(struct udphdr); |
375 |
|
|
} |
376 |
|
|
|
377 |
|
|
// Calculate the size of data in an IR-package. |
378 |
|
|
// return : size or zero. |
379 |
|
|
int udp_lite_detect_ir_size(unsigned char * first_byte, int second_byte_add) |
380 |
|
|
{ |
381 |
|
|
int ret = 14; |
382 |
|
|
int d = GET_BIT_0(first_byte); |
383 |
|
|
if (d) ret += 9 + 2; |
384 |
|
|
if (first_byte[second_byte_add + 2] != 0x40) return 0; |
385 |
|
|
if (first_byte[second_byte_add + 3] == PROTOCOL_IP_IN_IP){ |
386 |
|
|
ret += 10; |
387 |
|
|
if (d) ret += 5; |
388 |
|
|
if (first_byte[second_byte_add + 12] != 0x40) return 0; |
389 |
|
|
} |
390 |
|
|
return ret; |
391 |
|
|
} |
392 |
|
|
|
393 |
|
|
// Calculate the size of data in an IR-package. |
394 |
|
|
// return : size or zero. |
395 |
|
|
int udp_lite_detect_ir_dyn_size(unsigned char * first_byte, struct sd_context *c) |
396 |
|
|
{ |
397 |
|
|
struct s_udp_lite_profile_data * pro = c->data; |
398 |
|
|
if (pro->active1->ip.protocol == PROTOCOL_IP_IN_IP) return 16; |
399 |
|
|
return 11; |
400 |
|
|
} |
401 |
|
|
|
402 |
|
|
// Decode all package except IR-package. |
403 |
|
|
int udp_lite_decode( |
404 |
|
|
struct sd_rohc * state, |
405 |
|
|
struct sd_context * context, |
406 |
|
|
unsigned char * src, |
407 |
|
|
int size, |
408 |
|
|
int second_byte, |
409 |
|
|
unsigned char * dest |
410 |
|
|
) |
411 |
|
|
{ |
412 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
413 |
|
|
// ---- DEBUG ---- |
414 |
|
|
int i; unsigned char * p; |
415 |
|
|
|
416 |
|
|
struct s_udp_lite_change * active1 = pro->active1; |
417 |
|
|
struct s_udp_lite_change * active2 = pro->active2; |
418 |
|
|
struct s_udp_lite_change * last1 = pro->last1; |
419 |
|
|
struct s_udp_lite_change * last2 = pro->last2; |
420 |
|
|
|
421 |
|
|
pro->current_packet_time = get_microseconds(); |
422 |
|
|
|
423 |
|
|
rohc_debugf(2,"(udp_lite_decode) nbo = %d rnd = %d\n", last1->nbo, last1->rnd); |
424 |
|
|
if (pro->multiple_ip){ |
425 |
|
|
rohc_debugf(2,"Multiple ip-header\n"); |
426 |
|
|
rohc_debugf(2,"(udp_lite_decode) nbo2 = %d rnd2 = %d\n", last2->nbo, last2->rnd); |
427 |
|
|
} |
428 |
|
|
if (memcmp(active1, last1, sizeof(struct s_udp_lite_change)) != 0) { |
429 |
|
|
rohc_debugf(0,"(udp_lite_decode) last1 and active1 struct is not synchronized\n"); |
430 |
|
|
p = (unsigned char *)last1; |
431 |
|
|
for (i = 0; i < sizeof(struct s_udp_lite_change); i++) { |
432 |
|
|
printk("%2x ", p[i]); |
433 |
|
|
} |
434 |
|
|
printk("\nvs\n"); |
435 |
|
|
p = (unsigned char *)active1; |
436 |
|
|
for (i = 0; i < sizeof(struct s_udp_lite_change); i++) { |
437 |
|
|
printk("%2x ", p[i]); |
438 |
|
|
} |
439 |
|
|
printk("\n"); |
440 |
|
|
} |
441 |
|
|
if (memcmp(active2, last2, sizeof(struct s_udp_lite_change)) != 0) { |
442 |
|
|
rohc_debugf(0,"(udp_lite_decode) last2 and active2 struct is not synchronized\n"); |
443 |
|
|
p = (unsigned char *)last2; |
444 |
|
|
for (i = 0; i < sizeof(struct s_udp_lite_change); i++) { |
445 |
|
|
printk("%2x ", p[i]); |
446 |
|
|
} |
447 |
|
|
printk("\nvs\n"); |
448 |
|
|
p = (unsigned char *)active2; |
449 |
|
|
for (i = 0; i < sizeof(struct s_udp_lite_change); i++) { |
450 |
|
|
printk("%2x ", p[i]); |
451 |
|
|
} |
452 |
|
|
printk("\n"); |
453 |
|
|
} |
454 |
|
|
// ---- DEBUG ---- |
455 |
|
|
|
456 |
|
|
if (context->state == ROHC_NO_CONTEXT) return ROHC_ERROR; |
457 |
|
|
|
458 |
|
|
pro->ce_packet = 0; |
459 |
|
|
if (*src == 0xf9){ // CE() |
460 |
|
|
rohc_debugf(2,"(udp_lite) CE()\n"); |
461 |
|
|
pro->ce_packet = PACKAGE_CE; |
462 |
|
|
}else if(*src == 0xfa){ // CE(ON) |
463 |
|
|
rohc_debugf(2,"(udp_lite) CE(ON)\n"); |
464 |
|
|
pro->coverage_present = 1; |
465 |
|
|
pro->ce_packet = PACKAGE_CE; |
466 |
|
|
}else if(*src == 0xfb){ // CE(OFF) |
467 |
|
|
rohc_debugf(2,"(udp_lite) CE(OFF)\n"); |
468 |
|
|
pro->coverage_present = 0; |
469 |
|
|
pro->ce_packet = PACKAGE_CE_OFF; |
470 |
|
|
} |
471 |
|
|
|
472 |
|
|
if(pro->ce_packet){ |
473 |
|
|
src += second_byte; |
474 |
|
|
size--; |
475 |
|
|
} |
476 |
|
|
|
477 |
|
|
switch(udp_lite_package_type(src)) { |
478 |
|
|
case PACKAGE_UO_0: |
479 |
|
|
pro->package_type = PACKAGE_UO_0; |
480 |
|
|
if (context->state == ROHC_STATIC_CONTEXT) return ROHC_ERROR; |
481 |
|
|
return udp_lite_decode_uo0(state, context, src, (pro->ce_packet ? src + 1 : src + second_byte), |
482 |
|
|
dest, size - second_byte); |
483 |
|
|
case PACKAGE_UO_1: |
484 |
|
|
pro->package_type = PACKAGE_UO_1; |
485 |
|
|
if (context->state == ROHC_STATIC_CONTEXT) return ROHC_ERROR; |
486 |
|
|
return udp_lite_decode_uo1(state, context, src, (pro->ce_packet ? src + 1 : src + second_byte), |
487 |
|
|
dest, size - second_byte); |
488 |
|
|
case PACKAGE_UOR_2: |
489 |
|
|
pro->package_type = PACKAGE_UOR_2; |
490 |
|
|
return udp_lite_decode_uor2(state, context, src, (pro->ce_packet ? src + 1 : src + second_byte), |
491 |
|
|
dest, size - second_byte); |
492 |
|
|
case PACKAGE_IR_DYN: |
493 |
|
|
return udp_lite_decode_irdyn(state, context, src, (pro->ce_packet ? src + 1 : src + second_byte), |
494 |
|
|
dest, size - second_byte); |
495 |
|
|
default: |
496 |
|
|
rohc_debugf(0,"(udp) Unknown package.\n"); |
497 |
|
|
return ROHC_ERROR; |
498 |
|
|
} |
499 |
|
|
} |
500 |
|
|
/* get the last SN reference value */ |
501 |
|
|
static int udp_lite_get_sn(struct sd_context * context) |
502 |
|
|
{ |
503 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
504 |
|
|
return d_get_lsb_ref(&pro->sn); |
505 |
|
|
} |
506 |
|
|
|
507 |
|
|
static struct s_profile d_udplite_profile = {8,"1.0", "UDP-Lite / Decompressor", |
508 |
|
|
udp_lite_decode, |
509 |
|
|
udp_lite_decode_ir, |
510 |
|
|
udp_lite_allocate_decode_data, |
511 |
|
|
udp_lite_free_decode_data, |
512 |
|
|
udp_lite_detect_ir_size, |
513 |
|
|
udp_lite_detect_ir_dyn_size, |
514 |
|
|
udp_lite_get_sn |
515 |
|
|
}; |
516 |
|
|
|
517 |
|
|
struct s_profile * udp_lite_profile_create() |
518 |
|
|
{ |
519 |
|
|
return &d_udplite_profile; |
520 |
|
|
} |
521 |
|
|
|
522 |
|
|
|
523 |
|
|
|
524 |
|
|
// --------------------------------------------------------- Package type decoder |
525 |
|
|
|
526 |
|
|
|
527 |
|
|
/* Decode a UO-0 package */ |
528 |
|
|
static int udp_lite_decode_uo0( |
529 |
|
|
struct sd_rohc * state, |
530 |
|
|
struct sd_context * context, |
531 |
|
|
unsigned char * head, |
532 |
|
|
unsigned char * src, |
533 |
|
|
unsigned char * dest, |
534 |
|
|
int payload_size |
535 |
|
|
) |
536 |
|
|
{ |
537 |
|
|
unsigned char *saved_src = src; |
538 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
539 |
|
|
|
540 |
|
|
int multiple_ip = pro->multiple_ip; |
541 |
|
|
int id, id2=-1, sn, sn_bits = GET_BIT_3_6(head), sn_size = 4; |
542 |
|
|
int calc_crc, real_crc = GET_BIT_0_2(head); |
543 |
|
|
int extra_fields = 0, org_payload_size = payload_size; |
544 |
|
|
|
545 |
|
|
// Do the decoding / find ip-id |
546 |
|
|
extra_fields = udp_lite_do_decode_uo0_and_uo1(context, src, dest, &payload_size, sn_bits, sn_size , &id, 0 , &id2, &sn, &calc_crc); |
547 |
|
|
|
548 |
|
|
if (calc_crc != real_crc) { |
549 |
|
|
rohc_debugf(0,"UO-0: CRC failure (calc) %x vs %x (real)\n", |
550 |
|
|
calc_crc, real_crc); |
551 |
|
|
|
552 |
|
|
payload_size = org_payload_size; |
553 |
|
|
udp_lite_crc_failure_action(0, context, src, dest, sn_size, &sn, sn_bits, &payload_size, &id, 0, &id2, &calc_crc, &real_crc, 0); |
554 |
|
|
|
555 |
|
|
return ROHC_ERROR_CRC; |
556 |
|
|
} |
557 |
|
|
|
558 |
|
|
if(pro->counter){ |
559 |
|
|
if(pro->counter==1){ |
560 |
|
|
|
561 |
|
|
rohc_debugf(2,"Throw away packet, just 2 packages right so far\n"); |
562 |
|
|
|
563 |
|
|
pro->counter++; |
564 |
|
|
// Update the inter-packet variable |
565 |
|
|
|
566 |
|
|
udp_lite_update_packet_time(pro); |
567 |
|
|
|
568 |
|
|
udp_lite_syncronize(pro); |
569 |
|
|
|
570 |
|
|
d_lsb_sync_ref(&pro->sn); |
571 |
|
|
d_lsb_update(&pro->sn, sn); |
572 |
|
|
d_ip_id_update(&pro->ip_id1, id, sn); |
573 |
|
|
if (pro->multiple_ip) |
574 |
|
|
d_ip_id_update(&pro->ip_id2, id2, sn); |
575 |
|
|
|
576 |
|
|
return ROHC_ERROR_CRC; |
577 |
|
|
|
578 |
|
|
}else if(pro->counter==2){ |
579 |
|
|
pro->counter = 0; |
580 |
|
|
rohc_debugf(2,"The rapair is deemed successful\n"); |
581 |
|
|
}else{ |
582 |
|
|
rohc_debugf(2,"XX Should not happen XX\n"); |
583 |
|
|
} |
584 |
|
|
} |
585 |
|
|
|
586 |
|
|
src += extra_fields; |
587 |
|
|
|
588 |
|
|
// Update the inter-packet variable |
589 |
|
|
udp_lite_update_packet_time(pro); |
590 |
|
|
|
591 |
|
|
udp_lite_syncronize(pro); |
592 |
|
|
|
593 |
|
|
// Update lsb and id structs on CRC-success |
594 |
|
|
d_lsb_sync_ref(&pro->sn); |
595 |
|
|
d_lsb_update(&pro->sn, sn); |
596 |
|
|
d_ip_id_update(&pro->ip_id1, id, sn); |
597 |
|
|
if (pro->multiple_ip){ |
598 |
|
|
d_ip_id_update(&pro->ip_id2, id2, sn); |
599 |
|
|
dest += sizeof(struct iphdr); |
600 |
|
|
} |
601 |
|
|
|
602 |
|
|
// Payload |
603 |
|
|
dest += sizeof(struct iphdr) + sizeof(struct udphdr); |
604 |
|
|
|
605 |
|
|
memcpy(dest, src, payload_size); |
606 |
|
|
|
607 |
|
|
// Statistics: |
608 |
|
|
context->header_compressed_size += src - saved_src; |
609 |
|
|
c_add_wlsb(context->header_16_compressed, 0,0, src - saved_src); |
610 |
|
|
context->header_uncompressed_size += (multiple_ip+1)*sizeof(struct iphdr) + sizeof(struct udphdr); |
611 |
|
|
c_add_wlsb(context->header_16_uncompressed, 0, 0, (multiple_ip+1)*sizeof(struct iphdr) + sizeof(struct udphdr)); |
612 |
|
|
|
613 |
|
|
return payload_size + (multiple_ip + 1) * sizeof(struct iphdr)+ sizeof(struct udphdr); |
614 |
|
|
} |
615 |
|
|
|
616 |
|
|
|
617 |
|
|
|
618 |
|
|
/* Decode a UO-1 package */ |
619 |
|
|
static int udp_lite_decode_uo1( |
620 |
|
|
struct sd_rohc * state, |
621 |
|
|
struct sd_context * context, |
622 |
|
|
unsigned char * head, |
623 |
|
|
unsigned char * src, |
624 |
|
|
unsigned char * dest, |
625 |
|
|
int payload_size |
626 |
|
|
) |
627 |
|
|
{ |
628 |
|
|
unsigned char *saved_src = src; |
629 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
630 |
|
|
|
631 |
|
|
int id = GET_BIT_0_5(head), id2=-1; |
632 |
|
|
int sn, sn_bits = GET_BIT_3_7(src), sn_size = 5; |
633 |
|
|
int org_payload_size, extra_fields=0; |
634 |
|
|
int calc_crc, real_crc = GET_BIT_0_2(src); |
635 |
|
|
src++; payload_size--; |
636 |
|
|
org_payload_size = payload_size; |
637 |
|
|
|
638 |
|
|
// Decode the id and sn |
639 |
|
|
extra_fields = udp_lite_do_decode_uo0_and_uo1(context, src, dest, &payload_size, sn_bits, sn_size , &id, 6 , &id2, &sn, &calc_crc); |
640 |
|
|
|
641 |
|
|
if (calc_crc != real_crc) { |
642 |
|
|
rohc_debugf(0,"UO-1: CRC failure (calc) %x vs %x (real)\n", |
643 |
|
|
calc_crc, real_crc); |
644 |
|
|
payload_size = org_payload_size; |
645 |
|
|
udp_lite_crc_failure_action(0, context, src, dest, sn_size, &sn, sn_bits, &payload_size, &id, 6, &id2, &calc_crc, &real_crc, 0); |
646 |
|
|
return ROHC_ERROR_CRC; |
647 |
|
|
} |
648 |
|
|
|
649 |
|
|
if(pro->counter){ |
650 |
|
|
if(pro->counter==1){ |
651 |
|
|
|
652 |
|
|
rohc_debugf(2,"Throw away packet, just 2 packages right so far\n"); |
653 |
|
|
|
654 |
|
|
pro->counter++; |
655 |
|
|
// Update the inter-packet variable |
656 |
|
|
|
657 |
|
|
udp_lite_update_packet_time(pro); |
658 |
|
|
|
659 |
|
|
udp_lite_syncronize(pro); |
660 |
|
|
|
661 |
|
|
d_lsb_sync_ref(&pro->sn); |
662 |
|
|
d_lsb_update(&pro->sn, sn); |
663 |
|
|
d_ip_id_update(&pro->ip_id1, id, sn); |
664 |
|
|
if (pro->multiple_ip) |
665 |
|
|
d_ip_id_update(&pro->ip_id2, id2, sn); |
666 |
|
|
|
667 |
|
|
|
668 |
|
|
return ROHC_ERROR_CRC; |
669 |
|
|
|
670 |
|
|
}else if(pro->counter==2){ |
671 |
|
|
pro->counter = 0; |
672 |
|
|
rohc_debugf(2,"The rapair is deemed successful\n"); |
673 |
|
|
}else{ |
674 |
|
|
rohc_debugf(2,"XX Should not happen XX\n"); |
675 |
|
|
} |
676 |
|
|
} |
677 |
|
|
|
678 |
|
|
src += extra_fields; |
679 |
|
|
|
680 |
|
|
// Update the inter-packet variable |
681 |
|
|
udp_lite_update_packet_time(pro); |
682 |
|
|
|
683 |
|
|
udp_lite_syncronize(pro); |
684 |
|
|
|
685 |
|
|
d_lsb_sync_ref(&pro->sn); |
686 |
|
|
d_lsb_update(&pro->sn, sn); |
687 |
|
|
d_ip_id_update(&pro->ip_id1, id, sn); |
688 |
|
|
|
689 |
|
|
if (pro->multiple_ip){ |
690 |
|
|
d_ip_id_update(&pro->ip_id2, id2, sn); |
691 |
|
|
dest += sizeof(struct iphdr); |
692 |
|
|
} |
693 |
|
|
|
694 |
|
|
// Payload |
695 |
|
|
dest += sizeof(struct iphdr)+ sizeof(struct udphdr); |
696 |
|
|
memcpy(dest, src, payload_size); |
697 |
|
|
|
698 |
|
|
// Statistics: |
699 |
|
|
context->header_compressed_size += src - saved_src; |
700 |
|
|
c_add_wlsb(context->header_16_compressed, 0,0, src - saved_src); |
701 |
|
|
context->header_uncompressed_size += (pro->multiple_ip+1)*sizeof(struct iphdr) + sizeof(struct udphdr); |
702 |
|
|
c_add_wlsb(context->header_16_uncompressed, 0, 0, (pro->multiple_ip+1)*sizeof(struct iphdr) + sizeof(struct udphdr)); |
703 |
|
|
|
704 |
|
|
return payload_size + (pro->multiple_ip + 1) * sizeof(struct iphdr) + sizeof(struct udphdr); |
705 |
|
|
} |
706 |
|
|
|
707 |
|
|
|
708 |
|
|
|
709 |
|
|
|
710 |
|
|
/* Decode a UO-2 package */ |
711 |
|
|
static int udp_lite_decode_uor2( |
712 |
|
|
struct sd_rohc * state, |
713 |
|
|
struct sd_context * context, |
714 |
|
|
unsigned char * head, |
715 |
|
|
unsigned char * src, |
716 |
|
|
unsigned char * dest, |
717 |
|
|
int payload_size |
718 |
|
|
) |
719 |
|
|
{ |
720 |
|
|
unsigned char *saved_src = src; |
721 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
722 |
|
|
|
723 |
|
|
int org_payload_size = payload_size, extra_fields = 0; |
724 |
|
|
int sn_size = 0; |
725 |
|
|
int id = 0, id2 = 0, multiple_id = pro->multiple_ip; |
726 |
|
|
int calc_crc = 0; |
727 |
|
|
|
728 |
|
|
int sn_bits = GET_BIT_0_4(head) , sn = 0; |
729 |
|
|
int real_crc = GET_BIT_0_6(src); |
730 |
|
|
int ext = GET_BIT_7(src); |
731 |
|
|
src++; |
732 |
|
|
|
733 |
|
|
// Decode |
734 |
|
|
extra_fields = udp_lite_do_decode_uor2(state, context, src, dest, &payload_size, &id, &id2, &sn, &sn_size, sn_bits, ext, &calc_crc); |
735 |
|
|
|
736 |
|
|
|
737 |
|
|
if (calc_crc != real_crc) { |
738 |
|
|
|
739 |
|
|
rohc_debugf(0,"UOR-2: CRC failure (calc) %x vs %x (real)\n", |
740 |
|
|
calc_crc, real_crc); |
741 |
|
|
|
742 |
|
|
payload_size = org_payload_size; |
743 |
|
|
id =0 ; id2 = 0; calc_crc = 0; |
744 |
|
|
udp_lite_crc_failure_action(state, context, src, dest, sn_size, &sn, sn_bits, &payload_size, &id, 0, &id2, &calc_crc, &real_crc, ext); |
745 |
|
|
return ROHC_ERROR_CRC; |
746 |
|
|
} |
747 |
|
|
|
748 |
|
|
if(pro->counter){ |
749 |
|
|
if(pro->counter==1){ |
750 |
|
|
|
751 |
|
|
rohc_debugf(2,"Throw away packet, just 2 packages right so far\n"); |
752 |
|
|
|
753 |
|
|
pro->counter++; |
754 |
|
|
// Update the inter-packet variable |
755 |
|
|
|
756 |
|
|
udp_lite_update_packet_time(pro); |
757 |
|
|
|
758 |
|
|
udp_lite_syncronize(pro); |
759 |
|
|
|
760 |
|
|
d_lsb_sync_ref(&pro->sn); |
761 |
|
|
d_lsb_update(&pro->sn, sn); |
762 |
|
|
d_ip_id_update(&pro->ip_id1, id, sn); |
763 |
|
|
if (pro->multiple_ip) |
764 |
|
|
d_ip_id_update(&pro->ip_id2, id2, sn); |
765 |
|
|
|
766 |
|
|
return ROHC_ERROR_CRC; |
767 |
|
|
|
768 |
|
|
}else if(pro->counter==2){ |
769 |
|
|
pro->counter = 0; |
770 |
|
|
rohc_debugf(2,"The rapair is deemed successful\n"); |
771 |
|
|
}else{ |
772 |
|
|
rohc_debugf(2,"XX Should not happen XX\n"); |
773 |
|
|
} |
774 |
|
|
} |
775 |
|
|
|
776 |
|
|
context->state = ROHC_FULL_CONTEXT; |
777 |
|
|
src += extra_fields; |
778 |
|
|
|
779 |
|
|
//if crc success |
780 |
|
|
udp_lite_syncronize(pro); |
781 |
|
|
|
782 |
|
|
// Update the inter-packet variable |
783 |
|
|
udp_lite_update_packet_time(pro); |
784 |
|
|
|
785 |
|
|
// Update |
786 |
|
|
d_lsb_sync_ref(&pro->sn); |
787 |
|
|
d_lsb_update(&pro->sn, sn); |
788 |
|
|
d_ip_id_update(&pro->ip_id1, id, sn); |
789 |
|
|
if (pro->multiple_ip){ |
790 |
|
|
d_ip_id_update(&pro->ip_id2, id2, sn); |
791 |
|
|
dest += sizeof(struct iphdr); |
792 |
|
|
} |
793 |
|
|
// Payload |
794 |
|
|
dest += sizeof(struct iphdr)+ sizeof(struct udphdr); |
795 |
|
|
memcpy(dest, src, payload_size); |
796 |
|
|
|
797 |
|
|
// Statistics: |
798 |
|
|
context->header_compressed_size += src - saved_src; |
799 |
|
|
c_add_wlsb(context->header_16_compressed, 0,0, src - saved_src); |
800 |
|
|
context->header_uncompressed_size += (multiple_id+1)*sizeof(struct iphdr) + sizeof(struct udphdr); |
801 |
|
|
c_add_wlsb(context->header_16_uncompressed, 0, 0, (multiple_id+1)*sizeof(struct iphdr) + sizeof(struct udphdr)); |
802 |
|
|
|
803 |
|
|
return payload_size + (multiple_id +1) *sizeof(struct iphdr)+ sizeof(struct udphdr); |
804 |
|
|
} |
805 |
|
|
|
806 |
|
|
|
807 |
|
|
/* Decode a IR-Dyn package */ |
808 |
|
|
static int udp_lite_decode_irdyn( |
809 |
|
|
struct sd_rohc * state, |
810 |
|
|
struct sd_context * context, |
811 |
|
|
unsigned char * head, |
812 |
|
|
unsigned char * src2, |
813 |
|
|
unsigned char * dest, |
814 |
|
|
int payload_size |
815 |
|
|
) |
816 |
|
|
{ |
817 |
|
|
unsigned char *saved_src = src2; |
818 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
819 |
|
|
struct s_udp_lite_change * active1 = pro->active1; |
820 |
|
|
struct s_udp_lite_change * active2 = pro->active2; |
821 |
|
|
int sn = 0,size = d_decode_dynamic_ip4(src2, &active1->ip, &active1->rnd, &active1->nbo); |
822 |
|
|
int udp_length = 0; |
823 |
|
|
src2 += size; payload_size -= size + 2; |
824 |
|
|
|
825 |
|
|
if( pro->multiple_ip ){ |
826 |
|
|
size = d_decode_dynamic_ip4(src2, &active2->ip, &active2->rnd, &active2->nbo); |
827 |
|
|
src2 += size; payload_size -= size; |
828 |
|
|
} |
829 |
|
|
|
830 |
|
|
size = d_decode_dynamic_udp_lite(src2, &active1->udp); |
831 |
|
|
|
832 |
|
|
payload_size -= size; src2 += size; |
833 |
|
|
|
834 |
|
|
udp_length = payload_size + sizeof(struct udphdr); |
835 |
|
|
|
836 |
|
|
// set the checksum coverage flags |
837 |
|
|
pro->coverage_present = (udp_length != ntohs(active1->udp.len)); |
838 |
|
|
pro->coverage_inferred = (udp_length == ntohs(active1->udp.len)); |
839 |
|
|
|
840 |
|
|
//debug |
841 |
|
|
rohc_debugf(2,"(decomp) coverage present %d\n",pro->coverage_present); |
842 |
|
|
rohc_debugf(2,"(decomp) coverage inferred %d\n",pro->coverage_inferred); |
843 |
|
|
sn = ntohs(* ((__u16 *)src2)); |
844 |
|
|
d_lsb_init(&pro->sn, sn, -1); |
845 |
|
|
d_ip_id_init(&pro->ip_id1,ntohs(active1->ip.id), sn); |
846 |
|
|
|
847 |
|
|
udp_lite_syncronize(pro); |
848 |
|
|
// reset the correction-counter |
849 |
|
|
pro->counter = 0; |
850 |
|
|
src2 += 2; |
851 |
|
|
|
852 |
|
|
// write header |
853 |
|
|
if (pro->multiple_ip){ |
854 |
|
|
d_ip_id_init(&pro->ip_id2,ntohs(active2->ip.id), sn); |
855 |
|
|
udp_lite_write_uncompressed_ip4(active1, ntohs(active1->ip.id), dest, payload_size+sizeof(struct iphdr)+sizeof(struct udphdr)); |
856 |
|
|
dest += sizeof(struct iphdr); |
857 |
|
|
udp_lite_write_uncompressed_ip4(active2, ntohs(active2->ip.id), dest, payload_size + sizeof(struct udphdr)); |
858 |
|
|
}else{ |
859 |
|
|
udp_lite_write_uncompressed_ip4(active1, ntohs(active1->ip.id), dest, payload_size+ sizeof(struct udphdr)); |
860 |
|
|
} |
861 |
|
|
context->state = ROHC_FULL_CONTEXT; |
862 |
|
|
|
863 |
|
|
dest += sizeof(struct iphdr); |
864 |
|
|
// write udp-header |
865 |
|
|
memcpy(dest, &active1->udp, sizeof(struct udphdr)); |
866 |
|
|
dest += sizeof(struct udphdr); |
867 |
|
|
|
868 |
|
|
// Update the inter-packet variable |
869 |
|
|
udp_lite_update_packet_time(pro); |
870 |
|
|
|
871 |
|
|
memcpy(dest, src2, payload_size); |
872 |
|
|
|
873 |
|
|
// Statistics: |
874 |
|
|
context->header_compressed_size += src2 - saved_src; |
875 |
|
|
c_add_wlsb(context->header_16_compressed, 0,0, src2 - saved_src); |
876 |
|
|
context->header_uncompressed_size += (pro->multiple_ip+1)*sizeof(struct iphdr) + sizeof(struct udphdr); |
877 |
|
|
c_add_wlsb(context->header_16_uncompressed, 0, 0, (pro->multiple_ip+1)*sizeof(struct iphdr) + sizeof(struct udphdr)); |
878 |
|
|
|
879 |
|
|
return payload_size + ((pro->multiple_ip)+1) * sizeof(struct iphdr) + sizeof(struct udphdr); |
880 |
|
|
} |
881 |
|
|
|
882 |
|
|
/* decode uo0 and uo1 packets */ |
883 |
|
|
static int udp_lite_do_decode_uo0_and_uo1( |
884 |
|
|
struct sd_context * context, |
885 |
|
|
unsigned char * src, |
886 |
|
|
unsigned char * dest, |
887 |
|
|
int * payload_size, |
888 |
|
|
int sn_bits, |
889 |
|
|
int number_of_sn_bits, |
890 |
|
|
int * id, |
891 |
|
|
int number_of_id_bits, |
892 |
|
|
int * id2, |
893 |
|
|
int * sn, |
894 |
|
|
int * calc_crc |
895 |
|
|
) |
896 |
|
|
{ |
897 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
898 |
|
|
struct s_udp_lite_change * active1 = pro->active1; |
899 |
|
|
struct s_udp_lite_change * active2 = pro->active2; |
900 |
|
|
int field_counter = 0; |
901 |
|
|
int checksum = 0; |
902 |
|
|
int coverage = 0; |
903 |
|
|
|
904 |
|
|
*sn = d_lsb_decode(&pro->sn, sn_bits, number_of_sn_bits); |
905 |
|
|
|
906 |
|
|
if (active1->rnd) { |
907 |
|
|
*id = ntohs(*((__u16 *)src)); |
908 |
|
|
src += 2; field_counter +=2; *payload_size -= 2; |
909 |
|
|
|
910 |
|
|
} else { |
911 |
|
|
if(number_of_id_bits) |
912 |
|
|
*id = d_ip_id_decode(&pro->ip_id1, *id, number_of_id_bits, *sn); |
913 |
|
|
else |
914 |
|
|
*id = d_ip_id_decode(&pro->ip_id1, 0, 0, *sn); |
915 |
|
|
} |
916 |
|
|
|
917 |
|
|
// If multiple ip header |
918 |
|
|
if (pro->multiple_ip){ |
919 |
|
|
|
920 |
|
|
if (active2->rnd) { |
921 |
|
|
*id2 = ntohs(*((__u16 *)src)); |
922 |
|
|
|
923 |
|
|
src +=2; field_counter +=2; *payload_size -= 2; |
924 |
|
|
} else { |
925 |
|
|
*id2 = d_ip_id_decode(&pro->ip_id2, 0, 0, *sn); |
926 |
|
|
} |
927 |
|
|
} |
928 |
|
|
|
929 |
|
|
// coverage checksum field |
930 |
|
|
if ((pro->coverage_present)||(pro->ce_packet)){ |
931 |
|
|
coverage = ntohs(* ((__u16 *)src)); |
932 |
|
|
if(pro->ce_packet == PACKAGE_CE_OFF){ |
933 |
|
|
active1->udp.len = * ((__u16 *)src); |
934 |
|
|
pro->coverage_inferred = ((*payload_size - 4 + sizeof(struct udphdr)) == ntohs(active1->udp.len)); |
935 |
|
|
} |
936 |
|
|
src += 2; field_counter +=2; *payload_size -= 2; |
937 |
|
|
} |
938 |
|
|
|
939 |
|
|
//debug |
940 |
|
|
rohc_debugf(2,"(decomp) coverage present %d\n",pro->coverage_present); |
941 |
|
|
rohc_debugf(2,"(decomp) coverage inferred %d\n",pro->coverage_inferred); |
942 |
|
|
|
943 |
|
|
//checksum |
944 |
|
|
checksum = *((__u16 *)src); |
945 |
|
|
src +=2; field_counter +=2; *payload_size -= 2; |
946 |
|
|
|
947 |
|
|
rohc_debugf(2,"(decomp) udp checksum %x payload size = %d\n",checksum,*payload_size); |
948 |
|
|
// Header write |
949 |
|
|
if (pro->multiple_ip){ |
950 |
|
|
udp_lite_write_uncompressed_ip4(active1, *id, dest, *payload_size+sizeof(struct iphdr)+sizeof(struct udphdr)); |
951 |
|
|
udp_lite_write_uncompressed_ip4(active2, *id2, dest+sizeof(struct iphdr),*payload_size+sizeof(struct udphdr)); |
952 |
|
|
|
953 |
|
|
}else{ |
954 |
|
|
udp_lite_write_uncompressed_ip4(active1, *id, dest, *payload_size+sizeof(struct udphdr)); |
955 |
|
|
} |
956 |
|
|
// udp-header write |
957 |
|
|
udp_lite_write_uncompressed_udp(pro, checksum, coverage, dest+(pro->multiple_ip + 1) * sizeof(struct iphdr), *payload_size); |
958 |
|
|
|
959 |
|
|
// Check CRC |
960 |
|
|
*calc_crc = crc_calculate(CRC_TYPE_3, dest, (pro->multiple_ip + 1) * sizeof(struct iphdr)+ sizeof(struct udphdr)); |
961 |
|
|
|
962 |
|
|
return field_counter; |
963 |
|
|
} |
964 |
|
|
|
965 |
|
|
/* decode uor2 packets */ |
966 |
|
|
static int udp_lite_do_decode_uor2( |
967 |
|
|
struct sd_rohc * state, |
968 |
|
|
struct sd_context * context, |
969 |
|
|
unsigned char * src2, |
970 |
|
|
unsigned char * dest, |
971 |
|
|
int * payload_size, |
972 |
|
|
int * id, |
973 |
|
|
int * id2, |
974 |
|
|
int * sn, |
975 |
|
|
int * sn_size, |
976 |
|
|
int sn_bits, |
977 |
|
|
int ext, |
978 |
|
|
int * calc_crc |
979 |
|
|
) |
980 |
|
|
{ |
981 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
982 |
|
|
struct s_udp_lite_change * active1 = pro->active1; |
983 |
|
|
struct s_udp_lite_change * active2 = pro->active2; |
984 |
|
|
int id2_updated = 0, size = 0; |
985 |
|
|
int no_ip_id_update = 0; |
986 |
|
|
int field_counter = 0; |
987 |
|
|
int checksum = 0, coverage = 0; |
988 |
|
|
|
989 |
|
|
*sn = sn_bits; |
990 |
|
|
|
991 |
|
|
if (ext) { |
992 |
|
|
// decode extention |
993 |
|
|
switch(udp_lite_extention_type(src2)) { |
994 |
|
|
case PACKAGE_EXT_0: |
995 |
|
|
size = udp_lite_decode_extention0(src2, sn, id); |
996 |
|
|
// ip_id_bits == 3 |
997 |
|
|
*sn_size = 8; |
998 |
|
|
*sn = d_lsb_decode(&pro->sn, *sn, *sn_size ); |
999 |
|
|
*id = d_ip_id_decode(&pro->ip_id1, *id, 3, *sn); |
1000 |
|
|
*id2 = d_ip_id_decode(&pro->ip_id2, 0, 0, *sn); |
1001 |
|
|
break; |
1002 |
|
|
|
1003 |
|
|
case PACKAGE_EXT_1: |
1004 |
|
|
size = udp_lite_decode_extention1(src2, sn, id); |
1005 |
|
|
// ip_id bits == 11 |
1006 |
|
|
*sn_size = 8; |
1007 |
|
|
*sn = d_lsb_decode(&pro->sn, *sn, *sn_size ); |
1008 |
|
|
*id = d_ip_id_decode(&pro->ip_id1, *id, 11, *sn); |
1009 |
|
|
*id2 = d_ip_id_decode(&pro->ip_id2, 0, 0, *sn); |
1010 |
|
|
break; |
1011 |
|
|
|
1012 |
|
|
case PACKAGE_EXT_2: |
1013 |
|
|
size = udp_lite_decode_extention2(src2, sn, id, id2); |
1014 |
|
|
// ip_id bits == 8 |
1015 |
|
|
*sn_size = 8; |
1016 |
|
|
*sn = d_lsb_decode(&pro->sn, *sn, *sn_size ); |
1017 |
|
|
*id2 = d_ip_id_decode(&pro->ip_id1, *id, 8, *sn); |
1018 |
|
|
*id = d_ip_id_decode(&pro->ip_id2, *id2, 11, *sn); |
1019 |
|
|
break; |
1020 |
|
|
|
1021 |
|
|
case PACKAGE_EXT_3: |
1022 |
|
|
*sn_size = 5; |
1023 |
|
|
size = udp_lite_decode_extention3(src2, state, context, sn, |
1024 |
|
|
sn_size, &no_ip_id_update, &id2_updated); |
1025 |
|
|
//udp_lite_syncronize(pro);//flytta denna, synka efter crc-success. |
1026 |
|
|
rohc_debugf(2,"(udp_lite_decode) after ext 3 : nbo = %d rnd = %d\n", active1->nbo, active1->rnd); |
1027 |
|
|
|
1028 |
|
|
*sn = d_lsb_decode(&pro->sn, *sn, *sn_size ); |
1029 |
|
|
|
1030 |
|
|
if (no_ip_id_update) { |
1031 |
|
|
*id = ntohs(active1->ip.id); |
1032 |
|
|
} else { |
1033 |
|
|
*id = d_ip_id_decode(&pro->ip_id1, 0, 0, *sn); |
1034 |
|
|
} |
1035 |
|
|
if( pro->multiple_ip ){ |
1036 |
|
|
rohc_debugf(2,"(udp_lite_decode) after ext 3 : nbo2 = %d rnd2 = %d\n", active2->nbo, active2->rnd); |
1037 |
|
|
|
1038 |
|
|
if (id2_updated){ |
1039 |
|
|
*id2 = ntohs(active2->ip.id); |
1040 |
|
|
}else |
1041 |
|
|
*id2 = d_ip_id_decode(&pro->ip_id2, 0, 0, *sn); |
1042 |
|
|
} |
1043 |
|
|
break; |
1044 |
|
|
} |
1045 |
|
|
|
1046 |
|
|
|
1047 |
|
|
src2 += size; |
1048 |
|
|
field_counter +=size; |
1049 |
|
|
*payload_size -= size + 1; |
1050 |
|
|
} else { |
1051 |
|
|
// No extention |
1052 |
|
|
*sn_size = 5; |
1053 |
|
|
*sn = d_lsb_decode(&pro->sn, *sn , *sn_size); |
1054 |
|
|
|
1055 |
|
|
*id = d_ip_id_decode(&pro->ip_id1, 0, 0, *sn); |
1056 |
|
|
if( pro->multiple_ip) |
1057 |
|
|
*id2 = d_ip_id_decode(&pro->ip_id1, 0, 0, *sn); |
1058 |
|
|
*payload_size -= 1; |
1059 |
|
|
} |
1060 |
|
|
|
1061 |
|
|
|
1062 |
|
|
// Random IP ID ? |
1063 |
|
|
if (active1->rnd) { |
1064 |
|
|
*id = ntohs(*((__u16 *)src2)); |
1065 |
|
|
src2 +=2; field_counter +=2; *payload_size -= 2; |
1066 |
|
|
} |
1067 |
|
|
|
1068 |
|
|
// Multiple ip-header |
1069 |
|
|
|
1070 |
|
|
if (( pro->multiple_ip )&&( active2->rnd )){ |
1071 |
|
|
|
1072 |
|
|
*id2 = ntohs(*((__u16 *)src2)); |
1073 |
|
|
src2 +=2; field_counter +=2; *payload_size -= 2; |
1074 |
|
|
} |
1075 |
|
|
|
1076 |
|
|
|
1077 |
|
|
// coverage checksum field |
1078 |
|
|
if ((pro->coverage_present)||(pro->ce_packet)){ |
1079 |
|
|
coverage = ntohs(* ((__u16 *)src2)); |
1080 |
|
|
if(pro->ce_packet == PACKAGE_CE_OFF){ |
1081 |
|
|
active1->udp.len = * ((__u16 *)src2); |
1082 |
|
|
pro->coverage_inferred = ((*payload_size - 4 + sizeof(struct udphdr)) == ntohs(active1->udp.len)); |
1083 |
|
|
|
1084 |
|
|
} |
1085 |
|
|
|
1086 |
|
|
src2 += 2; field_counter +=2; *payload_size -= 2; |
1087 |
|
|
} |
1088 |
|
|
|
1089 |
|
|
//debug |
1090 |
|
|
rohc_debugf(2,"(decomp) coverage present %d\n",pro->coverage_present); |
1091 |
|
|
rohc_debugf(2,"(decomp) coverage inferred %d\n",pro->coverage_inferred); |
1092 |
|
|
|
1093 |
|
|
|
1094 |
|
|
checksum = *((__u16 *)src2); |
1095 |
|
|
src2 +=2; field_counter +=2; *payload_size -= 2; |
1096 |
|
|
|
1097 |
|
|
|
1098 |
|
|
// Header write |
1099 |
|
|
if (pro->multiple_ip){ |
1100 |
|
|
udp_lite_write_uncompressed_ip4(active1, *id, dest, *payload_size+sizeof(struct iphdr)+sizeof(struct udphdr)); |
1101 |
|
|
udp_lite_write_uncompressed_ip4(active2, *id2, dest+sizeof(struct iphdr),*payload_size+sizeof(struct udphdr)); |
1102 |
|
|
}else{ |
1103 |
|
|
udp_lite_write_uncompressed_ip4(active1, *id, dest, *payload_size+sizeof(struct udphdr)); |
1104 |
|
|
} |
1105 |
|
|
|
1106 |
|
|
// udp-header write |
1107 |
|
|
udp_lite_write_uncompressed_udp(pro, checksum, coverage, dest+(pro->multiple_ip + 1) * sizeof(struct iphdr), *payload_size); |
1108 |
|
|
|
1109 |
|
|
// CRC-check |
1110 |
|
|
*calc_crc = crc_calculate(CRC_TYPE_7, dest, (pro->multiple_ip + 1) * sizeof(struct iphdr) + sizeof(struct udphdr)); |
1111 |
|
|
|
1112 |
|
|
return field_counter; |
1113 |
|
|
} |
1114 |
|
|
|
1115 |
|
|
|
1116 |
|
|
|
1117 |
|
|
|
1118 |
|
|
|
1119 |
|
|
// --------------------------------------------------------- |
1120 |
|
|
|
1121 |
|
|
/* Decode extention 0. |
1122 |
|
|
* - SN value is expanded with 3 lower bits |
1123 |
|
|
* - IP-ID is replaced with 3 bits |
1124 |
|
|
*/ |
1125 |
|
|
static int udp_lite_decode_extention0(unsigned char * src, int * sn, int * ip_id) |
1126 |
|
|
{ |
1127 |
|
|
*sn = *sn << 3 | GET_BIT_3_5(src); |
1128 |
|
|
*ip_id = GET_BIT_0_2(src); |
1129 |
|
|
return 1; |
1130 |
|
|
} |
1131 |
|
|
|
1132 |
|
|
/* Decode extention 1 |
1133 |
|
|
* - SN value is expanded with 3 lower bits |
1134 |
|
|
* - IP-ID is replaced with 11 bits |
1135 |
|
|
*/ |
1136 |
|
|
static int udp_lite_decode_extention1(unsigned char * src, int * sn, int * ip_id) |
1137 |
|
|
{ |
1138 |
|
|
*sn = *sn << 3 | GET_BIT_3_5(src); |
1139 |
|
|
*ip_id = GET_BIT_0_2(src); |
1140 |
|
|
|
1141 |
|
|
src++; |
1142 |
|
|
*ip_id = *ip_id << 8 | *src; |
1143 |
|
|
|
1144 |
|
|
return 2; |
1145 |
|
|
} |
1146 |
|
|
|
1147 |
|
|
/* Decode extention 2 |
1148 |
|
|
* - SN value is expanded with 3 lower bits |
1149 |
|
|
* - IP-ID is replaced with 8 bits |
1150 |
|
|
*/ |
1151 |
|
|
static int udp_lite_decode_extention2(unsigned char * src, int * sn, int * ip_id, int * ip_id2) |
1152 |
|
|
{ |
1153 |
|
|
|
1154 |
|
|
*sn = *sn << 3 | GET_BIT_3_5(src); |
1155 |
|
|
*ip_id2 = GET_BIT_0_2(src); |
1156 |
|
|
src++; |
1157 |
|
|
|
1158 |
|
|
*ip_id2 = *ip_id2 << 8 | *src; |
1159 |
|
|
src++; |
1160 |
|
|
|
1161 |
|
|
*ip_id = *src; |
1162 |
|
|
return 3; |
1163 |
|
|
} |
1164 |
|
|
|
1165 |
|
|
/* Decode extention 3 |
1166 |
|
|
* - Updates random fields in the s_udp_change |
1167 |
|
|
* - May update the SN value with 8 lower bits. sn_size is then changed |
1168 |
|
|
*/ |
1169 |
|
|
static int udp_lite_decode_extention3( |
1170 |
|
|
unsigned char * src, |
1171 |
|
|
struct sd_rohc * state, |
1172 |
|
|
struct sd_context * context, |
1173 |
|
|
int * sn, |
1174 |
|
|
int * sn_size, |
1175 |
|
|
int * ip_id_changed, |
1176 |
|
|
int * update_id2) |
1177 |
|
|
{ |
1178 |
|
|
struct s_iponly_profile_data * pro = context->data; |
1179 |
|
|
struct s_iponly_change * active1 = pro->active1; |
1180 |
|
|
struct s_iponly_change * active2 = pro->active2; |
1181 |
|
|
unsigned char * org = src; |
1182 |
|
|
unsigned char * fields = src + 1; |
1183 |
|
|
int S = GET_BIT_5(src); |
1184 |
|
|
int mode = GET_BIT_3_4(src); |
1185 |
|
|
int I = GET_BIT_2(src); |
1186 |
|
|
int ip = GET_BIT_1(src); |
1187 |
|
|
int ip2 = GET_BIT_0(src); |
1188 |
|
|
int size; |
1189 |
|
|
|
1190 |
|
|
src++; |
1191 |
|
|
|
1192 |
|
|
if (ip) fields++; |
1193 |
|
|
if (ip2) fields++; |
1194 |
|
|
|
1195 |
|
|
if (S) { |
1196 |
|
|
*sn = (*sn << 8) + *fields; |
1197 |
|
|
*sn_size += 8; |
1198 |
|
|
fields++; |
1199 |
|
|
} |
1200 |
|
|
|
1201 |
|
|
if (ip) { |
1202 |
|
|
if(pro->multiple_ip){ |
1203 |
|
|
size = decode_inner_header_flags(src, fields, &active2->ip, |
1204 |
|
|
&active2->rnd, &active2->nbo); |
1205 |
|
|
}else |
1206 |
|
|
size = udp_lite_decode_inner_header_flags(src, fields, &active1->ip, |
1207 |
|
|
&active1->rnd, &active1->nbo); |
1208 |
|
|
fields += size; |
1209 |
|
|
} |
1210 |
|
|
if (I) { |
1211 |
|
|
if(pro->multiple_ip){ |
1212 |
|
|
active2->ip.id = *((__u16 *)fields); |
1213 |
|
|
fields += 2; |
1214 |
|
|
*update_id2 = 1; |
1215 |
|
|
}else{ |
1216 |
|
|
active1->ip.id = *((__u16 *)fields); |
1217 |
|
|
fields += 2; |
1218 |
|
|
*ip_id_changed = 1; |
1219 |
|
|
} |
1220 |
|
|
} |
1221 |
|
|
if (ip2) { |
1222 |
|
|
size = udp_lite_decode_outer_header_flags(src, fields, &active1->ip, |
1223 |
|
|
&active1->rnd, &active1->nbo, ip_id_changed ); |
1224 |
|
|
fields += size; |
1225 |
|
|
} |
1226 |
|
|
|
1227 |
|
|
if(mode != context->mode){ |
1228 |
|
|
rohc_debugf(2,"mode is not equal on decomp and comp.\n"); |
1229 |
|
|
d_change_mode_feedback(state, context); |
1230 |
|
|
} |
1231 |
|
|
return fields - org; |
1232 |
|
|
} |
1233 |
|
|
|
1234 |
|
|
// --------------------------------------------------------- Local utils functions |
1235 |
|
|
|
1236 |
|
|
// Deceide what package type |
1237 |
|
|
static int udp_lite_package_type(const unsigned char * p) |
1238 |
|
|
{ |
1239 |
|
|
if (!GET_BIT_7(p)) { |
1240 |
|
|
return PACKAGE_UO_0; |
1241 |
|
|
} else if (!GET_BIT_6(p)) { |
1242 |
|
|
return PACKAGE_UO_1; |
1243 |
|
|
} else if (GET_BIT_5_7(p) == 6) { |
1244 |
|
|
return PACKAGE_UOR_2; |
1245 |
|
|
} else if (*p == 0xf8) { |
1246 |
|
|
return PACKAGE_IR_DYN; |
1247 |
|
|
} else if ( (*p & 0xfe) == 0xfc) { |
1248 |
|
|
return PACKAGE_IR; |
1249 |
|
|
} else { |
1250 |
|
|
return PACKAGE_UNKNOWN; |
1251 |
|
|
} |
1252 |
|
|
} |
1253 |
|
|
|
1254 |
|
|
// Copy the active-structs to last-structs. |
1255 |
|
|
void udp_lite_syncronize(struct s_udp_lite_profile_data * pro) |
1256 |
|
|
{ |
1257 |
|
|
memcpy(pro->last1, pro->active1, sizeof(struct s_udp_lite_change)); |
1258 |
|
|
memcpy(pro->last2, pro->active2, sizeof(struct s_udp_lite_change)); |
1259 |
|
|
} |
1260 |
|
|
|
1261 |
|
|
// Copy the last-structs to the active-structs. |
1262 |
|
|
void udp_lite_sync_on_failure(struct s_udp_lite_profile_data * pro) |
1263 |
|
|
{ |
1264 |
|
|
memcpy(pro->active1, pro->last1, sizeof(struct s_udp_lite_change)); |
1265 |
|
|
memcpy(pro->active2, pro->last2, sizeof(struct s_udp_lite_change)); |
1266 |
|
|
} |
1267 |
|
|
|
1268 |
|
|
// Deceide the extention type |
1269 |
|
|
static int udp_lite_extention_type(const unsigned char * p) |
1270 |
|
|
{ |
1271 |
|
|
return GET_BIT_6_7(p); |
1272 |
|
|
} |
1273 |
|
|
|
1274 |
|
|
/* Decode inner IP flags and fields. Storage the values |
1275 |
|
|
* in a IP-head struct. |
1276 |
|
|
*/ |
1277 |
|
|
static int udp_lite_decode_inner_header_flags( |
1278 |
|
|
unsigned char * flags, |
1279 |
|
|
unsigned char * fields, |
1280 |
|
|
struct iphdr * ip, |
1281 |
|
|
int * rnd, int * nbo) |
1282 |
|
|
{ |
1283 |
|
|
int size = 0; |
1284 |
|
|
if (GET_BIT_7(flags)) { |
1285 |
|
|
ip->tos = *fields; |
1286 |
|
|
fields++; size++; |
1287 |
|
|
} |
1288 |
|
|
if (GET_BIT_6(flags)) { |
1289 |
|
|
ip->ttl = *fields; |
1290 |
|
|
fields++; size++; |
1291 |
|
|
} |
1292 |
|
|
if (GET_BIT_5(flags)) { |
1293 |
|
|
ip->frag_off = htons(IP_DF); |
1294 |
|
|
} else { |
1295 |
|
|
ip->frag_off = 0; |
1296 |
|
|
} |
1297 |
|
|
if (GET_BIT_4(flags)) { |
1298 |
|
|
ip->protocol = *fields; |
1299 |
|
|
fields++; size++; |
1300 |
|
|
} |
1301 |
|
|
if (GET_BIT_3(flags)) { |
1302 |
|
|
// TODO, list compression |
1303 |
|
|
rohc_debugf(0,"Listcompression is not supported\n"); |
1304 |
|
|
} |
1305 |
|
|
*nbo = GET_BIT_2(flags); |
1306 |
|
|
*rnd = GET_BIT_1(flags); |
1307 |
|
|
return size; |
1308 |
|
|
} |
1309 |
|
|
|
1310 |
|
|
/* Decode outer IP flags and fields. Storage the values |
1311 |
|
|
* in a IP-head struct. |
1312 |
|
|
*/ |
1313 |
|
|
static int udp_lite_decode_outer_header_flags( |
1314 |
|
|
unsigned char * flags, |
1315 |
|
|
unsigned char * fields, |
1316 |
|
|
struct iphdr * ip, |
1317 |
|
|
int * rnd, int * nbo, |
1318 |
|
|
int * updated_id) |
1319 |
|
|
{ |
1320 |
|
|
int size = 0; |
1321 |
|
|
if (GET_BIT_7(flags)) { |
1322 |
|
|
ip->tos = *fields; |
1323 |
|
|
fields++; size++; |
1324 |
|
|
} |
1325 |
|
|
if (GET_BIT_6(flags)) { |
1326 |
|
|
ip->ttl = *fields; |
1327 |
|
|
fields++; size++; |
1328 |
|
|
} |
1329 |
|
|
if (GET_BIT_5(flags)) { |
1330 |
|
|
ip->frag_off = htons(IP_DF); |
1331 |
|
|
} else { |
1332 |
|
|
ip->frag_off = 0; |
1333 |
|
|
} |
1334 |
|
|
if (GET_BIT_4(flags)) { |
1335 |
|
|
ip->protocol = *fields; |
1336 |
|
|
fields++; size++; |
1337 |
|
|
} |
1338 |
|
|
if (GET_BIT_3(flags)) { |
1339 |
|
|
// TODO, list compression |
1340 |
|
|
rohc_debugf(0,"Listcompression is not supported\n"); |
1341 |
|
|
} |
1342 |
|
|
*nbo = GET_BIT_2(flags); |
1343 |
|
|
*rnd = GET_BIT_1(flags); |
1344 |
|
|
|
1345 |
|
|
if (GET_BIT_0(flags)) { |
1346 |
|
|
ip->id = *((__u16 *)fields); |
1347 |
|
|
fields += 2; size += 2; |
1348 |
|
|
*updated_id = 1; |
1349 |
|
|
} |
1350 |
|
|
return size; |
1351 |
|
|
} |
1352 |
|
|
|
1353 |
|
|
/* Write a uncompressed IP v4 header */ |
1354 |
|
|
static void udp_lite_write_uncompressed_ip4( |
1355 |
|
|
struct s_udp_lite_change * active, |
1356 |
|
|
int ip_id, |
1357 |
|
|
unsigned char * dest, |
1358 |
|
|
int payload_size |
1359 |
|
|
) |
1360 |
|
|
{ |
1361 |
|
|
struct iphdr * ip = (struct iphdr *)dest; |
1362 |
|
|
|
1363 |
|
|
// --- static & some changing |
1364 |
|
|
memcpy(dest, &active->ip, sizeof(struct iphdr)); |
1365 |
|
|
|
1366 |
|
|
// --- ip-id |
1367 |
|
|
ip->id = htons(ip_id); |
1368 |
|
|
if (!active->nbo) ip->id = __swab16(ip->id); |
1369 |
|
|
|
1370 |
|
|
//--- Static-known fields |
1371 |
|
|
ip->ihl = 5; |
1372 |
|
|
|
1373 |
|
|
//--- inferred fields |
1374 |
|
|
ip->tot_len = htons(payload_size + (ip->ihl * 4)); |
1375 |
|
|
ip->check = 0; |
1376 |
|
|
ip->check = ip_fast_csum(dest, ip->ihl); |
1377 |
|
|
} |
1378 |
|
|
|
1379 |
|
|
static void udp_lite_write_uncompressed_udp( |
1380 |
|
|
struct s_udp_lite_profile_data *pro, |
1381 |
|
|
int checksum, |
1382 |
|
|
int coverage, |
1383 |
|
|
unsigned char * dest, |
1384 |
|
|
int payload_size |
1385 |
|
|
) |
1386 |
|
|
{ |
1387 |
|
|
struct s_udp_lite_change * active = pro->active1; |
1388 |
|
|
struct udphdr * udp = (struct udphdr *)dest; |
1389 |
|
|
|
1390 |
|
|
// --- static & some changing |
1391 |
|
|
memcpy(dest, &active->udp, sizeof(struct udphdr)); |
1392 |
|
|
|
1393 |
|
|
// --- udp-checksum |
1394 |
|
|
udp->check = checksum; |
1395 |
|
|
|
1396 |
|
|
// len is checksum coverage in udp-lite |
1397 |
|
|
if ((pro->coverage_present)||(pro->ce_packet)) |
1398 |
|
|
udp->len = htons(coverage); |
1399 |
|
|
else{ |
1400 |
|
|
if(pro->coverage_inferred) |
1401 |
|
|
udp->len = htons(payload_size + sizeof(struct udphdr)); |
1402 |
|
|
} |
1403 |
|
|
} |
1404 |
|
|
|
1405 |
|
|
/* This function try to repair the SN in one of two different ways |
1406 |
|
|
* |
1407 |
|
|
*/ |
1408 |
|
|
static int udp_lite_crc_failure_action( |
1409 |
|
|
struct sd_rohc * state, |
1410 |
|
|
struct sd_context * context, |
1411 |
|
|
unsigned char * src, |
1412 |
|
|
unsigned char * dest, |
1413 |
|
|
int sn_size, |
1414 |
|
|
int * sn, |
1415 |
|
|
int sn_bits, |
1416 |
|
|
int * payload_size, |
1417 |
|
|
int * id, |
1418 |
|
|
int id_size, |
1419 |
|
|
int * id2, |
1420 |
|
|
int * calc_crc, |
1421 |
|
|
int * real_crc, |
1422 |
|
|
int ext |
1423 |
|
|
) |
1424 |
|
|
{ |
1425 |
|
|
struct s_udp_lite_profile_data * pro = context->data; |
1426 |
|
|
|
1427 |
|
|
int sn_ref = 0, intervall = 0; |
1428 |
|
|
int sn_curr2 = 0, sn_curr1; |
1429 |
|
|
int sn_update = 0; |
1430 |
|
|
|
1431 |
|
|
udp_lite_sync_on_failure(pro); |
1432 |
|
|
|
1433 |
|
|
if(CRC_ACTION){ |
1434 |
|
|
rohc_debugf(0,"Try to repair the CRC\n"); |
1435 |
|
|
|
1436 |
|
|
if(pro->last_packet_time) //if last packettime == 0 then IR was just sent and ... |
1437 |
|
|
intervall = pro->current_packet_time - pro->last_packet_time; |
1438 |
|
|
|
1439 |
|
|
|
1440 |
|
|
if(intervall > ((1 << sn_size)*(pro->inter_arrival_time))){ |
1441 |
|
|
rohc_debugf(0,"ROHC: repair with the assumption: SN LSB wraparound\n"); |
1442 |
|
|
rohc_debugf(2,"inter_arrival_time = %d and current intervall is = %d\n", pro->inter_arrival_time, intervall); |
1443 |
|
|
rohc_debugf(2,"add %d to SN\n", 1 << sn_size); |
1444 |
|
|
|
1445 |
|
|
// Try and update the SN |
1446 |
|
|
sn_ref = d_get_lsb_ref(&pro->sn); |
1447 |
|
|
sn_ref += (1 << sn_size); |
1448 |
|
|
d_lsb_sync_ref(&pro->sn); |
1449 |
|
|
d_lsb_update(&pro->sn, sn_ref); |
1450 |
|
|
*sn = d_lsb_decode(&pro->sn, sn_bits, sn_size ); |
1451 |
|
|
|
1452 |
|
|
pro->counter = 0; |
1453 |
|
|
|
1454 |
|
|
}else{ |
1455 |
|
|
// try with the old sn_ref value |
1456 |
|
|
|
1457 |
|
|
rohc_debugf(0,"ROHC: repair with the assumption: incorrect SN-updates\n"); |
1458 |
|
|
|
1459 |
|
|
// save current referece value |
1460 |
|
|
sn_curr1 = d_get_lsb_ref(&pro->sn); |
1461 |
|
|
// test with old reference value |
1462 |
|
|
d_lsb_update(&pro->sn, d_get_lsb_old_ref(&pro->sn)); |
1463 |
|
|
|
1464 |
|
|
sn_curr2 = d_lsb_decode(&pro->sn, sn_bits, sn_size ); |
1465 |
|
|
|
1466 |
|
|
if( sn_curr2 == *sn) |
1467 |
|
|
return ROHC_ERROR_CRC; |
1468 |
|
|
//*sn = sn_curr2; |
1469 |
|
|
d_lsb_update(&pro->sn, sn_curr2); |
1470 |
|
|
sn_update = 1; |
1471 |
|
|
pro->counter = 0; |
1472 |
|
|
} |
1473 |
|
|
|
1474 |
|
|
// Try a new decompression with another SN |
1475 |
|
|
switch (pro->package_type){ |
1476 |
|
|
case PACKAGE_UO_0: |
1477 |
|
|
//fall through |
1478 |
|
|
case PACKAGE_UO_1: |
1479 |
|
|
udp_lite_do_decode_uo0_and_uo1(context, src, dest, payload_size, sn_bits, sn_size , id, id_size, id2, sn, calc_crc); |
1480 |
|
|
break; |
1481 |
|
|
case PACKAGE_UOR_2: |
1482 |
|
|
*sn = sn_bits; |
1483 |
|
|
udp_lite_do_decode_uor2(state, context, src, dest, payload_size, id, id2, sn, &sn_size, sn_bits, ext, calc_crc); |
1484 |
|
|
break; |
1485 |
|
|
default: |
1486 |
|
|
rohc_debugf(0,"(Ip-only) A existing packet?\n"); |
1487 |
|
|
d_lsb_update(&pro->sn, sn_curr1); |
1488 |
|
|
return ROHC_ERROR_CRC; |
1489 |
|
|
} |
1490 |
|
|
|
1491 |
|
|
if (*calc_crc != *real_crc){ |
1492 |
|
|
rohc_debugf(0,"ROHC: CRC failure also on the second attempt (calc) %x vs %x (real)\n",*calc_crc, *real_crc); |
1493 |
|
|
pro->counter = 0; |
1494 |
|
|
if(sn_update) |
1495 |
|
|
d_lsb_update(&pro->sn, sn_curr1); //reference curr1 should be used |
1496 |
|
|
udp_lite_sync_on_failure(pro); |
1497 |
|
|
return ROHC_ERROR_CRC; |
1498 |
|
|
|
1499 |
|
|
} |
1500 |
|
|
|
1501 |
|
|
rohc_debugf(2,"Update and sync with the new SN then throw away the packet\n"); |
1502 |
|
|
pro->counter++; |
1503 |
|
|
udp_lite_update_packet_time(pro); |
1504 |
|
|
|
1505 |
|
|
udp_lite_syncronize(pro); |
1506 |
|
|
if(!sn_update){ |
1507 |
|
|
d_lsb_sync_ref(&pro->sn); |
1508 |
|
|
d_lsb_update(&pro->sn, *sn); |
1509 |
|
|
}else |
1510 |
|
|
d_lsb_update(&pro->sn, sn_curr2); |
1511 |
|
|
|
1512 |
|
|
d_ip_id_update(&pro->ip_id1, *id, *sn); |
1513 |
|
|
if (pro->multiple_ip) |
1514 |
|
|
d_ip_id_update(&pro->ip_id2, *id2, *sn); |
1515 |
|
|
|
1516 |
|
|
return ROHC_ERROR_CRC; |
1517 |
|
|
|
1518 |
|
|
}else{ |
1519 |
|
|
return ROHC_ERROR_CRC; |
1520 |
|
|
} |
1521 |
|
|
} |
1522 |
|
|
|
1523 |
|
|
/* Update the inter-packet time, a sort of averange over the last inter-packet times */ |
1524 |
|
|
static void udp_lite_update_packet_time(struct s_udp_lite_profile_data * pro) |
1525 |
|
|
{ |
1526 |
|
|
int last_time = pro->last_packet_time; |
1527 |
|
|
int delta = 0; |
1528 |
|
|
rohc_debugf(2,"current time = %d and last time = %d\n", pro->current_packet_time, last_time); |
1529 |
|
|
|
1530 |
|
|
if (last_time) |
1531 |
|
|
delta = pro->current_packet_time - last_time; |
1532 |
|
|
pro->last_packet_time = pro->current_packet_time; |
1533 |
|
|
if (pro->inter_arrival_time){ |
1534 |
|
|
pro->inter_arrival_time = (pro->inter_arrival_time >> WEIGHT_OLD) + (delta >> WEIGHT_NEW); |
1535 |
|
|
|
1536 |
|
|
} else |
1537 |
|
|
pro->inter_arrival_time = delta; |
1538 |
|
|
|
1539 |
|
|
rohc_debugf(2,"inter_arrival_time = %d and current arrival delta is = %d\n", pro->inter_arrival_time, delta); |
1540 |
|
|
} |