| /* |
| * This file is part of the UWB stack for linux. |
| * |
| * Copyright (c) 2020-2022 Qorvo US, Inc. |
| * |
| * This software is provided under the GNU General Public License, version 2 |
| * (GPLv2), as well as under a Qorvo commercial license. |
| * |
| * You may choose to use this software under the terms of the GPLv2 License, |
| * version 2 ("GPLv2"), as published by the Free Software Foundation. |
| * You should have received a copy of the GPLv2 along with this program. If |
| * not, see <http://www.gnu.org/licenses/>. |
| * |
| * This program is distributed under the GPLv2 in the hope that it will be |
| * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of |
| * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GPLv2 for more |
| * details. |
| * |
| * If you cannot meet the requirements of the GPLv2, you may not use this |
| * software for any purpose without first obtaining a commercial license from |
| * Qorvo. Please contact Qorvo to inquire about licensing terms. |
| */ |
| |
| #include "fira_frame.h" |
| #include "fira_session.h" |
| #include "fira_crypto.h" |
| #include "fira_trace.h" |
| |
| #include <asm/unaligned.h> |
| #include <linux/bitfield.h> |
| #include <linux/errno.h> |
| #include <linux/ieee802154.h> |
| #include <linux/math64.h> |
| #include <net/af_ieee802154.h> |
| #include <net/mcps802154_frame.h> |
| |
| #include "warn_return.h" |
| |
| bool fira_frame_check_n_controlees(const struct fira_session *session, |
| size_t n_controlees, bool active) |
| { |
| /* |
| * TODO: use more parameters (embedded mode, ranging mode, device |
| * type...) to calculate the size of frames. |
| * Currently only SS-TWR vs DS-TWR mode is considered. |
| * The computation MUST stay "pessimistic" (aka strict). |
| * E.g.: for control frame, each new controlee consumes 8 bytes so |
| * we need AT LEAST 8 * n_controlee bytes of "free space". |
| */ |
| const struct fira_session_params *params = &session->params; |
| size_t mrm_size, rcm_size; |
| size_t n_msg_controller; |
| size_t n_msg_controlee = 2; |
| |
| if (n_controlees > FIRA_CONTROLEES_MAX) |
| return false; |
| if (!active) |
| return true; |
| |
| if (params->ranging_round_usage == FIRA_RANGING_ROUND_USAGE_DSTWR) { |
| mrm_size = FIRA_FRAME_WITHOUT_PAYLOAD_LEN + |
| FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE1_LEN( |
| 1, n_controlees); |
| n_msg_controller = 4; |
| } else { |
| mrm_size = FIRA_FRAME_WITHOUT_PAYLOAD_LEN + |
| FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE2_LEN( |
| 1, 0, n_controlees); |
| n_msg_controller = 3; |
| } |
| |
| rcm_size = FIRA_FRAME_WITHOUT_PAYLOAD_LEN + |
| FIRA_IE_PAYLOAD_CONTROL_LEN(n_msg_controller + |
| n_msg_controlee * n_controlees); |
| |
| return mrm_size <= IEEE802154_MTU && rcm_size <= IEEE802154_MTU; |
| } |
| |
| void fira_frame_header_put(const struct fira_local *local, |
| const struct fira_slot *slot, struct sk_buff *skb) |
| { |
| const struct fira_session *session = local->current_session; |
| u16 fc = (IEEE802154_FC_TYPE_DATA | IEEE802154_FC_SECEN | |
| IEEE802154_FC_INTRA_PAN | IEEE802154_FC_NO_SEQ | |
| (IEEE802154_ADDR_SHORT << IEEE802154_FC_DAMODE_SHIFT) | |
| (2 << IEEE802154_FC_VERSION_SHIFT) | |
| (IEEE802154_ADDR_NONE << IEEE802154_FC_SAMODE_SHIFT)); |
| u8 *p; |
| int i; |
| u8 *p_hie; |
| |
| p = skb_put(skb, IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN + |
| IEEE802154_SCF_LEN); |
| put_unaligned_le16(fc, p); |
| p += IEEE802154_FC_LEN; |
| put_unaligned_le16(local->dst_short_addr, p); |
| p += IEEE802154_SHORT_ADDR_LEN; |
| *p = IEEE802154_SCF_NO_FRAME_COUNTER; |
| |
| p_hie = skb->data + skb->len; |
| mcps802154_ie_put_begin(skb); |
| p = mcps802154_ie_put_header_ie(skb, IEEE802154_IE_HEADER_VENDOR_ID, |
| FIRA_IE_HEADER_LEN); |
| put_unaligned_le24(FIRA_IE_VENDOR_OUI, p); |
| p += FIRA_IE_VENDOR_OUI_LEN; |
| for (i = 0; i < FIRA_IE_HEADER_PADDING_LEN; i++) |
| *p++ = FIRA_IE_HEADER_PADDING; |
| put_unaligned_le32(session->id, p); |
| p += FIRA_IE_HEADER_SESSION_ID_LEN; |
| put_unaligned_le32(fira_sts_get_phy_sts_index(session, slot), p); |
| fira_sts_encrypt_hie(local->current_session, slot, skb, p_hie - skb->data, |
| FIRA_IE_HEADER_LEN + IEEE802154_IE_HEADER_LEN); |
| } |
| |
| static u8 *fira_frame_common_payload_put(struct sk_buff *skb, unsigned int len, |
| enum fira_message_id message_id) |
| { |
| u8 *p; |
| |
| p = mcps802154_ie_put_payload_ie(skb, IEEE802154_IE_PAYLOAD_VENDOR_GID, |
| len); |
| WARN_RETURN_ON(!p, NULL); |
| |
| put_unaligned_le24(FIRA_IE_VENDOR_OUI, p); |
| p += FIRA_IE_VENDOR_OUI_LEN; |
| *p++ = message_id; |
| |
| return p; |
| } |
| |
| void fira_frame_control_payload_put(const struct fira_local *local, |
| const struct fira_slot *slot, |
| struct sk_buff *skb) |
| { |
| const struct fira_session *session = local->current_session; |
| int n_mngt; |
| u8 *p; |
| int i; |
| |
| n_mngt = local->access.n_frames - 1 + local->n_stopped_controlees; |
| |
| p = fira_frame_common_payload_put(skb, |
| FIRA_IE_PAYLOAD_CONTROL_LEN(n_mngt), |
| FIRA_MESSAGE_ID_CONTROL); |
| |
| *p++ = n_mngt; |
| *p++ = 0; |
| *p++ = session->block_stride_len; |
| |
| for (i = 0; i < local->access.n_frames - 1; i++) { |
| const struct fira_slot *slot = &local->slots[i + 1]; |
| int initiator = slot->controller_tx; |
| int slot_index = slot->index; |
| __le16 short_addr = slot->controller_tx ? |
| local->src_short_addr : |
| slot->controlee->short_addr; |
| int message_id = slot->message_id; |
| u32 mngt = FIELD_PREP(FIRA_MNGT_RANGING_ROLE, initiator) | |
| FIELD_PREP(FIRA_MNGT_SLOT_INDEX, slot_index) | |
| FIELD_PREP(FIRA_MNGT_SHORT_ADDR, short_addr) | |
| FIELD_PREP(FIRA_MNGT_MESSAGE_ID, message_id); |
| put_unaligned_le32(mngt, p); |
| p += sizeof(u32); |
| } |
| |
| for (i = 0; i < local->n_stopped_controlees; i++) { |
| __le16 short_addr = local->stopped_controlees[i]; |
| u32 mngt = FIELD_PREP(FIRA_MNGT_SHORT_ADDR, short_addr) | |
| FIELD_PREP(FIRA_MNGT_STOP, 1); |
| put_unaligned_le32(mngt, p); |
| p += sizeof(u32); |
| } |
| } |
| |
| void fira_frame_measurement_report_payload_put(const struct fira_local *local, |
| const struct fira_slot *slot, |
| struct sk_buff *skb) |
| { |
| const struct fira_session *session = local->current_session; |
| const struct fira_session_params *params = &session->params; |
| const struct fira_ranging_info *ranging_info = |
| &local->ranging_info[slot->ranging_index]; |
| u8 *p; |
| int hopping_mode = params->round_hopping; |
| int round_index_present = 1; |
| int reply_time_present = 0; /* for initiator */ |
| int n_reply_time = local->n_ranging_valid; |
| int i; |
| u32 first_round_trip_time; |
| u32 reply_time; |
| u64 initiation_rctu, response_rctu, final_rctu; |
| bool double_sided = params->ranging_round_usage == |
| FIRA_RANGING_ROUND_USAGE_DSTWR; |
| |
| p = fira_frame_common_payload_put( |
| skb, |
| (double_sided ? FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE1_LEN( |
| round_index_present, n_reply_time) : |
| FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE2_LEN( |
| round_index_present, reply_time_present, |
| n_reply_time)), |
| FIRA_MESSAGE_ID_MEASUREMENT_REPORT); |
| |
| *p++ = FIELD_PREP(FIRA_MEASUREMENT_REPORT_CONTROL_HOPPING_MODE, |
| hopping_mode) | |
| FIELD_PREP(FIRA_MEASUREMENT_REPORT_CONTROL_ROUND_INDEX_PRESENT, |
| round_index_present) | |
| FIELD_PREP(FIRA_MEASUREMENT_REPORT_CONTROL_N_REPLY_TIME, |
| n_reply_time); |
| |
| if (!double_sided) |
| *p++ = FIELD_PREP( |
| FIRA_MEASUREMENT_REPORT_CONTROL_REPLY_TIME_PRESENT, |
| reply_time_present); |
| |
| put_unaligned_le16(session->next_round_index, p); |
| p += sizeof(u16); |
| |
| /* |
| * No handling for failed measurement, as there is only one, a failed |
| * measurement will cancel the ranging round. |
| * With several measurements, make sure a later measurement can still be |
| * done if an earlier one is failed. |
| */ |
| initiation_rctu = |
| ranging_info |
| ->timestamps_rctu[FIRA_MESSAGE_ID_RANGING_INITIATION]; |
| final_rctu = |
| ranging_info->timestamps_rctu[FIRA_MESSAGE_ID_RANGING_FINAL]; |
| |
| /* Retrieve first measurement. */ |
| for (i = 0; i < local->n_ranging_info; i++) { |
| ranging_info = &local->ranging_info[i]; |
| if (!ranging_info->status) |
| break; |
| } |
| response_rctu = |
| ranging_info->timestamps_rctu[FIRA_MESSAGE_ID_RANGING_RESPONSE]; |
| if (double_sided) { |
| /* Add first round trip measurement. */ |
| first_round_trip_time = mcps802154_difference_timestamp_rctu( |
| local->llhw, response_rctu, initiation_rctu); |
| put_unaligned_le32(first_round_trip_time, p); |
| p += sizeof(u32); |
| } |
| /* Retrieve reply measurement. */ |
| for (; i < local->n_ranging_info; i++) { |
| ranging_info = &local->ranging_info[i]; |
| if (ranging_info->status) |
| continue; |
| put_unaligned_le16(ranging_info->short_addr, p); |
| p += sizeof(u16); |
| response_rctu = ranging_info->timestamps_rctu |
| [FIRA_MESSAGE_ID_RANGING_RESPONSE]; |
| if (double_sided) { |
| reply_time = mcps802154_difference_timestamp_rctu( |
| local->llhw, final_rctu, response_rctu); |
| } else { |
| reply_time = mcps802154_difference_timestamp_rctu( |
| local->llhw, response_rctu, initiation_rctu); |
| } |
| put_unaligned_le32(reply_time, p); |
| p += sizeof(u32); |
| } |
| } |
| |
| void fira_frame_result_report_payload_put(const struct fira_local *local, |
| const struct fira_slot *slot, |
| struct sk_buff *skb) |
| { |
| const struct fira_session *session = local->current_session; |
| const struct fira_session_params *params = &session->params; |
| const struct fira_ranging_info *ranging_info = |
| &local->ranging_info[slot->ranging_index]; |
| bool tof_present, aoa_azimuth_present, aoa_elevation_present, |
| aoa_fom_present, neg_tof_present; |
| u8 *p; |
| |
| tof_present = ranging_info->tof_present && params->report_tof; |
| aoa_azimuth_present = ranging_info->local_aoa_azimuth.present && |
| params->report_aoa_azimuth; |
| aoa_elevation_present = ranging_info->local_aoa_elevation.present && |
| params->report_aoa_elevation; |
| aoa_fom_present = (ranging_info->local_aoa_azimuth.aoa_fom || |
| ranging_info->local_aoa_elevation.aoa_fom) && |
| params->report_aoa_fom; |
| neg_tof_present = tof_present && (ranging_info->tof_rctu < 0); |
| p = fira_frame_common_payload_put( |
| skb, |
| FIRA_IE_PAYLOAD_RESULT_REPORT_LEN( |
| tof_present, aoa_azimuth_present, aoa_elevation_present, |
| aoa_fom_present, neg_tof_present), |
| FIRA_MESSAGE_ID_RESULT_REPORT); |
| |
| *p++ = FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_TOF_PRESENT, tof_present) | |
| FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_AOA_AZIMUTH_PRESENT, |
| aoa_azimuth_present) | |
| FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_AOA_ELEVATION_PRESENT, |
| aoa_elevation_present) | |
| FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_AOA_FOM_PRESENT, |
| aoa_fom_present) | |
| FIELD_PREP(FIRA_RESULT_REPORT_CONTROL_NEG_TOF_PRESENT, |
| neg_tof_present); |
| |
| if (tof_present) { |
| put_unaligned_le32( |
| ranging_info->tof_rctu > 0 ? ranging_info->tof_rctu : 0, |
| p); |
| p += sizeof(u32); |
| } |
| if (aoa_azimuth_present) { |
| put_unaligned_le16(ranging_info->local_aoa_azimuth.aoa_2pi, p); |
| p += sizeof(u16); |
| if (aoa_fom_present) { |
| *p = ranging_info->local_aoa_azimuth.aoa_fom; |
| p++; |
| } |
| } |
| if (aoa_elevation_present) { |
| put_unaligned_le16( |
| ranging_info->local_aoa_elevation.aoa_2pi * 2, p); |
| p += sizeof(u16); |
| if (aoa_fom_present) { |
| *p = ranging_info->local_aoa_elevation.aoa_fom; |
| p++; |
| } |
| } |
| if (neg_tof_present) { |
| put_unaligned_le32(-ranging_info->tof_rctu, p); |
| p += sizeof(u32); |
| } |
| } |
| |
| void fira_frame_rframe_payload_put(struct fira_local *local, |
| struct sk_buff *skb) |
| { |
| struct fira_session *session = local->current_session; |
| const struct fira_session_params *params = &session->params; |
| u8 *p; |
| |
| if (session->data_payload.seq == params->data_payload_seq) |
| return; |
| |
| p = mcps802154_ie_put_payload_ie(skb, IEEE802154_IE_PAYLOAD_VENDOR_GID, |
| FIRA_IE_VENDOR_OUI_LEN + |
| params->data_payload_len); |
| WARN_RETURN_VOID_ON(!p); |
| put_unaligned_le24(params->data_vendor_oui, p); |
| p += FIRA_IE_VENDOR_OUI_LEN; |
| memcpy(p, params->data_payload, params->data_payload_len); |
| session->data_payload.seq = params->data_payload_seq; |
| session->data_payload.sent = true; |
| } |
| |
| bool fira_frame_header_check(struct fira_local *local, |
| const struct fira_slot *slot, struct sk_buff *skb, |
| struct mcps802154_ie_get_context *ie_get, |
| u32 *phy_sts_index, u32 *session_id) |
| { |
| struct fira_session *session = local->current_session; |
| u16 fc = (IEEE802154_FC_TYPE_DATA | IEEE802154_FC_SECEN | |
| IEEE802154_FC_INTRA_PAN | IEEE802154_FC_NO_SEQ | |
| IEEE802154_FC_IE_PRESENT | |
| (IEEE802154_ADDR_SHORT << IEEE802154_FC_DAMODE_SHIFT) | |
| (2 << IEEE802154_FC_VERSION_SHIFT) | |
| (IEEE802154_ADDR_NONE << IEEE802154_FC_SAMODE_SHIFT)); |
| u8 ciphered_hie[FIRA_IE_HEADER_PADDING_LEN + |
| FIRA_IE_HEADER_SESSION_ID_LEN + |
| FIRA_IE_HEADER_STS_INDEX_LEN] = { 0 }; |
| bool fira_header_seen = false; |
| int r; |
| u8 *p; |
| |
| p = skb->data; |
| if (!skb_pull(skb, IEEE802154_FC_LEN + IEEE802154_SHORT_ADDR_LEN + |
| IEEE802154_SCF_LEN) || |
| get_unaligned_le16(p) != fc) |
| return false; |
| |
| if (fira_sts_prepare_decrypt(session, slot, skb)) |
| return false; |
| |
| for (r = mcps802154_ie_get(skb, ie_get); r == 0 && !ie_get->in_payload; |
| r = mcps802154_ie_get(skb, ie_get)) { |
| p = skb->data; |
| ie_get->mlme_len = 0; |
| |
| if (ie_get->id == IEEE802154_IE_HEADER_VENDOR_ID && |
| ie_get->len >= FIRA_IE_VENDOR_OUI_LEN) { |
| u32 vendor; |
| |
| vendor = get_unaligned_le24(p); |
| p += FIRA_IE_VENDOR_OUI_LEN; |
| if (vendor != FIRA_IE_VENDOR_OUI) |
| goto next; |
| if (fira_header_seen) |
| goto hie_error; |
| if (ie_get->len != FIRA_IE_HEADER_LEN) |
| goto hie_error; |
| |
| memcpy(ciphered_hie, skb->data + FIRA_IE_VENDOR_OUI_LEN, |
| sizeof(ciphered_hie)); |
| if (fira_sts_decrypt_hie( |
| session, slot, skb, FIRA_IE_VENDOR_OUI_LEN, |
| ie_get->len - FIRA_IE_VENDOR_OUI_LEN)) |
| goto hie_error; |
| p += FIRA_IE_HEADER_PADDING_LEN; |
| *session_id = get_unaligned_le32(p); |
| p += FIRA_IE_HEADER_SESSION_ID_LEN; |
| *phy_sts_index = get_unaligned_le32(p); |
| p += FIRA_IE_HEADER_STS_INDEX_LEN; |
| fira_header_seen = true; |
| memcpy(skb->data + FIRA_IE_VENDOR_OUI_LEN, ciphered_hie, |
| ie_get->len - FIRA_IE_VENDOR_OUI_LEN); |
| memzero_explicit(ciphered_hie, sizeof(ciphered_hie)); |
| } |
| next: |
| skb_pull(skb, ie_get->len); |
| } |
| |
| return r >= 0 && fira_header_seen; |
| |
| hie_error: |
| skb_pull(skb, ie_get->len); |
| return false; |
| } |
| |
| static bool fira_frame_control_read(struct fira_local *local, u8 *p, |
| unsigned int ie_len, unsigned int *n_slots, |
| bool *stop, int *block_stride_len) |
| { |
| const struct fira_session *session = local->current_session; |
| struct fira_slot *slot, last; |
| int n_mngt, i; |
| u16 msg_ids = 0; |
| bool stop_found = false; |
| const struct fira_measurement_sequence_step *step = |
| fira_session_get_meas_seq_step(session); |
| |
| n_mngt = *p++; |
| if (ie_len < FIRA_IE_PAYLOAD_CONTROL_LEN(n_mngt)) |
| return false; |
| p++; |
| |
| *block_stride_len = *p++; |
| |
| slot = local->slots; |
| last = *slot++; |
| for (i = 0; i < n_mngt; i++) { |
| u32 mngt; |
| bool initiator; |
| int slot_index; |
| __le16 short_addr; |
| enum fira_message_id message_id; |
| bool stop_ranging; |
| bool is_rframe; |
| |
| mngt = get_unaligned_le32(p); |
| p += sizeof(u32); |
| |
| initiator = !!(mngt & FIRA_MNGT_RANGING_ROLE); |
| slot_index = FIELD_GET(FIRA_MNGT_SLOT_INDEX, mngt); |
| short_addr = FIELD_GET(FIRA_MNGT_SHORT_ADDR, mngt); |
| message_id = FIELD_GET(FIRA_MNGT_MESSAGE_ID, mngt); |
| stop_ranging = !!(mngt & FIRA_MNGT_STOP); |
| |
| is_rframe = message_id <= FIRA_MESSAGE_ID_RFRAME_MAX; |
| if (stop_ranging) { |
| if (short_addr == local->src_short_addr) { |
| stop_found = true; |
| } |
| continue; |
| } |
| |
| if (slot_index <= last.index || |
| slot_index >= session->params.round_duration_slots) |
| return false; |
| if (initiator && short_addr == local->src_short_addr) |
| return false; |
| |
| last.index = slot_index; |
| if (message_id <= FIRA_MESSAGE_ID_MAX && |
| (initiator || short_addr == local->src_short_addr)) { |
| u16 msg_id = 1 << message_id; |
| if (message_id == FIRA_MESSAGE_ID_CONTROL_UPDATE && |
| !initiator) |
| msg_id <<= 1; |
| if (msg_id < msg_ids || msg_id & msg_ids) |
| return false; |
| msg_ids |= msg_id; |
| if (slot == local->slots + FIRA_CONTROLEE_FRAMES_MAX) |
| return false; |
| last.controller_tx = initiator; |
| last.ranging_index = 0; |
| last.message_id = message_id; |
| if (!initiator) { |
| last.tx_ant_set = |
| is_rframe ? step->tx_ant_set_ranging : |
| step->tx_ant_set_nonranging; |
| } else { |
| last.rx_ant_set = fira_session_get_rx_ant_set( |
| session, message_id); |
| } |
| *slot++ = last; |
| } |
| } |
| *stop = stop_found; |
| *n_slots = slot - local->slots; |
| |
| return true; |
| } |
| |
| bool fira_frame_control_payload_check(struct fira_local *local, |
| struct sk_buff *skb, |
| struct mcps802154_ie_get_context *ie_get, |
| unsigned int *n_slots, bool *stop_ranging, |
| int *block_stride_len) |
| { |
| bool fira_payload_seen = false; |
| int r; |
| u8 *p; |
| |
| for (r = mcps802154_ie_get(skb, ie_get); r == 0; |
| r = mcps802154_ie_get(skb, ie_get)) { |
| p = skb->data; |
| skb_pull(skb, ie_get->len); |
| |
| if (ie_get->id == IEEE802154_IE_PAYLOAD_VENDOR_GID && |
| ie_get->len >= FIRA_IE_VENDOR_OUI_LEN) { |
| u32 vendor; |
| int message_id; |
| |
| vendor = get_unaligned_le24(p); |
| p += FIRA_IE_VENDOR_OUI_LEN; |
| if (vendor != FIRA_IE_VENDOR_OUI) |
| continue; |
| |
| if (ie_get->len < FIRA_IE_PAYLOAD_CONTROL_LEN(0)) |
| return false; |
| message_id = (*p++) & 0xf; |
| if (message_id != FIRA_MESSAGE_ID_CONTROL) |
| return false; |
| |
| if (fira_payload_seen) |
| return false; |
| |
| if (!fira_frame_control_read(local, p, ie_get->len, |
| n_slots, stop_ranging, |
| block_stride_len)) |
| return false; |
| |
| fira_payload_seen = true; |
| } |
| } |
| |
| return r >= 0 && fira_payload_seen; |
| } |
| |
| static bool |
| fira_frame_measurement_report_fill_ranging_info(struct fira_local *local, |
| const struct fira_slot *slot, |
| u8 *p, unsigned int ie_len) |
| { |
| struct fira_session *session = local->current_session; |
| struct fira_ranging_info *ranging_info = |
| &local->ranging_info[slot->ranging_index]; |
| u8 control; |
| bool hopping_mode, round_index_present, reply_time_present; |
| unsigned int n_reply_time; |
| u32 remote_round_trip_rctu, remote_reply_rctu = 0; |
| u64 rx_initiation_rctu, tx_response_rctu, rx_final_rctu; |
| u32 local_round_trip_rctu, local_reply_rctu; |
| int tof_rctu, i; |
| bool double_sided = session->params.ranging_round_usage == |
| FIRA_RANGING_ROUND_USAGE_DSTWR; |
| control = *p++; |
| hopping_mode = FIELD_GET(FIRA_MEASUREMENT_REPORT_CONTROL_HOPPING_MODE, |
| control); |
| round_index_present = FIELD_GET( |
| FIRA_MEASUREMENT_REPORT_CONTROL_ROUND_INDEX_PRESENT, control); |
| n_reply_time = FIELD_GET(FIRA_MEASUREMENT_REPORT_CONTROL_N_REPLY_TIME, |
| control); |
| |
| if (!double_sided) { |
| control = *p++; |
| /* Is reply time present? Not supported. */ |
| reply_time_present = FIELD_GET( |
| FIRA_MEASUREMENT_REPORT_CONTROL_REPLY_TIME_PRESENT, |
| control); |
| if (reply_time_present) { |
| trace_fira_nondeferred_not_supported(session); |
| return false; |
| } |
| } |
| |
| if (ie_len < (double_sided ? |
| FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE1_LEN( |
| round_index_present, n_reply_time) : |
| FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE2_LEN( |
| round_index_present, reply_time_present, |
| n_reply_time))) |
| return false; |
| |
| if (round_index_present) { |
| int next_round_index; |
| |
| next_round_index = get_unaligned_le16(p); |
| p += sizeof(u16); |
| |
| session->controlee.next_round_index_valid = true; |
| session->next_round_index = next_round_index; |
| } |
| |
| if (double_sided) { |
| /* Remote_round_trip = first_round_trip + first_reply - my_reply. */ |
| remote_round_trip_rctu = get_unaligned_le32(p); |
| p += sizeof(u32); |
| /* Add first_reply. */ |
| remote_round_trip_rctu += get_unaligned_le32(p + sizeof(u16)); |
| } |
| |
| for (i = 0; i < n_reply_time; i++) { |
| __le16 short_addr = get_unaligned_le16(p); |
| p += sizeof(u16); |
| if (local->src_short_addr == short_addr) { |
| remote_reply_rctu = get_unaligned_le32(p); |
| break; |
| } |
| p += sizeof(u32); |
| } |
| /* Reply time not found. */ |
| if (i == n_reply_time) |
| return false; |
| if (double_sided) |
| /* Substract my_reply. */ |
| remote_round_trip_rctu -= remote_reply_rctu; |
| else |
| remote_round_trip_rctu = remote_reply_rctu; |
| |
| rx_initiation_rctu = |
| ranging_info |
| ->timestamps_rctu[FIRA_MESSAGE_ID_RANGING_INITIATION]; |
| tx_response_rctu = |
| ranging_info->timestamps_rctu[FIRA_MESSAGE_ID_RANGING_RESPONSE]; |
| local_reply_rctu = mcps802154_difference_timestamp_rctu( |
| local->llhw, tx_response_rctu, rx_initiation_rctu); |
| |
| if (double_sided) { |
| rx_final_rctu = |
| ranging_info |
| ->timestamps_rctu[FIRA_MESSAGE_ID_RANGING_FINAL]; |
| local_round_trip_rctu = mcps802154_difference_timestamp_rctu( |
| local->llhw, rx_final_rctu, tx_response_rctu); |
| tof_rctu = div64_s64( |
| (s64)remote_round_trip_rctu * local_round_trip_rctu - |
| (s64)remote_reply_rctu * local_reply_rctu, |
| (s64)remote_round_trip_rctu + local_round_trip_rctu + |
| remote_reply_rctu + local_reply_rctu); |
| } else { |
| static const s32 Q26 = 1 << 26; |
| s32 adjusted_reply_rctu = |
| (ranging_info->clock_offset_present) ? |
| (((u64)local_reply_rctu * Q26) / |
| (Q26 - ranging_info->clock_offset_q26)) : |
| local_reply_rctu; |
| tof_rctu = |
| ((s32)remote_round_trip_rctu - adjusted_reply_rctu) / 2; |
| } |
| ranging_info->tof_rctu = (!slot->controller_tx) ? -tof_rctu : tof_rctu; |
| ranging_info->tof_present = true; |
| session->controlee.hopping_mode = hopping_mode; |
| return true; |
| } |
| |
| bool fira_frame_measurement_report_payload_check( |
| struct fira_local *local, const struct fira_slot *slot, |
| struct sk_buff *skb, struct mcps802154_ie_get_context *ie_get) |
| { |
| const struct fira_session *session = local->current_session; |
| const struct fira_session_params *params = &session->params; |
| bool fira_payload_seen = false; |
| unsigned int minimum_payload_len; |
| int r; |
| u8 *p; |
| |
| if (params->ranging_round_usage == FIRA_RANGING_ROUND_USAGE_DSTWR) |
| minimum_payload_len = |
| FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE1_LEN(false, 0); |
| else |
| minimum_payload_len = |
| FIRA_IE_PAYLOAD_MEASUREMENT_REPORT_TYPE2_LEN(false, |
| false, 0); |
| |
| for (r = mcps802154_ie_get(skb, ie_get); r == 0; |
| r = mcps802154_ie_get(skb, ie_get)) { |
| p = skb->data; |
| skb_pull(skb, ie_get->len); |
| |
| if (ie_get->id == IEEE802154_IE_PAYLOAD_VENDOR_GID && |
| ie_get->len >= FIRA_IE_VENDOR_OUI_LEN) { |
| u32 vendor; |
| int message_id; |
| |
| vendor = get_unaligned_le24(p); |
| p += FIRA_IE_VENDOR_OUI_LEN; |
| if (vendor != FIRA_IE_VENDOR_OUI) |
| continue; |
| |
| if (ie_get->len < minimum_payload_len) |
| return false; |
| message_id = (*p++) & 0xf; |
| if (message_id != FIRA_MESSAGE_ID_MEASUREMENT_REPORT) |
| return false; |
| |
| if (fira_payload_seen) |
| return false; |
| |
| if (!fira_frame_measurement_report_fill_ranging_info( |
| local, slot, p, ie_get->len)) |
| return false; |
| |
| fira_payload_seen = true; |
| } |
| } |
| |
| return r >= 0 && fira_payload_seen; |
| } |
| |
| static bool |
| fira_frame_result_report_fill_ranging_info(struct fira_local *local, |
| const struct fira_slot *slot, u8 *p, |
| unsigned int ie_len) |
| { |
| struct fira_ranging_info *ranging_info = |
| &local->ranging_info[slot->ranging_index]; |
| u8 control; |
| bool tof_present, neg_tof_present, aoa_azimuth_present, aoa_elevation_present, |
| aoa_fom_present; |
| |
| control = *p++; |
| tof_present = !!(control & FIRA_RESULT_REPORT_CONTROL_TOF_PRESENT); |
| aoa_azimuth_present = |
| !!(control & FIRA_RESULT_REPORT_CONTROL_AOA_AZIMUTH_PRESENT); |
| aoa_elevation_present = |
| !!(control & FIRA_RESULT_REPORT_CONTROL_AOA_ELEVATION_PRESENT); |
| aoa_fom_present = |
| !!(control & FIRA_RESULT_REPORT_CONTROL_AOA_FOM_PRESENT); |
| neg_tof_present = !!(control & FIRA_RESULT_REPORT_CONTROL_NEG_TOF_PRESENT); |
| if (ie_len < FIRA_IE_PAYLOAD_RESULT_REPORT_LEN( |
| tof_present, aoa_azimuth_present, |
| aoa_elevation_present, aoa_fom_present, neg_tof_present)) |
| return false; |
| |
| if (tof_present) { |
| ranging_info->tof_present = true; |
| ranging_info->tof_rctu = get_unaligned_le32(p); |
| p += sizeof(u32); |
| } |
| if (aoa_azimuth_present) { |
| ranging_info->remote_aoa_azimuth_present = true; |
| ranging_info->remote_aoa_azimuth_2pi = get_unaligned_le16(p); |
| p += sizeof(s16); |
| } |
| if (aoa_elevation_present) { |
| ranging_info->remote_aoa_elevation_present = true; |
| ranging_info->remote_aoa_elevation_pi = get_unaligned_le16(p); |
| p += sizeof(s16); |
| } |
| if (aoa_fom_present) { |
| ranging_info->remote_aoa_fom_present = true; |
| if (aoa_azimuth_present) |
| ranging_info->remote_aoa_azimuth_fom = *p++; |
| if (aoa_elevation_present) |
| ranging_info->remote_aoa_elevation_fom = *p++; |
| } |
| if (neg_tof_present) { |
| /* When negative ToF is present at end of frame, |
| * ToF read ahead MUST be 0, so, is safe to overwrite */ |
| ranging_info->tof_rctu = -get_unaligned_le32(p); |
| p += sizeof(u32); |
| } |
| |
| return true; |
| } |
| |
| bool fira_frame_result_report_payload_check( |
| struct fira_local *local, const struct fira_slot *slot, |
| struct sk_buff *skb, struct mcps802154_ie_get_context *ie_get) |
| { |
| bool fira_payload_seen = false; |
| int r; |
| u8 *p; |
| |
| for (r = mcps802154_ie_get(skb, ie_get); r == 0; |
| r = mcps802154_ie_get(skb, ie_get)) { |
| p = skb->data; |
| skb_pull(skb, ie_get->len); |
| |
| if (ie_get->id == IEEE802154_IE_PAYLOAD_VENDOR_GID && |
| ie_get->len >= FIRA_IE_VENDOR_OUI_LEN) { |
| u32 vendor; |
| int message_id; |
| |
| vendor = get_unaligned_le24(p); |
| p += FIRA_IE_VENDOR_OUI_LEN; |
| if (vendor != FIRA_IE_VENDOR_OUI) |
| continue; |
| |
| if (ie_get->len < FIRA_IE_PAYLOAD_RESULT_REPORT_LEN( |
| false, false, false, false, false)) |
| return false; |
| message_id = (*p++) & 0xf; |
| if (message_id != FIRA_MESSAGE_ID_RESULT_REPORT) |
| return false; |
| |
| if (fira_payload_seen) |
| return false; |
| |
| if (!fira_frame_result_report_fill_ranging_info( |
| local, slot, p, ie_get->len)) |
| return false; |
| |
| fira_payload_seen = true; |
| } |
| } |
| |
| return r >= 0 && fira_payload_seen; |
| } |
| |
| bool fira_frame_rframe_payload_check(struct fira_local *local, |
| const struct fira_slot *slot, |
| struct sk_buff *skb, |
| struct mcps802154_ie_get_context *ie_get) |
| { |
| const struct fira_session *session = local->current_session; |
| const struct fira_session_params *params = &session->params; |
| struct fira_ranging_info *ranging_info = |
| &local->ranging_info[slot->ranging_index]; |
| bool rframe_payload_seen = false; |
| int r; |
| u8 *p; |
| |
| for (r = mcps802154_ie_get(skb, ie_get); r == 0; |
| r = mcps802154_ie_get(skb, ie_get)) { |
| p = skb->data; |
| skb_pull(skb, ie_get->len); |
| |
| if (ie_get->id == IEEE802154_IE_PAYLOAD_VENDOR_GID && |
| ie_get->len >= FIRA_IE_VENDOR_OUI_LEN && |
| ie_get->len <= FIRA_IE_VENDOR_OUI_LEN + FIRA_DATA_PAYLOAD_SIZE_MAX) { |
| u32 vendor; |
| unsigned int data_len; |
| |
| vendor = get_unaligned_le24(p); |
| p += FIRA_IE_VENDOR_OUI_LEN; |
| if (vendor != params->data_vendor_oui) |
| continue; |
| |
| if (ie_get->len < FIRA_IE_VENDOR_OUI_LEN + 1) |
| continue; |
| |
| if (rframe_payload_seen) |
| return false; |
| |
| data_len = ie_get->len - FIRA_IE_VENDOR_OUI_LEN; |
| memcpy(&ranging_info->data_payload, p, data_len); |
| ranging_info->data_payload_len = data_len; |
| |
| rframe_payload_seen = true; |
| } |
| } |
| |
| return r >= 0; |
| } |
| |
| struct fira_session *fira_rx_frame_control_header_check( |
| struct fira_local *local, const struct fira_slot *slot, |
| struct sk_buff *skb, struct mcps802154_ie_get_context *ie_get, |
| u32 *phy_sts_index) |
| { |
| const struct fira_session *session = local->current_session; |
| struct fira_session *session_found = NULL; |
| u32 session_id; |
| |
| if (!fira_frame_header_check(local, slot, skb, ie_get, phy_sts_index, |
| &session_id)) |
| return NULL; |
| if (session->id == session_id) { |
| session_found = local->current_session; |
| } else if (session->controlee.synchronised) { |
| return NULL; |
| } else { |
| session_found = |
| fira_get_session_by_session_id(local, session_id); |
| if (!session_found || |
| session_found->params.device_type != |
| FIRA_DEVICE_TYPE_CONTROLEE || |
| !fira_session_is_active(session_found)) |
| return NULL; |
| /* |
| * FIXME: The previous session will not sent a ranging round |
| * report failure. |
| * |
| * The most simple is probably to remove a round ranging? |
| * or keep somewhere, previous value. |
| * or choice number 3. |
| * ``` |
| * int remove_blocks = session->block_stride_len + 1; |
| * |
| * session->block_start_dtu -= remove_blocks * |
| * params->block_duration_dtu; |
| * session->block_index -= remove_blocks; |
| * ``` |
| */ |
| } |
| /* Update current and allow content of session to be updated. */ |
| local->current_session = session_found; |
| return session_found; |
| } |
| |
| int fira_frame_header_check_decrypt(struct fira_local *local, |
| const struct fira_slot *slot, |
| struct sk_buff *skb, |
| struct mcps802154_ie_get_context *ie_get) |
| { |
| struct fira_session *session = local->current_session; |
| int header_len; |
| __le16 src_short_addr; |
| u32 phy_sts_index; |
| u32 session_id; |
| u8 *header; |
| |
| header = skb->data; |
| |
| if (!fira_frame_header_check(local, slot, skb, ie_get, &phy_sts_index, |
| &session_id)) |
| return -EBADMSG; |
| if (session_id != session->id) |
| return -EBADMSG; |
| |
| if (phy_sts_index != fira_sts_get_phy_sts_index(session, slot)) |
| return -EBADMSG; |
| |
| header_len = skb->data - header; |
| src_short_addr = slot->controller_tx ? local->dst_short_addr : |
| slot->controlee->short_addr; |
| return fira_sts_decrypt_frame(session, slot, skb, header_len, src_short_addr); |
| } |