void init_para_prob(ipv6_addr_t *src, ipv6_addr_t *dest, uint8_t code, uint32_t pointer, uint8_t *packet, uint8_t packet_len)
{
struct para_prob_t *para_prob_buf;
packet_length = IPV6_HDR_LEN + ICMPV6_HDR_LEN + PARA_PROB_LEN;
memcpy(&(ipv6_buf[packet_length]), packet, min(MTU - packet_length, packet_len));
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;
ipv6_ext_hdr_len = 0;
icmp_buf = get_icmpv6_buf(ipv6_ext_hdr_len);
icmp_buf->type = ICMP_PARA_PROB;
icmp_buf->code = code;
memcpy(&(ipv6_buf->destaddr.uint8[0]), &(dest->uint8[0]), 16);
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &(src->uint8[0]), 16);
para_prob_buf = get_para_prob_buf(ipv6_ext_hdr_len);
para_prob_buf->pointer = pointer;
packet_length += min(MTU - packet_length, packet_len);
ipv6_buf->length = packet_length - IPV6_HDR_LEN;
icmp_buf->checksum = 0;
icmp_buf->checksum = ~icmpv6_csum(PROTO_NUM_ICMPV6);
}
void init_nbr_adv(ipv6_addr_t *src, ipv6_addr_t *dst, ipv6_addr_t *tgt,
uint8_t rso, uint8_t sllao, uint8_t aro, uint8_t aro_state)
{
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;
ipv6_ext_hdr_len = 0;
icmp_buf = get_icmpv6_buf(ipv6_ext_hdr_len);
icmp_buf->type = ICMP_NBR_ADV;
icmp_buf->code = 0;
memcpy(&(ipv6_buf->destaddr.uint8[0]), &(dst->uint8[0]), 16);
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &(src->uint8[0]), 16);
nbr_adv_buf = get_nbr_adv_buf(ipv6_ext_hdr_len);
nbr_adv_buf->rso = rso;
memset(&(nbr_adv_buf->reserved[0]), 0, 3);
memcpy(&(nbr_adv_buf->tgtaddr.uint8[0]), &(tgt->uint8[0]), 16);
packet_length = IPV6_HDR_LEN + ICMPV6_HDR_LEN + NBR_ADV_LEN;
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; /* TODO */
opt_aro_buf->reserved1 = 0;
opt_aro_buf->reserved2 = 0;
memcpy(&(opt_aro_buf->eui64), mac_get_eui(dst), 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);
}
uint16_t icmpv6_csum(uint8_t proto)
{
ipv6_buf = get_ipv6_buf();
uint16_t sum;
uint16_t len = ipv6_buf->length;
sum = len + proto;
sum = csum(sum, (uint8_t *)&ipv6_buf->srcaddr, 2 * sizeof(ipv6_addr_t));
sum = csum(sum, (uint8_t *)get_icmpv6_buf(0), len);
return (sum == 0) ? 0xffff : HTONS(sum);
}
/* 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 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);
}
void *ipv6_process(void *arg)
{
(void) arg;
msg_t m_recv_lowpan, m_send_lowpan;
msg_t m_recv, m_send;
uint8_t i;
uint16_t packet_length;
msg_init_queue(ip_msg_queue, IP_PKT_RECV_BUF_SIZE);
while (1) {
msg_receive(&m_recv_lowpan);
ipv6_buf = (ipv6_hdr_t *)m_recv_lowpan.content.ptr;
/* identifiy packet */
nextheader = &ipv6_buf->nextheader;
for (i = 0; i < SIXLOWIP_MAX_REGISTERED; i++) {
if (sixlowip_reg[i]) {
msg_t m_send;
m_send.type = IPV6_PACKET_RECEIVED;
m_send.content.ptr = (char *) ipv6_buf;
msg_send(&m_send, sixlowip_reg[i], 1);
}
}
/* destination is our address */
if (is_our_address(&ipv6_buf->destaddr)) {
switch (*nextheader) {
case (IPV6_PROTO_NUM_ICMPV6): {
icmp_buf = get_icmpv6_buf(ipv6_ext_hdr_len);
/* checksum test*/
if (ipv6_csum(ipv6_buf, (uint8_t *) icmp_buf, NTOHS(ipv6_buf->length),
IPV6_PROTO_NUM_ICMPV6) != 0xffff) {
DEBUG("ERROR: wrong checksum\n");
}
icmpv6_demultiplex(icmp_buf);
break;
}
case (IPV6_PROTO_NUM_TCP): {
if (tcp_packet_handler_pid != KERNEL_PID_UNDEF) {
m_send.content.ptr = (char *) ipv6_buf;
msg_send_receive(&m_send, &m_recv, tcp_packet_handler_pid);
}
else {
DEBUG("INFO: No TCP handler registered.\n");
}
break;
}
case (IPV6_PROTO_NUM_UDP): {
if (udp_packet_handler_pid != KERNEL_PID_UNDEF) {
m_send.content.ptr = (char *) ipv6_buf;
msg_send_receive(&m_send, &m_recv, udp_packet_handler_pid);
}
else {
DEBUG("INFO: No UDP handler registered.\n");
}
break;
}
case (IPV6_PROTO_NUM_NONE): {
DEBUG("INFO: Packet with no Header following the IPv6 Header received.\n");
break;
}
default:
DEBUG("INFO: Unknown next header\n");
break;
}
}
/* destination is foreign address */
else {
DEBUG("That's not for me, destination is %s\n", ipv6_addr_to_str(addr_str, IPV6_MAX_ADDR_STR_LEN, &ipv6_buf->destaddr));
packet_length = IPV6_HDR_LEN + NTOHS(ipv6_buf->length);
ndp_neighbor_cache_t *nce;
ipv6_addr_t *dest;
if (ip_get_next_hop == NULL) {
dest = &ipv6_buf->destaddr;
}
else {
dest = ip_get_next_hop(&ipv6_buf->destaddr);
}
if ((dest == NULL) || ((--ipv6_buf->hoplimit) == 0)) {
DEBUG("!!! Packet not for me, routing handler is set, but I "\
" have no idea where to send or the hop limit is exceeded.\n");
msg_reply(&m_recv_lowpan, &m_send_lowpan);
continue;
}
nce = ndp_get_ll_address(dest);
/* copy received packet to send buffer */
memcpy(ipv6_get_buf_send(), ipv6_get_buf(), packet_length);
/* send packet to node ID derived from dest IP */
if (nce != NULL) {
sixlowpan_lowpan_sendto(nce->if_id, &nce->lladdr,
nce->lladdr_len,
(uint8_t *)ipv6_get_buf_send(),
packet_length);
} else {
/* XXX: this is wrong, but until ND does work correctly,
* this is the only way (aka the old way)*/
uint16_t raddr = dest->uint16[7];
sixlowpan_lowpan_sendto(0, &raddr, 2, (uint8_t *)ipv6_get_buf_send(), packet_length);
}
}
msg_reply(&m_recv_lowpan, &m_send_lowpan);
}
}