You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

172 lines
4.9 KiB

/*
* Copyright (C) 2016 Freie Universität Berlin
*
* This file is subject to the terms and conditions of the GNU Lesser
* General Public License v2.1. See the file LICENSE in the top level
* directory for more details.
*/
/**
* @{
*
* @file
* @author Martine Lenders <mlenders@inf.fu-berlin.de>
*/
#include "msg.h"
#include "net/gnrc/ipv6.h"
#include "net/gnrc/netif/hdr.h"
#include "net/gnrc/netreg.h"
#include "net/gnrc/udp.h"
#include "net/sock.h"
#include "sched.h"
#include "stack.h"
#define _MSG_QUEUE_SIZE (4)
static msg_t _msg_queue[_MSG_QUEUE_SIZE];
static gnrc_netreg_entry_t _udp_handler;
void _net_init(void)
{
msg_init_queue(_msg_queue, _MSG_QUEUE_SIZE);
gnrc_netreg_entry_init_pid(&_udp_handler, GNRC_NETREG_DEMUX_CTX_ALL,
sched_active_pid);
}
void _prepare_send_checks(void)
{
gnrc_netreg_register(GNRC_NETTYPE_UDP, &_udp_handler);
}
static gnrc_pktsnip_t *_build_udp_packet(const ipv6_addr_t *src,
const ipv6_addr_t *dst,
uint16_t src_port, uint16_t dst_port,
void *data, size_t data_len,
uint16_t netif)
{
gnrc_pktsnip_t *netif_hdr, *ipv6, *udp;
udp_hdr_t *udp_hdr;
ipv6_hdr_t *ipv6_hdr;
uint16_t csum = 0;
if ((netif > INT16_MAX) || ((sizeof(udp_hdr_t) + data_len) > UINT16_MAX)) {
return NULL;
}
udp = gnrc_pktbuf_add(NULL, NULL, sizeof(udp_hdr_t) + data_len,
GNRC_NETTYPE_UNDEF);
if (udp == NULL) {
return NULL;
}
udp_hdr = udp->data;
udp_hdr->src_port = byteorder_htons(src_port);
udp_hdr->dst_port = byteorder_htons(dst_port);
udp_hdr->length = byteorder_htons((uint16_t)udp->size);
udp_hdr->checksum.u16 = 0;
memcpy(udp_hdr + 1, data, data_len);
csum = inet_csum(csum, (uint8_t *)udp->data, udp->size);
ipv6 = gnrc_ipv6_hdr_build(NULL, src, dst);
if (ipv6 == NULL) {
return NULL;
}
ipv6_hdr = ipv6->data;
ipv6_hdr->len = byteorder_htons((uint16_t)udp->size);
ipv6_hdr->nh = PROTNUM_UDP;
ipv6_hdr->hl = 64;
csum = ipv6_hdr_inet_csum(csum, ipv6_hdr, PROTNUM_UDP, (uint16_t)udp->size);
if (csum == 0xffff) {
udp_hdr->checksum = byteorder_htons(csum);
}
else {
udp_hdr->checksum = byteorder_htons(~csum);
}
LL_APPEND(udp, ipv6);
netif_hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif_hdr == NULL) {
return NULL;
}
((gnrc_netif_hdr_t *)netif_hdr->data)->if_pid = (kernel_pid_t)netif;
LL_APPEND(udp, netif_hdr);
return udp;
}
bool _inject_packet(const ipv6_addr_t *src, const ipv6_addr_t *dst,
uint16_t src_port, uint16_t dst_port,
void *data, size_t data_len, uint16_t netif)
{
gnrc_pktsnip_t *pkt = _build_udp_packet(src, dst, src_port, dst_port,
data, data_len, netif);
if (pkt == NULL) {
return false;
}
return (gnrc_netapi_dispatch_receive(GNRC_NETTYPE_UDP,
GNRC_NETREG_DEMUX_CTX_ALL, pkt) > 0);
}
bool _check_net(void)
{
return (gnrc_pktbuf_is_sane() && gnrc_pktbuf_is_empty());
}
static inline bool _res(gnrc_pktsnip_t *pkt, bool res)
{
gnrc_pktbuf_release(pkt);
return res;
}
bool _check_packet(const ipv6_addr_t *src, const ipv6_addr_t *dst,
uint16_t src_port, uint16_t dst_port,
void *data, size_t data_len, uint16_t iface,
bool random_src_port)
{
gnrc_pktsnip_t *pkt, *ipv6, *udp;
ipv6_hdr_t *ipv6_hdr;
udp_hdr_t *udp_hdr;
msg_t msg;
msg_receive(&msg);
if (msg.type != GNRC_NETAPI_MSG_TYPE_SND) {
return false;
}
pkt = msg.content.ptr;
if (iface != SOCK_ADDR_ANY_NETIF) {
gnrc_netif_hdr_t *netif_hdr;
if (pkt->type != GNRC_NETTYPE_NETIF) {
return _res(pkt, false);
}
netif_hdr = pkt->data;
if (netif_hdr->if_pid != iface) {
return _res(pkt, false);
}
ipv6 = pkt->next;
}
else {
ipv6 = pkt;
}
if (ipv6->type != GNRC_NETTYPE_IPV6) {
return _res(pkt, false);
}
ipv6_hdr = ipv6->data;
udp = gnrc_pktsnip_search_type(ipv6, GNRC_NETTYPE_UDP);
if (udp == NULL) {
return _res(pkt, false);
}
udp_hdr = udp->data;
return _res(pkt, (memcmp(src, &ipv6_hdr->src, sizeof(ipv6_addr_t)) == 0) &&
(memcmp(dst, &ipv6_hdr->dst, sizeof(ipv6_addr_t)) == 0) &&
(ipv6_hdr->nh == PROTNUM_UDP) &&
(random_src_port || (src_port == byteorder_ntohs(udp_hdr->src_port))) &&
(dst_port == byteorder_ntohs(udp_hdr->dst_port)) &&
(udp->next != NULL) &&
(data_len == udp->next->size) &&
(memcmp(data, udp->next->data, data_len) == 0));
}
/** @} */