void gnrc_icmpv6_echo_req_handle(gnrc_netif_t *netif, ipv6_hdr_t *ipv6_hdr,
icmpv6_echo_t *echo, uint16_t len)
{
uint8_t *payload = ((uint8_t *)echo) + sizeof(icmpv6_echo_t);
gnrc_pktsnip_t *hdr, *pkt;
if ((echo == NULL) || (len < sizeof(icmpv6_echo_t))) {
DEBUG("icmpv6_echo: echo was NULL or len (%" PRIu16
") was < sizeof(icmpv6_echo_t)\n", len);
return;
}
pkt = gnrc_icmpv6_echo_build(ICMPV6_ECHO_REP, byteorder_ntohs(echo->id),
byteorder_ntohs(echo->seq), payload,
len - sizeof(icmpv6_echo_t));
if (pkt == NULL) {
DEBUG("icmpv6_echo: no space left in packet buffer\n");
return;
}
if (ipv6_addr_is_multicast(&ipv6_hdr->dst)) {
hdr = gnrc_ipv6_hdr_build(pkt, NULL, &ipv6_hdr->src);
}
else {
hdr = gnrc_ipv6_hdr_build(pkt, &ipv6_hdr->dst, &ipv6_hdr->src);
}
if (hdr == NULL) {
DEBUG("icmpv6_echo: no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return;
}
pkt = hdr;
hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (netif != NULL) {
((gnrc_netif_hdr_t *)hdr->data)->if_pid = netif->pid;
}
else {
/* ipv6_hdr->dst is loopback address */
((gnrc_netif_hdr_t *)hdr->data)->if_pid = KERNEL_PID_UNDEF;
}
LL_PREPEND(pkt, hdr);
if (!gnrc_netapi_dispatch_send(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL,
pkt)) {
DEBUG("icmpv6_echo: no receivers for IPv6 packets\n");
gnrc_pktbuf_release(pkt);
}
}
gnrc_pktsnip_t *gnrc_netreg_hdr_build(gnrc_nettype_t type, gnrc_pktsnip_t *payload,
uint8_t *src, uint8_t src_len,
uint8_t *dst, uint8_t dst_len)
{
switch (type) {
#ifdef MODULE_GNRC_IPV6
case GNRC_NETTYPE_IPV6:
return gnrc_ipv6_hdr_build(payload, src, src_len, dst, dst_len);
#endif
#ifdef MODULE_GNRC_TCP
case GNRC_NETTYPE_TCP:
return gnrc_tcp_hdr_build(payload, src, src_len, dst, dst_len);
#endif
#ifdef MODULE_GNRC_UDP
case GNRC_NETTYPE_UDP:
return gnrc_udp_hdr_build(payload, src, src_len, dst, dst_len);
#endif
default:
(void)payload;
(void)src;
(void)src_len;
(void)dst;
(void)dst_len;
return NULL;
}
}
static gnrc_pktsnip_t *_build_ipv6_packet(const ipv6_addr_t *src,
const ipv6_addr_t *dst, uint8_t nh,
void *data, size_t data_len,
uint16_t netif)
{
gnrc_pktsnip_t *netif_hdr, *ipv6, *payload;
ipv6_hdr_t *ipv6_hdr;
if ((netif > INT16_MAX) || (data_len > UINT16_MAX)) {
return NULL;
}
payload = gnrc_pktbuf_add(NULL, data, data_len, GNRC_NETTYPE_UNDEF);
if (payload == NULL) {
return NULL;
}
ipv6 = gnrc_ipv6_hdr_build(NULL, src, dst);
if (ipv6 == NULL) {
return NULL;
}
ipv6_hdr = ipv6->data;
ipv6_hdr->len = byteorder_htons((uint16_t)payload->size);
ipv6_hdr->nh = nh;
ipv6_hdr->hl = 64;
LL_APPEND(payload, 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(payload, netif_hdr);
return payload;
}
void _gnrc_rpl_send(gnrc_pktsnip_t *pkt, ipv6_addr_t *src, ipv6_addr_t *dst,
ipv6_addr_t *dodag_id)
{
gnrc_pktsnip_t *hdr;
ipv6_addr_t all_RPL_nodes = GNRC_RPL_ALL_NODES_ADDR, ll_addr;
kernel_pid_t iface = gnrc_ipv6_netif_find_by_addr(NULL, &all_RPL_nodes);
if (iface == KERNEL_PID_UNDEF) {
DEBUG("RPL: no suitable interface found for this destination address\n");
gnrc_pktbuf_release(pkt);
return;
}
if (src == NULL) {
ipv6_addr_t *tmp = NULL;
if (dodag_id != NULL) {
tmp = gnrc_ipv6_netif_match_prefix(iface, dodag_id);
}
else if (dodag_id == NULL) {
tmp = gnrc_ipv6_netif_find_best_src_addr(iface, &all_RPL_nodes);
}
if (tmp == NULL) {
DEBUG("RPL: no suitable src address found\n");
gnrc_pktbuf_release(pkt);
return;
}
memcpy(&ll_addr, tmp, sizeof(ll_addr));
ipv6_addr_set_link_local_prefix(&ll_addr);
src = &ll_addr;
}
if (dst == NULL) {
dst = &all_RPL_nodes;
}
hdr = gnrc_ipv6_hdr_build(pkt, (uint8_t *)src, sizeof(ipv6_addr_t), (uint8_t *)dst,
sizeof(ipv6_addr_t));
if (hdr == NULL) {
DEBUG("RPL: Send - no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return;
}
if (!gnrc_netapi_dispatch_send(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL,hdr)) {
DEBUG("RPL: cannot send packet: no subscribers found.\n");
gnrc_pktbuf_release(hdr);
}
}
void gnrc_rpl_send(gnrc_pktsnip_t *pkt, kernel_pid_t iface, ipv6_addr_t *src, ipv6_addr_t *dst,
ipv6_addr_t *dodag_id)
{
(void)dodag_id;
gnrc_pktsnip_t *hdr;
if (iface == KERNEL_PID_UNDEF) {
if ((iface = gnrc_ipv6_netif_find_by_addr(NULL, &ipv6_addr_all_rpl_nodes))
== KERNEL_PID_UNDEF) {
DEBUG("RPL: no suitable interface found for this destination address\n");
gnrc_pktbuf_release(pkt);
return;
}
}
if (src == NULL) {
src = gnrc_ipv6_netif_match_prefix(iface, &ipv6_addr_link_local_prefix);
if (src == NULL) {
DEBUG("RPL: no suitable src address found\n");
gnrc_pktbuf_release(pkt);
return;
}
}
if (dst == NULL) {
dst = (ipv6_addr_t *) &ipv6_addr_all_rpl_nodes;
}
hdr = gnrc_ipv6_hdr_build(pkt, src, dst);
if (hdr == NULL) {
DEBUG("RPL: Send - no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return;
}
pkt = hdr;
hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
((gnrc_netif_hdr_t *)hdr->data)->if_pid = iface;
LL_PREPEND(pkt, hdr);
if (!gnrc_netapi_dispatch_send(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL, pkt)) {
DEBUG("RPL: cannot send packet: no subscribers found.\n");
gnrc_pktbuf_release(pkt);
}
}
int conn_udp_sendto(const void *data, size_t len, const void *src, size_t src_len,
const void *dst, size_t dst_len, int family, uint16_t sport,
uint16_t dport)
{
gnrc_pktsnip_t *pkt, *hdr = NULL;
pkt = gnrc_pktbuf_add(NULL, (void *)data, len, GNRC_NETTYPE_UNDEF); /* data will only be copied */
hdr = gnrc_udp_hdr_build(pkt, (uint8_t *)&sport, sizeof(uint16_t), (uint8_t *)&dport,
sizeof(uint16_t));
if (hdr == NULL) {
gnrc_pktbuf_release(pkt);
return -ENOMEM;
}
pkt = hdr;
switch (family) {
#ifdef MODULE_GNRC_IPV6
case AF_INET6:
if (((src != NULL) && (src_len != sizeof(ipv6_addr_t))) ||
(dst_len != sizeof(ipv6_addr_t))) {
gnrc_pktbuf_release(pkt);
return -EINVAL;
}
/* addr will only be copied */
hdr = gnrc_ipv6_hdr_build(pkt, (uint8_t *)src, src_len, (uint8_t *)dst, dst_len);
if (hdr == NULL) {
gnrc_pktbuf_release(pkt);
return -ENOMEM;
}
pkt = hdr;
break;
#endif /* MODULE_GNRC_IPV6 */
default:
(void)hdr;
(void)src;
(void)src_len;
(void)dst;
(void)dst_len;
gnrc_pktbuf_release(pkt);
return -EAFNOSUPPORT;
}
gnrc_netapi_dispatch_send(GNRC_NETTYPE_UDP, GNRC_NETREG_DEMUX_CTX_ALL, pkt);
return len;
}
int conn_ip_sendto(const void *data, size_t len, const void *src, size_t src_len,
void *dst, size_t dst_len, int family, int proto)
{
gnrc_pktsnip_t *pkt, *hdr = NULL;
gnrc_nettype_t l3_type;
pkt = gnrc_pktbuf_add(NULL, (void *)data, len, GNRC_NETTYPE_UNDEF); /* data will only be copied */
switch (family) {
#ifdef MODULE_GNRC_IPV6
case AF_INET6:
if (((src != NULL) && (src_len != sizeof(ipv6_addr_t))) ||
(dst_len != sizeof(ipv6_addr_t)) ||
(((unsigned)proto) > 256U)) {
gnrc_pktbuf_release(pkt);
return -EINVAL;
}
/* addr will only be copied */
hdr = gnrc_ipv6_hdr_build(pkt, (uint8_t *)src, src_len, (uint8_t *)dst, dst_len);
if (hdr == NULL) {
gnrc_pktbuf_release(pkt);
return -ENOMEM;
}
/* set next header to connection's proto */
ipv6_hdr_t *ipv6_hdr = hdr->data;
ipv6_hdr->nh = (uint8_t)proto;
pkt = hdr;
l3_type = GNRC_NETTYPE_IPV6;
break;
#endif /* MODULE_GNRC_IPV6 */
default:
(void)src;
(void)src_len;
(void)dst;
(void)dst_len;
(void)proto;
(void)hdr;
gnrc_pktbuf_release(pkt);
return -EAFNOSUPPORT;
}
gnrc_netapi_dispatch_send(l3_type, GNRC_NETREG_DEMUX_CTX_ALL, pkt);
return len;
}
static gnrc_pktsnip_t *_build_headers(kernel_pid_t iface, gnrc_pktsnip_t *payload,
ipv6_addr_t *dst, ipv6_addr_t *src)
{
gnrc_pktsnip_t *l2hdr;
gnrc_pktsnip_t *iphdr = gnrc_ipv6_hdr_build(payload, (uint8_t *)src, sizeof(ipv6_addr_t),
(uint8_t *)dst, sizeof(ipv6_addr_t));
if (iphdr == NULL) {
DEBUG("ndp internal: error allocating IPv6 header.\n");
return NULL;
}
((ipv6_hdr_t *)iphdr->data)->hl = 255;
/* add netif header for send interface specification */
l2hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
if (l2hdr == NULL) {
DEBUG("ndp internal: error allocating netif header.\n");
gnrc_pktbuf_remove_snip(iphdr, iphdr);
return NULL;
}
((gnrc_netif_hdr_t *)l2hdr->data)->if_pid = iface;
LL_PREPEND(iphdr, l2hdr);
return l2hdr;
}
tftp_state _tftp_send(gnrc_pktsnip_t *buf, tftp_context_t *ctxt, size_t len)
{
network_uint16_t src_port, dst_port;
gnrc_pktsnip_t *udp, *ip;
assert(len <= TFTP_DEFAULT_DATA_SIZE);
/* down-size the packet to it's used size */
if (len > TFTP_DEFAULT_DATA_SIZE) {
DEBUG("tftp: can't reallocate to bigger packet, buffer overflowed\n");
gnrc_pktbuf_release(buf);
if (ctxt->stop_cb) {
ctxt->stop_cb(TFTP_INTERN_ERROR, "buffer overflowed");
}
return TS_FAILED;
}
else if (gnrc_pktbuf_realloc_data(buf, len) != 0) {
assert(false);
DEBUG("tftp: failed to reallocate data snippet\n");
gnrc_pktbuf_release(buf);
/* inform the user that we can't reallocate */
if (ctxt->stop_cb) {
ctxt->stop_cb(TFTP_INTERN_ERROR, "no reallocate");
}
return TS_FAILED;
}
/* allocate UDP header, set source port := destination port */
src_port.u16 = ctxt->src_port;
dst_port.u16 = ctxt->dst_port;
udp = gnrc_udp_hdr_build(buf, src_port.u16, dst_port.u16);
if (udp == NULL) {
DEBUG("tftp: error unable to allocate UDP header\n");
gnrc_pktbuf_release(buf);
if (ctxt->stop_cb) {
ctxt->stop_cb(TFTP_INTERN_ERROR, "no udp allocate");
}
return TS_FAILED;
}
/* allocate IPv6 header */
ip = gnrc_ipv6_hdr_build(udp, NULL, &(ctxt->peer));
if (ip == NULL) {
DEBUG("tftp: error unable to allocate IPv6 header\n");
gnrc_pktbuf_release(udp);
if (ctxt->stop_cb) {
ctxt->stop_cb(TFTP_INTERN_ERROR, "no ip allocate");
}
return TS_FAILED;
}
/* send packet */
if (gnrc_netapi_dispatch_send(GNRC_NETTYPE_UDP, GNRC_NETREG_DEMUX_CTX_ALL,
ip) == 0) {
/* if send failed inform the user */
DEBUG("tftp: error unable to locate UDP thread\n");
gnrc_pktbuf_release(ip);
if (ctxt->stop_cb) {
ctxt->stop_cb(TFTP_INTERN_ERROR, "no dispatch send");
}
return TS_FAILED;
}
/* only set timeout if enabled for this block */
if (ctxt->block_timeout) {
ctxt->timer_msg.type = TFTP_TIMEOUT_MSG;
xtimer_set_msg(&(ctxt->timer), ctxt->block_timeout, &(ctxt->timer_msg), thread_getpid());
DEBUG("tftp: set timeout %" PRIu32 " ms\n", ctxt->block_timeout / MS_IN_USEC);
}
return TS_BUSY;
}
void gnrc_icmpv6_echo_req_handle(kernel_pid_t iface, ipv6_hdr_t *ipv6_hdr,
icmpv6_echo_t *echo, uint16_t len)
{
uint8_t *payload = ((uint8_t *)echo) + sizeof(icmpv6_echo_t);
gnrc_pktsnip_t *hdr, *pkt;
gnrc_netreg_entry_t *sendto = NULL;
if ((echo == NULL) || (len < sizeof(icmpv6_echo_t))) {
DEBUG("icmpv6_echo: echo was NULL or len (%" PRIu16
") was < sizeof(icmpv6_echo_t)\n", len);
return;
}
pkt = gnrc_icmpv6_echo_build(ICMPV6_ECHO_REP, byteorder_ntohs(echo->id),
byteorder_ntohs(echo->seq), payload,
len - sizeof(icmpv6_echo_t));
if (pkt == NULL) {
DEBUG("icmpv6_echo: no space left in packet buffer\n");
return;
}
if (ipv6_addr_is_multicast(&ipv6_hdr->dst)) {
hdr = gnrc_ipv6_hdr_build(pkt, NULL, 0, (uint8_t *)&ipv6_hdr->src,
sizeof(ipv6_addr_t));
}
else {
hdr = gnrc_ipv6_hdr_build(pkt, (uint8_t *)&ipv6_hdr->dst,
sizeof(ipv6_addr_t), (uint8_t *)&ipv6_hdr->src,
sizeof(ipv6_addr_t));
}
if (hdr == NULL) {
DEBUG("icmpv6_echo: no space left in packet buffer\n");
gnrc_pktbuf_release(pkt);
return;
}
pkt = hdr;
hdr = gnrc_netif_hdr_build(NULL, 0, NULL, 0);
((gnrc_netif_hdr_t *)hdr->data)->if_pid = iface;
LL_PREPEND(pkt, hdr);
sendto = gnrc_netreg_lookup(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL);
if (sendto == NULL) {
DEBUG("icmpv6_echo: no receivers for IPv6 packets\n");
gnrc_pktbuf_release(pkt);
return;
}
/* ICMPv6 is not interested anymore so `- 1` */
gnrc_pktbuf_hold(pkt, gnrc_netreg_num(GNRC_NETTYPE_IPV6, GNRC_NETREG_DEMUX_CTX_ALL) - 1);
while (sendto != NULL) {
if (gnrc_netapi_send(sendto->pid, pkt) < 1) {
DEBUG("icmpv6_echo: unable to send packet\n");
gnrc_pktbuf_release(pkt);
}
sendto = gnrc_netreg_getnext(sendto);
}
}
