static inline void _addr_set_multicast(uint8_t *dst, gnrc_pktsnip_t *payload)
{
switch (payload->type) {
#ifdef MODULE_IPV6
case GNRC_NETTYPE_IPV6:
dst[0] = 0x33;
dst[1] = 0x33;
memcpy(dst + 2, ((uint8_t *)payload->data) + _IPV6_DST_OFFSET, 4);
/* TODO change to proper types when gnrc_ipv6_hdr_t got merged */
break;
#endif
default:
_addr_set_broadcast(dst);
break;
}
}
static inline void _addr_set_multicast(uint8_t *dst, ng_pktsnip_t *payload)
{
switch (payload->type) {
#ifdef MODULE_NG_IPV6
case NG_NETTYPE_IPV6:
dst[0] = 0x33;
dst[1] = 0x33;
if ((payload != NULL) && (payload->data != NULL)) {
ipv6_hdr_t *hdr = payload->data;
uint16_t *prefix = (uint16_t *)(&dst[2]);
prefix[0] = hdr->dst.u16[6].u16;
prefix[1] = hdr->dst.u16[7].u16;
}
break;
#endif
default:
_addr_set_broadcast(dst);
break;
}
}
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;
}
static int _send(gnrc_netdev2_t *gnrc_netdev2, gnrc_pktsnip_t *pkt)
{
ethernet_hdr_t hdr;
gnrc_netif_hdr_t *netif_hdr;
gnrc_pktsnip_t *payload;
netdev2_t *dev = gnrc_netdev2->dev;
if (pkt == NULL) {
DEBUG("gnrc_netdev2_eth: pkt was NULL");
return -EINVAL;
}
payload = pkt->next;
if (pkt->type != GNRC_NETTYPE_NETIF) {
DEBUG("gnrc_netdev2_eth: First header was not generic netif header\n");
return -EBADMSG;
}
if (payload) {
hdr.type = byteorder_htons(gnrc_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, gnrc_netif_hdr_get_src_addr(netif_hdr),
netif_hdr->src_l2addr_len);
}
else {
dev->driver->get(dev, NETOPT_ADDRESS, hdr.src, ETHERNET_ADDR_LEN);
}
if (netif_hdr->flags & GNRC_NETIF_HDR_FLAGS_BROADCAST) {
_addr_set_broadcast(hdr.dst);
}
else if (netif_hdr->flags & GNRC_NETIF_HDR_FLAGS_MULTICAST) {
_addr_set_multicast(hdr.dst, payload);
}
else if (netif_hdr->dst_l2addr_len == ETHERNET_ADDR_LEN) {
memcpy(hdr.dst, gnrc_netif_hdr_get_dst_addr(netif_hdr),
ETHERNET_ADDR_LEN);
}
else {
DEBUG("gnrc_netdev2_eth: destination address had unexpected format\n");
return -EBADMSG;
}
DEBUG("gnrc_netdev2_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]);
size_t n;
pkt = gnrc_pktbuf_get_iovec(pkt, &n);
struct iovec *vector = (struct iovec *)pkt->data;
vector[0].iov_base = (char*)&hdr;
vector[0].iov_len = sizeof(ethernet_hdr_t);
dev->driver->send(dev, vector, n);
gnrc_pktbuf_release(pkt);
return 0;
}