767 lines
24 KiB
C
767 lines
24 KiB
C
/****************************************************************************
|
|
* net/sixlowpan/sixlowpan_framelist.c
|
|
*
|
|
* Copyright (C) 2017-2018 Gregory Nutt. All rights reserved.
|
|
* Author: Gregory Nutt <gnutt@nuttx.org>
|
|
*
|
|
* Parts of this file derive from Contiki:
|
|
*
|
|
* Copyright (c) 2008, Swedish Institute of Computer Science.
|
|
* All rights reserved.
|
|
* Authors: Adam Dunkels <adam@sics.se>
|
|
* Nicolas Tsiftes <nvt@sics.se>
|
|
* Niclas Finne <nfi@sics.se>
|
|
* Mathilde Durvy <mdurvy@cisco.com>
|
|
* Julien Abeille <jabeille@cisco.com>
|
|
* Joakim Eriksson <joakime@sics.se>
|
|
* Joel Hoglund <joel@sics.se>
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
* 3. Neither the name of the Institute nor the names of its contributors
|
|
* may be used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
|
|
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
|
|
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
|
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
|
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
|
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
|
* SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Included Files
|
|
****************************************************************************/
|
|
|
|
#include <nuttx/config.h>
|
|
|
|
#include <string.h>
|
|
#include <assert.h>
|
|
#include <errno.h>
|
|
#include <debug.h>
|
|
|
|
#include <nuttx/mm/iob.h>
|
|
#include <nuttx/net/netdev.h>
|
|
#include <nuttx/net/radiodev.h>
|
|
#include <nuttx/wireless/ieee802154/ieee802154_mac.h>
|
|
|
|
#include "utils/utils.h"
|
|
#include "sixlowpan/sixlowpan_internal.h"
|
|
|
|
#ifdef CONFIG_NET_6LOWPAN
|
|
|
|
/****************************************************************************
|
|
* Pre-processor Definitions
|
|
****************************************************************************/
|
|
|
|
/* Configuration ************************************************************/
|
|
|
|
/* A single IOB must be big enough to hold a full frame. This we have to
|
|
* check at run time. A IOB must also be big enough to hold the maximum MAC
|
|
* header (25 bytes?) plus the FCS and have some amount of space left for
|
|
* the payload.
|
|
*/
|
|
|
|
#define MAX_MACHDR 25 /* REVISIT: This is IEEE 802.15.4 specific */
|
|
#if CONFIG_IOB_BUFSIZE < (SIXLOWPAN_MAC_FCSSIZE + MAX_MACHDR)
|
|
# error CONFIG_IOB_BUFSIZE too small to hold a IEEE802.14.5 frame
|
|
#endif
|
|
|
|
/* There must be at least enough IOBs to hold the full MTU. Probably still
|
|
* won't work unless there are a few more.
|
|
*/
|
|
|
|
#if CONFIG_NET_6LOWPAN_PKTSIZE > (CONFIG_IOB_BUFSIZE * CONFIG_IOB_NBUFFERS)
|
|
# error Not enough IOBs to hold one full 6LoWPAN packet
|
|
#endif
|
|
|
|
/****************************************************************************
|
|
* Private Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: sixlowpan_compress_ipv6hdr
|
|
*
|
|
* Description:
|
|
* IPv6 dispatch "compression" function. Packets "Compression" when only
|
|
* IPv6 dispatch is used
|
|
*
|
|
* There is no compression in this case, all fields are sent
|
|
* inline. We just add the IPv6 dispatch byte before the packet.
|
|
*
|
|
* 0 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
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* | IPv6 Dsp | IPv6 header and payload ...
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
*
|
|
* Input Parameters:
|
|
* ipv6hdr - Pointer to the IPv6 header to "compress"
|
|
* fptr - Pointer to the beginning of the frame under construction
|
|
*
|
|
* Returned Value:
|
|
* None
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int sixlowpan_compress_ipv6hdr(FAR const struct ipv6_hdr_s *ipv6hdr,
|
|
FAR uint8_t *fptr)
|
|
{
|
|
/* Indicate the IPv6 dispatch and length */
|
|
|
|
fptr[g_frame_hdrlen] = SIXLOWPAN_DISPATCH_IPV6;
|
|
g_frame_hdrlen += SIXLOWPAN_IPV6_HDR_LEN;
|
|
|
|
/* Copy the IPv6 header and adjust pointers */
|
|
|
|
memcpy(&fptr[g_frame_hdrlen], ipv6hdr, IPv6_HDRLEN);
|
|
g_frame_hdrlen += IPv6_HDRLEN;
|
|
g_uncomp_hdrlen += IPv6_HDRLEN;
|
|
|
|
return COMPRESS_HDR_INLINE;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: sixlowpan_protosize
|
|
*
|
|
* Description:
|
|
* Get the size of any uncompressed protocol header that follows the
|
|
* IPv6 header.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static uint16_t sixlowpan_protosize(FAR const struct ipv6_hdr_s *ipv6hdr,
|
|
FAR uint8_t *fptr)
|
|
{
|
|
uint16_t protosize;
|
|
|
|
/* Copy the following protocol header, */
|
|
|
|
switch (ipv6hdr->proto)
|
|
{
|
|
#ifdef CONFIG_NET_TCP
|
|
case IP_PROTO_TCP:
|
|
{
|
|
FAR struct tcp_hdr_s *tcp =
|
|
&((FAR struct ipv6tcp_hdr_s *)ipv6hdr)->tcp;
|
|
|
|
/* The TCP header length is encoded in the top 4 bits of the
|
|
* tcpoffset field (in units of 32-bit words).
|
|
*/
|
|
|
|
protosize = ((uint16_t)tcp->tcpoffset >> 4) << 2;
|
|
}
|
|
break;
|
|
#endif
|
|
|
|
#ifdef CONFIG_NET_UDP
|
|
case IP_PROTO_UDP:
|
|
protosize = UDP_HDRLEN;
|
|
break;
|
|
#endif
|
|
|
|
#ifdef CONFIG_NET_ICMPv6
|
|
case IP_PROTO_ICMP6:
|
|
protosize = ICMPv6_HDRLEN;
|
|
break;
|
|
#endif
|
|
|
|
default:
|
|
nwarn("WARNING: Unrecognized proto: %u\n", ipv6hdr->proto);
|
|
protosize = 0;
|
|
break;
|
|
}
|
|
|
|
return protosize;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: sixlowpan_ieee802154_metadata
|
|
*
|
|
* Description:
|
|
* Create the meta data that describes the IEEE 802.15.4 MAC header.
|
|
*
|
|
* Input Parameters:
|
|
* radio - The radio network driver instance
|
|
* destmac - The IEEE802.15.4 MAC address of the destination
|
|
* meta - Location to return the final metadata.
|
|
*
|
|
* Returned Value:
|
|
* OK is returned on success; Otherwise a negated errno value is returned.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#ifdef CONFIG_WIRELESS_IEEE802154
|
|
static int sixlowpan_ieee802154_metadata(FAR struct radio_driver_s *radio,
|
|
FAR const struct netdev_varaddr_s *destmac,
|
|
FAR union sixlowpan_metadata_u *meta)
|
|
{
|
|
struct ieee802_txmetadata_s pktmeta;
|
|
int ret;
|
|
|
|
/* Reset frame meta data */
|
|
|
|
memset(&pktmeta, 0, sizeof(struct ieee802_txmetadata_s));
|
|
pktmeta.xmits = CONFIG_NET_6LOWPAN_MAX_MACTRANSMITS;
|
|
|
|
/* Set stream mode for all TCP packets, except FIN packets. */
|
|
|
|
#if 0 /* Currently the frame type is always data */
|
|
if (ipv6->proto == IP_PROTO_TCP)
|
|
{
|
|
FAR const struct tcp_hdr_s *tcp =
|
|
&((FAR const struct ipv6tcp_hdr_s *)ipv6)->tcp;
|
|
|
|
if ((tcp->flags & TCP_FIN) == 0 &&
|
|
(tcp->flags & TCP_CTL) != TCP_ACK)
|
|
{
|
|
pktmeta.type = FRAME_ATTR_TYPE_STREAM;
|
|
}
|
|
else if ((tcp->flags & TCP_FIN) == TCP_FIN)
|
|
{
|
|
pktmeta.type = FRAME_ATTR_TYPE_STREAM_END;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
/* Set the source and destination address. The source MAC address
|
|
* is a fixed size, determined by a configuration setting. The
|
|
* destination MAC address many be either short or extended.
|
|
*/
|
|
|
|
#ifdef CONFIG_NET_6LOWPAN_EXTENDEDADDR
|
|
pktmeta.sextended = TRUE;
|
|
sixlowpan_eaddrcopy(pktmeta.source.nm_addr,
|
|
&radio->r_dev.d_mac.radio.nv_addr);
|
|
#else
|
|
sixlowpan_saddrcopy(pktmeta.source.nm_addr,
|
|
&radio->r_dev.d_mac.radio.nv_addr);
|
|
#endif
|
|
|
|
/* Copy the destination node address into the meta data */
|
|
|
|
if (destmac->nv_addrlen == NET_6LOWPAN_EADDRSIZE)
|
|
{
|
|
pktmeta.dextended = TRUE;
|
|
sixlowpan_eaddrcopy(pktmeta.dest.nm_addr, destmac->nv_addr);
|
|
}
|
|
else
|
|
{
|
|
DEBUGASSERT(destmac->nv_addrlen == NET_6LOWPAN_SADDRSIZE);
|
|
sixlowpan_saddrcopy(pktmeta.dest.nm_addr, destmac->nv_addr);
|
|
}
|
|
|
|
/* Get the destination PAN ID.
|
|
*
|
|
* REVISIT: For now I am assuming that the source and destination
|
|
* PAN IDs are the same.
|
|
*/
|
|
|
|
sixlowpan_src_panid(radio, pktmeta.dpanid);
|
|
|
|
/* Based on the collected attributes and addresses, construct the MAC meta
|
|
* data structure that we need to interface with the IEEE802.15.4 MAC (we
|
|
* will update the MSDU payload size when the IOB has been setup).
|
|
*/
|
|
|
|
ret = sixlowpan_meta_data(radio, &pktmeta, &meta->ieee802154);
|
|
if (ret < 0)
|
|
{
|
|
nerr("ERROR: sixlowpan_meta_data() failed: %d\n", ret);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
#endif
|
|
|
|
/****************************************************************************
|
|
* Name: sixlowpan_pktradio_metadata
|
|
*
|
|
* Description:
|
|
* Create the meta data that describes the MAC header for a generic radio.
|
|
*
|
|
* Input Parameters:
|
|
* radio - The radio network driver instance
|
|
* destmac - The radio-specific MAC address of the destination
|
|
* meta - Location to return the final metadata.
|
|
*
|
|
* Returned Value:
|
|
* OK is returned on success; Otherwise a negated errno value is returned.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#ifdef CONFIG_WIRELESS_PKTRADIO
|
|
static int sixlowpan_pktradio_metadata(FAR struct radio_driver_s *radio,
|
|
FAR const struct netdev_varaddr_s *destmac,
|
|
FAR union sixlowpan_metadata_u *meta)
|
|
{
|
|
FAR struct pktradio_metadata_s *pktmeta = &meta->pktradio;
|
|
|
|
/* Reset the meta data */
|
|
|
|
memset(pktmeta, 0, sizeof(struct pktradio_metadata_s));
|
|
|
|
/* Set the source address */
|
|
|
|
pktmeta->pm_src.pa_addrlen = radio->r_dev.d_mac.radio.nv_addrlen;
|
|
memcpy(pktmeta->pm_src.pa_addr,
|
|
radio->r_dev.d_mac.radio.nv_addr,
|
|
radio->r_dev.d_mac.radio.nv_addrlen);
|
|
|
|
/* Set the destination address.
|
|
* REVISIT: Do we need to check for multicast or broadcast addresses
|
|
* here?
|
|
*/
|
|
|
|
pktmeta->pm_dest.pa_addrlen = destmac->nv_addrlen;
|
|
memcpy(pktmeta->pm_dest.pa_addr, destmac->nv_addr, destmac->nv_addrlen);
|
|
return OK;
|
|
}
|
|
#endif
|
|
|
|
/****************************************************************************
|
|
* Public Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: sixlowpan_queue_frames
|
|
*
|
|
* Description:
|
|
* Process an outgoing UDP, TCP, or ICMPv6 packet. This function is
|
|
* called from send event handler when a TX poll is received. It
|
|
* formats the list of frames to be sent by the IEEE802.15.4 MAC driver.
|
|
*
|
|
* The payload data is in the caller 'buf' and is of length 'buflen'.
|
|
* Compressed headers will be added and if necessary the packet is
|
|
* fragmented. The resulting packet/fragments are submitted to the MAC
|
|
* where they are queue for transfer.
|
|
*
|
|
* Input Parameters:
|
|
* radio - The radio network driver instance
|
|
* ipv6 - IPv6 header followed by TCP, UDP, or ICMPv6 header.
|
|
* buf - Beginning of the packet packet to send (with IPv6 + protocol
|
|
* headers)
|
|
* buflen - Length of data to send (includes IPv6 and protocol headers)
|
|
* destmac - The IEEE802.15.4 MAC address of the destination
|
|
*
|
|
* Returned Value:
|
|
* Ok is returned on success; Otherwise a negated errno value is returned.
|
|
* This function is expected to fail if the driver is not an IEEE802.15.4
|
|
* MAC network driver. In that case, the UDP/TCP will fall back to normal
|
|
* IPv4/IPv6 formatting.
|
|
*
|
|
* Assumptions:
|
|
* Called with the network locked.
|
|
*
|
|
****************************************************************************/
|
|
|
|
int sixlowpan_queue_frames(FAR struct radio_driver_s *radio,
|
|
FAR const struct ipv6_hdr_s *ipv6,
|
|
FAR const void *buf, size_t buflen,
|
|
FAR const struct netdev_varaddr_s *destmac)
|
|
{
|
|
union sixlowpan_metadata_u meta;
|
|
FAR struct iob_s *iob;
|
|
FAR uint8_t *fptr;
|
|
int framer_hdrlen;
|
|
struct netdev_varaddr_s bcastmac;
|
|
uint16_t framelen;
|
|
uint16_t pktlen;
|
|
uint16_t paysize;
|
|
uint16_t outlen = 0;
|
|
uint8_t protosize;
|
|
int ret;
|
|
|
|
ninfo("buflen=%lu\n", (unsigned long)buflen);
|
|
|
|
/* Initialize global data. Locking the network guarantees that we have
|
|
* exclusive use of the global values for intermediate calculations.
|
|
*/
|
|
|
|
g_uncomp_hdrlen = 0;
|
|
g_frame_hdrlen = 0;
|
|
protosize = 0;
|
|
|
|
/* The destination address will be tagged to each outbound packet. If the
|
|
* argument destmac is NULL, we are sending a broadcast packet.
|
|
*/
|
|
|
|
if (destmac == NULL)
|
|
{
|
|
memset(&bcastmac, 0, sizeof(struct netdev_varaddr_s));
|
|
destmac = &bcastmac;
|
|
}
|
|
|
|
/* Pre-allocate the IOB to hold frame or the first fragment, waiting if
|
|
* necessary.
|
|
*/
|
|
|
|
iob = net_ioballoc(false);
|
|
DEBUGASSERT(iob != NULL);
|
|
|
|
fptr = iob->io_data;
|
|
|
|
ninfo("Sending packet length %zd\n", buflen);
|
|
|
|
/* Get the metadata that describes the MAC header on the packet */
|
|
|
|
#ifdef CONFIG_WIRELESS_IEEE802154
|
|
#ifdef CONFIG_WIRELESS_PKTRADIO
|
|
if (radio->r_dev.d_lltype == NET_LL_IEEE802154)
|
|
#endif
|
|
{
|
|
ret = sixlowpan_ieee802154_metadata(radio, destmac, &meta);
|
|
}
|
|
#endif
|
|
#ifdef CONFIG_WIRELESS_PKTRADIO
|
|
#ifdef CONFIG_WIRELESS_IEEE802154
|
|
else
|
|
#endif
|
|
{
|
|
ret = sixlowpan_pktradio_metadata(radio, destmac, &meta);
|
|
}
|
|
#endif
|
|
|
|
/* Pre-calculate frame header length. */
|
|
|
|
framer_hdrlen = sixlowpan_frame_hdrlen(radio, &meta);
|
|
if (framer_hdrlen < 0)
|
|
{
|
|
/* Failed to determine the size of the header failed. */
|
|
|
|
nerr("ERROR: sixlowpan_frame_hdrlen() failed: %d\n", framer_hdrlen);
|
|
return framer_hdrlen;
|
|
}
|
|
|
|
/* This sill be the initial offset into io_data. Valid data begins at
|
|
* this offset and must be reflected in io_offset.
|
|
*/
|
|
|
|
g_frame_hdrlen = framer_hdrlen;
|
|
iob->io_offset = framer_hdrlen;
|
|
|
|
#ifndef CONFIG_NET_6LOWPAN_COMPRESSION_IPv6
|
|
if (buflen >= CONFIG_NET_6LOWPAN_COMPRESSION_THRESHOLD)
|
|
{
|
|
/* Try to compress the headers */
|
|
|
|
#if defined(CONFIG_NET_6LOWPAN_COMPRESSION_HC1)
|
|
ret = sixlowpan_compresshdr_hc1(radio, ipv6, destmac, fptr);
|
|
#elif defined(CONFIG_NET_6LOWPAN_COMPRESSION_HC06)
|
|
ret = sixlowpan_compresshdr_hc06(radio, ipv6, destmac, fptr);
|
|
#else
|
|
# error No compression specified
|
|
#endif
|
|
}
|
|
else
|
|
#endif /* !CONFIG_NET_6LOWPAN_COMPRESSION_IPv6 */
|
|
{
|
|
/* Small.. use IPv6 dispatch (no compression) */
|
|
|
|
ret = sixlowpan_compress_ipv6hdr(ipv6, fptr);
|
|
}
|
|
|
|
/* Get the size of any uncompressed protocol headers */
|
|
|
|
if (ret == COMPRESS_HDR_INLINE)
|
|
{
|
|
protosize = sixlowpan_protosize(ipv6, fptr);
|
|
}
|
|
|
|
ninfo("Header of length=%u protosize=%u\n", g_frame_hdrlen, protosize);
|
|
|
|
/* Get the maximum packet size supported by this radio. */
|
|
|
|
ret = sixlowpan_radio_framelen(radio);
|
|
if (ret < 0)
|
|
{
|
|
nerr("ERROR: sixlowpan_radio_framelen() failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
/* Limit to the maximum size supported by the IOBs */
|
|
|
|
if (ret > CONFIG_IOB_BUFSIZE)
|
|
{
|
|
ret = CONFIG_IOB_BUFSIZE;
|
|
}
|
|
|
|
/* Reserve space at the end for any FCS that the hardware may include
|
|
* in the payload.
|
|
*/
|
|
|
|
ret -= SIXLOWPAN_MAC_FCSSIZE;
|
|
if (ret < MAX_MACHDR || ret > UINT16_MAX)
|
|
{
|
|
nerr("ERROR: Invalid frame size: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
framelen = (uint16_t)ret;
|
|
|
|
/* Check if we need to fragment the packet into several frames.
|
|
* We may need to reserve space at the end of the frame for a 2-byte FCS
|
|
*/
|
|
|
|
if (buflen > (framelen - g_frame_hdrlen - protosize))
|
|
{
|
|
/* qhead will hold the generated frame list; frames will be
|
|
* added at qtail.
|
|
*/
|
|
|
|
FAR struct sixlowpan_reassbuf_s *reass;
|
|
FAR struct iob_s *qhead;
|
|
FAR struct iob_s *qtail;
|
|
FAR uint8_t *frame1;
|
|
FAR uint8_t *fragptr;
|
|
uint16_t frag1_hdrlen;
|
|
|
|
/* Recover the reassembly buffer from the driver d_buf. */
|
|
|
|
reass = (FAR struct sixlowpan_reassbuf_s *)radio->r_dev.d_buf;
|
|
DEBUGASSERT(reass != NULL);
|
|
|
|
/* The outbound IPv6 packet is too large to fit into a single 15.4
|
|
* packet, so we fragment it into multiple packets and send them.
|
|
* The first fragment contains frag1 dispatch, then
|
|
* IPv6/HC1/HC06/HC_UDP dispatchs/headers.
|
|
* The following fragments contain only the fragn dispatch.
|
|
*/
|
|
|
|
ninfo("Sending fragmented packet length %zd\n", buflen);
|
|
|
|
/* Create 1st Fragment */
|
|
|
|
/* Move HC1/HC06/IPv6 header to make space for the FRAG1 header at the
|
|
* beginning of the frame.
|
|
*/
|
|
|
|
fragptr = fptr + framer_hdrlen;
|
|
memmove(fragptr + SIXLOWPAN_FRAG1_HDR_LEN, fragptr,
|
|
g_frame_hdrlen - framer_hdrlen);
|
|
|
|
/* Setup up the fragment header.
|
|
*
|
|
* The fragment header contains three fields: Datagram size, datagram
|
|
* tag and datagram offset:
|
|
*
|
|
* 1. Datagram size describes the total (un-fragmented) payload.
|
|
* 2. Datagram tag identifies the set of fragments and is used to
|
|
* match fragments of the same payload.
|
|
* 3. Datagram offset identifies the fragment's offset within the
|
|
* unfragmented payload.
|
|
*
|
|
* The fragment header length is 4 bytes for the first header and 5
|
|
* bytes for all subsequent headers.
|
|
*/
|
|
|
|
pktlen = buflen + g_uncomp_hdrlen + protosize;
|
|
PUTHOST16(fragptr, SIXLOWPAN_FRAG_DISPATCH_SIZE,
|
|
((SIXLOWPAN_DISPATCH_FRAG1 << 8) | pktlen));
|
|
PUTHOST16(fragptr, SIXLOWPAN_FRAG_TAG, reass->rb_dgramtag);
|
|
|
|
g_frame_hdrlen += SIXLOWPAN_FRAG1_HDR_LEN;
|
|
|
|
/* Copy any uncompressed protocol headers that must appear only in th
|
|
* first fragment.
|
|
*/
|
|
|
|
if (protosize > 0)
|
|
{
|
|
FAR uint8_t *src = (FAR uint8_t *)ipv6 + IPv6_HDRLEN;
|
|
memcpy(fptr + g_frame_hdrlen, src, protosize);
|
|
}
|
|
|
|
/* Copy payload and enqueue. NOTE that the size is a multiple of eight
|
|
* bytes.
|
|
*/
|
|
|
|
paysize = (framelen - g_frame_hdrlen) & ~7;
|
|
memcpy(fptr + g_frame_hdrlen + protosize, buf, paysize - protosize);
|
|
|
|
/* Set outlen to what we already sent from the IP payload */
|
|
|
|
iob->io_len = paysize + g_frame_hdrlen;
|
|
outlen = paysize;
|
|
|
|
ninfo("First fragment: length %d, tag %d\n",
|
|
paysize, reass->rb_dgramtag);
|
|
sixlowpan_dumpbuffer("Outgoing frame",
|
|
(FAR const uint8_t *)iob->io_data, iob->io_len);
|
|
|
|
/* Add the first frame to the IOB queue */
|
|
|
|
ninfo("Queuing frame io_len=%u io_offset=%u\n",
|
|
iob->io_len, iob->io_offset);
|
|
|
|
qhead = iob;
|
|
qtail = iob;
|
|
|
|
/* Keep track of the total amount of data queue */
|
|
|
|
iob->io_pktlen = iob->io_len;
|
|
|
|
/* Create following fragments */
|
|
|
|
frame1 = iob->io_data;
|
|
frag1_hdrlen = g_frame_hdrlen;
|
|
|
|
while (outlen < (buflen + protosize))
|
|
{
|
|
uint16_t fragn_hdrlen;
|
|
|
|
/* Allocate an IOB to hold the next fragment, waiting if
|
|
* necessary.
|
|
*/
|
|
|
|
iob = net_ioballoc(false);
|
|
DEBUGASSERT(iob != NULL);
|
|
|
|
/* Initialize the IOB */
|
|
|
|
iob->io_offset = framer_hdrlen;
|
|
fptr = iob->io_data;
|
|
|
|
/* Copy the HC1/HC06/IPv6 header the frame header from first
|
|
* frame, into the correct location after the FRAGN header
|
|
* of subsequent frames.
|
|
*/
|
|
|
|
fragptr = fptr + framer_hdrlen;
|
|
memcpy(fragptr + SIXLOWPAN_FRAGN_HDR_LEN,
|
|
frame1 + framer_hdrlen + SIXLOWPAN_FRAG1_HDR_LEN,
|
|
frag1_hdrlen - framer_hdrlen);
|
|
fragn_hdrlen = frag1_hdrlen - SIXLOWPAN_FRAG1_HDR_LEN;
|
|
|
|
/* Setup up the FRAGN header after the frame header. */
|
|
|
|
PUTHOST16(fragptr, SIXLOWPAN_FRAG_DISPATCH_SIZE,
|
|
((SIXLOWPAN_DISPATCH_FRAGN << 8) | pktlen));
|
|
PUTHOST16(fragptr, SIXLOWPAN_FRAG_TAG, reass->rb_dgramtag);
|
|
fragptr[SIXLOWPAN_FRAG_OFFSET] = outlen >> 3;
|
|
|
|
fragn_hdrlen += SIXLOWPAN_FRAGN_HDR_LEN;
|
|
|
|
/* Copy payload and enqueue.
|
|
*
|
|
* Check for the last fragment.
|
|
*/
|
|
|
|
paysize = (framelen - fragn_hdrlen) & SIXLOWPAN_DISPATCH_FRAG_MASK;
|
|
if (paysize > buflen - outlen + protosize)
|
|
{
|
|
/* Last fragment, truncate to the correct length */
|
|
|
|
paysize = buflen - outlen + protosize;
|
|
}
|
|
|
|
memcpy(fptr + fragn_hdrlen, buf + outlen - protosize, paysize);
|
|
|
|
/* Set outlen to what we already sent from the IP payload */
|
|
|
|
iob->io_len = paysize + fragn_hdrlen;
|
|
outlen += paysize;
|
|
|
|
ninfo("Fragment offset=%d, paysize=%d, rb_dgramtag=%d\n",
|
|
outlen >> 3, paysize, reass->rb_dgramtag);
|
|
|
|
sixlowpan_dumpbuffer("Outgoing frame",
|
|
(FAR const uint8_t *)iob->io_data,
|
|
iob->io_len);
|
|
|
|
/* Add the next frame to the tail of the IOB queue */
|
|
|
|
ninfo("Queuing frame io_len=%u io_offset=%u\n",
|
|
iob->io_len, iob->io_offset);
|
|
|
|
qtail->io_flink = iob;
|
|
qtail = iob;
|
|
|
|
/* Keep track of the total amount of data queue */
|
|
|
|
qhead->io_pktlen += iob->io_len;
|
|
}
|
|
|
|
/* Submit all of the fragments to the MAC. We send all frames back-
|
|
* to-back like this to minimize any possible condition where some
|
|
* frame which is not a fragment from this sequence from intervening.
|
|
*/
|
|
|
|
for (iob = qhead; iob != NULL; iob = qhead)
|
|
{
|
|
/* Remove the IOB containing the frame from the list */
|
|
|
|
qhead = iob->io_flink;
|
|
iob->io_flink = NULL;
|
|
|
|
/* And submit the frame to the MAC */
|
|
|
|
ninfo("Submitting frame\n");
|
|
ret = sixlowpan_frame_submit(radio, &meta, iob);
|
|
if (ret < 0)
|
|
{
|
|
nerr("ERROR: sixlowpan_frame_submit() failed: %d\n", ret);
|
|
}
|
|
}
|
|
|
|
/* Update the datagram TAG value */
|
|
|
|
reass->rb_dgramtag++;
|
|
}
|
|
else
|
|
{
|
|
/* The packet does not need to be fragmented just copy the "payload"
|
|
* and send in one frame.
|
|
*/
|
|
|
|
/* Copy any uncompressed protocol headers that must appear only in th
|
|
* first fragment.
|
|
*/
|
|
|
|
if (protosize > 0)
|
|
{
|
|
FAR uint8_t *src = (FAR uint8_t *)ipv6 + IPv6_HDRLEN;
|
|
memcpy(fptr + g_frame_hdrlen, src, protosize);
|
|
}
|
|
|
|
/* Copy the payload into the frame. */
|
|
|
|
memcpy(fptr + g_frame_hdrlen + protosize, buf, buflen);
|
|
iob->io_len = buflen + g_frame_hdrlen + protosize;
|
|
iob->io_pktlen = iob->io_len;
|
|
|
|
ninfo("Non-fragmented: length %d\n", iob->io_len);
|
|
sixlowpan_dumpbuffer("Outgoing frame",
|
|
(FAR const uint8_t *)iob->io_data, iob->io_len);
|
|
|
|
/* And submit the frame to the MAC */
|
|
|
|
ninfo("Submitting frame length=%u io_offset=%u\n",
|
|
iob->io_len, iob->io_offset);
|
|
|
|
ret = sixlowpan_frame_submit(radio, &meta, iob);
|
|
if (ret < 0)
|
|
{
|
|
nerr("ERROR: sixlowpan_frame_submit() failed: %d\n", ret);
|
|
}
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
#endif /* CONFIG_NET_6LOWPAN */
|