subsys/net/ip/l2/ieee802154/ieee802154_fragment.c

/** @file
 * @brief 802.15.4 fragment related functions
 */

/*
 * Copyright (c) 2016 Intel Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#if defined(CONFIG_NET_DEBUG_L2_IEEE802154_FRAGMENT)
#define SYS_LOG_DOMAIN "net/ieee802154"
#define NET_LOG_ENABLED 1
#endif

#include <errno.h>
#include <net/net_core.h>
#include <net/net_pkt.h>
#include <net/net_if.h>
#include <net/net_stats.h>
#include <net/udp.h>

#include "ieee802154_fragment.h"

#include "net_private.h"
#include "6lo.h"
#include "6lo_private.h"

#define FRAG_REASSEMBLY_TIMEOUT (MSEC_PER_SEC * \
				 CONFIG_NET_L2_IEEE802154_REASSEMBLY_TIMEOUT)
#define REASS_CACHE_SIZE CONFIG_NET_L2_IEEE802154_FRAGMENT_REASS_CACHE_SIZE

static u16_t datagram_tag;

/**
 *  Reassemble cache : Depends on cache size it used for reassemble
 *  IPv6 packets simultaneously.
 */
struct frag_cache {
	struct k_delayed_work timer;	/* Reassemble timer */
	struct net_pkt *pkt;		/* Reassemble packet */
	u16_t size;			/* Datagram size */
	u16_t tag;			/* Datagram tag */
	bool used;
};

static struct frag_cache cache[REASS_CACHE_SIZE];

/**
 *  RFC 4944, section 5.3
 *  If an entire payload (e.g., IPv6) datagram fits within a single 802.15.4
 *  frame, it is unfragmented and the LoWPAN encapsulation should not contain
 *  a fragmentation header.  If the datagram does not fit within a single
 *  IEEE 802.15.4 frame, it SHALL be broken into link fragments.  As the
 *  fragment offset can only express multiples of eight bytes, all link
 *  fragments for a datagram except the last one MUST be multiples of eight
 *  bytes in length.
 *
 *  RFC 7668, section 3 (IPv6 over Bluetooth Low Energy)
 *  Functionality is comprised of link-local IPv6 addresses and stateless
 *  IPv6 address autoconfiguration, Neighbor Discovery, and header compression
 *  Fragmentation features from 6LoWPAN standards are not used due to Bluetooth
 *  LE's link-layer fragmentation support.
 */

/**
 *                     1                   2                   3
 *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |1 1 0 0 0|    datagram_size    |         datagram_tag          |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 *                     1                   2                   3
 *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |1 1 0 0 0|    datagram_size    |         datagram_tag          |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |datagram_offset|
 *  +-+-+-+-+-+-+-+-+
 */

static inline struct net_buf *prepare_new_fragment(struct net_pkt *pkt,
						   u8_t offset)
{
	struct net_buf *frag;

	frag = net_pkt_get_frag(pkt, K_FOREVER);
	if (!frag) {
		return NULL;
	}

	/* Reserve space for fragmentation header */
	if (!offset) {
		net_buf_add(frag, NET_6LO_FRAG1_HDR_LEN);
	} else {
		net_buf_add(frag, NET_6LO_FRAGN_HDR_LEN);
	}

	net_pkt_frag_add(pkt, frag);

	return frag;
}

static inline void set_datagram_size(u8_t *ptr, u16_t size)
{
	ptr[0] |= ((size & 0x7FF) >> 8);
	ptr[1] = (u8_t) size;
}

static inline void set_datagram_tag(u8_t *ptr, u16_t tag)
{
	ptr[0] = tag >> 8;
	ptr[1] = (u8_t) tag;
}

static inline void set_up_frag_hdr(struct net_buf *frag, u16_t size,
				   u8_t offset)
{
	u8_t pos = 0;

	if (offset) {
		frag->data[pos] = NET_6LO_DISPATCH_FRAGN;
	} else {
		frag->data[pos] = NET_6LO_DISPATCH_FRAG1;
	}

	set_datagram_size(frag->data, size);
	pos += NET_6LO_FRAG_DATAGRAM_SIZE_LEN;

	set_datagram_tag(frag->data + pos, datagram_tag);
	pos += NET_6LO_FRAG_DATAGRAM_OFFSET_LEN;

	if (offset) {
		frag->data[pos] = offset;
	}
}

static inline u8_t calc_max_payload(struct net_pkt *pkt,
				    struct net_buf *frag,
				    u8_t offset)
{
	u8_t max;

	max = frag->size - net_pkt_ll_reserve(pkt);
	max -= offset ? NET_6LO_FRAGN_HDR_LEN : NET_6LO_FRAG1_HDR_LEN;

	return (max & 0xF8);
}

static inline u8_t move_frag_data(struct net_buf *frag,
				  struct net_buf *next,
				  u8_t max,
				  bool first,
				  int hdr_diff,
				  u8_t *room_left)
{
	u8_t room;
	u8_t move;
	u8_t occupied;

	/* First fragment */
	if (first) {
		occupied = frag->len - NET_6LO_FRAG1_HDR_LEN;
	} else {
		occupied = frag->len - NET_6LO_FRAGN_HDR_LEN;
	}

	/* Remaining room for data */
	room = max - occupied;

	if (first) {
		room -= hdr_diff;
	}

	/* Calculate remaining room space for data to move */
	move = next->len > room ? room : next->len;

	memmove(frag->data + frag->len, next->data, move);

	net_buf_add(frag, move);

	/* Room left in current fragment */
	*room_left = room - move;

	return move;
}

static inline void compact_frag(struct net_buf *frag, u8_t moved)
{
	u8_t remaining = frag->len - moved;

	/* Move remaining data next to fragmentation header,
	 * (leave space for header).
	 */
	if (remaining) {
		memmove(frag->data, frag->data + moved, remaining);
	}

	frag->len = remaining;
}

/**
 *  ch  : compressed (IPv6) header(s)
 *  fh  : fragment header (dispatch + size + tag + [offset])
 *  p   : payload (first fragment holds IPv6 hdr as payload)
 *  e   : empty space
 *
 *  Input to ieee802154_fragment() buf chain looks like below
 *
 *  | ch + p | p | p | p | p | p + e |
 *
 *  After complete fragmentation buf chain looks like below
 *
 *  |fh + p + e | fh + p + e | fh + p + e | fh + p + e | fh + p + e |
 *
 *  Space in every fragment is because fragment payload should be multiple
 *  of 8 octets (we have predefined packets at compile time, data packet mtu
 *  is set already).
 *
 *  Create the first fragment, add fragmentation header and insert
 *  fragment at beginning of pkt, move data from next fragments to
 *  previous one, from here on insert fragmentation header and adjust
 *  data on subsequent packets.
 */
bool ieee802154_fragment(struct net_pkt *pkt, int hdr_diff)
{
	struct net_buf *frag;
	struct net_buf *next;
	u16_t processed;
	u16_t offset;
	u16_t size;
	u8_t room;
	u8_t move;
	u8_t max;
	bool first;

	if (!pkt || !pkt->frags) {
		return false;
	}

	/* If it is a single fragment do not add fragmentation header */
	if (!pkt->frags->frags) {
		return true;
	}

	/* Datagram_size: total length before compression */
	size = net_pkt_get_len(pkt) + hdr_diff;

	room = 0;
	offset = 0;
	processed = 0;
	first = true;
	datagram_tag++;

	next = pkt->frags;
	pkt->frags = NULL;

	/* First fragment has compressed header, but SIZE and OFFSET
	 * values in fragmentation header are based on uncompressed
	 * IP packet.
	 */
	while (1) {
		if (!room) {
			/* Prepare new fragment based on offset */
			frag = prepare_new_fragment(pkt, offset);
			if (!frag) {
				return false;
			}

			/* Set fragmentation header in the beginning */
			set_up_frag_hdr(frag, size, offset);

			/* Calculate max payload in multiples of 8 bytes */
			max = calc_max_payload(pkt, frag, offset);

			/* Calculate how much data is processed */
			processed += max;

			offset = processed >> 3;
		}

		/* Move data from next fragment to current fragment */
		move = move_frag_data(frag, next, max, first, hdr_diff, &room);
		first = false;

		/* Compact the next fragment */
		compact_frag(next, move);

		if (!next->len) {
			next = net_pkt_frag_del(pkt, NULL, next);
			if (!next) {
				break;
			}
		}
	}

	return true;
}

static inline u16_t get_datagram_size(u8_t *ptr)
{
	return ((ptr[0] & 0x1F) << 8) | ptr[1];
}

static inline u16_t get_datagram_tag(u8_t *ptr)
{
	return (ptr[0] << 8) | ptr[1];
}

static inline void remove_frag_header(struct net_buf *frag, u8_t hdr_len)
{
	memmove(frag->data, frag->data + hdr_len, frag->len - hdr_len);
	frag->len -= hdr_len;
}

static void update_protocol_header_lengths(struct net_pkt *pkt, u16_t size)
{
	net_pkt_set_ip_hdr_len(pkt, NET_IPV6H_LEN);

	NET_IPV6_HDR(pkt)->len[0] = (size - NET_IPV6H_LEN) >> 8;
	NET_IPV6_HDR(pkt)->len[1] = (u8_t) (size - NET_IPV6H_LEN);

	if (NET_IPV6_HDR(pkt)->nexthdr == IPPROTO_UDP) {
		struct net_udp_hdr hdr, *udp_hdr;

		udp_hdr = net_udp_get_hdr(pkt, &hdr);

		NET_ASSERT(udp_hdr);

		udp_hdr->len = htons(size - NET_IPV6H_LEN);

		net_udp_set_hdr(pkt, udp_hdr);
	}
}

static inline void clear_reass_cache(u16_t size, u16_t tag)
{
	u8_t i;

	for (i = 0; i < REASS_CACHE_SIZE; i++) {
		if (!(cache[i].size == size && cache[i].tag == tag)) {
			continue;
		}

		if (cache[i].pkt) {
			net_pkt_unref(cache[i].pkt);
		}

		cache[i].pkt = NULL;
		cache[i].size = 0;
		cache[i].tag = 0;
		cache[i].used = false;
		k_delayed_work_cancel(&cache[i].timer);
	}
}

/**
 *  If the reassembly not completed within reassembly timeout discard
 *  the whole packet.
 */
static void reass_timeout(struct k_work *work)
{
	struct frag_cache *cache = CONTAINER_OF(work, struct frag_cache, timer);

	if (cache->pkt) {
		net_pkt_unref(cache->pkt);
	}

	cache->pkt = NULL;
	cache->size = 0;
	cache->tag = 0;
	cache->used = false;
}

/**
 *  Upon reception of first fragment with respective of size and tag
 *  create a new cache. If number of unused cache are out then
 *  discard the fragments.
 */
static inline struct frag_cache *set_reass_cache(struct net_pkt *pkt,
						 u16_t size, u16_t tag)
{
	int i;

	for (i = 0; i < REASS_CACHE_SIZE; i++) {
		if (cache[i].used) {
			continue;
		}

		cache[i].pkt = pkt;
		cache[i].size = size;
		cache[i].tag = tag;
		cache[i].used = true;

		k_delayed_work_init(&cache[i].timer, reass_timeout);
		k_delayed_work_submit(&cache[i].timer, FRAG_REASSEMBLY_TIMEOUT);
		return &cache[i];
	}

	return NULL;
}

/**
 *  Return cache if it matches with size and tag of stored caches,
 *  otherwise return NULL.
 */
static inline struct frag_cache *get_reass_cache(u16_t size, u16_t tag)
{
	u8_t i;

	for (i = 0; i < REASS_CACHE_SIZE; i++) {
		if (cache[i].used) {
			if (cache[i].size == size &&
			    cache[i].tag == tag) {
				return &cache[i];
			}
		}
	}

	return NULL;
}

/* Helper function to write fragment data to Rx packet based on offset. */
static inline bool copy_frag(struct net_pkt *pkt,
			     struct net_buf *frag,
			     u16_t offset)
{
	struct net_buf *input = frag;
	struct net_buf *write;
	u16_t pos = offset;

	write = pkt->frags;

	while (input) {
		write = net_pkt_write(pkt, write, pos, &pos, input->len,
				       input->data, NET_6LO_RX_PKT_TIMEOUT);
		if (!write && pos == 0xffff) {
			/* Free the new bufs we tried to get, we need to discard
			 * the whole fragment chain.
			 */
			net_pkt_frag_unref(pkt->frags);
			pkt->frags = NULL;

			return false;
		}

		input = input->frags;
	}

	net_pkt_frag_unref(frag);

	return true;
}

/**
 *  Parse size and tag from the fragment, check if we have any cache
 *  related to it. If not create a new cache.
 *  Remove the fragmentation header and uncompress IPv6 and related headers.
 *  Cache Rx part of fragment along with data buf for the first fragment
 *  in the cache, remaining fragments just cache data fragment, unref
 *  RX pkt. So in both the cases caller can assume packet is consumed.
 */
static inline enum net_verdict add_frag_to_cache(struct net_pkt *pkt,
						 bool first)
{
	struct frag_cache *cache;
	struct net_buf *frag;
	u16_t size;
	u16_t tag;
	u16_t offset = 0;
	u8_t pos = 0;

	/* Parse total size of packet */
	size = get_datagram_size(pkt->frags->data);
	pos += NET_6LO_FRAG_DATAGRAM_SIZE_LEN;

	/* Parse the datagram tag */
	tag = get_datagram_tag(pkt->frags->data + pos);
	pos += NET_6LO_FRAG_DATAGRAM_OFFSET_LEN;

	if (!first) {
		offset = ((u16_t)pkt->frags->data[pos]) << 3;
		pos++;
	}

	/* Remove frag header and update data */
	remove_frag_header(pkt->frags, pos);

	/* Uncompress the IP headers */
	if (first && !net_6lo_uncompress(pkt)) {
		NET_ERR("Could not uncompress first frag's 6lo hdr");
		clear_reass_cache(size, tag);

		return NET_DROP;
	}

	/* If there are no fragments in the cache means this frag
	 * is the first one. So cache Rx pkt otherwise not.
	 * Write data fragment data to cached Rx based on offset parameter.
	 * (Detach data fragment from incoming Rx and copy that data).
	 */

	frag = pkt->frags;
	pkt->frags = NULL;

	cache = get_reass_cache(size, tag);
	if (!cache) {
		cache = set_reass_cache(pkt, size, tag);
		if (!cache) {
			NET_ERR("Could not get a cache entry");
			pkt->frags = frag;
			return NET_DROP;
		}

		/* If write failed, then attach frag back to incoming packet
		 * and return NET_DROP, caller will take care of freeing it.
		 */
		if (!copy_frag(cache->pkt, frag, offset)) {
			pkt->frags = frag;

			/* Initialize to NULL to prevent duble free. It's only
			 * needed here because this is the first fragment.
			 */
			cache->pkt = NULL;

			clear_reass_cache(size, tag);

			NET_ERR("Copying frag failed");

			return NET_DROP;
		}

		NET_DBG("packet inserted into cache");

		return NET_OK;
	}

	/* Add data packet to reassembly packet */
	if (!copy_frag(cache->pkt, frag, offset)) {
		pkt->frags = frag;

		clear_reass_cache(size, tag);

		return NET_DROP;
	}

	/* Check if all the fragments are received or not */
	if (net_pkt_get_len(cache->pkt) == size) {
		/* Assign frags back to input packet. */
		pkt->frags = cache->pkt->frags;
		cache->pkt->frags = NULL;

		/* Lengths are elided in compression, so calculate it. */
		update_protocol_header_lengths(pkt, cache->size);

		/* Once reassemble is done, cache is no longer needed. */
		clear_reass_cache(size, tag);

		NET_DBG("All fragments received and reassembled");

		return NET_CONTINUE;
	}

	/* Unref Rx part of original packet */
	net_pkt_unref(pkt);

	return NET_OK;
}

enum net_verdict ieee802154_reassemble(struct net_pkt *pkt)
{
	if (!pkt || !pkt->frags) {
		NET_ERR("Nothing to reassemble");
		return NET_DROP;
	}

	switch (pkt->frags->data[0] & 0xF0) {
	case NET_6LO_DISPATCH_FRAG1:
		/* First fragment with IP headers */
		return add_frag_to_cache(pkt, true);
	case NET_6LO_DISPATCH_FRAGN:
		/* Further fragments */
		return add_frag_to_cache(pkt, false);
	default:
		NET_DBG("No frag dispatch (%02x)", pkt->frags->data[0]);
		/* Received unfragmented packet, uncompress */
		if (net_6lo_uncompress(pkt)) {
			return NET_CONTINUE;
		}

		NET_ERR("Could not uncompress. Bogus packet?");
	}

	return NET_DROP;
}