diff --git a/source/MAC/IEEE802_15_4/mac_header_helper_functions.c b/source/MAC/IEEE802_15_4/mac_header_helper_functions.c index 7865cc7cf70..5f8273bb6b7 100644 --- a/source/MAC/IEEE802_15_4/mac_header_helper_functions.c +++ b/source/MAC/IEEE802_15_4/mac_header_helper_functions.c @@ -267,6 +267,9 @@ const uint8_t *mac_header_parse_fcf_dsn(mac_fcf_sequence_t *header, const uint8_ } else { header->DSN = 0; } + //Check PanID presents at header + header->DstPanPresents = mac_dst_panid_present(header); + header->SrcPanPresents = mac_src_panid_present(header); return ptr; } diff --git a/source/MAC/IEEE802_15_4/mac_mcps_sap.c b/source/MAC/IEEE802_15_4/mac_mcps_sap.c index 60f0fdb16ff..df5e954e61e 100644 --- a/source/MAC/IEEE802_15_4/mac_mcps_sap.c +++ b/source/MAC/IEEE802_15_4/mac_mcps_sap.c @@ -362,8 +362,6 @@ static int8_t mac_virtual_data_req_handler(protocol_interface_rf_mac_setup_s *rf } mac_header_parse_fcf_dsn(&buffer->fcf_dsn, data_ptr); - buffer->fcf_dsn.DstPanPresents = mac_dst_panid_present(&buffer->fcf_dsn); - buffer->fcf_dsn.SrcPanPresents = mac_src_panid_present(&buffer->fcf_dsn); // Use MAC sequence as handle buffer->msduHandle = buffer->fcf_dsn.DSN; memcpy(buffer->mac_payload, data_ptr, data_length); @@ -1607,9 +1605,8 @@ static int8_t mcps_generic_packet_build(protocol_interface_rf_mac_setup_s *rf_pt return 0; } -int8_t mcps_generic_ack_build(protocol_interface_rf_mac_setup_s *rf_ptr, const mac_fcf_sequence_t *fcf, const uint8_t *data_ptr, const mcps_ack_data_payload_t *ack_payload, uint32_t rx_time) +int8_t mcps_generic_ack_build(protocol_interface_rf_mac_setup_s *rf_ptr, const mac_fcf_sequence_t *fcf, const uint8_t *data_ptr, const mcps_ack_data_payload_t *ack_payload) { - (void)rx_time; phy_device_driver_s *dev_driver = rf_ptr->dev_driver->phy_driver; dev_driver_tx_buffer_s *tx_buf = &rf_ptr->dev_driver_tx_buffer; @@ -2071,7 +2068,9 @@ static mac_pre_build_frame_t *mcps_sap_pd_req_queue_read(protocol_interface_rf_m void mcps_sap_pre_parsed_frame_buffer_free(mac_pre_parsed_frame_t *buf) { - ns_dyn_mem_free(buf); + if (buf) { + ns_dyn_mem_free(buf); + } } mac_pre_parsed_frame_t *mcps_sap_pre_parsed_frame_buffer_get(const uint8_t *data_ptr, uint16_t frame_length) diff --git a/source/MAC/IEEE802_15_4/mac_mcps_sap.h b/source/MAC/IEEE802_15_4/mac_mcps_sap.h index ea8acee933d..a4f4baeb73c 100644 --- a/source/MAC/IEEE802_15_4/mac_mcps_sap.h +++ b/source/MAC/IEEE802_15_4/mac_mcps_sap.h @@ -214,6 +214,6 @@ uint8_t mcps_sap_purge_reg_handler(struct protocol_interface_rf_mac_setup *rf_ma int8_t mcps_pd_data_rebuild(struct protocol_interface_rf_mac_setup *rf_ptr, mac_pre_build_frame_t *buffer); -int8_t mcps_generic_ack_build(struct protocol_interface_rf_mac_setup *rf_ptr, const mac_fcf_sequence_t *fcf, const uint8_t *data_ptr, const mcps_ack_data_payload_t *ack_payload, uint32_t rx_time); +int8_t mcps_generic_ack_build(struct protocol_interface_rf_mac_setup *rf_ptr, const mac_fcf_sequence_t *fcf, const uint8_t *data_ptr, const mcps_ack_data_payload_t *ack_payload); #endif /* MAC_IEEE802_15_4_MAC_MCPS_SAP_H_ */ diff --git a/source/MAC/IEEE802_15_4/mac_pd_sap.c b/source/MAC/IEEE802_15_4/mac_pd_sap.c index 9a0e02c32cd..71443a9c78e 100644 --- a/source/MAC/IEEE802_15_4/mac_pd_sap.c +++ b/source/MAC/IEEE802_15_4/mac_pd_sap.c @@ -252,6 +252,9 @@ void mac_pd_sap_set_phy_tx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup, */ static uint32_t mac_pd_sap_get_phy_rx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup) { + if (!rf_mac_setup->rf_csma_extension_supported) { + return 0; + } uint8_t rx_time_buffer[4]; rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_READ_RX_TIME, rx_time_buffer); return common_read_32_bit(rx_time_buffer); @@ -625,198 +628,198 @@ static bool mac_pd_sap_ack_validation(protocol_interface_rf_mac_setup_s *rf_ptr, return true; } -int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message) +static int8_t mac_pd_sap_validate_fcf(protocol_interface_rf_mac_setup_s *rf_ptr, mac_fcf_sequence_t fcf_read, arm_pd_sap_generic_ind_t *pd_data_ind) { - protocol_interface_rf_mac_setup_s *rf_ptr = (protocol_interface_rf_mac_setup_s *)identifier; - mac_pre_parsed_frame_t *buffer = NULL; - - if (!rf_ptr || !message) { - return -1; - } - - if (!rf_ptr->macUpState) { - return -2; + switch (fcf_read.frametype) { + case FC_DATA_FRAME: + if (fcf_read.SrcAddrMode == MAC_ADDR_MODE_NONE) { + return -1; + } else if (fcf_read.DstAddrMode == MAC_ADDR_MODE_NONE && fcf_read.frameVersion != MAC_FRAME_VERSION_2015) { + return -1; + } + break; + case FC_BEACON_FRAME: + if (fcf_read.SrcAddrMode == MAC_ADDR_MODE_NONE || fcf_read.DstAddrMode != MAC_ADDR_MODE_NONE) { + return -1; + } + break; + case FC_ACK_FRAME: + // Only accept version 2015 Acks + if (fcf_read.frameVersion != MAC_FRAME_VERSION_2015) { + return -1; + } + //Validate Ack doesn't request Ack + if (fcf_read.ackRequested) { + return -1; + } + //Validate ACK + if (!mac_pd_sap_ack_validation(rf_ptr, &fcf_read, pd_data_ind->data_ptr)) { + return -1; + } + break; + case FC_CMD_FRAME: + break; + default: + return -1; } + return 0; +} - if (message->id == MAC15_4_PD_SAP_DATA_IND) { - const uint8_t *ptr; - arm_pd_sap_generic_ind_t *pd_data_ind = &(message->message.generic_data_ind); - - if (pd_data_ind->data_len < 3) { +static int8_t mac_pd_sap_generate_ack(protocol_interface_rf_mac_setup_s *rf_ptr, mac_fcf_sequence_t fcf_read, arm_pd_sap_generic_ind_t *pd_data_ind) +{ + //Generate ACK when Extension is enabled and ACK is requested + if (rf_ptr->mac_extension_enabled && fcf_read.ackRequested && fcf_read.frameVersion == MAC_FRAME_VERSION_2015) { + //SEND ACK here + if (rf_ptr->mac_ack_tx_active) { return -1; } - ptr = pd_data_ind->data_ptr; - - uint32_t time_stamp = 0; - if (rf_ptr->rf_csma_extension_supported) { - time_stamp = mac_pd_sap_get_phy_rx_time(rf_ptr); - } - mac_fcf_sequence_t fcf_read; - ptr = mac_header_parse_fcf_dsn(&fcf_read, ptr); - //Check PanID presents at header - fcf_read.DstPanPresents = mac_dst_panid_present(&fcf_read); - fcf_read.SrcPanPresents = mac_src_panid_present(&fcf_read); - int16_t length = pd_data_ind->data_len; - if (!rf_ptr->macProminousMode) { - //Unsupported Frame - if (fcf_read.frametype > FC_CMD_FRAME || (fcf_read.frametype == FC_ACK_FRAME && fcf_read.frameVersion != MAC_FRAME_VERSION_2015)) { - goto ERROR_HANDLER; - } + mcps_ack_data_payload_t ack_payload; + mac_api_t *mac_api = get_sw_mac_api(rf_ptr); + mac_api->enhanced_ack_data_req_cb(mac_api, &ack_payload, pd_data_ind->dbm, pd_data_ind->link_quality); + //Calculate Delta time - switch (fcf_read.frametype) { - case FC_DATA_FRAME: - if (fcf_read.SrcAddrMode == MAC_ADDR_MODE_NONE) { - return -1; - } else if (fcf_read.DstAddrMode == MAC_ADDR_MODE_NONE && fcf_read.frameVersion != MAC_FRAME_VERSION_2015) { - return -1; - } - break; - case FC_BEACON_FRAME: - if (fcf_read.SrcAddrMode == MAC_ADDR_MODE_NONE || fcf_read.DstAddrMode != MAC_ADDR_MODE_NONE) { - return -1; - } - break; - case FC_ACK_FRAME: - //Validate here that we are waiting ack - if (fcf_read.ackRequested) { - return -1; - } + if (mcps_generic_ack_build(rf_ptr, &fcf_read, pd_data_ind->data_ptr, &ack_payload) != 0) { + return -1; + } + } + return 0; +} - //Validate ACK - if (!mac_pd_sap_ack_validation(rf_ptr, &fcf_read, pd_data_ind->data_ptr)) { - return -1; - } - break; +static mac_pre_parsed_frame_t *mac_pd_sap_allocate_receive_buffer(protocol_interface_rf_mac_setup_s *rf_ptr, mac_fcf_sequence_t fcf_read, arm_pd_sap_generic_ind_t *pd_data_ind) +{ + mac_pre_parsed_frame_t *buffer = mcps_sap_pre_parsed_frame_buffer_get(pd_data_ind->data_ptr, pd_data_ind->data_len); + if (!buffer) { + return NULL; + } + + //Copy Pre Parsed values + buffer->fcf_dsn = fcf_read; + buffer->timestamp = mac_pd_sap_get_phy_rx_time(rf_ptr); + buffer->ack_pendinfg_status = mac_data_interface_read_last_ack_pending_status(rf_ptr); + /* Set default flags */ + buffer->dbm = pd_data_ind->dbm; + buffer->LQI = pd_data_ind->link_quality; + buffer->mac_class_ptr = rf_ptr; + return buffer; +} - default: - break; - } +static int8_t mac_pd_sap_parse_length_fields(mac_pre_parsed_frame_t *buffer, arm_pd_sap_generic_ind_t *pd_data_ind, const uint8_t *parse_ptr) +{ + if (buffer->fcf_dsn.frametype > FC_CMD_FRAME) { + return -1; + } + buffer->mac_header_length = parse_ptr - pd_data_ind->data_ptr; + int16_t length = pd_data_ind->data_len; + buffer->mac_header_length += mac_header_address_length(&buffer->fcf_dsn); + length -= buffer->mac_header_length; + if (length < 0) { + return -1; + } - //Generate ACK when Extension is enabled and ACK is requested - if (rf_ptr->mac_extension_enabled && fcf_read.ackRequested && fcf_read.frameVersion == MAC_FRAME_VERSION_2015) { - //SEND ACK here - if (rf_ptr->mac_ack_tx_active) { - return -1; - } + buffer->mac_payload_length = (buffer->frameLength - buffer->mac_header_length); - mcps_ack_data_payload_t ack_payload; - mac_api_t *mac_api = get_sw_mac_api(rf_ptr); - mac_api->enhanced_ack_data_req_cb(mac_api, &ack_payload, pd_data_ind->dbm, pd_data_ind->link_quality); - //Calculate Delta time + if (buffer->fcf_dsn.securityEnabled) { + //Read KEYID Mode + uint8_t key_id_mode, security_level, mic_len; + uint8_t *security_ptr = &buffer->buf[buffer->mac_header_length]; + uint8_t auxBaseHeader = *security_ptr; + key_id_mode = (auxBaseHeader >> 3) & 3; + security_level = auxBaseHeader & 7; - if (mcps_generic_ack_build(rf_ptr, &fcf_read, pd_data_ind->data_ptr, &ack_payload, time_stamp) != 0) { - return -1; + switch (key_id_mode) { + case MAC_KEY_ID_MODE_IMPLICIT: + if (security_level) { + buffer->security_aux_header_length = 5; + } else { + buffer->security_aux_header_length = 1; } - } + break; + case MAC_KEY_ID_MODE_IDX: + buffer->security_aux_header_length = 6; + break; + case MAC_KEY_ID_MODE_SRC4_IDX: + buffer->security_aux_header_length = 10; + break; + default: + buffer->security_aux_header_length = 14; + break; } - buffer = mcps_sap_pre_parsed_frame_buffer_get(pd_data_ind->data_ptr, pd_data_ind->data_len); + length -= buffer->security_aux_header_length; + mic_len = mac_security_mic_length_get(security_level); - if (!buffer) { - sw_mac_stats_update(rf_ptr, STAT_MAC_RX_DROP, 0); - return -3; + length -= mic_len; + + //Verify that data length is not negative + if (length < 0) { + return -1; } - //Copy Pre Parsed values - buffer->fcf_dsn = fcf_read; - buffer->timestamp = time_stamp; + buffer->mac_payload_length -= (buffer->security_aux_header_length + mic_len); + } + //Do not accept command frame with length 0 + if (buffer->fcf_dsn.frametype == FC_CMD_FRAME && length == 0) { + return -1; + } + return 0; +} - buffer->ack_pendinfg_status = mac_data_interface_read_last_ack_pending_status(rf_ptr); +int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message) +{ + protocol_interface_rf_mac_setup_s *rf_ptr = (protocol_interface_rf_mac_setup_s *)identifier; + if (!rf_ptr || !message) { + return -1; + } - // Upward direction functions assume no headroom and are trusting that removed bytes are still valid. - // see mac.c:655 + if (!rf_ptr->macUpState) { + return -2; + } - /* Set default flags */ - buffer->dbm = pd_data_ind->dbm; - buffer->LQI = pd_data_ind->link_quality; - buffer->mac_class_ptr = rf_ptr; - //Dnamic calculation for FCF + SEQ parse - buffer->mac_header_length = ptr - pd_data_ind->data_ptr; + if (message->id == MAC15_4_PD_SAP_DATA_IND) { + arm_pd_sap_generic_ind_t *pd_data_ind = &(message->message.generic_data_ind); + if (pd_data_ind->data_len < 3) { + return -1; + } + mac_fcf_sequence_t fcf_read; + const uint8_t *ptr = mac_header_parse_fcf_dsn(&fcf_read, pd_data_ind->data_ptr); + mac_pre_parsed_frame_t *buffer = mac_pd_sap_allocate_receive_buffer(rf_ptr, fcf_read, pd_data_ind); if (!rf_ptr->macProminousMode) { - - if (buffer->fcf_dsn.frametype > FC_CMD_FRAME) { + if (mac_pd_sap_validate_fcf(rf_ptr, fcf_read, pd_data_ind)) { goto ERROR_HANDLER; } - - buffer->mac_header_length += mac_header_address_length(&buffer->fcf_dsn); - - length -= buffer->mac_header_length; - - if (length < 0) { + if (mac_pd_sap_generate_ack(rf_ptr, fcf_read, pd_data_ind)) { goto ERROR_HANDLER; } - - buffer->mac_payload_length = (buffer->frameLength - buffer->mac_header_length); - - if (buffer->fcf_dsn.securityEnabled) { - //Read KEYID Mode - uint8_t key_id_mode, security_level, mic_len; - uint8_t *security_ptr = &buffer->buf[buffer->mac_header_length]; - uint8_t auxBaseHeader = *security_ptr; - key_id_mode = (auxBaseHeader >> 3) & 3; - security_level = auxBaseHeader & 7; - - switch (key_id_mode) { - case MAC_KEY_ID_MODE_IMPLICIT: - if (security_level) { - buffer->security_aux_header_length = 5; - } else { - buffer->security_aux_header_length = 1; - } - break; - case MAC_KEY_ID_MODE_IDX: - buffer->security_aux_header_length = 6; - break; - case MAC_KEY_ID_MODE_SRC4_IDX: - buffer->security_aux_header_length = 10; - break; - default: - buffer->security_aux_header_length = 14; - break; + if (buffer) { + if (mac_pd_sap_parse_length_fields(buffer, pd_data_ind, ptr)) { + goto ERROR_HANDLER; } - - length -= buffer->security_aux_header_length; - mic_len = mac_security_mic_length_get(security_level); - - length -= mic_len; - - //Verify that data length is not negative - if (length < 0) { + if (!mac_header_information_elements_parse(buffer)) { goto ERROR_HANDLER; } - - buffer->mac_payload_length -= (buffer->security_aux_header_length + mic_len); - } - - //Do not accept command frame with length 0 - if (fcf_read.frametype == FC_CMD_FRAME && length == 0) { - goto ERROR_HANDLER; - } - - //Parse IE Elements - if (!mac_header_information_elements_parse(buffer)) { - goto ERROR_HANDLER; + if (buffer->fcf_dsn.frametype == FC_ACK_FRAME) { + if (mac_data_interface_tx_done_by_ack_cb(rf_ptr, buffer)) { + mcps_sap_pre_parsed_frame_buffer_free(buffer); + } + return 0; + } } } - - if (!rf_ptr->macProminousMode && buffer->fcf_dsn.frametype == FC_ACK_FRAME) { - if (mac_data_interface_tx_done_by_ack_cb(rf_ptr, buffer)) { - mcps_sap_pre_parsed_frame_buffer_free(buffer); - } + if (!buffer) { + sw_mac_stats_update(rf_ptr, STAT_MAC_RX_DROP, 0); + return -3; + } + if (mcps_sap_pd_ind(buffer) == 0) { return 0; - } else { - if (mcps_sap_pd_ind(buffer) == 0) { - return 0; - } } ERROR_HANDLER: mcps_sap_pre_parsed_frame_buffer_free(buffer); + sw_mac_stats_update(rf_ptr, STAT_MAC_RX_DROP, 0); return -1; - } else if (message->id == MAC15_4_PD_SAP_DATA_TX_CONFIRM) { arm_pd_sap_15_4_confirm_with_params_t *pd_data_cnf = &(message->message.mac15_4_pd_sap_confirm); return mac_data_interface_tx_done_cb(rf_ptr, pd_data_cnf->status, pd_data_cnf->cca_retry, pd_data_cnf->tx_retry); diff --git a/test/nanostack/unittest/mac/mac_mcps_sap/test_mac_mcps_sap.c b/test/nanostack/unittest/mac/mac_mcps_sap/test_mac_mcps_sap.c index 745f9b136c0..a793d0a527a 100644 --- a/test/nanostack/unittest/mac/mac_mcps_sap/test_mac_mcps_sap.c +++ b/test/nanostack/unittest/mac/mac_mcps_sap/test_mac_mcps_sap.c @@ -2342,7 +2342,7 @@ bool test_mcps_sap_data_req_ext_handler() mcps_pd_data_rebuild(rf_mac_setup, rf_mac_setup->active_pd_data_request); mcps_ack_data_payload_t ack_data; memset(&ack_data, 0, sizeof(mcps_ack_data_payload_t)); - mcps_generic_ack_build(rf_mac_setup, &rf_mac_setup->active_pd_data_request->fcf_dsn, rf_mac_setup->active_pd_data_request->mac_payload, &ack_data, 100); + mcps_generic_ack_build(rf_mac_setup, &rf_mac_setup->active_pd_data_request->fcf_dsn, rf_mac_setup->active_pd_data_request->mac_payload, &ack_data); mac_mcps_buffer_queue_free(rf_mac_setup); diff --git a/test/nanostack/unittest/stub/mac_mcps_sap_stub.c b/test/nanostack/unittest/stub/mac_mcps_sap_stub.c index a436e6d5c97..ebe3d72f510 100644 --- a/test/nanostack/unittest/stub/mac_mcps_sap_stub.c +++ b/test/nanostack/unittest/stub/mac_mcps_sap_stub.c @@ -167,7 +167,7 @@ int8_t mcps_pd_data_rebuild(struct protocol_interface_rf_mac_setup *rf_ptr, mac_ return mac_mcps_sap_stub.int8_value; } -int8_t mcps_generic_ack_build(struct protocol_interface_rf_mac_setup *rf_ptr, const mac_fcf_sequence_t *fcf, const uint8_t *data_ptr, const mcps_ack_data_payload_t *ack_payload, uint32_t rx_time) +int8_t mcps_generic_ack_build(struct protocol_interface_rf_mac_setup *rf_ptr, const mac_fcf_sequence_t *fcf, const uint8_t *data_ptr, const mcps_ack_data_payload_t *ack_payload) { return 0; }