void fhss_beacon_decode_raw(fhss_synchronization_beacon_payload_s* dest, const uint8_t* buffer)
{
dest->data_start_delimeter = *buffer++;
dest->channel_index = *buffer++;
dest->sender_unicast_channel = *buffer++;
dest->current_superframe = common_read_16_bit(buffer);
buffer += BEACON_FIELD_SIZE(current_superframe);
dest->remaining_slots = common_read_16_bit(buffer);
buffer += BEACON_FIELD_SIZE(remaining_slots);
dest->channel_list_counter = common_read_16_bit(buffer);
buffer += BEACON_FIELD_SIZE(channel_list_counter);
dest->hop_count = *buffer++;
dest->number_of_broadcast_channels = *buffer++;
dest->number_of_tx_slots = *buffer++;
dest->time_since_last_beacon = common_read_32_bit(buffer);
buffer += BEACON_FIELD_SIZE(time_since_last_beacon);
dest->processing_delay += common_read_16_bit(buffer);
buffer += BEACON_FIELD_SIZE(processing_delay);
dest->superframe_length = common_read_16_bit(buffer);
buffer += BEACON_FIELD_SIZE(superframe_length);
dest->number_of_superframes_per_channel = *buffer;
}
static void thread_nd_coap_notification_callback(int8_t interface_id, const uint8_t ip_addr[16], uint16_t loc_addr, const uint8_t ml_eid[8])
{
protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
if (!cur) {
return;
}
/* First check to see if we have an existing entry with different RLOC - we need to unicast error
* notification to that old entry if so. */
ipv6_neighbour_t *entry = ipv6_neighbour_lookup(&cur->ipv6_neighbour_cache, ip_addr);
if (entry && entry->ll_type == ADDR_802_15_4_SHORT) {
uint16_t old_entry_rloc = common_read_16_bit(entry->ll_address + 2);
if (old_entry_rloc != loc_addr) {
uint8_t old_entry_ip[16];
thread_addr_write_mesh_local_16(old_entry_ip, common_read_16_bit(entry->ll_address + 2), cur->thread_info);
tr_warn("Proactive address change %s %04x->%04x", trace_ipv6(ip_addr), old_entry_rloc, loc_addr);
thread_resolution_client_address_error(interface_id, old_entry_ip, ip_addr, ml_eid);
}
}
/* Now treat as an unsolicited update (by address, because entry may be NULL) */
uint8_t ll_addr[4];
common_write_16_bit(cur->mac_parameters->pan_id, ll_addr + 0);
common_write_16_bit(loc_addr, ll_addr + 2);
ipv6_neighbour_update_unsolicited(&cur->ipv6_neighbour_cache, ip_addr, ADDR_802_15_4_SHORT, ll_addr);
if (nd_proxy_enabled_for_upstream(cur->id)) {
ipv6_route_add(ip_addr, 128, cur->id, NULL, ROUTE_THREAD_PROXIED_HOST, 3600, 0);
}
}
bool pana_header_parse(uint8_t *ptr, uint16_t data_length, pana_header_t *header)
{
if (data_length < 16) {
return false;
}
if (common_read_16_bit(ptr) != PANA_HEADER_START) {
return false;
}
header->payload_len = common_read_16_bit(ptr + 2);
ptr += 4;
if (header->payload_len != data_length) {
return false;
}
header->flags = common_read_16_bit(ptr);
header->type = common_read_16_bit(ptr + 2);
ptr += 4;
header->session_id = common_read_32_bit(ptr);
ptr += 4;
header->seq = common_read_32_bit(ptr);
return true;
}
int mle_message_malformed_check(uint8_t *ptr, uint16_t data_len)
{
uint8_t *dptr;
uint16_t length;
dptr = ptr;
while (data_len) {
if (data_len >= 2) {
dptr += 1; //Skip TLV Type
length = *dptr++;
if (length == 0xff) {
// Long length format
data_len -= 2;
if (data_len < 2) {
return -1;
}
length = common_read_16_bit(dptr);
dptr += 2;
}
data_len -= 2;
if (data_len >= length) {
if (length) {
data_len -= length;
dptr += length;
}
} else {
// buffer is overrun this is malformed.
return -1;
}
} else {
return -1;
}
}
return 0;
}
bool eapol_parse_pdu_header(uint8_t *ptr, uint16_t data_length, eapol_pdu_t *eapol_pdu)
{
//Validate MIN length
if (data_length < EAPOL_BASE_LENGTH) {
return false;
}
//Validate Protocol version
uint8_t protocol = *ptr++;
if (protocol != EAPOL_PROTOCOL_VERSION) {
return false;
}
eapol_pdu->packet_type = *ptr++;
eapol_pdu->packet_length = common_read_16_bit(ptr);
ptr += 2;
//Validate Body Length
if (eapol_pdu->packet_length > data_length - EAPOL_BASE_LENGTH) {
return false;
}
eapol_pdu->packet_body = ptr;
if (eapol_pdu->packet_type == EAPOL_EAP_TYPE) {
return eapol_parse_eap_packet(eapol_pdu);
} else if (eapol_pdu->packet_type == EAPOL_KEY_TYPE) {
return eapol_parse_key_packet(eapol_pdu);
} else {
return false;
}
}
static bool eapol_parse_key_packet(eapol_pdu_t *eapol_pdu)
{
if (eapol_pdu->packet_length < EAPOL_KEY_FRAME_BASE_SIZE) {
return false;
}
uint8_t *ptr = eapol_pdu->packet_body;
eapol_key_frame_t *key_frame = &eapol_pdu->msg.key;
key_frame->key_description = *ptr++;
if (key_frame->key_description != EAPOL_RSN_KEY_DESCRIPTION) {
return false;
}
ptr = eapol_key_information_read(&key_frame->key_information, ptr);
if (key_frame->key_information.description_version != KEY_DESCRIPTION_HMAC_SHA1_MIC_AES_ENC) {
return false;
}
key_frame->key_length = common_read_16_bit(ptr);
ptr += 2;
key_frame->replay_counter = common_read_64_bit(ptr);
ptr += 8;
key_frame->key_nonce = ptr;
ptr += 32;
key_frame->key_iv = ptr;
ptr += 16;
key_frame->key_rsc = ptr;
ptr += 16; //Skip 8 byte RSC + RESERVED 8
key_frame->key_mic = ptr;
ptr += 16;
key_frame->key_data_length = common_read_16_bit(ptr);
ptr += 2;
key_frame->key_data = ptr;
if (key_frame->key_data_length > (eapol_pdu->packet_length - EAPOL_KEY_FRAME_BASE_SIZE)) {
return false;
}
return true;
}
bool mle_tlv_read_16_bit_tlv(mle_tlv_type_t reqType, uint8_t *ptr, uint16_t data_len, uint16_t *buffer)
{
mle_tlv_info_t tlv_info;
if (mle_tlv_option_discover(ptr, data_len, reqType, &tlv_info) >= 2) {
*buffer = common_read_16_bit(tlv_info.dataPtr);
return true;
}
return false;
}
uint8_t thread_meshcop_tlv_data_get_uint16(const uint8_t *ptr, uint16_t length, uint8_t type, uint16_t *data_ptr)
{
uint8_t result_len;
uint8_t *result_ptr;
result_len = thread_meshcop_tlv_find(ptr, length, type, &result_ptr);
if (result_len >= 2 && data_ptr) {
*data_ptr = common_read_16_bit(result_ptr);
}
return result_len;
}
uint16_t thread_meshcop_tlv_find(const uint8_t *ptr, uint16_t length, uint8_t type, uint8_t **result_ptr)
{
const uint8_t *p;
if (!ptr || length < 2) {
return 0;
}
//tr_info("tlv_find length: %d, type: %d", length, type);
p = ptr;
while (p != NULL) {
const uint8_t *tlv_data_ptr;
uint16_t tlv_data_length;
//tr_info("tlv_find first check");
// check if we have enough length for normal length tlv
if (p + 2 > ptr + length) {
break; //must have at least type and short length
}
if (p[1] == 0xff) {
// Long length format
if (p + 4 > ptr + length) {
break; // check if enough length for long length
}
tlv_data_length = common_read_16_bit(&p[2]);
tlv_data_ptr = p + 4;
} else {
tlv_data_length = p[1];
tlv_data_ptr = p + 2;
}
//tr_info("tlv_find check: %d, type: %d", tlv_data_length, *p);
// check if length of tlv is correct
if (tlv_data_ptr + tlv_data_length > ptr + length) {
break; //length goes past the data block
}
if (*p == type) {
// Correct TLV found
//tr_info("tlv_find Found: %d, type: %d", tlv_data_length, *p);
if (result_ptr != NULL) {
*result_ptr = (uint8_t *)tlv_data_ptr;
}
// return the correct tlv data
return tlv_data_length;
}
p = tlv_data_ptr + tlv_data_length;
}
return 0;
}
static buffer_t *udp_checksum_check(buffer_t *buf)
{
uint8_t *ptr = buffer_data_pointer(buf) + 6;
uint16_t check = common_read_16_bit(ptr);
// We refuse checksum field 0000, as per IPv6 (RFC 2460). Would have
// to accept this if handling IPv4.
if (check == 0 || buffer_ipv6_fcf(buf, IPV6_NH_UDP)) {
tr_err("CKSUM ERROR - src=%s", trace_ipv6(buf->src_sa.address));
protocol_stats_update(STATS_IP_CKSUM_ERROR, 1);
buf = buffer_free(buf);
}
return buf;
}
int16_t thread_meshcop_tlv_length_required(const uint8_t *ptr, uint16_t length)
{
if (length < 2) {
return -1;
}
if (ptr[1] == 0xff) {
// Long length format
if (length < 4) {
return -1;
}
return 4 + common_read_16_bit(&ptr[2]);
}
return 2 + ptr[1];
}
static uint8_t *eapol_key_information_read(eapol_key_information_t *key_information, uint8_t *ptr)
{
uint16_t key_info = common_read_16_bit(ptr);
key_information->description_version = ((key_info & KEY_INFO_VERSION_BIT_MASK) >> KEY_INFO_VERSION_BIT_SHIFT);
key_information->pairwise_key = ((key_info & KEY_INFO_KEY_TYPE_BIT_MASK) >> KEY_INFO_KEY_TYPE_BIT_SHIFT);
key_information->install = ((key_info & KEY_INFO_INSTALL_BIT_MASK) >> KEY_INFO_INSTALL_BIT_SHIFT);
key_information->key_ack = ((key_info & KEY_INFO_ACK_BIT_MASK) >> KEY_INFO_ACK_BIT_SHIFT);
key_information->key_mic = ((key_info & KEY_INFO_MIC_MASK) >> KEY_INFO_MIC_SHIFT);
key_information->secured_key_frame = ((key_info & KEY_INFO_SECURE_MASK) >> KEY_INFO_SECURE_SHIFT);
key_information->error = ((key_info & KEY_INFO_ERROR_MASK) >> KEY_INFO_ERROR_SHIFT);
key_information->request = ((key_info & KEY_INFO_REQUEST_MASK) >> KEY_INFO_REQUEST_SHIFT);
key_information->encrypted_key_data = ((key_info & KEY_INFO_ENC_KEY_DATA_MASK) >> KEY_INFO_ENC_KEY_DATA_SHIFT);
key_information->smk_handshake = ((key_info & KEY_INFO_SMK_MASK) >> KEY_INFO_SMK_SHIFT);
return ptr + 2;
}
static const uint8_t *thread_meshcop_next_tlv(const uint8_t *ptr, uint16_t length)
{
// This fails if returned pointer would go past the length and it must have 2 bytes room
if (length < 4) {
return NULL;
}
if (ptr[1] == 0xff) {
// Long length format
if (length < 6) {
return NULL;
}
return ptr + 4 + common_read_16_bit(&ptr[2]);
}
return ptr + 2 + ptr[1];
}
static void thread_parse_annoucement(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg)
{
uint64_t timestamp;
uint16_t panid;
uint8_t *ptr;
uint8_t channel_page;
uint16_t channel;
link_configuration_s *linkConfiguration = thread_joiner_application_get_config(cur->id);
tr_info("Recv Dataset Announce");
if (8 > thread_tmfcop_tlv_data_get_uint64(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_ACTIVE_TIMESTAMP,×tamp)) {
tr_error("Missing timestamp TLV");
return;
}
if (2 > thread_tmfcop_tlv_data_get_uint16(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_PANID,&panid)) {
tr_error("Missing Panid TLV");
return;
}
if (3 > thread_tmfcop_tlv_find(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_CHANNEL,&ptr)) {
tr_error("Missing Channel TLV");
return;
}
channel_page = ptr[0];
channel = common_read_16_bit(&ptr[1]);
if (linkConfiguration->timestamp == timestamp) {
// We received same timestamp
tr_debug("Same timestamp");
return;
}
if (cur->thread_info->announcement_info && cur->thread_info->announcement_info->timestamp == timestamp){
// We received same timestamp again
tr_debug("Processing announce with same timestamp");
return;
}
if (linkConfiguration->timestamp > timestamp) {
// We received older time stamp we just announce back to originator channel
thread_bootstrap_announce_send(cur, linkConfiguration->channel_page, linkConfiguration->rfChannel, linkConfiguration->panId, linkConfiguration->timestamp, channel);
return;
}
tr_debug("New configuration received");
thread_bootstrap_temporary_attach(cur,channel_page, channel, panid, timestamp);
}
bool thread_meshcop_tlv_exist(const uint8_t *ptr, const uint16_t length, const uint8_t type)
{
const uint8_t *p;
if (!ptr || length < 2) {
return false;
}
p = ptr;
while (p != NULL) {
const uint8_t *tlv_data_ptr;
uint16_t tlv_data_length;
// check if we have enough length for normal length tlv
if (p + 2 > ptr + length) {
break; //must have at least type and short length
}
if (p[1] == 0xff) {
// Long length format
if (p + 4 > ptr + length) {
break; // check if enough length for long length
}
tlv_data_length = common_read_16_bit(&p[2]);
tlv_data_ptr = p + 4;
} else {
tlv_data_length = p[1];
tlv_data_ptr = p + 2;
}
// check if length of tlv is correct
if (tlv_data_ptr + tlv_data_length > ptr + length) {
break; //length goes past the data block
}
if (*p == type) {
// Correct TLV found
return true;
}
p = tlv_data_ptr + tlv_data_length;
}
return false;
}
int mle_tlv_option_discover(uint8_t *ptr, uint16_t data_len, mle_tlv_type_t discovered_type, mle_tlv_info_t *option_info)
{
uint8_t *dptr;
uint16_t length;
mle_tlv_type_t type;
option_info->tlvLen = 0;
option_info->tlvType = MLE_TYPE_UNASSIGNED;
option_info->dataPtr = NULL;
dptr = ptr;
while (data_len) {
type = (mle_tlv_type_t) * dptr++;
length = *dptr++;
if (length == 0xff) {
// Long length format
data_len -= 2;
if (data_len < 2) {
return -1;
}
length = common_read_16_bit(dptr);
dptr += 2;
}
data_len -= 2;
if (data_len >= length) {
if (type == discovered_type) {
option_info->tlvLen = length;
option_info->tlvType = type;
option_info->dataPtr = dptr;
return length;
} else {
data_len -= length;
dptr += length;
}
}
}
return -1;
}
void mac_helper_coordinator_address_set(protocol_interface_info_entry_t *interface, addrtype_t adr_type, uint8_t *adr_ptr)
{
mlme_set_t set_req;
set_req.attr_index = 0;
if (adr_type == ADDR_802_15_4_SHORT) {
memcpy(interface->mac_parameters->mac_cordinator_info.mac_mlme_coord_address, adr_ptr, 2);
interface->mac_parameters->mac_cordinator_info.cord_adr_mode = MAC_ADDR_MODE_16_BIT;
uint16_t short_addr = common_read_16_bit(interface->mac_parameters->mac_cordinator_info.mac_mlme_coord_address);
set_req.attr = macCoordShortAddress;
set_req.value_pointer = &short_addr;
set_req.value_size = 2;
} else if (adr_type == ADDR_802_15_4_LONG) {
memcpy(interface->mac_parameters->mac_cordinator_info.mac_mlme_coord_address, adr_ptr, 8);
interface->mac_parameters->mac_cordinator_info.cord_adr_mode = MAC_ADDR_MODE_64_BIT;
set_req.attr = macCoordExtendedAddress;
set_req.value_pointer = &interface->mac_parameters->mac_cordinator_info.mac_mlme_coord_address;
set_req.value_size = 8;
}
if (interface->mac_api) {
interface->mac_api->mlme_req(interface->mac_api, MLME_SET, &set_req);
}
}
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;
}
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;
}
uint8_t mac_indirect_data_req_handle(mac_pre_parsed_frame_t *buf, protocol_interface_rf_mac_setup_s *mac_ptr)
{
if (!mac_ptr || !buf) {
return 1;
}
if (buf->fcf_dsn.DstAddrMode == MAC_ADDR_MODE_NONE || buf->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_NONE) {
return 1;
}
uint8_t srcAddress[8];
memset(&srcAddress, 0, 8);
mac_header_get_src_address(&buf->fcf_dsn, mac_header_message_start_pointer(buf), srcAddress);
//Call COMM status
mac_api_t *mac_api = get_sw_mac_api(mac_ptr);
if (mac_api) {
mlme_comm_status_t comm_status;
memset(&comm_status, 0, sizeof(mlme_comm_status_t));
comm_status.status = MLME_DATA_POLL_NOTIFICATION;
//Call com status
comm_status.PANId = mac_header_get_dst_panid(&buf->fcf_dsn, mac_header_message_start_pointer(buf));
comm_status.DstAddrMode = buf->fcf_dsn.DstAddrMode;;
mac_header_get_dst_address(&buf->fcf_dsn, mac_header_message_start_pointer(buf), comm_status.DstAddr);
comm_status.SrcAddrMode = buf->fcf_dsn.SrcAddrMode;
mac_header_get_src_address(&buf->fcf_dsn, mac_header_message_start_pointer(buf), comm_status.SrcAddr);
mac_header_security_components_read(buf, &comm_status.Key);
if( mac_api->mlme_ind_cb ){
mac_api->mlme_ind_cb(mac_api, MLME_COMM_STATUS, &comm_status);
}
}
/* If the Ack we sent for the Data Request didn't have frame pending set, we shouldn't transmit - child may have slept */
if (!buf->ack_pendinfg_status) {
//tr_debug("Drop by pending");
if (mac_ptr->indirect_pd_data_request_queue) {
tr_error("Wrongly dropped");
}
//Free Buffer
return 1;
}
uint8_t address_cmp_ok = 0;
uint8_t len;
mac_pre_build_frame_t *b_prev = NULL;
if (buf->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_16_BIT) {
len = 2;
} else {
len = 8;
}
mac_pre_build_frame_t *b = mac_ptr->indirect_pd_data_request_queue;
while (b) {
if (buf->neigh_info) {
uint16_t compare_short;
if (b->fcf_dsn.DstAddrMode == MAC_ADDR_MODE_16_BIT) {
compare_short = common_read_16_bit(b->DstAddr);
if (compare_short == buf->neigh_info->ShortAddress) {
address_cmp_ok = 1;
}
} else {
if (memcmp(b->DstAddr, buf->neigh_info->ExtAddress, 8) == 0) {
address_cmp_ok = 1;
}
}
} else {
if (b->fcf_dsn.DstAddrMode == buf->fcf_dsn.SrcAddrMode) {
if (memcmp(b->DstAddr, srcAddress, len) == 0) {
address_cmp_ok = 1;
}
}
}
if (address_cmp_ok) {
if (b_prev) {
b_prev->next = b->next;
} else {
mac_ptr->indirect_pd_data_request_queue = b->next;
}
b->next = NULL;
b->priority = MAC_PD_DATA_MEDIUM_PRIORITY;
mcps_sap_pd_req_queue_write(mac_ptr, b);
return 1;
} else {
b_prev = b;
b = b->next;
}
}
return 0;
}
/* 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;
}
/*
* 0 1 2 3
* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | Option Type | Opt Data Len |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* |O|R|F|0|0|0|0|0| RPLInstanceID | SenderRank |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
* | (sub-TLVs) |
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
*
* Figure 1: RPL Option
*/
static buffer_t *rpl_data_exthdr_provider_hbh_2(buffer_t *buf, rpl_instance_t *instance, rpl_neighbour_t *neighbour, ipv6_exthdr_stage_t stage, int16_t *result)
{
ipv6_route_info_t *route_info = &buf->route->route_info;
/* This can be called both for routes which only use HbH headers (eg DIO)
* as well as one-hop DAO_SR routes which would normally use source routing
* headers, if there was more than one hop. For DAO_SR, neighbour will be
* NULL.
*/
rpl_dodag_t *dodag = rpl_instance_current_dodag(instance);
if (!dodag) {
*result = -1;
return buf;
}
bool destination_in_instance = false;
uint16_t ext_size = 0;
if (addr_ipv6_equal(route_info->next_hop_addr, buf->dst_sa.address) ||
addr_ipv6_equal(buf->dst_sa.address, dodag->id)) {
destination_in_instance = true;
if (buf->rpl_option) {
/* Forwarding an existing option - preserve it */
uint8_t opt_size = buf->rpl_option[0];
ext_size = 2 + opt_size;
ext_size = (ext_size + 7) &~ 7;
} else {
/* Generating our own option - fixed size, no TLVs */
ext_size = 8;
}
}
switch (stage) {
case IPV6_EXTHDR_SIZE:
*result = ext_size;
return buf;
case IPV6_EXTHDR_INSERT: {
if (!destination_in_instance) {
/* We don't add a header - we'll do it on the tunnel */
*result = 0;
return buf;
}
buf = buffer_headroom(buf, ext_size);
if (!buf) {
return NULL;
}
uint8_t *ext = buffer_data_reserve_header(buf, ext_size);
ext[0] = buf->options.type;
buf->options.type = IPV6_NH_HOP_BY_HOP;
ext[1] = ext_size / 8 - 1;
uint8_t *opt = ext + 2;
opt[0] = IPV6_OPTION_RPL;
if (buf->rpl_option) {
/* Get back the RPL option we stripped off an outer IP header */
memcpy(opt + 1, buf->rpl_option, 1 + buf->rpl_option[0]);
ns_dyn_mem_free(buf->rpl_option);
buf->rpl_option = NULL;
} else {
opt[1] = 4; // option length
opt[2] = 0; // placeholder
opt[3] = instance->id;
/* For upwards routes we can deduce that DODAGID must be
* the destination, so set the D flag.
*/
if (rpl_instance_id_is_local(instance->id) && !rpl_data_is_rpl_downward_route(route_info->source)) {
opt[3] |= RPL_INSTANCE_DEST;
}
common_write_16_bit(RPL_RANK_INFINITE, opt + 4); // SenderRank (placeholder)
}
/* Pad HbH header if necessary. */
uint8_t pad_len = ext + ext_size - (opt + 2 + opt[1]);
if (pad_len == 1) {
opt[0] = IPV6_OPTION_PAD1;
} else if (pad_len > 1) {
opt[0] = IPV6_OPTION_PADN;
opt[1] = pad_len - 2;
memset(opt + 2, 0, pad_len - 2);
}
// don't forget to set the "RPL option present" marker
buf->options.ip_extflags |= IPEXT_HBH_RPL;
*result = 0;
return buf;
}
case IPV6_EXTHDR_MODIFY: {
uint8_t *opt;
uint16_t sender_rank;
rpl_data_locate_info(buf, &opt, NULL);
if (!opt) {
*result = IPV6_EXTHDR_MODIFY_TUNNEL;
// Tunnel to next hop in general case, but if going to DODAGID,
// it can tunnel all the way (and it HAS to if it is a local
// DODAG).
if (!addr_ipv6_equal(buf->dst_sa.address, dodag->id)) {
memcpy(buf->dst_sa.address, route_info->next_hop_addr, 16);
}
buf->src_sa.addr_type = ADDR_NONE; // force auto-selection
return buf;
}
if (buf->ip_routed_up) {
/* Check for rank errors - RFC 6550 11.2.2.2. */
/* Note that RPL spec does not say that packets from nodes of
* equal rank are errors, but we treat them as such to get
* reliable sibling loop detection - we require sender rank to be
* strictly less for Down packets and strictly greater for Up.
*/
sender_rank = common_read_16_bit(opt + 4);
rpl_cmp_t cmp = rpl_rank_compare_dagrank_rank(dodag, sender_rank, instance->current_rank);
rpl_cmp_t expected_cmp = (opt[2] & RPL_OPT_DOWN) ? RPL_CMP_LESS : RPL_CMP_GREATER;
if (cmp != expected_cmp) {
/* Set the Rank-Error bit; if already set, drop */
if (opt[2] & RPL_OPT_RANK_ERROR) {
protocol_stats_update(STATS_RPL_ROUTELOOP, 1);
tr_info("Forwarding inconsistency R");
rpl_instance_inconsistency(instance);
*result = -1;
return buf;
} else {
opt[2] |= RPL_OPT_RANK_ERROR;
}
}
}
if (buf->rpl_flag_error & RPL_OPT_FWD_ERROR) {
opt[2] |= RPL_OPT_FWD_ERROR;
} else if (rpl_data_is_rpl_downward_route(route_info->source)) {
opt[2] |= RPL_OPT_DOWN;
} else {
opt[2] &= ~RPL_OPT_DOWN;
}
/* Set the D flag for local instances */
if (rpl_instance_id_is_local(instance->id)) {
if (addr_ipv6_equal(dodag->id, buf->dst_sa.address)) {
opt[3] |= RPL_INSTANCE_DEST;
} else if (addr_ipv6_equal(dodag->id, buf->src_sa.address)) {
opt[3] &=~ RPL_INSTANCE_DEST;
} else {
tr_error("Local instance invalid %s[%d]: %s -> %s", trace_ipv6(dodag->id), instance->id, trace_ipv6(buf->src_sa.address), trace_ipv6(buf->dst_sa.address));
*result = -1;
return buf;
}
}
/* RPL 11.2.2.2. says we set SenderRank to infinite when forwarding
* across a version discontinuity. (Must be up - we don't know versions
* of downward routes).
*/
if ((buf->rpl_flag_error & RPL_OPT_FWD_ERROR) || rpl_data_is_rpl_downward_route(route_info->source) || !neighbour || neighbour->dodag_version == instance->current_dodag_version) {
sender_rank = nrpl_dag_rank(dodag, instance->current_rank);
} else {
sender_rank = RPL_RANK_INFINITE;
}
common_write_16_bit(sender_rank, opt + 4);
*result = 0;
return buf;
}
default:
return buffer_free(buf);
}
}
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);
}