uint8_t rpl_init(transceiver_type_t trans, uint16_t rpl_address)
{
mutex_init(&rpl_send_mutex);
mutex_init(&rpl_recv_mutex);
if(rpl_address == 0) {
return SIXLOWERROR_ADDRESS;
}
/* initialize routing table */
rpl_clear_routing_table();
init_trickle();
rpl_process_pid = thread_create(rpl_process_buf, RPL_PROCESS_STACKSIZE,
PRIORITY_MAIN - 1, CREATE_STACKTEST,
rpl_process, "rpl_process");
/* INSERT NEW OBJECTIVE FUNCTIONS HERE */
objective_functions[0] = rpl_get_of0();
/* objective_functions[1] = rpl_get_of_ETX() */
sixlowpan_init(trans, rpl_address, 0);
/* Wir benötigen einen Link Local prefix, um unsere entsprechende Addresse im Netz abzufragen */
ipv6_addr_t ll_address;
ipv6_set_ll_prefix(&ll_address);
ipv6_get_saddr(&my_address, &ll_address);
set_rpl_process_pid(rpl_process_pid);
return SUCCESS;
}
static int ip6_dst_lookup_tail(struct sock *sk,
struct dst_entry **dst, struct flowi *fl)
{
int err;
if (*dst == NULL)
*dst = ip6_route_output(sk, fl);
if ((err = (*dst)->error))
goto out_err_release;
if (ipv6_addr_any(&fl->fl6_src)) {
err = ipv6_get_saddr(*dst, &fl->fl6_dst, &fl->fl6_src);
if (err)
goto out_err_release;
}
#ifdef CONFIG_IPV6_OPTIMISTIC_DAD
/*
* Here if the dst entry we've looked up
* has a neighbour entry that is in the INCOMPLETE
* state and the src address from the flow is
* marked as OPTIMISTIC, we release the found
* dst entry and replace it instead with the
* dst entry of the nexthop router
*/
if (!((*dst)->neighbour->nud_state & NUD_VALID)) {
struct inet6_ifaddr *ifp;
struct flowi fl_gw;
int redirect;
ifp = ipv6_get_ifaddr(&fl->fl6_src, (*dst)->dev, 1);
redirect = (ifp && ifp->flags & IFA_F_OPTIMISTIC);
if (ifp)
in6_ifa_put(ifp);
if (redirect) {
/*
* We need to get the dst entry for the
* default router instead
*/
dst_release(*dst);
memcpy(&fl_gw, fl, sizeof(struct flowi));
memset(&fl_gw.fl6_dst, 0, sizeof(struct in6_addr));
*dst = ip6_route_output(sk, &fl_gw);
if ((err = (*dst)->error))
goto out_err_release;
}
}
#endif
return 0;
out_err_release:
dst_release(*dst);
*dst = NULL;
return err;
}
void rpl_send(ipv6_addr_t *destination, uint8_t *payload, uint16_t p_len, uint8_t next_header, void *tcp_socket)
{
uint8_t *p_ptr;
ipv6_send_buf = get_rpl_send_ipv6_buf();
p_ptr = get_rpl_send_payload_buf(ipv6_ext_hdr_len);
packet_length = 0;
ipv6_send_buf->version_trafficclass = IPV6_VER;
ipv6_send_buf->trafficclass_flowlabel = 0;
ipv6_send_buf->flowlabel = 0;
ipv6_send_buf->nextheader = next_header;
ipv6_send_buf->hoplimit = MULTIHOP_HOPLIMIT;
ipv6_send_buf->length = p_len;
memcpy(&(ipv6_send_buf->destaddr), destination, 16);
ipv6_get_saddr(&(ipv6_send_buf->srcaddr), &(ipv6_send_buf->destaddr));
/* Wenn das Paket in der rpl.c "zusammegebaut" wurde, wurde dafür ohnehin
* der rpl_send_buf verwendet. In diesem Fall muss also keine memcopy
* Aktion durchgeführt werden, da sich der payload bereits im richtigen
* Speicherbereich befindet. */
if(p_ptr != payload) {
memcpy(p_ptr, payload, p_len);
}
packet_length = IPV6_HDR_LEN + p_len;
if(ipv6_prefix_mcast_match(&ipv6_send_buf->destaddr)) {
lowpan_init((ieee_802154_long_t *)&(ipv6_send_buf->destaddr.uint16[4]), (uint8_t *)ipv6_send_buf);
}
else {
/* find right next hop before sending */
ipv6_addr_t *next_hop = rpl_get_next_hop(&ipv6_send_buf->destaddr);
if(next_hop == NULL) {
if(i_am_root) {
/* oops... ich bin root und weiß nicht wohin mit dem paketn */
printf("[Error] destination unknown\n");
return;
}
else {
next_hop = rpl_get_my_preferred_parent();
if(next_hop == NULL) {
/* kein preferred parent eingetragen */
puts("[Error] no preferred parent, dropping package");
return;
}
}
}
lowpan_init((ieee_802154_long_t *)&(next_hop->uint16[4]), (uint8_t *)ipv6_send_buf);
}
}
/* send router solicitation message - RFC4861 section 4.1 */
void init_rtr_sol(uint8_t sllao)
{
ipv6_buf = get_ipv6_buf();
icmp_buf = get_icmpv6_buf(ipv6_ext_hdr_len);
packet_length = 0;
icmp_buf->type = ICMP_RTR_SOL;
icmp_buf->code = 0;
ipv6_buf->version_trafficclass = IPV6_VER;
ipv6_buf->trafficclass_flowlabel = 0;
ipv6_buf->flowlabel = 0;
ipv6_buf->nextheader = PROTO_NUM_ICMPV6;
ipv6_buf->hoplimit = ND_HOPLIMIT;
ipv6_set_all_rtrs_mcast_addr(&ipv6_buf->destaddr);
//iface_find_src_ipaddr(&ipv6_buf->srcaddr, ADDR_STATE_PREFERRED,
/* ADDR_TYPE_MULTICAST); */
ipv6_get_saddr(&(ipv6_buf->srcaddr), &(ipv6_buf->destaddr));
opt_hdr_len = RTR_SOL_LEN;
ipv6_buf->length = ICMPV6_HDR_LEN + RTR_SOL_LEN + OPT_STLLAO_MAX_LEN;
if (sllao == OPT_SLLAO) {
opt_stllao_buf = get_opt_stllao_buf(ipv6_ext_hdr_len, opt_hdr_len);
set_llao(opt_stllao_buf, OPT_SLLAO_TYPE, 2);
packet_length = IPV6_HDR_LEN + ICMPV6_HDR_LEN + ipv6_ext_hdr_len +
RTR_SOL_LEN + OPT_STLLAO_MAX_LEN;
}
else {
packet_length = IPV6_HDR_LEN + ICMPV6_HDR_LEN + ipv6_ext_hdr_len +
RTR_SOL_LEN;
}
icmp_buf->checksum = 0;
icmp_buf->checksum = ~icmpv6_csum(PROTO_NUM_ICMPV6);
#if ENABLE_DEBUG
printf("INFO: send router solicitation to: ");
ipv6_print_addr(&ipv6_buf->destaddr);
#endif
lowpan_init((ieee_802154_long_t *)&(ipv6_buf->destaddr.uint16[4]),
(uint8_t *)ipv6_buf);
}
void etx_radio(void) {
msg_t m;
radio_packet_t *p;
ieee802154_frame_t frame;
msg_init_queue(msg_que, ETX_RCV_QUEUE_SIZE);
ipv6_addr_t ll_address;
ipv6_addr_t candidate_addr;
ipv6_set_ll_prefix(&ll_address);
ipv6_get_saddr(&candidate_addr, &ll_address);
while (1) {
msg_receive(&m);
if (m.type == PKT_PENDING) {
p = (radio_packet_t*) m.content.ptr;
read_802154_frame(p->data, &frame, p->length);
if (frame.payload[0] == ETX_PKT_OPTVAL) {
//copy to receive buffer
memcpy(etx_rec_buf, &frame.payload[0], frame.payload_len);
//create IPv6 address from radio packet
//we can do the cast here since rpl nodes can only have addr
//up to 8 bits
candidate_addr.uint8[ETX_IPV6_LAST_BYTE] = (uint8_t) p->src;
//handle the beacon
mutex_lock(&etx_mutex);
etx_handle_beacon(&candidate_addr);
mutex_unlock(&etx_mutex,1);
}
p->processing--;
}
else if (m.type == ENOBUFFER) {
puts("Transceiver buffer full");
}
else {
//packet is not for me, whatever
}
}
}
static int xfrm6_get_saddr(xfrm_address_t *saddr, xfrm_address_t *daddr)
{
struct rt6_info *rt;
struct flowi fl_tunnel = {
.nl_u = {
.ip6_u = {
.daddr = *(struct in6_addr *)&daddr->a6,
},
},
};
if (!xfrm6_dst_lookup((struct xfrm_dst **)&rt, &fl_tunnel)) {
ipv6_get_saddr(&rt->u.dst, (struct in6_addr *)&daddr->a6,
(struct in6_addr *)&saddr->a6);
dst_release(&rt->u.dst);
return 0;
}
return -EHOSTUNREACH;
}
static int ip6_dst_lookup_tail(struct sock *sk,
struct dst_entry **dst, struct flowi *fl)
{
int err;
if (*dst == NULL)
*dst = ip6_route_output(sk, fl);
if ((err = (*dst)->error))
goto out_err_release;
if (ipv6_addr_any(&fl->fl6_src)) {
err = ipv6_get_saddr(*dst, &fl->fl6_dst, &fl->fl6_src);
if (err)
goto out_err_release;
}
return 0;
out_err_release:
dst_release(*dst);
*dst = NULL;
return err;
}
int ip6_dst_lookup(struct sock *sk, struct dst_entry **dst, struct flowi *fl)
{
int err = 0;
*dst = NULL;
if (sk) {
struct ipv6_pinfo *np = inet6_sk(sk);
*dst = sk_dst_check(sk, np->dst_cookie);
if (*dst) {
struct rt6_info *rt = (struct rt6_info*)*dst;
/* Yes, checking route validity in not connected
case is not very simple. Take into account,
that we do not support routing by source, TOS,
and MSG_DONTROUTE --ANK (980726)
1. If route was host route, check that
cached destination is current.
If it is network route, we still may
check its validity using saved pointer
to the last used address: daddr_cache.
We do not want to save whole address now,
(because main consumer of this service
is tcp, which has not this problem),
so that the last trick works only on connected
sockets.
2. oif also should be the same.
*/
if (((rt->rt6i_dst.plen != 128 ||
!ipv6_addr_equal(&fl->fl6_dst, &rt->rt6i_dst.addr))
&& (np->daddr_cache == NULL ||
!ipv6_addr_equal(&fl->fl6_dst, np->daddr_cache)))
|| (fl->oif && fl->oif != (*dst)->dev->ifindex)) {
dst_release(*dst);
*dst = NULL;
}
}
}
if (*dst == NULL)
*dst = ip6_route_output(sk, fl);
if ((err = (*dst)->error))
goto out_err_release;
if (ipv6_addr_any(&fl->fl6_src)) {
err = ipv6_get_saddr(*dst, &fl->fl6_dst, &fl->fl6_src);
if (err) {
#if IP6_DEBUG >= 2
printk(KERN_DEBUG "ip6_dst_lookup: "
"no available source address\n");
#endif
goto out_err_release;
}
}
return 0;
out_err_release:
dst_release(*dst);
*dst = NULL;
return err;
}
int ip6_build_xmit(struct sock *sk, inet_getfrag_t getfrag, const void *data,
struct flowi *fl, unsigned length,
struct ipv6_txoptions *opt, int hlimit, int flags)
{
struct ipv6_pinfo *np = &sk->net_pinfo.af_inet6;
struct in6_addr *final_dst = NULL;
struct dst_entry *dst;
int err = 0;
unsigned int pktlength, jumbolen, mtu;
struct in6_addr saddr;
if (opt && opt->srcrt) {
struct rt0_hdr *rt0 = (struct rt0_hdr *) opt->srcrt;
final_dst = fl->fl6_dst;
fl->fl6_dst = rt0->addr;
}
if (!fl->oif && ipv6_addr_is_multicast(fl->nl_u.ip6_u.daddr))
fl->oif = np->mcast_oif;
dst = __sk_dst_check(sk, np->dst_cookie);
if (dst) {
struct rt6_info *rt = (struct rt6_info*)dst;
/* Yes, checking route validity in not connected
case is not very simple. Take into account,
that we do not support routing by source, TOS,
and MSG_DONTROUTE --ANK (980726)
1. If route was host route, check that
cached destination is current.
If it is network route, we still may
check its validity using saved pointer
to the last used address: daddr_cache.
We do not want to save whole address now,
(because main consumer of this service
is tcp, which has not this problem),
so that the last trick works only on connected
sockets.
2. oif also should be the same.
*/
if (((rt->rt6i_dst.plen != 128 ||
ipv6_addr_cmp(fl->fl6_dst, &rt->rt6i_dst.addr))
&& (np->daddr_cache == NULL ||
ipv6_addr_cmp(fl->fl6_dst, np->daddr_cache)))
|| (fl->oif && fl->oif != dst->dev->ifindex)) {
dst = NULL;
} else
dst_hold(dst);
}
if (dst == NULL)
dst = ip6_route_output(sk, fl);
if (dst->error) {
IP6_INC_STATS(Ip6OutNoRoutes);
dst_release(dst);
return -ENETUNREACH;
}
if (fl->fl6_src == NULL) {
err = ipv6_get_saddr(dst, fl->fl6_dst, &saddr);
if (err) {
#if IP6_DEBUG >= 2
printk(KERN_DEBUG "ip6_build_xmit: "
"no available source address\n");
#endif
goto out;
}
fl->fl6_src = &saddr;
}
pktlength = length;
if (hlimit < 0) {
if (ipv6_addr_is_multicast(fl->fl6_dst))
hlimit = np->mcast_hops;
else
hlimit = np->hop_limit;
if (hlimit < 0)
hlimit = ((struct rt6_info*)dst)->rt6i_hoplimit;
}
jumbolen = 0;
if (!sk->protinfo.af_inet.hdrincl) {
pktlength += sizeof(struct ipv6hdr);
if (opt)
pktlength += opt->opt_flen + opt->opt_nflen;
if (pktlength > 0xFFFF + sizeof(struct ipv6hdr)) {
/* Jumbo datagram.
It is assumed, that in the case of hdrincl
jumbo option is supplied by user.
*/
pktlength += 8;
jumbolen = pktlength - sizeof(struct ipv6hdr);
}
}
mtu = dst->pmtu;
if (np->frag_size < mtu) {
if (np->frag_size)
mtu = np->frag_size;
else if (np->pmtudisc == IPV6_PMTUDISC_DONT)
mtu = IPV6_MIN_MTU;
}
/* Critical arithmetic overflow check.
FIXME: may gcc optimize it out? --ANK (980726)
*/
if (pktlength < length) {
ipv6_local_error(sk, EMSGSIZE, fl, mtu);
err = -EMSGSIZE;
goto out;
}
if (flags&MSG_CONFIRM)
dst_confirm(dst);
if (pktlength <= mtu) {
struct sk_buff *skb;
struct ipv6hdr *hdr;
struct net_device *dev = dst->dev;
err = 0;
if (flags&MSG_PROBE)
goto out;
skb = sock_alloc_send_skb(sk, pktlength + 15 +
dev->hard_header_len,
flags & MSG_DONTWAIT, &err);
if (skb == NULL) {
IP6_INC_STATS(Ip6OutDiscards);
goto out;
}
skb->dst = dst_clone(dst);
skb_reserve(skb, (dev->hard_header_len + 15) & ~15);
hdr = (struct ipv6hdr *) skb->tail;
skb->nh.ipv6h = hdr;
if (!sk->protinfo.af_inet.hdrincl) {
ip6_bld_1(sk, skb, fl, hlimit,
jumbolen ? sizeof(struct ipv6hdr) : pktlength);
if (opt || jumbolen) {
u8 *prev_hdr = &hdr->nexthdr;
prev_hdr = ipv6_build_nfrag_opts(skb, prev_hdr, opt, final_dst, jumbolen);
if (opt && opt->opt_flen)
ipv6_build_frag_opts(skb, prev_hdr, opt);
}
}
skb_put(skb, length);
err = getfrag(data, &hdr->saddr,
((char *) hdr) + (pktlength - length),
0, length);
if (!err) {
IP6_INC_STATS(Ip6OutRequests);
err = NF_HOOK(PF_INET6, NF_IP6_LOCAL_OUT, skb, NULL, dst->dev, ip6_maybe_reroute);
} else {
err = -EFAULT;
kfree_skb(skb);
}
} else {
if (sk->protinfo.af_inet.hdrincl || jumbolen ||
np->pmtudisc == IPV6_PMTUDISC_DO) {
ipv6_local_error(sk, EMSGSIZE, fl, mtu);
err = -EMSGSIZE;
goto out;
}
err = ip6_frag_xmit(sk, getfrag, data, dst, fl, opt, final_dst, hlimit,
flags, length, mtu);
}
/*
* cleanup
*/
out:
ip6_dst_store(sk, dst, fl->nl_u.ip6_u.daddr == &np->daddr ? &np->daddr : NULL);
if (err > 0)
err = np->recverr ? net_xmit_errno(err) : 0;
return err;
}
void init_nbr_sol(ipv6_addr_t *src, ipv6_addr_t *dest, ipv6_addr_t *targ,
uint8_t sllao, uint8_t aro)
{
ipv6_buf = get_ipv6_buf();
ipv6_buf->version_trafficclass = IPV6_VER;
ipv6_buf->trafficclass_flowlabel = 0;
ipv6_buf->flowlabel = 0;
ipv6_buf->nextheader = PROTO_NUM_ICMPV6;
ipv6_buf->hoplimit = ND_HOPLIMIT;
if (dest == NULL) {
ipv6_set_sol_node_mcast_addr(targ, &(ipv6_buf->destaddr));
}
else {
memcpy(&(ipv6_buf->destaddr.uint8[0]), &(dest->uint8[0]), 16);
}
ipv6_ext_hdr_len = 0;
icmp_buf = get_icmpv6_buf(ipv6_ext_hdr_len);
icmp_buf->type = ICMP_NBR_SOL;
icmp_buf->code = 0;
nbr_sol_buf = get_nbr_sol_buf(ipv6_ext_hdr_len);
nbr_sol_buf->reserved = 0;
memcpy(&(nbr_sol_buf->tgtaddr), targ, 16);
opt_hdr_len = NBR_SOL_LEN;
packet_length = IPV6_HDR_LEN + ICMPV6_HDR_LEN + NBR_SOL_LEN;
if (ipv6_iface_addr_match(targ) == NULL) {
if (src == NULL) {
ipv6_get_saddr(&(ipv6_buf->srcaddr), &(ipv6_buf->destaddr));
}
else {
memcpy(&(ipv6_buf->srcaddr), src, 16);
}
if (sllao == OPT_SLLAO) {
/* set sllao option */
opt_stllao_buf = get_opt_stllao_buf(ipv6_ext_hdr_len, opt_hdr_len);
set_llao(opt_stllao_buf, OPT_SLLAO_TYPE, 1);
opt_hdr_len += OPT_STLLAO_MIN_LEN;
packet_length += OPT_STLLAO_MIN_LEN;
}
}
if (aro == OPT_ARO) {
/* set aro option */
opt_aro_buf = get_opt_aro_buf(ipv6_ext_hdr_len, opt_hdr_len);
opt_aro_buf->type = OPT_ARO_TYPE;
opt_aro_buf->length = OPT_ARO_LEN;
opt_aro_buf->status = 0;
opt_aro_buf->reserved1 = 0;
opt_aro_buf->reserved2 = 0;
memcpy(&(opt_aro_buf->eui64), mac_get_eui(src), 8);
opt_hdr_len += OPT_ARO_HDR_LEN;
packet_length += OPT_ARO_HDR_LEN;
}
ipv6_buf->length = packet_length - IPV6_HDR_LEN;
icmp_buf->checksum = 0;
icmp_buf->checksum = ~icmpv6_csum(PROTO_NUM_ICMPV6);
}
void init_rtr_adv(ipv6_addr_t *addr, uint8_t sllao, uint8_t mtu, uint8_t pi,
uint8_t sixco, uint8_t abro)
{
lowpan_context_t *contexts = NULL;
abr_cache_t *msg_abr = NULL;
ipv6_buf = get_ipv6_buf();
icmp_buf = get_icmpv6_buf(ipv6_ext_hdr_len);
ipv6_buf->version_trafficclass = IPV6_VER;
ipv6_buf->trafficclass_flowlabel = 0;
ipv6_buf->flowlabel = 0;
ipv6_buf->nextheader = PROTO_NUM_ICMPV6;
ipv6_buf->hoplimit = ND_HOPLIMIT;
if (addr == NULL) {
/* not solicited */
ipv6_set_all_nds_mcast_addr(&ipv6_buf->destaddr);
}
else {
memcpy(&ipv6_buf->destaddr, addr, 16);
}
ipv6_get_saddr(&(ipv6_buf->srcaddr), &(ipv6_buf->destaddr));
icmp_buf->type = ICMP_RTR_ADV;
icmp_buf->code = 0;
//TODO: gethoplimit func, set current ttl
rtr_adv_buf = get_rtr_adv_buf(ipv6_ext_hdr_len);
rtr_adv_buf->hoplimit = MULTIHOP_HOPLIMIT;
/* set M and O flag, last 6 bits are zero */
rtr_adv_buf->autoconfig_flags = (RTR_ADV_M_FLAG << 7) | (RTR_ADV_O_FLAG << 6);
rtr_adv_buf->router_lifetime = HTONS(RTR_ADV_MAX_INTERVAL * RTR_ADV_MAX);
rtr_adv_buf->reachable_time = 0;
rtr_adv_buf->retrans_timer = 0;
opt_hdr_len = RTR_ADV_LEN;
packet_length = IPV6_HDR_LEN + ICMPV6_HDR_LEN + RTR_ADV_LEN;
if (sllao == OPT_SLLAO) {
/* set link layer address option */
opt_stllao_buf = get_opt_stllao_buf(ipv6_ext_hdr_len, opt_hdr_len);
set_llao(opt_stllao_buf, OPT_SLLAO_TYPE, 2);
opt_hdr_len += OPT_STLLAO_MAX_LEN;
packet_length += OPT_STLLAO_MAX_LEN;
}
if (mtu == OPT_MTU) {
/* set MTU options */
opt_mtu_buf = get_opt_mtu_buf(ipv6_ext_hdr_len, opt_hdr_len);
opt_mtu_buf->type = OPT_MTU_TYPE;
opt_mtu_buf->length = OPT_MTU_LEN;
opt_mtu_buf->reserved = 0;
opt_mtu_buf->mtu = HTONL(1500);
opt_hdr_len += OPT_MTU_HDR_LEN;
packet_length += OPT_MTU_HDR_LEN;
}
/* set payload length field */
if (abro == OPT_ABRO) {
/* set authoritive border router option */
if (abr_count > 0) {
msg_abr = abr_get_most_current();
opt_abro_buf = get_opt_abro_buf(ipv6_ext_hdr_len, opt_hdr_len);
opt_abro_buf->type = OPT_ABRO_TYPE;
opt_abro_buf->length = OPT_ABRO_LEN;
opt_abro_buf->version = HTONS(msg_abr->version);
opt_abro_buf->reserved = 0;
memcpy(&(opt_abro_buf->addr), &(msg_abr->abr_addr), sizeof(ipv6_addr_t));
}
}
if (sixco == OPT_6CO) {
/* set 6lowpan context option */
int contexts_len = 0;
mutex_lock(&lowpan_context_mutex);
if (msg_abr == NULL) {
contexts = lowpan_context_get();
contexts_len = lowpan_context_len();
}
else {
lowpan_context_t c_tmp[LOWPAN_CONTEXT_MAX];
contexts_len = 0;
for (int i = 0; i < LOWPAN_CONTEXT_MAX; i++) {
lowpan_context_t *ctx = abr_get_context(msg_abr, i);
if (ctx != NULL) {
memcpy(&(c_tmp[contexts_len++]), ctx, sizeof(lowpan_context_t));
}
}
contexts = (lowpan_context_t *)calloc(contexts_len, sizeof(lowpan_context_t));
memcpy(contexts, c_tmp, contexts_len);
}
for (int i = 0; i < contexts_len; i++) {
opt_6co_hdr_buf = get_opt_6co_hdr_buf(ipv6_ext_hdr_len, opt_hdr_len);
opt_6co_hdr_buf->type = OPT_6CO_TYPE;
if (contexts[i].length > 64) {
opt_6co_hdr_buf->length = OPT_6CO_MAX_LEN;
}
else {
opt_6co_hdr_buf->length = OPT_6CO_MIN_LEN;
}
opt_6co_hdr_buf->c_length = contexts[i].length;
opt_6co_hdr_buf->c_flags = set_opt_6co_flags(contexts[i].comp, contexts[i].num);
opt_6co_hdr_buf->reserved = 0;
opt_6co_hdr_buf->val_ltime = HTONS(contexts[i].lifetime);
opt_hdr_len += OPT_6CO_HDR_LEN;
packet_length += OPT_6CO_HDR_LEN;
/* attach prefixes */
opt_6co_prefix_buf = get_opt_6co_prefix_buf(ipv6_ext_hdr_len, opt_hdr_len);
if (opt_6co_hdr_buf->c_length > 64) {
memset((void *)opt_6co_prefix_buf, 0, 16);
memcpy((void *)opt_6co_prefix_buf, (void *) & (contexts[i].prefix.uint8[0]), opt_6co_hdr_buf->c_length / 8);
opt_hdr_len += 16;
packet_length += 16;
}
else {
memset((void *)opt_6co_prefix_buf, 0, 8);
memcpy((void *)opt_6co_prefix_buf, (void *) & (contexts[i].prefix.uint8[0]), opt_6co_hdr_buf->c_length / 8);
opt_hdr_len += 8;
packet_length += 8;
}
}
if (msg_abr != NULL && contexts != NULL) {
free(contexts);
}
mutex_unlock(&lowpan_context_mutex, 0);
}
if (pi == OPT_PI) {
/* set prefix option */
for (int i = 0; i < OPT_PI_LIST_LEN; i++) {
if (plist[i].inuse && plist[i].adv) {
opt_pi_buf = get_opt_pi_buf(ipv6_ext_hdr_len, opt_hdr_len);
memcpy(&(opt_pi_buf->addr.uint8[0]), &(plist[i].addr.uint8[0]), 16);
opt_pi_buf->type = OPT_PI_TYPE;
opt_pi_buf->length = OPT_PI_LEN;
opt_pi_buf->prefix_length = plist[i].length;
opt_pi_buf->l_a_reserved1 = plist[i].l_a_reserved1;
opt_pi_buf->val_ltime = HTONL(plist[i].val_ltime);
opt_pi_buf->pref_ltime = HTONL(plist[i].pref_ltime);
opt_pi_buf->reserved2 = 0;
packet_length += OPT_PI_HDR_LEN;
opt_hdr_len += OPT_PI_HDR_LEN;
}
}
}
ipv6_buf->length = packet_length - IPV6_HDR_LEN;
/* calculate checksum */
icmp_buf->checksum = 0;
icmp_buf->checksum = ~icmpv6_csum(PROTO_NUM_ICMPV6);
}
int udpv6_connect(struct sock *sk, struct sockaddr *uaddr, int addr_len)
{
struct sockaddr_in6 *usin = (struct sockaddr_in6 *) uaddr;
struct ipv6_pinfo *np = &sk->net_pinfo.af_inet6;
struct in6_addr *daddr;
struct in6_addr saddr;
struct dst_entry *dst;
struct flowi fl;
struct ip6_flowlabel *flowlabel = NULL;
int addr_type;
int err;
if (usin->sin6_family == AF_INET) {
err = udp_connect(sk, uaddr, addr_len);
goto ipv4_connected;
}
if (addr_len < SIN6_LEN_RFC2133)
return -EINVAL;
if (usin->sin6_family != AF_INET6)
return -EAFNOSUPPORT;
fl.fl6_flowlabel = 0;
if (np->sndflow) {
fl.fl6_flowlabel = usin->sin6_flowinfo&IPV6_FLOWINFO_MASK;
if (fl.fl6_flowlabel&IPV6_FLOWLABEL_MASK) {
flowlabel = fl6_sock_lookup(sk, fl.fl6_flowlabel);
if (flowlabel == NULL)
return -EINVAL;
ipv6_addr_copy(&usin->sin6_addr, &flowlabel->dst);
}
}
addr_type = ipv6_addr_type(&usin->sin6_addr);
if (addr_type == IPV6_ADDR_ANY) {
/*
* connect to self
*/
usin->sin6_addr.s6_addr[15] = 0x01;
}
daddr = &usin->sin6_addr;
if (addr_type == IPV6_ADDR_MAPPED) {
struct sockaddr_in sin;
sin.sin_family = AF_INET;
sin.sin_addr.s_addr = daddr->s6_addr32[3];
sin.sin_port = usin->sin6_port;
err = udp_connect(sk, (struct sockaddr*) &sin, sizeof(sin));
ipv4_connected:
if (err < 0)
return err;
ipv6_addr_set(&np->daddr, 0, 0,
__constant_htonl(0x0000ffff),
sk->daddr);
if(ipv6_addr_any(&np->saddr)) {
ipv6_addr_set(&np->saddr, 0, 0,
__constant_htonl(0x0000ffff),
sk->saddr);
}
if(ipv6_addr_any(&np->rcv_saddr)) {
ipv6_addr_set(&np->rcv_saddr, 0, 0,
__constant_htonl(0x0000ffff),
sk->rcv_saddr);
}
return 0;
}
if (addr_type&IPV6_ADDR_LINKLOCAL) {
if (addr_len >= sizeof(struct sockaddr_in6) &&
usin->sin6_scope_id) {
if (sk->bound_dev_if && sk->bound_dev_if != usin->sin6_scope_id) {
fl6_sock_release(flowlabel);
return -EINVAL;
}
sk->bound_dev_if = usin->sin6_scope_id;
}
/* Connect to link-local address requires an interface */
if (sk->bound_dev_if == 0)
return -EINVAL;
}
ipv6_addr_copy(&np->daddr, daddr);
np->flow_label = fl.fl6_flowlabel;
sk->dport = usin->sin6_port;
/*
* Check for a route to destination an obtain the
* destination cache for it.
*/
fl.proto = IPPROTO_UDP;
fl.fl6_dst = &np->daddr;
fl.fl6_src = &saddr;
fl.oif = sk->bound_dev_if;
fl.uli_u.ports.dport = sk->dport;
fl.uli_u.ports.sport = sk->sport;
if (flowlabel) {
if (flowlabel->opt && flowlabel->opt->srcrt) {
struct rt0_hdr *rt0 = (struct rt0_hdr *) flowlabel->opt->srcrt;
fl.fl6_dst = rt0->addr;
}
} else if (np->opt && np->opt->srcrt) {
struct rt0_hdr *rt0 = (struct rt0_hdr *) np->opt->srcrt;
fl.fl6_dst = rt0->addr;
}
dst = ip6_route_output(sk, &fl);
if ((err = dst->error) != 0) {
dst_release(dst);
fl6_sock_release(flowlabel);
return err;
}
ip6_dst_store(sk, dst, fl.fl6_dst);
/* get the source adddress used in the apropriate device */
err = ipv6_get_saddr(dst, daddr, &saddr);
if (err == 0) {
if(ipv6_addr_any(&np->saddr))
ipv6_addr_copy(&np->saddr, &saddr);
if(ipv6_addr_any(&np->rcv_saddr)) {
ipv6_addr_copy(&np->rcv_saddr, &saddr);
sk->rcv_saddr = LOOPBACK4_IPV6;
}
sk->state = TCP_ESTABLISHED;
}
fl6_sock_release(flowlabel);
return err;
}
/**
* @brief Parse a network packet
*
* @param[out] packet The parsed packet
* @param data The data to parse
* @param trace_cb The function to call for printing traces
* @param trace_cb_priv An optional private context, may be NULL
* @param trace_entity The entity that emits the traces
* @return true if the packet was successfully parsed,
* false if a problem occurred (a malformed packet is
* not considered as an error)
*/
bool net_pkt_parse(struct net_pkt *const packet,
const struct rohc_buf data,
rohc_trace_callback2_t trace_cb,
void *const trace_cb_priv,
rohc_trace_entity_t trace_entity)
{
packet->data = rohc_buf_data(data);
packet->len = data.len;
packet->ip_hdr_nr = 0;
packet->key = 0;
/* traces */
packet->trace_callback = trace_cb;
packet->trace_callback_priv = trace_cb_priv;
/* create the outer IP packet from raw data */
if(!ip_create(&packet->outer_ip, rohc_buf_data(data), data.len))
{
rohc_warning(packet, trace_entity, ROHC_PROFILE_GENERAL,
"cannot create the outer IP header");
goto error;
}
packet->ip_hdr_nr++;
rohc_debug(packet, trace_entity, ROHC_PROFILE_GENERAL,
"outer IP header: %u bytes", ip_get_totlen(&packet->outer_ip));
rohc_debug(packet, trace_entity, ROHC_PROFILE_GENERAL,
"outer IP header: version %d", ip_get_version(&packet->outer_ip));
if(packet->outer_ip.nh.data != NULL)
{
rohc_debug(packet, trace_entity, ROHC_PROFILE_GENERAL,
"outer IP header: next header is of type %d",
packet->outer_ip.nh.proto);
if(packet->outer_ip.nl.data != NULL)
{
rohc_debug(packet, trace_entity, ROHC_PROFILE_GENERAL,
"outer IP header: next layer is of type %d",
packet->outer_ip.nl.proto);
}
}
/* build the hash key for the packet */
if(ip_get_version(&packet->outer_ip) == IPV4)
{
packet->key ^= ipv4_get_saddr(&packet->outer_ip);
packet->key ^= ipv4_get_daddr(&packet->outer_ip);
}
else if(ip_get_version(&packet->outer_ip) == IPV6)
{
const struct ipv6_addr *const saddr = ipv6_get_saddr(&packet->outer_ip);
const struct ipv6_addr *const daddr = ipv6_get_daddr(&packet->outer_ip);
packet->key ^= saddr->addr.u32[0];
packet->key ^= saddr->addr.u32[1];
packet->key ^= saddr->addr.u32[2];
packet->key ^= saddr->addr.u32[3];
packet->key ^= daddr->addr.u32[0];
packet->key ^= daddr->addr.u32[1];
packet->key ^= daddr->addr.u32[2];
packet->key ^= daddr->addr.u32[3];
}
/* get the transport protocol */
packet->transport = &packet->outer_ip.nl;
/* is there any inner IP header? */
if(rohc_is_tunneling(packet->transport->proto))
{
/* create the second IP header */
if(!ip_get_inner_packet(&packet->outer_ip, &packet->inner_ip))
{
rohc_warning(packet, trace_entity, ROHC_PROFILE_GENERAL,
"cannot create the inner IP header");
goto error;
}
packet->ip_hdr_nr++;
rohc_debug(packet, trace_entity, ROHC_PROFILE_GENERAL,
"inner IP header: %u bytes", ip_get_totlen(&packet->inner_ip));
rohc_debug(packet, trace_entity, ROHC_PROFILE_GENERAL,
"inner IP header: version %d", ip_get_version(&packet->inner_ip));
if(packet->inner_ip.nh.data != NULL)
{
rohc_debug(packet, trace_entity, ROHC_PROFILE_GENERAL,
"inner IP header: next header is of type %d",
packet->inner_ip.nh.proto);
if(packet->inner_ip.nl.data != NULL)
{
rohc_debug(packet, trace_entity, ROHC_PROFILE_GENERAL,
"inner IP header: next layer is of type %d",
packet->inner_ip.nl.proto);
}
}
/* get the transport protocol */
packet->transport = &packet->inner_ip.nl;
}
return true;
error:
return false;
}
