/* 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; }