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;
}
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_parse_child_update_response(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, mle_security_header_t *security_headers, uint8_t linkMargin)
{
uint8_t mode;
uint32_t timeout;
mle_neigh_table_entry_t *entry_temp;
thread_leader_data_t leaderData = {0};
uint8_t status;
bool leader_data_received;
tr_debug("Child Update Response");
leader_data_received = thread_leader_data_parse(mle_msg->data_ptr, mle_msg->data_length, &leaderData);
entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false);
if (mle_tlv_read_8_bit_tlv(MLE_TYPE_STATUS, mle_msg->data_ptr, mle_msg->data_length, &status) &&
status == 1 && thread_check_is_this_my_parent(cur, entry_temp)) {
tr_debug("parent has connection error");
thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
return;
}
if (!entry_temp) {
tr_debug("Not Neighbor");
mle_tlv_info_t challengeTlv;
mle_tlv_read_tlv(MLE_TYPE_CHALLENGE, mle_msg->data_ptr, mle_msg->data_length, &challengeTlv);
thread_host_bootstrap_child_update_negative_response(cur, mle_msg->packet_src_address, &challengeTlv);
return;
}
if ((security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX)
&& (thread_instance_id_matches(cur, &leaderData))) {
thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
} else {
tr_debug("Key ID Mode 2 not used; dropped.");
return;
}
if (!mle_tlv_read_8_bit_tlv(MLE_TYPE_MODE, mle_msg->data_ptr, mle_msg->data_length, &mode)) {
tr_debug("No Mode");
return;
}
if (mle_tlv_read_32_bit_tlv(MLE_TYPE_TIMEOUT, mle_msg->data_ptr, mle_msg->data_length, &timeout)) {
entry_temp->holdTime = 90;
tr_debug("Setting child timeout, value=%"PRIu32, timeout);
mle_entry_timeout_update(entry_temp, timeout);
thread_info(cur)->thread_endnode_parent->childUpdateProcessStatus = true;
}
tr_debug("Keep-Alive -->Respond from Parent");
mle_entry_timeout_refresh(entry_temp);
//Save possible new Leader Data
if (leader_data_received) {
thread_save_leader_data(cur, &leaderData);
}
mac_data_poll_protocol_poll_mode_decrement(cur);
}
bool mle_tlv_read_32_bit_tlv(mle_tlv_type_t reqType, uint8_t *ptr, uint16_t data_len, uint32_t *buffer)
{
mle_tlv_info_t tlv_info;
if (mle_tlv_option_discover(ptr, data_len, reqType, &tlv_info) >= 4) {
*buffer = common_read_32_bit(tlv_info.dataPtr);
return true;
}
return false;
}
uint8_t thread_meshcop_tlv_data_get_uint32(const uint8_t *ptr, uint16_t length, uint8_t type, uint32_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 >= 4 && data_ptr) {
*data_ptr = common_read_32_bit(result_ptr);
}
return result_len;
}
static void thread_discover_security_material_update(protocol_interface_info_entry_t *cur, const mlme_security_t *security_params)
{
if (!security_params || !cur) {
return;
}
if (security_params->SecurityLevel != SEC_ENC_MIC32 || security_params->KeyIdMode != MAC_KEY_ID_MODE_SRC4_IDX || security_params->KeyIndex != THREAD_DISCOVERY_SECURITY_KEY_INDEX) {
return;
}
mac_description_storage_size_t buffer;
if (common_read_32_bit(security_params->Keysource) != THREAD_DISCOVERY_SECURITY_KEY_SOURCE) {
return;
}
if (!cur->mac_api || !cur->mac_api->mac_storage_sizes_get || cur->mac_api->mac_storage_sizes_get(cur->mac_api, &buffer) != 0) {
return;
}
thread_discover_device_descriptor_set(cur->mac_api, thread_discovery_extented_address, buffer.device_decription_table_size - 1);
}
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);
}
static void thread_parse_accept(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, mle_security_header_t *security_headers, uint8_t linkMargin)
{
uint32_t llFrameCounter;
uint32_t mleFrameCounter;
uint16_t version, shortAddress;
uint16_t messageId;
uint8_t linkMarginfronNeigh;
mle_neigh_table_entry_t *entry_temp;
bool createNew;
tr_info("MLE LINK ACCEPT");
messageId = mle_tlv_validate_response(mle_msg->data_ptr, mle_msg->data_length);
if (messageId == 0) {
tr_debug("Not for me");
return;
}
if (!addr_is_ipv6_multicast(mle_service_get_msg_destination_address_pointer(messageId))) {
//Free Response only if it is unicast
mle_service_msg_free(messageId);
}
// TODO: Check missing TLV's
if ((!mle_tlv_read_16_bit_tlv(MLE_TYPE_VERSION, mle_msg->data_ptr, mle_msg->data_length, &version)) ||
(!mle_tlv_read_32_bit_tlv(MLE_TYPE_LL_FRAME_COUNTER, mle_msg->data_ptr, mle_msg->data_length, &llFrameCounter)) ||
(!mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &shortAddress))) {
return;
}
/* Call to determine whether or not we should create a new link */
createNew = thread_bootstrap_link_create_check(cur, shortAddress);
entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, createNew);
if (!entry_temp) {
thread_link_reject_send(cur, mle_msg->packet_src_address);
return;
}
if (security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
}
//If MLE MLE_TYPE_MLE_FRAME_COUNTER TLV is present then use it for validating further messages else use link layer frame counter
if ((!mle_tlv_read_32_bit_tlv(MLE_TYPE_MLE_FRAME_COUNTER, mle_msg->data_ptr, mle_msg->data_length, &mleFrameCounter))) {
mleFrameCounter = llFrameCounter;
}
entry_temp->threadNeighbor = true;
entry_temp->short_adr = shortAddress;
entry_temp->mle_frame_counter = mleFrameCounter;
// Set full data as REED needs full data and SED will not make links
entry_temp->mode |= MLE_THREAD_REQ_FULL_DATA_SET;
mac_helper_devicetable_set(entry_temp, cur, llFrameCounter, security_headers->KeyIndex);
if (entry_temp->timeout_rx) {
mle_entry_timeout_refresh(entry_temp);
} else {
mle_entry_timeout_update(entry_temp, THREAD_DEFAULT_LINK_LIFETIME);
}
if (thread_i_am_router(cur) && thread_is_router_addr(entry_temp->short_adr)) {
// If we both are routers, mark the link as 2-way
entry_temp->handshakeReady = 1;
tr_debug("Marked link as 2-way, handshakeReady=%d", entry_temp->handshakeReady);
} else {
tr_debug("Marked link as 1-way, handshakeReady=%d", entry_temp->handshakeReady);
}
blacklist_update(mle_msg->packet_src_address, true);
if (mle_tlv_read_8_bit_tlv(MLE_TYPE_RSSI, mle_msg->data_ptr, mle_msg->data_length, &linkMarginfronNeigh)) {
thread_routing_update_link_margin(cur, entry_temp->short_adr, linkMargin, linkMarginfronNeigh);
}
}
static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, mle_security_header_t *security_headers, uint8_t linkMargin)
{
mle_tlv_info_t routeTlv;
thread_leader_data_t leaderData;
uint16_t shortAddress;
mle_neigh_table_entry_t *entry_temp;
// Check device mode & bootstrap state
if ((thread_info(cur)->thread_device_mode == THREAD_DEVICE_MODE_SLEEPY_END_DEVICE) ||
(cur->nwk_bootstrap_state != ER_BOOTSRAP_DONE && cur->nwk_bootstrap_state != ER_MLE_ATTACH_READY)) {
return;
}
tr_debug("Received Advertisement");
// Validate and parse TLV's
if (!thread_router_advertiment_tlv_analyze(mle_msg->data_ptr, mle_msg->data_length, &leaderData, &shortAddress, &routeTlv)) {
return;
}
// Get MLE entry
entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false);
if ((security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX)
&& (thread_instance_id_matches(cur, &leaderData))) {
thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
}
// Check parent status
if (!thread_attach_active_router(cur)) {
//processing for non routers
if (thread_check_is_this_my_parent(cur, entry_temp)) {
//advertisement from parent
if ((thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) ||
(thread_info(cur)->thread_leader_data->weighting != leaderData.weighting)) {
//parent changed partition/weight - reset own routing information
thread_old_partition_data_purge(cur);
}
//check if network data needs to be requested
if (!thread_bootstrap_request_network_data(cur, &leaderData, shortAddress)) {
tr_debug("Parent short address changed - re-attach");
thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
return;
}
}
}
// Update MLE entry
thread_update_mle_entry(cur, mle_msg, security_headers, entry_temp, shortAddress);
// Process advertisement
if (thread_info(cur)->thread_device_mode != THREAD_DEVICE_MODE_END_DEVICE) {
if (!thread_attach_active_router(cur)) {
// REED and FED
if (!entry_temp && thread_bootstrap_link_create_check(cur, shortAddress) && thread_bootstrap_link_create_allowed(cur, shortAddress, mle_msg->packet_src_address)) {
if ((thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId) &&
(thread_info(cur)->thread_leader_data->weighting == leaderData.weighting)) {
// Create link to new neighbor no other processing allowed
thread_link_request_start(cur, mle_msg->packet_src_address);
return;
}
if (!thread_router_leader_data_process(cur, mle_msg->packet_src_address, &leaderData, &routeTlv, entry_temp)) {
// better partition found or new network data learn started
return;
}
}
// process REED advertisement from higher partition
if (thread_reed_partitions_merge(cur, shortAddress, leaderData)) {
return;
}
} else {
//Router
if (!thread_router_leader_data_process(cur, mle_msg->packet_src_address, &leaderData, &routeTlv, entry_temp) ) {
return;
}
thread_router_bootstrap_advertiment_analyze(cur, mle_msg->packet_src_address, entry_temp, shortAddress);
}
}
// Process route TLV
if ((entry_temp && routeTlv.dataPtr && routeTlv.tlvLen) &&
(thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId)){
tr_debug("Update Route TLV %x", entry_temp->short_adr);
thread_router_bootstrap_route_tlv_push(cur, routeTlv.dataPtr, routeTlv.tlvLen , linkMargin, entry_temp);
}
}
