/* TODO: generalize and move to ng_ieee802154 */
static size_t _make_data_frame_hdr(ng_at86rf2xx_t *dev, uint8_t *buf,
ng_netif_hdr_t *hdr)
{
int pos = 0;
/* we are building a data frame here */
buf[0] = NG_IEEE802154_FCF_TYPE_DATA;
buf[1] = NG_IEEE802154_FCF_VERS_V1;
/* if AUTOACK is enabled, then we also expect ACKs for this packet */
if (!(hdr->flags & NG_NETIF_HDR_FLAGS_BROADCAST) &&
!(hdr->flags & NG_NETIF_HDR_FLAGS_MULTICAST) &&
(dev->options & NG_AT86RF2XX_OPT_AUTOACK)) {
buf[0] |= NG_IEEE802154_FCF_ACK_REQ;
}
/* fill in destination PAN ID */
pos = 3;
buf[pos++] = (uint8_t)((dev->pan) & 0xff);
buf[pos++] = (uint8_t)((dev->pan) >> 8);
/* fill in destination address */
if (hdr->flags &
(NG_NETIF_HDR_FLAGS_BROADCAST | NG_NETIF_HDR_FLAGS_MULTICAST)) {
buf[1] |= NG_IEEE802154_FCF_DST_ADDR_SHORT;
buf[pos++] = 0xff;
buf[pos++] = 0xff;
}
else if (hdr->dst_l2addr_len == 2) {
uint8_t *dst_addr = ng_netif_hdr_get_dst_addr(hdr);
buf[1] |= NG_IEEE802154_FCF_DST_ADDR_SHORT;
buf[pos++] = dst_addr[1];
buf[pos++] = dst_addr[0];
}
else if (hdr->dst_l2addr_len == 8) {
buf[1] |= NG_IEEE802154_FCF_DST_ADDR_LONG;
uint8_t *dst_addr = ng_netif_hdr_get_dst_addr(hdr);
for (int i = 7; i >= 0; i--) {
buf[pos++] = dst_addr[i];
}
}
else {
/* unsupported address length */
return 0;
}
/* fill in source PAN ID (if applicable */
if (dev->options & NG_AT86RF2XX_OPT_USE_SRC_PAN) {
buf[pos++] = (uint8_t)((dev->pan) & 0xff);
buf[pos++] = (uint8_t)((dev->pan) >> 8);
} else {
void ng_netif_hdr_print(ng_netif_hdr_t *hdr)
{
char addr_str[NG_NETIF_HDR_L2ADDR_MAX_LEN * 3];
printf("if_pid: %" PRIkernel_pid " ", hdr->if_pid);
printf("rssi: %" PRIu8 " ", hdr->rssi);
printf("lqi: %" PRIu8 "\n", hdr->lqi);
if (hdr->src_l2addr_len > 0) {
printf("src_l2addr: %s\n",
ng_netif_addr_to_str(addr_str, sizeof(addr_str),
ng_netif_hdr_get_src_addr(hdr),
(size_t)hdr->src_l2addr_len));
}
else {
puts("src_l2addr: (nil)");
}
if (hdr->dst_l2addr_len > 0) {
printf("dst_l2addr: %s\n",
ng_netif_addr_to_str(addr_str, sizeof(addr_str),
ng_netif_hdr_get_dst_addr(hdr),
(size_t)hdr->dst_l2addr_len));
}
else {
puts("dst_l2addr: (nil)");
}
}
static ng_pktsnip_t *_build_frag_pkt(ng_pktsnip_t *pkt, size_t payload_len,
size_t size)
{
ng_netif_hdr_t *hdr = pkt->data, *new_hdr;
ng_pktsnip_t *netif, *frag;
netif = ng_netif_hdr_build(ng_netif_hdr_get_src_addr(hdr), hdr->src_l2addr_len,
ng_netif_hdr_get_dst_addr(hdr), hdr->dst_l2addr_len);
if (netif == NULL) {
DEBUG("6lo frag: error allocating new link-layer header\n");
return NULL;
}
new_hdr = netif->data;
new_hdr->if_pid = hdr->if_pid;
new_hdr->flags = hdr->flags;
new_hdr->rssi = hdr->rssi;
new_hdr->lqi = hdr->lqi;
frag = ng_pktbuf_add(NULL, NULL, _min(size, payload_len),
NG_NETTYPE_SIXLOWPAN);
if (frag == NULL) {
DEBUG("6lo frag: error allocating first fragment\n");
ng_pktbuf_release(netif);
return NULL;
}
LL_PREPEND(frag, netif);
return frag;
}
static void _dump_netif_hdr(ng_netif_hdr_t *hdr)
{
char addr_str[ADDR_STR_MAX];
printf("if_pid: %" PRIkernel_pid " ", hdr->if_pid);
printf("rssi: %" PRIu8 " ", hdr->rssi);
printf("lqi: %" PRIu8 "\n", hdr->lqi);
printf("src_l2addr: %s\n",
ng_netif_addr_to_str(addr_str, sizeof(addr_str),
ng_netif_hdr_get_src_addr(hdr),
(size_t)hdr->src_l2addr_len));
printf("dst_l2addr: %s\n",
ng_netif_addr_to_str(addr_str, sizeof(addr_str),
ng_netif_hdr_get_dst_addr(hdr),
(size_t)hdr->dst_l2addr_len));
}
static int _marshall_ethernet(ng_netdev_eth_t *dev, uint8_t *buffer, ng_pktsnip_t *pkt)
{
int data_len = 0;
ethernet_hdr_t *hdr = (ethernet_hdr_t *)buffer;
ng_netif_hdr_t *netif_hdr;
ng_pktsnip_t *payload;
if (pkt == NULL) {
DEBUG("ng_netdev_eth: pkt was NULL");
return -EINVAL;
}
payload = pkt->next;
if (pkt->type != NG_NETTYPE_NETIF) {
DEBUG("ng_netdev_eth: First header was not generic netif header\n");
return -EBADMSG;
}
if (payload) {
hdr->type = byteorder_htons(ng_nettype_to_ethertype(payload->type));
}
else {
hdr->type = byteorder_htons(ETHERTYPE_UNKNOWN);
}
netif_hdr = pkt->data;
/* set ethernet header */
if (netif_hdr->src_l2addr_len == ETHERNET_ADDR_LEN) {
memcpy(hdr->dst, ng_netif_hdr_get_src_addr(netif_hdr),
netif_hdr->src_l2addr_len);
}
else {
dev_eth_t *ethdev = dev->ethdev;
ethdev->driver->get_mac_addr(ethdev, hdr->src);
}
if (netif_hdr->flags & NG_NETIF_HDR_FLAGS_BROADCAST) {
_addr_set_broadcast(hdr->dst);
}
else if (netif_hdr->flags & NG_NETIF_HDR_FLAGS_MULTICAST) {
_addr_set_multicast(hdr->dst, payload);
}
else if (netif_hdr->dst_l2addr_len == ETHERNET_ADDR_LEN) {
memcpy(hdr->dst, ng_netif_hdr_get_dst_addr(netif_hdr),
ETHERNET_ADDR_LEN);
}
else {
DEBUG("ng_netdev_eth: destination address had unexpected format\n");
return -EBADMSG;
}
DEBUG("ng_netdev_eth: send to %02x:%02x:%02x:%02x:%02x:%02x\n",
hdr->dst[0], hdr->dst[1], hdr->dst[2],
hdr->dst[3], hdr->dst[4], hdr->dst[5]);
data_len += sizeof(ethernet_hdr_t);
while (payload != NULL) {
if ((data_len + payload->size) > ETHERNET_MAX_LEN) {
DEBUG("ng_netdev_eth: Packet too big for ethernet frame\n");
return -ENOBUFS;
}
memcpy(send_buffer + data_len, payload->data, payload->size);
data_len += payload->size;
payload = payload->next;
}
/* Pad to minimum payload size.
* Linux does this on its own, but it doesn't hurt to do it here.
* As of now only tuntaposx needs this. */
if (data_len < (ETHERNET_MIN_LEN)) {
DEBUG("ng_netdev_eth: padding data! (%d -> ", data_len);
memset(send_buffer + data_len, 0, ETHERNET_MIN_LEN - data_len);
data_len = ETHERNET_MIN_LEN;
DEBUG("%d)\n", data_len);
}
return data_len;
}
void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_pktsnip_t *pkt,
size_t frag_size, size_t offset)
{
rbuf_t *entry;
/* cppcheck is clearly wrong here */
/* cppcheck-suppress variableScope */
unsigned int data_offset = 0;
ng_sixlowpan_frag_t *frag = pkt->data;
rbuf_int_t *ptr;
uint8_t *data = ((uint8_t *)pkt->data) + sizeof(ng_sixlowpan_frag_t);
_rbuf_gc();
entry = _rbuf_get(ng_netif_hdr_get_src_addr(netif_hdr), netif_hdr->src_l2addr_len,
ng_netif_hdr_get_dst_addr(netif_hdr), netif_hdr->dst_l2addr_len,
byteorder_ntohs(frag->disp_size) & NG_SIXLOWPAN_FRAG_SIZE_MASK,
byteorder_ntohs(frag->tag));
if (entry == NULL) {
DEBUG("6lo rbuf: reassembly buffer full.\n");
return;
}
ptr = entry->ints;
/* dispatches in the first fragment are ignored */
if (offset == 0) {
if (data[0] == NG_SIXLOWPAN_UNCOMPRESSED) {
data++; /* skip 6LoWPAN dispatch */
frag_size--;
}
#ifdef MODULE_NG_SIXLOWPAN_IPHC
else if (ng_sixlowpan_iphc_is(data)) {
size_t iphc_len;
iphc_len = ng_sixlowpan_iphc_decode(entry->pkt, pkt,
sizeof(ng_sixlowpan_frag_t));
if (iphc_len == 0) {
DEBUG("6lo rfrag: could not decode IPHC dispatch\n");
ng_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
data += iphc_len; /* take remaining data as data */
frag_size -= iphc_len; /* and reduce frag size by IPHC dispatch length */
frag_size += sizeof(ipv6_hdr_t); /* but add IPv6 header length */
data_offset += sizeof(ipv6_hdr_t); /* start copying after IPv6 header */
}
#endif
}
else {
data++; /* FRAGN header is one byte longer (offset) */
}
if ((offset + frag_size) > entry->pkt->size) {
DEBUG("6lo rfrag: fragment too big for resulting datagram, discarding datagram\n");
ng_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
while (ptr != NULL) {
if (_rbuf_int_in(ptr, offset, offset + frag_size - 1)) {
DEBUG("6lo rfrag: overlapping or same intervals, discarding datagram\n");
ng_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
ptr = ptr->next;
}
if (_rbuf_update_ints(entry, offset, frag_size)) {
DEBUG("6lo rbuf: add fragment data\n");
entry->cur_size += (uint16_t)frag_size;
memcpy(((uint8_t *)entry->pkt->data) + offset + data_offset, data,
frag_size - data_offset);
}
if (entry->cur_size == entry->pkt->size) {
kernel_pid_t iface = netif_hdr->if_pid;
ng_pktsnip_t *netif = ng_netif_hdr_build(entry->src, entry->src_len,
entry->dst, entry->dst_len);
if (netif == NULL) {
DEBUG("6lo rbuf: error allocating netif header\n");
ng_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
netif_hdr = netif->data;
netif_hdr->if_pid = iface;
LL_APPEND(entry->pkt, netif);
if (!ng_netapi_dispatch_receive(NG_NETTYPE_IPV6, NG_NETREG_DEMUX_CTX_ALL,
entry->pkt)) {
DEBUG("6lo rbuf: No receivers for this packet found\n");
ng_pktbuf_release(entry->pkt);
}
_rbuf_rem(entry);
}
}
void rbuf_add(ng_netif_hdr_t *netif_hdr, ng_sixlowpan_frag_t *frag,
size_t frag_size, size_t offset)
{
rbuf_t *entry;
rbuf_int_t *ptr;
uint8_t *data = ((uint8_t *)frag) + sizeof(ng_sixlowpan_frag_t);
uint16_t dg_frag_size = frag_size; /* may differ on first fragment */
_rbuf_gc();
entry = _rbuf_get(ng_netif_hdr_get_src_addr(netif_hdr), netif_hdr->src_l2addr_len,
ng_netif_hdr_get_dst_addr(netif_hdr), netif_hdr->dst_l2addr_len,
byteorder_ntohs(frag->disp_size) & NG_SIXLOWPAN_FRAG_SIZE_MASK,
byteorder_ntohs(frag->tag));
if (entry == NULL) {
DEBUG("6lo rbuf: reassembly buffer full.\n");
return;
}
ptr = entry->ints;
/* dispatches in the first fragment are ignored */
if (offset != 0) {
switch (((uint8_t *)(entry->pkt->data))[0]) {
case NG_SIXLOWPAN_UNCOMPRESSED:
offset++;
break;
default:
break;
}
data++; /* also don't take offset field */
}
else {
switch (data[0]) {
case NG_SIXLOWPAN_UNCOMPRESSED:
dg_frag_size--;
break;
default:
break;
}
}
if ((offset + frag_size) > entry->pkt->size) {
DEBUG("6lo rfrag: fragment too big for resulting datagram, discarding datagram\n");
ng_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
while (ptr != NULL) {
if (_rbuf_int_in(ptr, offset, offset + dg_frag_size - 1)) {
DEBUG("6lo rfrag: overlapping or same intervals, discarding datagram\n");
ng_pktbuf_release(entry->pkt);
_rbuf_rem(entry);
return;
}
ptr = ptr->next;
}
if (_rbuf_update_ints(entry, offset, dg_frag_size)) {
if (dg_frag_size < frag_size) {
/* some dispatches do not count to datagram size and we need
* more space because of that */
if (ng_pktbuf_realloc_data(entry->pkt, entry->pkt->size +
(frag_size - dg_frag_size)) < 0) {
DEBUG("6lo rbuf: could not reallocate packet data.\n");
return;
}
/* move already inserted fragments (frag_size - dg_frag_size) to the right */
if (entry->cur_size > 0) {
for (int i = entry->pkt->size - (frag_size - dg_frag_size); i > 0; i--) {
uint8_t *d = ((uint8_t *)(entry->pkt->data)) + i;
*d = *(d - 1);
}
}
}
DEBUG("6lo rbuf: add fragment data\n");
entry->cur_size += (uint16_t)dg_frag_size;
memcpy(((uint8_t *)entry->pkt->data) + offset, data, frag_size);
}
if (entry->cur_size == entry->datagram_size) {
kernel_pid_t iface = netif_hdr->if_pid;
ng_pktsnip_t *netif = ng_netif_hdr_build(entry->src, entry->src_len,
entry->dst, entry->dst_len);
if (netif == NULL) {
DEBUG("6lo rbuf: error allocating netif header\n");
ng_pktbuf_release(entry->pkt);
return;
}
netif_hdr = netif->data;
netif_hdr->if_pid = iface;
LL_APPEND(entry->pkt, netif);
DEBUG("6lo rbuf: datagram complete, send to self\n");
ng_netapi_receive(thread_getpid(), entry->pkt);
_rbuf_rem(entry);
}
}
int _send(ng_netdev_t *dev, ng_pktsnip_t *pkt)
{
(void)dev;
size_t size;
size_t pos = 0;
uint8_t *dst_addr;
ng_netif_hdr_t *hdr;
ng_pktsnip_t *payload;
/* check packet */
if (pkt == NULL || pkt->next == NULL) {
DEBUG("nrfmin: Error sending packet: packet incomplete\n");
return -ENOMSG;
}
/* check if payload is withing length bounds */
size = ng_pkt_len(pkt->next);
if (size > CONF_PAYLOAD_LEN) {
ng_pktbuf_release(pkt);
DEBUG("nrfmin: Error sending packet: payload to large\n");
return -EOVERFLOW;
}
/* get netif header and check address length */
hdr = (ng_netif_hdr_t *)pkt->data;
if (hdr->dst_l2addr_len != 2) {
DEBUG("nrfmin: Error sending packet: dest address has invalid size\n");
ng_pktbuf_release(pkt);
return -ENOMSG;
}
dst_addr = ng_netif_hdr_get_dst_addr(hdr);
DEBUG("nrfmin: Sending packet to %02x:%02x - size %u\n",
dst_addr[0], dst_addr[1], size);
/* wait for any ongoing transmission to finish */
while (_state == STATE_TX);
/* write data into TX buffer */
payload = pkt->next;
_tx_buf.length = 6 + size;
_tx_buf.src_addr[0] = (uint8_t)(_addr >> 8);
_tx_buf.src_addr[1] = (uint8_t)(_addr);
_tx_buf.dst_addr[0] = dst_addr[0];
_tx_buf.dst_addr[1] = dst_addr[1];
_tx_buf.proto = _nettype_to_nrftype(payload->type);
while (payload) {
memcpy(&(_tx_buf.payload[pos]), payload->data, payload->size);
pos += payload->size;
payload = payload->next;
}
/* save old state and switch to idle if applicable */
_tx_prestate = _state;
if (_tx_prestate == STATE_RX) {
_switch_to_idle();
}
/* set packet pointer to TX buffer and write destination address */
NRF_RADIO->PACKETPTR = (uint32_t)(&_tx_buf);
NRF_RADIO->BASE0 &= ~(0xffff);
NRF_RADIO->BASE0 |= ((((uint16_t)dst_addr[0]) << 8) | dst_addr[1]);
/* start transmission */
_state = STATE_TX;
NRF_RADIO->TASKS_TXEN = 1;
/* release packet */
ng_pktbuf_release(pkt);
return (int)size;
}
