Index: uspace/srv/net/inetsrv/pdu.c
===================================================================
--- uspace/srv/net/inetsrv/pdu.c	(revision 1d94e2155b4f5c86363ef8e3d6b3ceb0827e95ff)
+++ uspace/srv/net/inetsrv/pdu.c	(revision bdae1989fc54c73bf035eed7898077d77d6dd9d5)
@@ -114,7 +114,8 @@
 	
 	size_t hdr_size = sizeof(ip_header_t);
-	
-	size_t data_offs = ROUND_UP(hdr_size, 4);
-	
+	if (hdr_size >= mtu)
+		return EINVAL;
+	
+	assert(hdr_size % 4 == 0);
 	assert(offs % FRAG_OFFS_UNIT == 0);
 	assert(offs / FRAG_OFFS_UNIT < fragoff_limit);
@@ -122,7 +123,4 @@
 	/* Value for the fragment offset field */
 	uint16_t foff = offs / FRAG_OFFS_UNIT;
-	
-	if (hdr_size >= mtu)
-		return EINVAL;
 	
 	/* Amount of space in the PDU available for payload */
@@ -170,5 +168,5 @@
 	
 	/* Copy payload */
-	memcpy((uint8_t *) data + data_offs, packet->data + offs, xfer_size);
+	memcpy((uint8_t *) data + hdr_size, packet->data + offs, xfer_size);
 	
 	*rdata = data;
@@ -194,5 +192,5 @@
  * @param rdata  Place to store pointer to allocated data buffer
  * @param rsize  Place to store size of allocated data buffer
- * @param roffs Place to store offset of remaning data
+ * @param roffs  Place to store offset of remaning data
  *
  */
@@ -200,6 +198,10 @@
     size_t offs, size_t mtu, void **rdata, size_t *rsize, size_t *roffs)
 {
+	/* IPv6 mandates a minimal MTU of 1280 bytes */
+	if (mtu < 1280)
+		return ELIMIT;
+	
 	/* Upper bound for fragment offset field */
-	size_t fragoff_limit = 1 << (FF_FRAGOFF_h - FF_FRAGOFF_l);
+	size_t fragoff_limit = 1 << (OF_FRAGOFF_h - OF_FRAGOFF_l);
 	
 	/* Verify that total size of datagram is within reasonable bounds */
@@ -207,20 +209,27 @@
 		return ELIMIT;
 	
-	size_t hdr_size = sizeof(ip6_header_t);
-	
-	size_t data_offs = ROUND_UP(hdr_size, 4);
-	
+	/* Determine whether we need the Fragment extension header */
+	bool fragment;
+	if (offs == 0)
+		fragment = (packet->size + sizeof(ip6_header_t) > mtu);
+	else
+		fragment = true;
+	
+	size_t hdr_size;
+	if (fragment)
+		hdr_size = sizeof(ip6_header_t) + sizeof(ip6_header_fragment_t);
+	else
+		hdr_size = sizeof(ip6_header_t);
+	
+	if (hdr_size >= mtu)
+		return EINVAL;
+	
+	assert(sizeof(ip6_header_t) % 8 == 0);
+	assert(hdr_size % 8 == 0);
 	assert(offs % FRAG_OFFS_UNIT == 0);
 	assert(offs / FRAG_OFFS_UNIT < fragoff_limit);
 	
-#if 0
-	// FIXME TODO fragmentation
-	
 	/* Value for the fragment offset field */
 	uint16_t foff = offs / FRAG_OFFS_UNIT;
-#endif
-	
-	if (hdr_size >= mtu)
-		return EINVAL;
 	
 	/* Amount of space in the PDU available for payload */
@@ -237,13 +246,8 @@
 	size_t rem_offs = offs + xfer_size;
 	
-#if 0
-	// FIXME TODO fragmentation
-	
 	/* Flags */
-	uint16_t flags_foff =
-	    (packet->df ? BIT_V(uint16_t, FF_FLAG_DF) : 0) +
-	    (rem_offs < packet->size ? BIT_V(uint16_t, FF_FLAG_MF) : 0) +
-	    (foff << FF_FRAGOFF_l);
-#endif
+	uint16_t offsmf =
+	    (rem_offs < packet->size ? BIT_V(uint16_t, OF_FLAG_M) : 0) +
+	    (foff << OF_FRAGOFF_l);
 	
 	void *data = calloc(size, 1);
@@ -256,6 +260,4 @@
 	hdr6->ver_tc = (6 << (VI_VERSION_l));
 	memset(hdr6->tc_fl, 0, 3);
-	hdr6->payload_len = host2uint16_t_be(packet->size);
-	hdr6->next = packet->proto;
 	hdr6->hop_limit = packet->ttl;
 	
@@ -263,6 +265,28 @@
 	host2addr128_t_be(dest, hdr6->dest_addr);
 	
+	/* Optionally encode Fragment extension header fields */
+	if (fragment) {
+		assert(offsmf != 0);
+		
+		hdr6->payload_len = host2uint16_t_be(packet->size +
+		    sizeof(ip6_header_fragment_t));
+		hdr6->next = IP6_NEXT_FRAGMENT;
+		
+		ip6_header_fragment_t *hdr6f = (ip6_header_fragment_t *)
+		    (hdr6 + 1);
+		
+		hdr6f->next = packet->proto;
+		hdr6f->reserved = 0;
+		hdr6f->offsmf = host2uint16_t_be(offsmf);
+		hdr6f->id = host2uint32_t_be(packet->ident);
+	} else {
+		assert(offsmf == 0);
+		
+		hdr6->payload_len = host2uint16_t_be(packet->size);
+		hdr6->next = packet->proto;
+	}
+	
 	/* Copy payload */
-	memcpy((uint8_t *) data + data_offs, packet->data + offs, xfer_size);
+	memcpy((uint8_t *) data + hdr_size, packet->data + offs, xfer_size);
 	
 	*rdata = data;
@@ -378,19 +402,33 @@
 	size_t payload_len = uint16_t_be2host(hdr6->payload_len);
 	if (payload_len + sizeof(ip6_header_t) > size) {
-		log_msg(LOG_DEFAULT, LVL_DEBUG, "Total Length = %zu > PDU size = %zu",
+		log_msg(LOG_DEFAULT, LVL_DEBUG, "Payload Length = %zu > PDU size = %zu",
 		    payload_len + sizeof(ip6_header_t), size);
 		return EINVAL;
 	}
 	
-#if 0
-	// FIXME TODO fragmentation
-	
-	uint16_t ident = uint16_t_be2host(hdr->id);
-	uint16_t flags_foff = uint16_t_be2host(hdr->flags_foff);
-	uint16_t foff = BIT_RANGE_EXTRACT(uint16_t, FF_FRAGOFF_h, FF_FRAGOFF_l,
-	    flags_foff);
-#endif
-	
-	/* XXX Checksum */
+	uint32_t ident;
+	uint16_t offsmf;
+	uint16_t foff;
+	uint16_t next;
+	size_t data_offs = sizeof(ip6_header_t);
+	
+	/* Fragment extension header */
+	if (hdr6->next == IP6_NEXT_FRAGMENT) {
+		ip6_header_fragment_t *hdr6f = (ip6_header_fragment_t *)
+		    (hdr6 + 1);
+		
+		ident = uint32_t_be2host(hdr6f->id);
+		offsmf = uint16_t_be2host(hdr6f->offsmf);
+		foff = BIT_RANGE_EXTRACT(uint16_t, OF_FRAGOFF_h, OF_FRAGOFF_l,
+		    offsmf);
+		next = hdr6f->next;
+		data_offs += sizeof(ip6_header_fragment_t);
+		payload_len -= sizeof(ip6_header_fragment_t);
+	} else {
+		ident = 0;
+		offsmf = 0;
+		foff = 0;
+		next = hdr6->next;
+	}
 	
 	addr128_t src;
@@ -404,24 +442,11 @@
 	
 	packet->tos = 0;
-	packet->proto = hdr6->next;
+	packet->proto = next;
 	packet->ttl = hdr6->hop_limit;
-	
-#if 0
-	// FIXME TODO fragmentation
-	
 	packet->ident = ident;
-	packet->df = (flags_foff & BIT_V(uint16_t, FF_FLAG_DF)) != 0;
-	packet->mf = (flags_foff & BIT_V(uint16_t, FF_FLAG_MF)) != 0;
+	
+	packet->df = 1;
+	packet->mf = (offsmf & BIT_V(uint16_t, OF_FLAG_M)) != 0;
 	packet->offs = foff * FRAG_OFFS_UNIT;
-	
-	/* XXX IP options */
-	size_t data_offs = sizeof(uint32_t) *
-	    BIT_RANGE_EXTRACT(uint8_t, VI_IHL_h, VI_IHL_l, hdr->ver_ihl);
-#endif
-	
-	packet->ident = 0;
-	packet->df = 0;
-	packet->mf = 0;
-	packet->offs = 0;
 	
 	packet->size = payload_len;
@@ -432,5 +457,5 @@
 	}
 	
-	memcpy(packet->data, (uint8_t *) data + sizeof(ip6_header_t), packet->size);
+	memcpy(packet->data, (uint8_t *) data + data_offs, packet->size);
 	
 	return EOK;
