buffer_t *build_pana_base(buffer_t *buf, pana_header_t *header, sec_suite_t *suite) { uint8_t *ptr; buf->session_ptr = NULL; buf = buffer_headroom(buf, PANA_HEADER_LENGTH); if (!buf) { return NULL; } buf = buffer_turnaround(buf); // In case we're reusing a buffer ptr = buffer_data_reserve_header(buf, PANA_HEADER_LENGTH); header->payload_len = buffer_data_length(buf); pana_header_write(ptr, header); buf->src_sa.port = UDP_PORT_PANA; buf->dst_sa.port = suite->session_port; buf->info = (buffer_info_t)(B_DIR_DOWN + B_FROM_APP + B_TO_UDP); if (header->type != PANA_MSG_RELAY || suite->pana_session.user_server) { buffer_socket_set(buf, socket_pointer_get(pana_socket)); buf->session_ptr = suite; } else { buf->socket = socket_dereference(buf->socket); buf->session_ptr = NULL; } //tr_debug("From Pana-> Core"); buf->interface = suite->interface; tr_debug("PANA len: %d", header->payload_len); return buf; }
void uv_udp_handle_t::write(request_udp_write_t& request) { int32_t err = UV_UNKNOWN; if (request.m_shared_write) { uv_udp_handle_t* handle = (uv_udp_handle_t*)singleton_ref(network_t).get_shared_write_socket(request.m_socket_fd); if (handle != NULL) { uint32_t length = request.m_length > 0 ? request.m_length : (uint32_t)buffer_data_length(request.m_buffer); if (uv_is_closing((uv_handle_t*)handle)) { err = NL_EUDPSCLOSED; } else { err = handle->write_handle(request); } } else { err = NL_EUDPNOWSHARED; } } else { err = request.m_socket_handle->write_handle(request); } if (err != UV_OK) { /* write error had been occurred */ if (request.m_length > 0) { nl_free((void*)request.m_string); } else { buffer_release(request.m_buffer); } if (request.m_session != LUA_REFNIL) { singleton_ref(node_lua_t).context_send(request.m_source, 0, request.m_session, RESPONSE_UDP_WRITE, (nl_err_code)err); } } }
static buffer_t *pana_eap_payload_to_avp(buffer_t *buf) { uint8_t *ptr; uint16_t eap_len, padding; eap_len = buffer_data_length(buf); padding = eap_len; if ((buf = buffer_headroom(buf, 8)) == 0) { return NULL; } buffer_data_reserve_header(buf, 8); //tr_debug("EAP AVP LEN: %02x", eap_len); ptr = buffer_data_pointer(buf); ptr = pana_avp_base_write(AVP_EAP_PAYLOAD_CODE, eap_len, ptr, 0, 0); //Check Padding padding %= 4; if (padding) { padding = 4 - padding; //tr_debug("Add Pad: %02x", padding); if ((buf = buffer_headroom(buf, padding)) != 0) { uint8_t *ptr2; buffer_data_reserve_header(buf, padding); ptr = buffer_data_pointer(buf); ptr2 = ptr; ptr += padding; memmove(ptr2, ptr, eap_len + 8); } else { return NULL; } } return buf; }
buffer_t *udp_down(buffer_t *buf) { if (buf->src_sa.addr_type != ADDR_IPV6) { //tr_debug("Create Address"); // if(protocol_stack_interface_get_address_by_prefix(buf->if_index, buf->src_sa.address,buf->dst_sa.address, 0) != 0) // { tr_debug("InterFace Address Get Fail--> free Buffer"); return buffer_free(buf); // } // else // { // buf->src_sa.addr_type = ADDR_IPV6; // } } buf = buffer_headroom(buf, 8); if (buf) { uint8_t *ptr; buf->buf_ptr -= 8; ptr = buffer_data_pointer(buf); ptr = common_write_16_bit(buf->src_sa.port, ptr); ptr = common_write_16_bit(buf->dst_sa.port, ptr); ptr = common_write_16_bit(buffer_data_length(buf), ptr); udp_checksum_write(buf); buf->IPHC_NH = 0; buf->info = (buffer_info_t)(B_FROM_UDP | B_TO_IPV6 | B_DIR_DOWN); buf->options.type = IPV6_NH_UDP; buf->options.code = 0; } return (buf); }
int32_t uv_udp_handle_t::write_handle(request_udp_write_t& request) { int result; uv_buf_t uv_buf; write_uv_request_t* uv_request = get_write_cached_request(); uv_request->m_source = request.m_source; uv_request->m_session = request.m_session; uv_request->m_length = request.m_length; if (request.m_length > 0) { uv_request->m_string = request.m_string; uv_buf.len = request.m_length; uv_buf.base = (char*)request.m_string; } else { uv_request->m_buffer = request.m_buffer; uv_buf.len = buffer_data_length(request.m_buffer); uv_buf.base = buffer_data_ptr(request.m_buffer); } if (!request.m_ipv6) { result = uv_udp_send(&uv_request->m_write_req, (uv_udp_t*)m_handle, &uv_buf, 1, uv_ip4_addr(REQUEST_SPARE_PTR(request), request.m_port), on_write); } else { result = uv_udp_send6(&uv_request->m_write_req, (uv_udp_t*)m_handle, &uv_buf, 1, uv_ip6_addr(REQUEST_SPARE_PTR(request), request.m_port), on_write); } if (result == 0) return UV_OK; put_write_cached_request(uv_request); return singleton_ref(network_t).last_error(); /* write error occurs */ }
/** * free buffer header * * \param b pointer to buffer * \param size required headroom space * * \return pdTRUE * \return pdFALSE packet too long */ portCHAR stack_buffer_free_upto_ptr( buffer_t * b) { if (b->buf_ptr < b->buf_end) { memmove(&(b->buf[0]), &(b->buf[b->buf_ptr]), buffer_data_length(b)); b->buf_end -= b->buf_ptr; b->buf_ptr = 0; return pdTRUE; } return pdFALSE; }
buffer_t *pana_relay_avp_build(buffer_t *buf, sec_suite_t *suite) { uint8_t *ptr, *adr_ptr; uint16_t relay_len, padding; relay_len = buffer_data_length(buf); padding = relay_len; buf->socket = socket_dereference(buf->socket); buf->session_ptr = NULL; if ((buf = buffer_headroom(buf, 36)) == 0) { return buf; } else { buffer_data_reserve_header(buf, 36); ptr = buffer_data_pointer(buf); ptr = pana_avp_base_write(AVP_PAC_INFO_CODE, 18, ptr, 0, 0); //SET Relay IPV6 address if (suite->pana_session.user_server) { memcpy(ptr, suite->session_address, 16); ptr += 16; ptr = common_write_16_bit(suite->session_port, ptr); adr_ptr = protocol_6lowpan_nd_border_router_address_get(buf->interface->nwk_id); if (adr_ptr) { memcpy(buf->src_sa.address, adr_ptr, 16); memcpy(buf->dst_sa.address, suite->pana_session.session_relay_address, 16); buf->dst_sa.port = suite->pana_session.relay_port; } } else { memcpy(ptr, buf->src_sa.address, 16); ptr += 16; ptr = common_write_16_bit(buf->src_sa.port, ptr); } //PADDING for PAC ptr = common_write_16_bit(0, ptr); //PANA Relay AVP header Write data is already there ptr = pana_avp_base_write(AVP_RELAY_MSG_CODE, relay_len, ptr, 0, 0); } //Enable security for relay allways by Default buf->options.ll_security_bypass_tx = false; padding %= 4; if (padding) { padding = 4 - padding; //tr_debug("Add Pad: %02x", padding); if ((buf = buffer_headroom(buf, padding)) != 0) { uint8_t *ptr2; buffer_data_reserve_header(buf, padding); ptr = buffer_data_pointer(buf); ptr2 = ptr; ptr += padding; memmove(ptr2, ptr, relay_len + 36); } } return buf; }
buffer_t *pana_auth_message_handler(buffer_t *buf, pana_header_t *header, sec_suite_t *suite) { if (!buf) { return NULL; } protocol_interface_info_entry_t *cur = buf->interface; if (!cur) { return buffer_free(buf); } uint16_t length = buffer_data_length(buf); uint8_t *ptr = buffer_data_pointer(buf); pana_avp_t avp_temp; if (suite->pana_session.session_ready) { return buffer_free(buf); } if (sec_check_suite_ptrs(suite) == 0) { tr_warn("SEC Lib Fail"); return buffer_free(buf); } avp_temp.code = AVP_NONCE_CODE; if (pana_avp_discover(ptr, length, &avp_temp)) { if (avp_temp.len == 16) { if ((header->flags & PANA_FLAGS_REQUEST) == PANA_FLAGS_RESPONSE) { memcpy(suite->pana_session.pana_heap->client_nonce, avp_temp.avp_ptr, 16); } else { memcpy(suite->pana_session.pana_heap->agent_nonce, avp_temp.avp_ptr, 16); } //tr_debug("Pana:A_NONCE OK"); } else { tr_debug("A_NONCE length fail, Len: %x", avp_temp.len); } } avp_temp.code = AVP_EAP_PAYLOAD_CODE; if (pana_avp_discover(ptr, length, &avp_temp)) { ptr = avp_temp.avp_ptr; if (avp_temp.len > 4) { buf->buf_ptr = 0; buf->buf_end = avp_temp.len; memmove(buf->buf, ptr, avp_temp.len); return buf; } } return buffer_free(buf); }
/** * Check buffer headroom. * * \param b pointer to buffer * \param size required headroom space * * \return pdTRUE * \return pdFALSE packet too long */ portCHAR stack_buffer_headroom( buffer_t * b, uint16_t size) { if ((b->buf_end + size) > b->size) { return pdFALSE; } memmove(&(b->buf[(b->buf_ptr)+size]), &(b->buf[b->buf_ptr]), buffer_data_length(b)); b->buf_end += size; b->buf_ptr += size; return pdTRUE; }
buffer_t *pana_relay_parse(buffer_t *buf) { uint8_t *ptr; buf->options.ll_security_bypass_tx = true; //tr_debug("Relay RX"); ptr = buffer_data_pointer(buf); uint16_t length = buffer_data_length(buf); pana_avp_t pac_info; pac_info.code = AVP_PAC_INFO_CODE; if (!pana_avp_discover(ptr, length, &pac_info) || pac_info.len != 18) { tr_debug("No Pac info"); return buffer_free(buf); } pana_avp_t relay_msg; relay_msg.code = AVP_RELAY_MSG_CODE; if (!pana_avp_discover(ptr, length, &relay_msg)) { tr_debug("No Relay MSG"); return buffer_free(buf); } //Set Message data to relay msg buffer_data_pointer_set(buf, relay_msg.avp_ptr); buffer_data_length_set(buf, relay_msg.len); //Set Destination to Pac Info ptr = pac_info.avp_ptr; memcpy(buf->dst_sa.address, ptr, 16); //buf->dst_sa.addr_type = ADDR_IPV6; ptr += 16; buf->dst_sa.port = common_read_16_bit(ptr); ptr += 2; //tr_debug("%s", trace_array(buf->dst_sa.address, 16) ); return buf; }
void pana_session_startms_parse(buffer_t *buf, pana_header_t *header, sec_suite_t *suite) { uint32_t prf_algorythm = 0; uint32_t integrity_algorythm = 12; uint32_t key_wrap = 0; bool key_wrap_parsed = false; uint16_t len = buffer_data_length(buf); uint8_t *ptr = buffer_data_pointer(buf); pana_avp_t avp_temp; //Read Resul and Key id if they are coming avp_temp.code = AVP_PRF_ALGORYTHM_CODE; avp_temp.len = 0; if (pana_avp_discover(ptr, len, &avp_temp) && avp_temp.len == 4) { prf_algorythm = common_read_32_bit(avp_temp.avp_ptr); } avp_temp.code = AVP_INTEGRIRTY_ALGORYTHM_CODE; avp_temp.len = 0; if (pana_avp_discover(ptr, len, &avp_temp) && avp_temp.len == 4) { integrity_algorythm = common_read_32_bit(avp_temp.avp_ptr); } avp_temp.code = AVP_KEY_WRAP_ALG_CODE; avp_temp.len = 0; if (pana_avp_discover(ptr, len, &avp_temp) && avp_temp.len == 4) { key_wrap = common_read_32_bit(avp_temp.avp_ptr); key_wrap_parsed = true; } bool drop_message = false; if ((header->flags & PANA_FLAGS_REQUEST) == PANA_FLAGS_RESPONSE) { if (prf_algorythm != suite->pana_session.prf_algorythm) { tr_debug("PRF!!"); drop_message = true; } else if (integrity_algorythm != suite->pana_session.integrity_algorythm) { tr_debug("int!!"); drop_message = true; } if (key_wrap_parsed && key_wrap != suite->pana_session.key_wrap) { tr_debug("key!!"); drop_message = true; } } else { if (prf_algorythm != 5) { drop_message = true; } else if (integrity_algorythm != 12) { drop_message = true; } } if (!drop_message) { if (key_wrap_parsed) { suite->pana_session.key_warp = true; suite->pana_session.key_wrap = key_wrap; } len += 16; ptr -= 16; //Shift Pana Headers back if ((header->flags & PANA_FLAGS_REQUEST) == PANA_FLAGS_RESPONSE) { sec_lib_state_machine_trig(suite, EAP_IDENTITY_REQ); pana_handshake_copy(ptr, len, false, suite); } else { suite->pana_session.integrity_algorythm = integrity_algorythm; suite->pana_session.prf_algorythm = prf_algorythm; sec_lib_state_machine_trig(suite, PANA_START_RESPONSE); pana_handshake_copy(ptr, len, true, suite); } suite->retry_counter = 0; } buffer_free(buf); }
void pana_down(buffer_t *buf, sec_suite_t *suite) { //Check Request Or Response pana_header_t header; header.type = PANA_MSG_PA; if (suite->pana_session.user_server) { if (suite->state == PANA_REQUEST_TX) { header.flags = PANA_FLAGS_START | PANA_FLAGS_REQUEST; } else if (suite->state == EAP_PANA_FINISH || suite->state == PANA_FAILURE) { header.flags = (PANA_FLAGS_REQUEST | PANA_FLAGS_COMPLETE); } else if (suite->state == PANA_FAILURE_RESPONSE) { header.flags = (PANA_FLAGS_RESPONSE | PANA_FLAGS_COMPLETE); } else { header.flags = PANA_FLAGS_REQUEST; } } else { if (suite->state == PANA_FAILURE) { header.flags = PANA_FLAGS_COMPLETE; } else if (suite->state == PANA_START_RESPONSE) { header.flags = PANA_FLAGS_START; } else { header.flags = 0; } } if (header.flags & PANA_FLAGS_REQUEST) { header.seq = suite->pana_session.req_seq; } else { header.seq = suite->pana_session.res_seq; } header.session_id = suite->pana_session.session_id; pana_set_agend_address(buf, false, suite); buf = build_pana_base(buf, &header, suite); if (buf) { /** * Copy Authentication start message */ if (header.flags & PANA_FLAGS_START) { uint16_t len = buffer_data_length(buf); uint8_t *ptr = buffer_data_pointer(buf); if (header.flags & PANA_FLAGS_REQUEST) { pana_handshake_copy(ptr, len, true, suite); } else { pana_handshake_copy(ptr, len, false, suite); } } if (suite->pana_session.address_status & 1) { tr_debug("Build Relay"); buf = pana_relay_avp_build(buf, suite); if (buf) { header.flags = 0; header.type = PANA_MSG_RELAY; header.session_id = 0; header.seq = 0; buf = build_pana_base(buf, &header, suite); } } protocol_push(buf); } }
/* We assume the packet is basically well-formed, as it will have either * cleared initial input parsing, or we formed it ourselves. hbh and srh * are set to point to the RPL Hop-by-Hop option and/or RPL Source Routing * Header, if present. */ static void rpl_data_locate_info(buffer_t *buf, uint8_t **hbh, uint8_t **srh) { uint8_t *ptr = buffer_data_pointer(buf); uint16_t len = buffer_data_length(buf); if (hbh) { *hbh = NULL; } if (srh) { *srh = NULL; } if (len < IPV6_HDRLEN) { return; } uint16_t ip_len = common_read_16_bit(ptr + IPV6_HDROFF_PAYLOAD_LENGTH); uint8_t nh = ptr[6]; ptr += IPV6_HDRLEN; len -= IPV6_HDRLEN; if (ip_len > len) { return; } len = ip_len; while (len) { uint16_t hdrlen; switch (nh) { case IPV6_NH_HOP_BY_HOP: { if (len < 8) { return; } nh = ptr[0]; hdrlen = (ptr[1] + 1) * 8; /* Move on if they're not interested in HbH (looking for SRH) */ if (!hbh) { break; } if (hdrlen > len) { return; } uint8_t *opt_ptr = ptr + 2; uint8_t *opt_end = ptr + hdrlen; while (opt_ptr < opt_end) { switch (opt_ptr[0]) { case IPV6_OPTION_PAD1: opt_ptr++; break; case IPV6_OPTION_RPL: *hbh = opt_ptr; goto found_option; default: opt_ptr += 2 + opt_ptr[1]; break; } } found_option: /* If they're not looking for SRH, finish now */ if (!srh) { return; } break; } case IPV6_NH_DEST_OPT: // Destination option permitted to appear before routing if (len < 8) { return; } nh = ptr[0]; hdrlen = (ptr[1] + 1) * 8; /* If they're not looking for SRH, finish now - past HbH */ if (!srh) { return; } break; case IPV6_NH_ROUTING: if (!srh) { return; } if (ptr[2] == IPV6_ROUTING_TYPE_RPL) { *srh = ptr; } // No need to examine past routing header return; default: // No other headers can appear before routing - last we care about return; } if (hdrlen > len) { return; } ptr += hdrlen; len -= hdrlen; } return; }
buffer_t *lowpan_up(buffer_t *buf) { protocol_interface_info_entry_t *cur = buf->interface; /* Reject: * Packets without address * Source broadcast PAN ID * Short source addresses 0xfffe (illegal) and 0xffff (broadcast) */ if (buf->dst_sa.addr_type == ADDR_NONE || buf->src_sa.addr_type == ADDR_NONE || common_read_16_bit(buf->src_sa.address) == 0xffff || (buf->dst_sa.addr_type == ADDR_802_15_4_SHORT && common_read_16_bit(buf->src_sa.address + 2) > 0xfffd)) { goto drop; } /* If our PAN ID is set to 0xffff (eg during beacon scan), the MAC will be * receiving all packets to all PANs. "Mute" 6LoWPAN reception in this state. */ if (cur->mac_parameters->pan_id == 0xffff) { goto drop; } const uint8_t *ip_hc = buffer_data_pointer(buf); //tr_debug("IP-UP"; if (buffer_data_length(buf) < 4 || addr_check_broadcast(buf->src_sa.address, buf->src_sa.addr_type) == 0) { tr_debug("cipv6_up() Too short or broadcast src"); goto drop; } else if ((ip_hc[0] & LOWPAN_FRAG_MASK) == LOWPAN_FRAG) { /* 11 x00xxx: FRAG1/FRAGN (RFC 4944) */ buf->info = (buffer_info_t)(B_DIR_UP | B_FROM_IPV6_TXRX | B_TO_FRAGMENTATION); return buf; } else if ((ip_hc[0] & LOWPAN_MESH_MASK) == LOWPAN_MESH) { /* 10 xxxxxx: MESH (RFC 4944) */ buf->info = (buffer_info_t)(B_DIR_UP | B_FROM_IPV6_TXRX | B_TO_MESH_ROUTING); return buf; } else if (ip_hc[0] == LOWPAN_DISPATCH_IPV6) { /* Send this to new handler */ buffer_data_strip_header(buf, 1); buf->ip_routed_up = true; buf->info = (buffer_info_t)(B_DIR_UP | B_FROM_IPV6_TXRX | B_TO_IPV6_FWD); return buf; } else if ((ip_hc[0] & LOWPAN_DISPATCH_IPHC_MASK) != LOWPAN_DISPATCH_IPHC) { /* Not handled: LOWPAN_HC1/LOWPAN_BC0/IPv6 (RFC 4944), or extension */ tr_debug("LOWPAN: %02x %02x", ip_hc[0], ip_hc[1]); goto drop; } /* Divert to new routing system - in final system, MAC/Mesh/Frag should send to IPV6_TXRX layer */ buf->ip_routed_up = true; buf = iphc_decompress(&cur->lowpan_contexts, buf); if (buf) { buf->info = (buffer_info_t)(B_DIR_UP | B_FROM_IPV6_TXRX | B_TO_IPV6_FWD); } return buf; drop: protocol_stats_update(STATS_IP_RX_DROP, 1); return buffer_free(buf); }
buffer_t *udp_up(buffer_t *buf) { //tr_debug("UDP UP"); const uint8_t *ip_hdr; if ((buf->info & B_FROM_MASK) == B_FROM_IPV6_FWD) { // New paths leave IP header on for us to permit ICMP response; // note the pointer and strip now. ip_hdr = buffer_data_pointer(buf); buffer_data_strip_header(buf, buf->offset); buf->offset = 0; } else { // We came from cipv6_up (or...?) - we have no real IP headers ip_hdr = NULL; } uint16_t ip_len = buffer_data_length(buf); if (ip_len < 8) { return buffer_free(buf); } const uint8_t *udp_hdr = buffer_data_pointer(buf); buf->src_sa.port = common_read_16_bit(udp_hdr + 0); buf->dst_sa.port = common_read_16_bit(udp_hdr + 2); uint16_t udp_len = common_read_16_bit(udp_hdr + 4); buf = nwk_udp_rx_security_check(buf); if (!buf) { return NULL; } if (udp_len < 8 || udp_len > ip_len) { return buffer_free(buf); } // Set UDP length - may trim the buffer buffer_data_length_set(buf, udp_len); buf = udp_checksum_check(buf); if (!buf) { return buf; } // Remove UDP header buffer_data_pointer_set(buf, udp_hdr + 8); if (buf->dst_sa.port == 0) { tr_err("UDP port 0"); protocol_stats_update(STATS_IP_RX_DROP, 1); return buffer_free(buf); } if (buf->dst_sa.port == UDP_PORT_ECHO && buf->src_sa.port != UDP_PORT_ECHO) { protocol_interface_info_entry_t *cur; tr_debug("UDP echo msg [%"PRIi16"]: %s%s", buffer_data_length(buf), trace_array( buffer_data_pointer(buf), (buffer_data_length(buf) > 64 ? 64 : buffer_data_length(buf))), (buffer_data_length(buf) > 64 ? "..." : "")); cur = buf->interface; if (addr_is_ipv6_multicast(buf->dst_sa.address)) { const uint8_t *ipv6_ptr; ipv6_ptr = addr_select_source(cur, buf->dst_sa.address, 0); if (!ipv6_ptr) { tr_debug("UDP Echo:No address"); return buffer_free(buf); } memcpy(buf->dst_sa.address, buf->src_sa.address, 16); memcpy(buf->src_sa.address, ipv6_ptr, 16); } else { memswap(buf->dst_sa.address, buf->src_sa.address, 16); } buf->dst_sa.port = buf->src_sa.port; buf->src_sa.port = UDP_PORT_ECHO; buf->info = (buffer_info_t)(B_FROM_UDP | B_TO_UDP | B_DIR_DOWN); buf->options.hop_limit = UNICAST_HOP_LIMIT_DEFAULT; buf->options.traffic_class = 0; buf->IPHC_NH = 0; return buffer_turnaround(buf); } if (ip_hdr) { /* New path generates port unreachable here, using the real IP headers * that we know the position of thanks to buf->offset. * * Old path has socket_up make port unreachable itself, creating a * fake IP header. */ if (!buf->socket) { buffer_socket_set(buf, socket_lookup_ipv6(IPV6_NH_UDP, &buf->dst_sa, &buf->src_sa, true)); } if (!buf->socket) { // Reconstruct original IP packet buffer_data_pointer_set(buf, udp_hdr); buffer_data_length_set(buf, ip_len); buffer_data_pointer_set(buf, ip_hdr); return icmpv6_error(buf, NULL, ICMPV6_TYPE_ERROR_DESTINATION_UNREACH, ICMPV6_CODE_DST_UNREACH_PORT_UNREACH, 0); } } buf->options.type = (uint8_t) SOCKET_FAMILY_IPV6; buf->options.code = IPV6_NH_UDP; buf->info = (buffer_info_t)(B_FROM_UDP | B_TO_APP | B_DIR_UP); return buf; }