f4f007b08910483885421127e1da3cd2a5b1a48d
[olsrd.git] / lib / bmf / src / Packet.c
1 /*
2  * OLSR Basic Multicast Forwarding (BMF) plugin.
3  * Copyright (c) 2005, 2006, Thales Communications, Huizen, The Netherlands.
4  * Written by Erik Tromp.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without 
8  * modification, are permitted provided that the following conditions 
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright 
12  *   notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above copyright 
14  *   notice, this list of conditions and the following disclaimer in 
15  *   the documentation and/or other materials provided with the 
16  *   distribution.
17  * * Neither the name of Thales, BMF nor the names of its 
18  *   contributors may be used to endorse or promote products derived 
19  *   from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 
22  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 
23  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
24  * IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
25  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 
27  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY 
28  * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 
29  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 
30  * OF THE POSSIBILITY OF SUCH DAMAGE.
31  */
32
33 /* -------------------------------------------------------------------------
34  * File       : Packet.c
35  * Description: IP packet and Ethernet frame processing functions
36  * Created    : 29 Jun 2006
37  *
38  * ------------------------------------------------------------------------- */
39
40 #include "Packet.h"
41
42 /* System includes */
43 #include <stddef.h> /* NULL */
44 #include <assert.h> /* assert() */
45 #include <string.h> /* memcpy() */
46 #include <sys/types.h> /* u_int8_t, u_int16_t, u_int32_t */
47 #include <netinet/in.h> /* ntohs(), htons() */
48 #include <netinet/ip.h> /* struct iphdr */
49
50 /* -------------------------------------------------------------------------
51  * Function   : IsIpFragment
52  * Description: Check if an IP packet is an IP fragment
53  * Input      : ipPacket - the IP packet
54  * Output     : none
55  * Return     : true (1) or false (0)
56  * Data Used  : none
57  * ------------------------------------------------------------------------- */
58 int IsIpFragment(unsigned char* ipPacket)
59 {
60   struct ip* iph;
61
62   assert(ipPacket != NULL);
63
64   iph = (struct ip*) ipPacket;
65   if ((ntohs(iph->ip_off) & IP_OFFMASK) != 0)
66   {
67     return 1;
68   }
69   return 0;
70 }
71
72 /* -------------------------------------------------------------------------
73  * Function   : GetTotalLength
74  * Description: Retrieve the total length of the IP packet (in bytes) of
75  *              an IP packet
76  * Input      : ipPacket - the IP packet
77  * Output     : none
78  * Return     : IP packet length
79  * Data Used  : none
80  * ------------------------------------------------------------------------- */
81 u_int16_t GetTotalLength(unsigned char* ipPacket)
82 {
83   struct iphdr* iph;
84
85   assert(ipPacket != NULL);
86
87   iph = (struct iphdr*) ipPacket;
88   return ntohs(iph->tot_len);
89 }
90
91 /* -------------------------------------------------------------------------
92  * Function   : GetHeaderLength
93  * Description: Retrieve the IP header length (in bytes) of an IP packet
94  * Input      : ipPacket - the IP packet
95  * Output     : none
96  * Return     : IP header length
97  * Data Used  : none
98  * ------------------------------------------------------------------------- */
99 unsigned int GetHeaderLength(unsigned char* ipPacket)
100 {
101   struct iphdr* iph;
102
103   assert(ipPacket != NULL);
104
105   iph = (struct iphdr*) ipPacket;
106   return iph->ihl << 2;
107 }
108
109 /* -------------------------------------------------------------------------
110  * Function   : GetTtl
111  * Description: Retrieve the TTL (Time To Live) value from the IP header of
112  *              an IP packet
113  * Input      : ipPacket - the IP packet
114  * Output     : none
115  * Return     : TTL value
116  * Data Used  : none
117  * ------------------------------------------------------------------------- */
118 u_int8_t GetTtl(unsigned char* ipPacket)
119 {
120   struct iphdr* iph;
121
122   assert(ipPacket != NULL);
123
124   iph = (struct iphdr*) ipPacket;
125   return iph->ttl;
126 }
127
128 /* -------------------------------------------------------------------------
129  * Function   : SaveTtlAndChecksum
130  * Description: Save the TTL (Time To Live) value and IP checksum as found in
131  *              the IP header of an IP packet
132  * Input      : ipPacket - the IP packet
133  * Output     : sttl - the TTL and checksum values
134  * Return     : none
135  * Data Used  : none
136  * ------------------------------------------------------------------------- */
137 void SaveTtlAndChecksum(unsigned char* ipPacket, struct TSaveTtl* sttl)
138 {
139   struct iphdr* iph;
140
141   assert(ipPacket != NULL && sttl != NULL);
142
143   iph = (struct iphdr*) ipPacket;
144   sttl->ttl = iph->ttl;
145   sttl->check = ntohs(iph->check);
146 }
147
148 /* -------------------------------------------------------------------------
149  * Function   : RestoreTtlAndChecksum
150  * Description: Restore the TTL (Time To Live) value and IP checksum in
151  *              the IP header of an IP packet
152  * Input      : ipPacket - the IP packet
153  *              sttl - the TTL and checksum values
154  * Output     : none
155  * Return     : none
156  * Data Used  : none
157  * ------------------------------------------------------------------------- */
158 void RestoreTtlAndChecksum(unsigned char* ipPacket, struct TSaveTtl* sttl)
159 {
160   struct iphdr* iph;
161
162   assert(ipPacket != NULL && sttl != NULL);
163
164   iph = (struct iphdr*) ipPacket;
165   iph->ttl = sttl->ttl;
166   iph->check = htons(sttl->check);
167 }
168
169 /* -------------------------------------------------------------------------
170  * Function   : DecreaseTtlAndUpdateHeaderChecksum
171  * Description: For an IP packet, decrement the TTL value and update the IP header
172  *              checksum accordingly.
173  * Input      : ipPacket - the IP packet
174  * Output     : none
175  * Return     : none
176  * Data Used  : none
177  * Notes      : See also RFC1141
178  * ------------------------------------------------------------------------- */
179 void DecreaseTtlAndUpdateHeaderChecksum(unsigned char* ipPacket)
180 {
181   struct iphdr* iph;
182   u_int32_t sum;
183
184   assert(ipPacket != NULL);
185
186   iph = (struct iphdr*) ipPacket;
187
188   iph->ttl--; /* decrement ttl */
189   sum = ntohs(iph->check) + 0x100; /* increment checksum high byte */
190   iph->check = htons(sum + (sum>>16)); /* add carry */
191 }
192
193 /* -------------------------------------------------------------------------
194  * Function   : GetEtherType
195  * Description: Retrieve the EtherType of an Ethernet frame
196  * Input      : ethernetFrame - the Ethernet frame
197  * Output     : none
198  * Return     : EtherType
199  * Data Used  : none
200  * ------------------------------------------------------------------------- */
201 u_int16_t GetEtherType(unsigned char* ethernetFrame)
202 {
203   u_int16_t type;
204   memcpy(&type, ethernetFrame + ETH_TYPE_OFFSET, 2);
205   return ntohs(type);
206 }
207
208 /* -------------------------------------------------------------------------
209  * Function   : GetIpHeader
210  * Description: Retrieve the IP header from an Ethernet frame
211  * Input      : ethernetFrame - the Ethernet frame
212  * Output     : none
213  * Return     : IP header
214  * Data Used  : none
215  * ------------------------------------------------------------------------- */
216 struct ip* GetIpHeader(unsigned char* ethernetFrame)
217 {
218   return (struct ip*)(ethernetFrame + IP_HDR_OFFSET);
219 }
220
221 /* -------------------------------------------------------------------------
222  * Function   : GetIpPacket
223  * Description: Retrieve the IP packet from an Ethernet frame
224  * Input      : ethernetFrame - the Ethernet frame
225  * Output     : none
226  * Return     : IP packet
227  * Data Used  : none
228  * ------------------------------------------------------------------------- */
229 unsigned char* GetIpPacket(unsigned char* ethernetFrame)
230 {
231   return ethernetFrame + IP_HDR_OFFSET;
232 }
233
234 /* -------------------------------------------------------------------------
235  * Function   : GetFrameLength
236  * Description: Return the Ethernet frame length of an Ethernet frame containing
237  *              an IP packet
238  * Input      : ethernetFrame - the Ethernet frame
239  * Output     : none
240  * Return     : The frame length
241  * Data Used  : none
242  * ------------------------------------------------------------------------- */
243 u_int16_t GetFrameLength(unsigned char* ethernetFrame)
244 {
245   return GetTotalLength(GetIpPacket(ethernetFrame)) + IP_HDR_OFFSET;
246 }
247
248 /* -------------------------------------------------------------------------
249  * Function   : SetFrameSourceMac
250  * Description: Set the source MAC address of an Ethernet frame
251  * Input      : ethernetFrame - the Ethernet frame
252               : srcMac - the source MAC address
253  * Output     : none
254  * Return     : none
255  * Data Used  : none
256  * ------------------------------------------------------------------------- */
257 void SetFrameSourceMac(unsigned char* ethernetFrame, unsigned char* srcMac)
258 {
259   memcpy(ethernetFrame + IFHWADDRLEN, srcMac, IFHWADDRLEN);
260 }
261
262 /* -------------------------------------------------------------------------
263  * Function   : GetEthernetFrame
264  * Description: Retrieve the Ethernet frame from BMF encapsulation UDP data
265  * Input      : encapsulationUdpData - the encapsulation UDP data
266  * Output     : none
267  * Return     : The Ethernet frame
268  * Data Used  : none
269  * ------------------------------------------------------------------------- */
270 unsigned char* GetEthernetFrame(unsigned char* encapsulationUdpData)
271 {
272   return encapsulationUdpData + ENCAP_HDR_LEN;
273 }
274
275 /* -------------------------------------------------------------------------
276  * Function   : GetEncapsulationUdpDataLength
277  * Description: Return the length of BMF encapsulation UDP data
278  * Input      : encapsulationUdpData - the encapsulation UDP data
279  * Output     : none
280  * Return     : The packet length
281  * Data Used  : none
282  * ------------------------------------------------------------------------- */
283 u_int16_t GetEncapsulationUdpDataLength(unsigned char* encapsulationUdpData)
284 {
285   return GetFrameLength(GetEthernetFrame(encapsulationUdpData)) + ENCAP_HDR_LEN;
286 }
287