src/rohc/d_util.c

/*
    ROHC Project 2003 at Lulea University of Technology, Sweden.
    Authors: Andreas Vernersson <andver-8@student.luth.se>
             Daniel Pettersson <danpet-7@student.luth.se>
             Erik Soderstrom <soderstrom@yahoo.com>
             Fredrik Lindstrom <frelin-9@student.luth.se>
             Johan Stenmark <johste-8@student.luth.se>
             Martin Juhlin <juhlin@users.sourceforge.net>
             Mikael Larsson <larmik-9@student.luth.se>
             Robert Maxe <robmax-1@student.luth.se>
             
    Copyright (C) 2003 Andreas Vernersson, Daniel Pettersson, 
    Erik Soderström, Fredrik Lindström, Johan Stenmark, 
    Martin Juhlin, Mikael Larsson, Robert Maxe.  

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/    

#include "d_util.h"

static const unsigned char D_PADDING = 0xe0;
static const unsigned char D_ADD_CID = 0xe;
static const unsigned char D_FEEDBACK = 0xf0 >> 3;
static const unsigned char D_IR_DYN_PACKET = 0xf8;
static const unsigned char D_IR_PACKET = 0xfc >> 1;
static const unsigned char D_SEGMENT = 0xfe >> 1;

// Decide if the field is a segment field
// Return: 1 = segment
//         0 = else
int d_is_segment(const unsigned char *data)
{
        if (GET_BIT_1_7(data) == D_SEGMENT)
                return 1;
        return 0;
}


// Decide if the field is a padding field
// Return: 1 = padding
//         0 = else
int d_is_paddning(const unsigned char *data)
{
        if (GET_BIT_0_7(data) == D_PADDING)
                return 1;
        return 0;
}

// Decide if the field is a feedback field
// Return: 1 = feedback header
//         0 = else
int d_is_feedback(const unsigned char *data)
{
        if (GET_BIT_3_7(data) == D_FEEDBACK)
                return 1;
        return 0;
}

// Return the size of the feedback
// In: Bytes of a feedback header
// Out: Size 
int d_feedback_size(const unsigned char *data)
{
        if(GET_BIT_0_2(data)!=(0x0 >> 1))
                return GET_BIT_0_2(data);
        else{
                data++; //change data to point on the next field
                return GET_BIT_0_7(data);
        }
}

// Decide how many bytes the feedback header is
// In: Bytes of a feedback header
// Out: the size in bytes (1-2)
int d_feedback_headersize(const unsigned char *data)
{
        if(GET_BIT_0_2(data)==(0x0 >> 1) )
                return 2;
        return 1;
}

// Check if a byte is a add-cid value 
// It is also possible to use d_decode_add_cid instead.
// In: Bytes
// Out: 1 = Add-cid value
//      0 = else
int d_is_add_cid(const unsigned char *data)
{
        if (GET_BIT_4_7(data) == D_ADD_CID)
                return 1;
        return 0;
}

// Decode the add-cid value
// In: Bytes
// Out: 1-15, cid value
//      0, no cid value
int d_decode_add_cid(const unsigned char *data)
{
        if (GET_BIT_4_7(data) == D_ADD_CID)
                return GET_BIT_0_3(data);
        return 0;
}

// Decide if a byte is a ir-field
// In: Bytes
// Out: 1 = IR, 0 = else
int d_is_ir(const unsigned char *data)
{
        if (GET_BIT_1_7(data) == D_IR_PACKET)
                return 1;
        return 0;
}

// Decide if a byte is a irdyn-field
// In: Bytes
// Out: 1 = IR-Dyn, 0 = else
int d_is_irdyn(const unsigned char *data)
{
        if (GET_BIT_0_7(data) == D_IR_DYN_PACKET)
                return 1;
        return 0;
}

// Decide the size of the self-decsribing variable-length value
// In: Bytes
// Out: 1-4
int d_sdvalue_size(const unsigned char *data)
{
        if(!GET_BIT_7( data ))                   //bit  == 0
                return 1;
        else if(GET_BIT_6_7(data) == (0x8 >> 2)) //bits == 10
                return 2;
        else if(GET_BIT_5_7(data) == (0xc >> 1)) //bits == 110
                return 3;
        else if(GET_BIT_5_7(data) == (0xe >> 1)) //bits == 111
                return 4;
        else
                return -1;  //should not happen
}

// Decode a self-describing variable-length value
// In: Bytes
// Out: The self-describing variable-length value
int d_sdvalue_decode(const unsigned char *data)
{
        if(!GET_BIT_7( data )){                   //bit  == 0
                
                return GET_BIT_0_6(data);
        }
        else if(GET_BIT_6_7(data) == (0x8 >> 2)){ //bits == 10
        
                return  (GET_BIT_0_5(data) << 8 |  GET_BIT_0_7((data+1)));
        }
        else if(GET_BIT_5_7(data) == (0xc >> 1)){ //bits == 110
          
                return (GET_BIT_0_4(data) << 16 |  GET_BIT_0_7((data+1)) << 8 
                       | GET_BIT_0_7((data+2))); 
        }
        else if(GET_BIT_5_7(data) == (0xe >> 1)){ //bits == 111
          
                return (GET_BIT_0_4(data) << 24 |  GET_BIT_0_7((data+1)) << 16 
                       | GET_BIT_0_7((data+2)) << 8 | GET_BIT_0_7((data+3)));  
        }
        else
                return -1;  //should not happen

}

//-----------------------------------------------------------------------------

// Decode the static part of a ipv4 rohc packet and store it in a ip-structure
// Return the number of used bytes
int d_decode_static_ip4(const unsigned char *data, struct iphdr * dest)
{
        dest->version = GET_BIT_4_7(data);
        if((dest->version)!=4)
                return -1;
        data++;
        dest->protocol = GET_BIT_0_7(data);
        data++;
        dest->saddr = *((u32 *)data);
        data += 4;
        dest->daddr = *((u32 *)data);
        return 10;
}

// Decode the static part in a udp rohc packet and store it in a udp-structure
// Return the number of used bytes
int d_decode_static_udp(const unsigned char *data, struct udphdr * dest)
{
        dest-> source = *((u16 *)data);
        data += 2;
        dest-> dest = *((u16 *)data);
        return 4;
}

// Decode the dynamic part in a ipv4 rohc packet and store it in a ip-structure
// Return the number of used bytes
int d_decode_dynamic_ip4(const unsigned char *data, struct iphdr * dest, 
                         int * rnd, int * nbo)
{
        dest->tos = GET_BIT_0_7(data);
        data++;
        dest->ttl = GET_BIT_0_7(data);
        data++;
        dest-> id = *((u16 *)data);
        data += 2;
        if(GET_BIT_7(data)){
                dest->frag_off = htons(0x4000);
        }else{
                dest->frag_off = htons(0x0000);
        }
        *nbo = GET_REAL(GET_BIT_5(data));
        *rnd = GET_REAL(GET_BIT_6(data));
        
        return 5;
}

// Decode the dynamic part in a udp rohc packet and store it in a udp-structure
// Return the number of used bytes
int d_decode_dynamic_udp(const unsigned char *data, struct udphdr * dest)
{
        dest-> check = *((u16 *)data);
        return 2;
}

// Decode the dynamic part in a udp-lite rohc packet and store it in a udp-structure
// Return the number of used bytes
int d_decode_dynamic_udp_lite(const unsigned char *data, struct udphdr * dest)
{
        dest-> len = *((u16 *)data);
        data += 2;
        dest-> check = *((u16 *)data);
        return 4;
}


// Initiate the lsb struct
// in: the value of v_ref_d and type of p value
void d_lsb_init(struct sd_lsb_decode * s,int v_ref_d, int p)
{
        s->p = p;
        s->v_ref_d = v_ref_d;
        s->old_v_ref_d = v_ref_d;
}

// Decode a LSB-value
// in: the struct were the old received value is present
//     m = the LSB-value to decode 
//     k = the length of m in bits
//     
// out:the decoded value
int d_lsb_decode(struct sd_lsb_decode * s, int m, int k)
{       
        int lower = (s->v_ref_d - s->p);
//      int higher = (ref - s->p) + 1 << k - 1;

        int bitmask = ~((1 << k) - 1);
        int sn = (s->v_ref_d & bitmask) | m;
        if (sn < lower) sn += 1 << k;

        return sn & 0xFFFF;
}

// update the reference value, called after a crc-success to update the 
// last decoded value, eg the sn number
void d_lsb_update(struct sd_lsb_decode * s, int v_ref_d)
{
        s->v_ref_d = v_ref_d;
}

// copy the value in v_ref_d to old_v_ref_d
void d_lsb_sync_ref(struct sd_lsb_decode * s){
        s->old_v_ref_d = s->v_ref_d;
}

// get the old value of v_ref_d
int d_get_lsb_old_ref(struct sd_lsb_decode * s){
        return s->old_v_ref_d;
}

// get the reference value
int d_get_lsb_ref(struct sd_lsb_decode * s)
{
        return s->v_ref_d;
}


// initiate the ip-id struct 
// in: the ip-id struct, the reference for ip-id, the sequenze number  
void d_ip_id_init(struct sd_ip_id_decode * s,int id_ref, int sn_ref)
{
        s->id_ref = id_ref;
        s->sn_ref = sn_ref;
}

// Decode the ip-id offset in packet m and return the ip_id
// in: the ip-id struct, the packet m containing the ip-id offset,
//     m = the length of the offset
//     k = number of bits in m
//     sn = the sequence number of packet m.
// out:ip_id
int d_ip_id_decode(struct sd_ip_id_decode * s, int m, int k, int sn)
{
        int offset_ref = (s->id_ref - s->sn_ref)%(65536);
        int offset_m = -1;
        
        int bitmask = ~((1 << k) - 1);
        int ip_id = (offset_ref & bitmask) | m;
        
        if (ip_id < offset_ref) ip_id += 1 << k;
        
        offset_m = ip_id & 0xFFFF;
        return ((sn + offset_m)%(65536));
}


// update the reference values for the ip-id and the sequence number
void d_ip_id_update(struct sd_ip_id_decode * s, int id_ref, int sn_ref)
{
        s->id_ref = id_ref;
        s->sn_ref = sn_ref;
}


Revision:	1.3
Committed:	Tue Apr 26 00:55:56 2005 UTC (19 years, 1 month ago) by pcg
Content type:	text/plain
Branch:	MAIN
CVS Tags:	rel-2_01, rel-3_0, rel-2_2, rel-2_0, rel-2_21, rel-2_22, rel-2_25, HEAD
Changes since 1.2:	+1 -1 lines
Log Message:	* empty log message *
#	Content
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 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