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 |
|
|
#include "d_util.h" |
32 |
|
|
|
33 |
|
|
static const unsigned char D_PADDING = 0xe0; |
34 |
|
|
static const unsigned char D_ADD_CID = 0xe; |
35 |
|
|
static const unsigned char D_FEEDBACK = 0xf0 >> 3; |
36 |
|
|
static const unsigned char D_IR_DYN_PACKET = 0xf8; |
37 |
|
|
static const unsigned char D_IR_PACKET = 0xfc >> 1; |
38 |
|
|
static const unsigned char D_SEGMENT = 0xfe >> 1; |
39 |
|
|
|
40 |
|
|
// Decide if the field is a segment field |
41 |
|
|
// Return: 1 = segment |
42 |
|
|
// 0 = else |
43 |
|
|
int d_is_segment(const unsigned char *data) |
44 |
|
|
{ |
45 |
|
|
if (GET_BIT_1_7(data) == D_SEGMENT) |
46 |
|
|
return 1; |
47 |
|
|
return 0; |
48 |
|
|
} |
49 |
|
|
|
50 |
|
|
|
51 |
|
|
// Decide if the field is a padding field |
52 |
|
|
// Return: 1 = padding |
53 |
|
|
// 0 = else |
54 |
|
|
int d_is_paddning(const unsigned char *data) |
55 |
|
|
{ |
56 |
|
|
if (GET_BIT_0_7(data) == D_PADDING) |
57 |
|
|
return 1; |
58 |
|
|
return 0; |
59 |
|
|
} |
60 |
|
|
|
61 |
|
|
// Decide if the field is a feedback field |
62 |
|
|
// Return: 1 = feedback header |
63 |
|
|
// 0 = else |
64 |
|
|
int d_is_feedback(const unsigned char *data) |
65 |
|
|
{ |
66 |
|
|
if (GET_BIT_3_7(data) == D_FEEDBACK) |
67 |
|
|
return 1; |
68 |
|
|
return 0; |
69 |
|
|
} |
70 |
|
|
|
71 |
|
|
// Return the size of the feedback |
72 |
|
|
// In: Bytes of a feedback header |
73 |
|
|
// Out: Size |
74 |
|
|
int d_feedback_size(const unsigned char *data) |
75 |
|
|
{ |
76 |
|
|
if(GET_BIT_0_2(data)!=(0x0 >> 1)) |
77 |
|
|
return GET_BIT_0_2(data); |
78 |
|
|
else{ |
79 |
|
|
data++; //change data to point on the next field |
80 |
|
|
return GET_BIT_0_7(data); |
81 |
|
|
} |
82 |
|
|
} |
83 |
|
|
|
84 |
|
|
// Decide how many bytes the feedback header is |
85 |
|
|
// In: Bytes of a feedback header |
86 |
|
|
// Out: the size in bytes (1-2) |
87 |
|
|
int d_feedback_headersize(const unsigned char *data) |
88 |
|
|
{ |
89 |
|
|
if(GET_BIT_0_2(data)==(0x0 >> 1) ) |
90 |
|
|
return 2; |
91 |
|
|
return 1; |
92 |
|
|
} |
93 |
|
|
|
94 |
|
|
// Check if a byte is a add-cid value |
95 |
|
|
// It is also possible to use d_decode_add_cid instead. |
96 |
|
|
// In: Bytes |
97 |
|
|
// Out: 1 = Add-cid value |
98 |
|
|
// 0 = else |
99 |
|
|
int d_is_add_cid(const unsigned char *data) |
100 |
|
|
{ |
101 |
|
|
if (GET_BIT_4_7(data) == D_ADD_CID) |
102 |
|
|
return 1; |
103 |
|
|
return 0; |
104 |
|
|
} |
105 |
|
|
|
106 |
|
|
// Decode the add-cid value |
107 |
|
|
// In: Bytes |
108 |
|
|
// Out: 1-15, cid value |
109 |
|
|
// 0, no cid value |
110 |
|
|
int d_decode_add_cid(const unsigned char *data) |
111 |
|
|
{ |
112 |
|
|
if (GET_BIT_4_7(data) == D_ADD_CID) |
113 |
|
|
return GET_BIT_0_3(data); |
114 |
|
|
return 0; |
115 |
|
|
} |
116 |
|
|
|
117 |
|
|
// Decide if a byte is a ir-field |
118 |
|
|
// In: Bytes |
119 |
|
|
// Out: 1 = IR, 0 = else |
120 |
|
|
int d_is_ir(const unsigned char *data) |
121 |
|
|
{ |
122 |
|
|
if (GET_BIT_1_7(data) == D_IR_PACKET) |
123 |
|
|
return 1; |
124 |
|
|
return 0; |
125 |
|
|
} |
126 |
|
|
|
127 |
|
|
// Decide if a byte is a irdyn-field |
128 |
|
|
// In: Bytes |
129 |
|
|
// Out: 1 = IR-Dyn, 0 = else |
130 |
|
|
int d_is_irdyn(const unsigned char *data) |
131 |
|
|
{ |
132 |
|
|
if (GET_BIT_0_7(data) == D_IR_DYN_PACKET) |
133 |
|
|
return 1; |
134 |
|
|
return 0; |
135 |
|
|
} |
136 |
|
|
|
137 |
|
|
// Decide the size of the self-decsribing variable-length value |
138 |
|
|
// In: Bytes |
139 |
|
|
// Out: 1-4 |
140 |
|
|
int d_sdvalue_size(const unsigned char *data) |
141 |
|
|
{ |
142 |
|
|
if(!GET_BIT_7( data )) //bit == 0 |
143 |
|
|
return 1; |
144 |
|
|
else if(GET_BIT_6_7(data) == (0x8 >> 2)) //bits == 10 |
145 |
|
|
return 2; |
146 |
|
|
else if(GET_BIT_5_7(data) == (0xc >> 1)) //bits == 110 |
147 |
|
|
return 3; |
148 |
|
|
else if(GET_BIT_5_7(data) == (0xe >> 1)) //bits == 111 |
149 |
|
|
return 4; |
150 |
|
|
else |
151 |
|
|
return -1; //should not happen |
152 |
|
|
} |
153 |
|
|
|
154 |
|
|
// Decode a self-describing variable-length value |
155 |
|
|
// In: Bytes |
156 |
|
|
// Out: The self-describing variable-length value |
157 |
|
|
int d_sdvalue_decode(const unsigned char *data) |
158 |
|
|
{ |
159 |
|
|
if(!GET_BIT_7( data )){ //bit == 0 |
160 |
|
|
|
161 |
|
|
return GET_BIT_0_6(data); |
162 |
|
|
} |
163 |
|
|
else if(GET_BIT_6_7(data) == (0x8 >> 2)){ //bits == 10 |
164 |
|
|
|
165 |
|
|
return (GET_BIT_0_5(data) << 8 | GET_BIT_0_7((data+1))); |
166 |
|
|
} |
167 |
|
|
else if(GET_BIT_5_7(data) == (0xc >> 1)){ //bits == 110 |
168 |
|
|
|
169 |
|
|
return (GET_BIT_0_4(data) << 16 | GET_BIT_0_7((data+1)) << 8 |
170 |
|
|
| GET_BIT_0_7((data+2))); |
171 |
|
|
} |
172 |
|
|
else if(GET_BIT_5_7(data) == (0xe >> 1)){ //bits == 111 |
173 |
|
|
|
174 |
|
|
return (GET_BIT_0_4(data) << 24 | GET_BIT_0_7((data+1)) << 16 |
175 |
|
|
| GET_BIT_0_7((data+2)) << 8 | GET_BIT_0_7((data+3))); |
176 |
|
|
} |
177 |
|
|
else |
178 |
|
|
return -1; //should not happen |
179 |
|
|
|
180 |
|
|
} |
181 |
|
|
|
182 |
|
|
//----------------------------------------------------------------------------- |
183 |
|
|
|
184 |
|
|
// Decode the static part of a ipv4 rohc packet and store it in a ip-structure |
185 |
|
|
// Return the number of used bytes |
186 |
|
|
int d_decode_static_ip4(const unsigned char *data, struct iphdr * dest) |
187 |
|
|
{ |
188 |
|
|
dest->version = GET_BIT_4_7(data); |
189 |
|
|
if((dest->version)!=4) |
190 |
|
|
return -1; |
191 |
|
|
data++; |
192 |
|
|
dest->protocol = GET_BIT_0_7(data); |
193 |
|
|
data++; |
194 |
|
|
dest->saddr = *((__u32 *)data); |
195 |
|
|
data += 4; |
196 |
|
|
dest->daddr = *((__u32 *)data); |
197 |
|
|
return 10; |
198 |
|
|
} |
199 |
|
|
|
200 |
|
|
// Decode the static part in a udp rohc packet and store it in a udp-structure |
201 |
|
|
// Return the number of used bytes |
202 |
|
|
int d_decode_static_udp(const unsigned char *data, struct udphdr * dest) |
203 |
|
|
{ |
204 |
|
|
dest-> source = *((__u16 *)data); |
205 |
|
|
data += 2; |
206 |
|
|
dest-> dest = *((__u16 *)data); |
207 |
|
|
return 4; |
208 |
|
|
} |
209 |
|
|
|
210 |
|
|
// Decode the dynamic part in a ipv4 rohc packet and store it in a ip-structure |
211 |
|
|
// Return the number of used bytes |
212 |
|
|
int d_decode_dynamic_ip4(const unsigned char *data, struct iphdr * dest, |
213 |
|
|
int * rnd, int * nbo) |
214 |
|
|
{ |
215 |
|
|
dest->tos = GET_BIT_0_7(data); |
216 |
|
|
data++; |
217 |
|
|
dest->ttl = GET_BIT_0_7(data); |
218 |
|
|
data++; |
219 |
|
|
dest-> id = *((__u16 *)data); |
220 |
|
|
data += 2; |
221 |
|
|
if(GET_BIT_7(data)){ |
222 |
|
|
dest->frag_off = htons(0x4000); |
223 |
|
|
}else{ |
224 |
|
|
dest->frag_off = htons(0x0000); |
225 |
|
|
} |
226 |
|
|
*nbo = GET_REAL(GET_BIT_5(data)); |
227 |
|
|
*rnd = GET_REAL(GET_BIT_6(data)); |
228 |
|
|
|
229 |
|
|
return 5; |
230 |
|
|
} |
231 |
|
|
|
232 |
|
|
// Decode the dynamic part in a udp rohc packet and store it in a udp-structure |
233 |
|
|
// Return the number of used bytes |
234 |
|
|
int d_decode_dynamic_udp(const unsigned char *data, struct udphdr * dest) |
235 |
|
|
{ |
236 |
|
|
dest-> check = *((__u16 *)data); |
237 |
|
|
return 2; |
238 |
|
|
} |
239 |
|
|
|
240 |
|
|
// Decode the dynamic part in a udp-lite rohc packet and store it in a udp-structure |
241 |
|
|
// Return the number of used bytes |
242 |
|
|
int d_decode_dynamic_udp_lite(const unsigned char *data, struct udphdr * dest) |
243 |
|
|
{ |
244 |
|
|
dest-> len = *((__u16 *)data); |
245 |
|
|
data += 2; |
246 |
|
|
dest-> check = *((__u16 *)data); |
247 |
|
|
return 4; |
248 |
|
|
} |
249 |
|
|
|
250 |
|
|
|
251 |
|
|
// Initiate the lsb struct |
252 |
|
|
// in: the value of v_ref_d and type of p value |
253 |
|
|
void d_lsb_init(struct sd_lsb_decode * s,int v_ref_d, int p) |
254 |
|
|
{ |
255 |
|
|
s->p = p; |
256 |
|
|
s->v_ref_d = v_ref_d; |
257 |
|
|
s->old_v_ref_d = v_ref_d; |
258 |
|
|
} |
259 |
|
|
|
260 |
|
|
// Decode a LSB-value |
261 |
|
|
// in: the struct were the old received value is present |
262 |
|
|
// m = the LSB-value to decode |
263 |
|
|
// k = the length of m in bits |
264 |
|
|
// |
265 |
|
|
// out:the decoded value |
266 |
|
|
int d_lsb_decode(struct sd_lsb_decode * s, int m, int k) |
267 |
|
|
{ |
268 |
|
|
int lower = (s->v_ref_d - s->p); |
269 |
|
|
// int higher = (ref - s->p) + 1 << k - 1; |
270 |
|
|
|
271 |
|
|
int bitmask = ~((1 << k) - 1); |
272 |
|
|
int sn = (s->v_ref_d & bitmask) | m; |
273 |
|
|
if (sn < lower) sn += 1 << k; |
274 |
|
|
|
275 |
|
|
return sn & 0xFFFF; |
276 |
|
|
} |
277 |
|
|
|
278 |
|
|
// update the reference value, called after a crc-success to update the |
279 |
|
|
// last decoded value, eg the sn number |
280 |
|
|
void d_lsb_update(struct sd_lsb_decode * s, int v_ref_d) |
281 |
|
|
{ |
282 |
|
|
s->v_ref_d = v_ref_d; |
283 |
|
|
} |
284 |
|
|
|
285 |
|
|
// copy the value in v_ref_d to old_v_ref_d |
286 |
|
|
void d_lsb_sync_ref(struct sd_lsb_decode * s){ |
287 |
|
|
s->old_v_ref_d = s->v_ref_d; |
288 |
|
|
} |
289 |
|
|
|
290 |
|
|
// get the old value of v_ref_d |
291 |
|
|
int d_get_lsb_old_ref(struct sd_lsb_decode * s){ |
292 |
|
|
return s->old_v_ref_d; |
293 |
|
|
} |
294 |
|
|
|
295 |
|
|
// get the reference value |
296 |
|
|
int d_get_lsb_ref(struct sd_lsb_decode * s) |
297 |
|
|
{ |
298 |
|
|
return s->v_ref_d; |
299 |
|
|
} |
300 |
|
|
|
301 |
|
|
|
302 |
|
|
// initiate the ip-id struct |
303 |
|
|
// in: the ip-id struct, the reference for ip-id, the sequenze number |
304 |
|
|
void d_ip_id_init(struct sd_ip_id_decode * s,int id_ref, int sn_ref) |
305 |
|
|
{ |
306 |
|
|
s->id_ref = id_ref; |
307 |
|
|
s->sn_ref = sn_ref; |
308 |
|
|
} |
309 |
|
|
|
310 |
|
|
// Decode the ip-id offset in packet m and return the ip_id |
311 |
|
|
// in: the ip-id struct, the packet m containing the ip-id offset, |
312 |
|
|
// m = the length of the offset |
313 |
|
|
// k = number of bits in m |
314 |
|
|
// sn = the sequence number of packet m. |
315 |
|
|
// out:ip_id |
316 |
|
|
int d_ip_id_decode(struct sd_ip_id_decode * s, int m, int k, int sn) |
317 |
|
|
{ |
318 |
|
|
int offset_ref = (s->id_ref - s->sn_ref)%(65536); |
319 |
|
|
int offset_m = -1; |
320 |
|
|
|
321 |
|
|
int bitmask = ~((1 << k) - 1); |
322 |
|
|
int ip_id = (offset_ref & bitmask) | m; |
323 |
|
|
|
324 |
|
|
if (ip_id < offset_ref) ip_id += 1 << k; |
325 |
|
|
|
326 |
|
|
offset_m = ip_id & 0xFFFF; |
327 |
|
|
return ((sn + offset_m)%(65536)); |
328 |
|
|
} |
329 |
|
|
|
330 |
|
|
|
331 |
|
|
// update the reference values for the ip-id and the sequence number |
332 |
|
|
void d_ip_id_update(struct sd_ip_id_decode * s, int id_ref, int sn_ref) |
333 |
|
|
{ |
334 |
|
|
s->id_ref = id_ref; |
335 |
|
|
s->sn_ref = sn_ref; |
336 |
|
|
} |
337 |
|
|
|
338 |
|
|
|
339 |
|
|
|
340 |
|
|
|
341 |
|
|
|
342 |
|
|
|
343 |
|
|
|
344 |
|
|
|
345 |
|
|
|
346 |
|
|
|
347 |
|
|
|
348 |
|
|
|
349 |
|
|
|
350 |
|
|
|
351 |
|
|
|
352 |
|
|
|
353 |
|
|
|
354 |
|
|
|
355 |
|
|
|