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, 2 months 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 *
#	User	Rev	Content
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	pcg	1.3	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
29	pcg	1.1	*/
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	pcg	1.2	dest->saddr = ((u32 )data);
195	pcg	1.1	data += 4;
196	pcg	1.2	dest->daddr = ((u32 )data);
197	pcg	1.1	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	pcg	1.2	dest-> source = ((u16 )data);
205	pcg	1.1	data += 2;
206	pcg	1.2	dest-> dest = ((u16 )data);
207	pcg	1.1	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	pcg	1.2	dest-> id = ((u16 )data);
220	pcg	1.1	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	pcg	1.2	dest-> check = ((u16 )data);
237	pcg	1.1	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	pcg	1.2	dest-> len = ((u16 )data);
245	pcg	1.1	data += 2;
246	pcg	1.2	dest-> check = ((u16 )data);
247	pcg	1.1	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