2 * Copyright (c) 2012 Jiri Svoboda
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
9 * - Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * - Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 * - The name of the author may not be used to endorse or promote products
15 * derived from this software without specific prior written permission.
17 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
18 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
19 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
20 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
21 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
22 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
26 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
39 #include <byteorder.h>
41 #include <fibril_synch.h>
46 #include <net/socket_codes.h>
51 /** One's complement addition.
53 * Result is a + b + carry.
55 static uint16_t inet_ocadd16(uint16_t a
, uint16_t b
)
59 s
= (uint32_t)a
+ (uint32_t)b
;
60 return (s
& 0xffff) + (s
>> 16);
63 uint16_t inet_checksum_calc(uint16_t ivalue
, void *data
, size_t size
)
72 bdata
= (uint8_t *)data
;
74 for (i
= 0; i
< words
; i
++) {
75 w
= ((uint16_t)bdata
[2*i
] << 8) | bdata
[2*i
+ 1];
76 sum
= inet_ocadd16(sum
, w
);
80 w
= ((uint16_t)bdata
[2*words
] << 8);
81 sum
= inet_ocadd16(sum
, w
);
89 * Encode internet packet into PDU (serialized form). Will encode a
90 * fragment of the payload starting at offset @a offs. The resulting
91 * PDU will have at most @a mtu bytes. @a *roffs will be set to the offset
92 * of remaining payload. If some data is remaining, the MF flag will
93 * be set in the header, otherwise the offset will equal @a packet->size.
95 * @param packet Packet to encode
96 * @param src Source address
97 * @param dest Destination address
98 * @param offs Offset into packet payload (in bytes)
99 * @param mtu MTU (Maximum Transmission Unit) in bytes
100 * @param rdata Place to store pointer to allocated data buffer
101 * @param rsize Place to store size of allocated data buffer
102 * @param roffs Place to store offset of remaning data
105 int inet_pdu_encode(inet_packet_t
*packet
, addr32_t src
, addr32_t dest
,
106 size_t offs
, size_t mtu
, void **rdata
, size_t *rsize
, size_t *roffs
)
108 /* Upper bound for fragment offset field */
109 size_t fragoff_limit
= 1 << (FF_FRAGOFF_h
- FF_FRAGOFF_l
);
111 /* Verify that total size of datagram is within reasonable bounds */
112 if (offs
+ packet
->size
> FRAG_OFFS_UNIT
* fragoff_limit
)
115 size_t hdr_size
= sizeof(ip_header_t
);
119 assert(hdr_size
% 4 == 0);
120 assert(offs
% FRAG_OFFS_UNIT
== 0);
121 assert(offs
/ FRAG_OFFS_UNIT
< fragoff_limit
);
123 /* Value for the fragment offset field */
124 uint16_t foff
= offs
/ FRAG_OFFS_UNIT
;
126 /* Amount of space in the PDU available for payload */
127 size_t spc_avail
= mtu
- hdr_size
;
128 spc_avail
-= (spc_avail
% FRAG_OFFS_UNIT
);
130 /* Amount of data (payload) to transfer */
131 size_t xfer_size
= min(packet
->size
- offs
, spc_avail
);
134 size_t size
= hdr_size
+ xfer_size
;
136 /* Offset of remaining payload */
137 size_t rem_offs
= offs
+ xfer_size
;
140 uint16_t flags_foff
=
141 (packet
->df
? BIT_V(uint16_t, FF_FLAG_DF
) : 0) +
142 (rem_offs
< packet
->size
? BIT_V(uint16_t, FF_FLAG_MF
) : 0) +
143 (foff
<< FF_FRAGOFF_l
);
145 void *data
= calloc(size
, 1);
149 /* Encode header fields */
150 ip_header_t
*hdr
= (ip_header_t
*) data
;
153 (4 << VI_VERSION_l
) | (hdr_size
/ sizeof(uint32_t));
154 hdr
->tos
= packet
->tos
;
155 hdr
->tot_len
= host2uint16_t_be(size
);
156 hdr
->id
= host2uint16_t_be(packet
->ident
);
157 hdr
->flags_foff
= host2uint16_t_be(flags_foff
);
158 hdr
->ttl
= packet
->ttl
;
159 hdr
->proto
= packet
->proto
;
161 hdr
->src_addr
= host2uint32_t_be(src
);
162 hdr
->dest_addr
= host2uint32_t_be(dest
);
164 /* Compute checksum */
165 uint16_t chksum
= inet_checksum_calc(INET_CHECKSUM_INIT
,
166 (void *) hdr
, hdr_size
);
167 hdr
->chksum
= host2uint16_t_be(chksum
);
170 memcpy((uint8_t *) data
+ hdr_size
, packet
->data
+ offs
, xfer_size
);
181 * Encode internet packet into PDU (serialized form). Will encode a
182 * fragment of the payload starting at offset @a offs. The resulting
183 * PDU will have at most @a mtu bytes. @a *roffs will be set to the offset
184 * of remaining payload. If some data is remaining, the MF flag will
185 * be set in the header, otherwise the offset will equal @a packet->size.
187 * @param packet Packet to encode
188 * @param src Source address
189 * @param dest Destination address
190 * @param offs Offset into packet payload (in bytes)
191 * @param mtu MTU (Maximum Transmission Unit) in bytes
192 * @param rdata Place to store pointer to allocated data buffer
193 * @param rsize Place to store size of allocated data buffer
194 * @param roffs Place to store offset of remaning data
197 int inet_pdu_encode6(inet_packet_t
*packet
, addr128_t src
, addr128_t dest
,
198 size_t offs
, size_t mtu
, void **rdata
, size_t *rsize
, size_t *roffs
)
200 /* IPv6 mandates a minimal MTU of 1280 bytes */
204 /* Upper bound for fragment offset field */
205 size_t fragoff_limit
= 1 << (OF_FRAGOFF_h
- OF_FRAGOFF_l
);
207 /* Verify that total size of datagram is within reasonable bounds */
208 if (offs
+ packet
->size
> FRAG_OFFS_UNIT
* fragoff_limit
)
211 /* Determine whether we need the Fragment extension header */
214 fragment
= (packet
->size
+ sizeof(ip6_header_t
) > mtu
);
220 hdr_size
= sizeof(ip6_header_t
) + sizeof(ip6_header_fragment_t
);
222 hdr_size
= sizeof(ip6_header_t
);
227 assert(sizeof(ip6_header_t
) % 8 == 0);
228 assert(hdr_size
% 8 == 0);
229 assert(offs
% FRAG_OFFS_UNIT
== 0);
230 assert(offs
/ FRAG_OFFS_UNIT
< fragoff_limit
);
232 /* Value for the fragment offset field */
233 uint16_t foff
= offs
/ FRAG_OFFS_UNIT
;
235 /* Amount of space in the PDU available for payload */
236 size_t spc_avail
= mtu
- hdr_size
;
237 spc_avail
-= (spc_avail
% FRAG_OFFS_UNIT
);
239 /* Amount of data (payload) to transfer */
240 size_t xfer_size
= min(packet
->size
- offs
, spc_avail
);
243 size_t size
= hdr_size
+ xfer_size
;
245 /* Offset of remaining payload */
246 size_t rem_offs
= offs
+ xfer_size
;
250 (rem_offs
< packet
->size
? BIT_V(uint16_t, OF_FLAG_M
) : 0) +
251 (foff
<< OF_FRAGOFF_l
);
253 void *data
= calloc(size
, 1);
257 /* Encode header fields */
258 ip6_header_t
*hdr6
= (ip6_header_t
*) data
;
260 hdr6
->ver_tc
= (6 << (VI_VERSION_l
));
261 memset(hdr6
->tc_fl
, 0, 3);
262 hdr6
->hop_limit
= packet
->ttl
;
264 host2addr128_t_be(src
, hdr6
->src_addr
);
265 host2addr128_t_be(dest
, hdr6
->dest_addr
);
267 /* Optionally encode Fragment extension header fields */
271 hdr6
->payload_len
= host2uint16_t_be(packet
->size
+
272 sizeof(ip6_header_fragment_t
));
273 hdr6
->next
= IP6_NEXT_FRAGMENT
;
275 ip6_header_fragment_t
*hdr6f
= (ip6_header_fragment_t
*)
278 hdr6f
->next
= packet
->proto
;
280 hdr6f
->offsmf
= host2uint16_t_be(offsmf
);
281 hdr6f
->id
= host2uint32_t_be(packet
->ident
);
285 hdr6
->payload_len
= host2uint16_t_be(packet
->size
);
286 hdr6
->next
= packet
->proto
;
290 memcpy((uint8_t *) data
+ hdr_size
, packet
->data
+ offs
, xfer_size
);
299 /** Decode IPv4 datagram
301 * @param data Serialized IPv4 datagram
302 * @param size Length of serialized IPv4 datagram
303 * @param packet IP datagram structure to be filled
305 * @return EOK on success
306 * @return EINVAL if the datagram is invalid or damaged
307 * @return ENOMEM if not enough memory
310 int inet_pdu_decode(void *data
, size_t size
, inet_packet_t
*packet
)
312 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "inet_pdu_decode()");
314 if (size
< sizeof(ip_header_t
)) {
315 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "PDU too short (%zu)", size
);
319 ip_header_t
*hdr
= (ip_header_t
*) data
;
321 uint8_t version
= BIT_RANGE_EXTRACT(uint8_t, VI_VERSION_h
,
322 VI_VERSION_l
, hdr
->ver_ihl
);
324 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "Version (%d) != 4", version
);
328 size_t tot_len
= uint16_t_be2host(hdr
->tot_len
);
329 if (tot_len
< sizeof(ip_header_t
)) {
330 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "Total Length too small (%zu)", tot_len
);
334 if (tot_len
> size
) {
335 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "Total Length = %zu > PDU size = %zu",
340 uint16_t ident
= uint16_t_be2host(hdr
->id
);
341 uint16_t flags_foff
= uint16_t_be2host(hdr
->flags_foff
);
342 uint16_t foff
= BIT_RANGE_EXTRACT(uint16_t, FF_FRAGOFF_h
, FF_FRAGOFF_l
,
346 inet_addr_set(uint32_t_be2host(hdr
->src_addr
), &packet
->src
);
347 inet_addr_set(uint32_t_be2host(hdr
->dest_addr
), &packet
->dest
);
348 packet
->tos
= hdr
->tos
;
349 packet
->proto
= hdr
->proto
;
350 packet
->ttl
= hdr
->ttl
;
351 packet
->ident
= ident
;
353 packet
->df
= (flags_foff
& BIT_V(uint16_t, FF_FLAG_DF
)) != 0;
354 packet
->mf
= (flags_foff
& BIT_V(uint16_t, FF_FLAG_MF
)) != 0;
355 packet
->offs
= foff
* FRAG_OFFS_UNIT
;
358 size_t data_offs
= sizeof(uint32_t) *
359 BIT_RANGE_EXTRACT(uint8_t, VI_IHL_h
, VI_IHL_l
, hdr
->ver_ihl
);
361 packet
->size
= tot_len
- data_offs
;
362 packet
->data
= calloc(packet
->size
, 1);
363 if (packet
->data
== NULL
) {
364 log_msg(LOG_DEFAULT
, LVL_WARN
, "Out of memory.");
368 memcpy(packet
->data
, (uint8_t *) data
+ data_offs
, packet
->size
);
373 /** Decode IPv6 datagram
375 * @param data Serialized IPv6 datagram
376 * @param size Length of serialized IPv6 datagram
377 * @param packet IP datagram structure to be filled
379 * @return EOK on success
380 * @return EINVAL if the datagram is invalid or damaged
381 * @return ENOMEM if not enough memory
384 int inet_pdu_decode6(void *data
, size_t size
, inet_packet_t
*packet
)
386 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "inet_pdu_decode6()");
388 if (size
< sizeof(ip6_header_t
)) {
389 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "PDU too short (%zu)", size
);
393 ip6_header_t
*hdr6
= (ip6_header_t
*) data
;
395 uint8_t version
= BIT_RANGE_EXTRACT(uint8_t, VI_VERSION_h
,
396 VI_VERSION_l
, hdr6
->ver_tc
);
398 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "Version (%d) != 6", version
);
402 size_t payload_len
= uint16_t_be2host(hdr6
->payload_len
);
403 if (payload_len
+ sizeof(ip6_header_t
) > size
) {
404 log_msg(LOG_DEFAULT
, LVL_DEBUG
, "Payload Length = %zu > PDU size = %zu",
405 payload_len
+ sizeof(ip6_header_t
), size
);
413 size_t data_offs
= sizeof(ip6_header_t
);
415 /* Fragment extension header */
416 if (hdr6
->next
== IP6_NEXT_FRAGMENT
) {
417 ip6_header_fragment_t
*hdr6f
= (ip6_header_fragment_t
*)
420 ident
= uint32_t_be2host(hdr6f
->id
);
421 offsmf
= uint16_t_be2host(hdr6f
->offsmf
);
422 foff
= BIT_RANGE_EXTRACT(uint16_t, OF_FRAGOFF_h
, OF_FRAGOFF_l
,
425 data_offs
+= sizeof(ip6_header_fragment_t
);
426 payload_len
-= sizeof(ip6_header_fragment_t
);
437 addr128_t_be2host(hdr6
->src_addr
, src
);
438 inet_addr_set6(src
, &packet
->src
);
440 addr128_t_be2host(hdr6
->dest_addr
, dest
);
441 inet_addr_set6(dest
, &packet
->dest
);
444 packet
->proto
= next
;
445 packet
->ttl
= hdr6
->hop_limit
;
446 packet
->ident
= ident
;
449 packet
->mf
= (offsmf
& BIT_V(uint16_t, OF_FLAG_M
)) != 0;
450 packet
->offs
= foff
* FRAG_OFFS_UNIT
;
452 packet
->size
= payload_len
;
453 packet
->data
= calloc(packet
->size
, 1);
454 if (packet
->data
== NULL
) {
455 log_msg(LOG_DEFAULT
, LVL_WARN
, "Out of memory.");
459 memcpy(packet
->data
, (uint8_t *) data
+ data_offs
, packet
->size
);
464 /** Encode NDP packet
466 * @param ndp NDP packet structure to be serialized
467 * @param dgram IPv6 datagram structure to be filled
469 * @return EOK on success
472 int ndp_pdu_encode(ndp_packet_t
*ndp
, inet_dgram_t
*dgram
)
474 inet_addr_set6(ndp
->sender_proto_addr
, &dgram
->src
);
475 inet_addr_set6(ndp
->target_proto_addr
, &dgram
->dest
);
477 dgram
->size
= sizeof(icmpv6_message_t
) + sizeof(ndp_message_t
);
479 dgram
->data
= calloc(1, dgram
->size
);
480 if (dgram
->data
== NULL
)
483 icmpv6_message_t
*icmpv6
= (icmpv6_message_t
*) dgram
->data
;
485 icmpv6
->type
= ndp
->opcode
;
487 memset(icmpv6
->un
.ndp
.reserved
, 0, 3);
489 ndp_message_t
*message
= (ndp_message_t
*) (icmpv6
+ 1);
491 if (ndp
->opcode
== ICMPV6_NEIGHBOUR_SOLICITATION
) {
492 host2addr128_t_be(ndp
->solicited_ip
, message
->target_address
);
494 icmpv6
->un
.ndp
.flags
= 0;
496 host2addr128_t_be(ndp
->sender_proto_addr
, message
->target_address
);
498 icmpv6
->un
.ndp
.flags
= NDP_FLAG_OVERRIDE
| NDP_FLAG_SOLICITED
;
502 addr48(ndp
->sender_hw_addr
, message
->mac
);
506 host2addr128_t_be(ndp
->sender_proto_addr
, phdr
.src_addr
);
507 host2addr128_t_be(ndp
->target_proto_addr
, phdr
.dest_addr
);
508 phdr
.length
= host2uint32_t_be(dgram
->size
);
509 memset(phdr
.zeroes
, 0, 3);
510 phdr
.next
= IP_PROTO_ICMPV6
;
513 inet_checksum_calc(INET_CHECKSUM_INIT
, &phdr
,
514 sizeof(icmpv6_phdr_t
));
516 uint16_t cs_all
= inet_checksum_calc(cs_phdr
, dgram
->data
,
519 icmpv6
->checksum
= host2uint16_t_be(cs_all
);
524 /** Decode NDP packet
526 * @param dgram Incoming IPv6 datagram encapsulating NDP packet
527 * @param ndp NDP packet structure to be filled
529 * @return EOK on success
530 * @return EINVAL if the Datagram is invalid
533 int ndp_pdu_decode(inet_dgram_t
*dgram
, ndp_packet_t
*ndp
)
535 uint16_t src_af
= inet_addr_get(&dgram
->src
, NULL
,
536 &ndp
->sender_proto_addr
);
537 if (src_af
!= AF_INET6
)
540 if (dgram
->size
< sizeof(icmpv6_message_t
) + sizeof(ndp_message_t
))
543 icmpv6_message_t
*icmpv6
= (icmpv6_message_t
*) dgram
->data
;
545 ndp
->opcode
= icmpv6
->type
;
547 ndp_message_t
*message
= (ndp_message_t
*) (icmpv6
+ 1);
549 addr128_t_be2host(message
->target_address
, ndp
->target_proto_addr
);
550 addr48(message
->mac
, ndp
->sender_hw_addr
);