From a2a6c47584ff7f57ffc9683ece1d803b74ecf4a4 Mon Sep 17 00:00:00 2001 From: MilissaImeneBOUDIAF Date: Thu, 17 Aug 2023 17:21:29 +0200 Subject: mac: declare invalid any FOM value superior than 100 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bug: 242879002 Change-Id: I4974cb866d844e6a5d86638bf284ac16f2662b65 Signed-off-by: Clément Viel --- mac/fira_frame.c | 51 ++++++++++++++++++++++++++++++++++----------------- 1 file changed, 34 insertions(+), 17 deletions(-) diff --git a/mac/fira_frame.c b/mac/fira_frame.c index 7bbf9d0..ffb27f8 100644 --- a/mac/fira_frame.c +++ b/mac/fira_frame.c @@ -425,6 +425,38 @@ hie_error: return false; } +static u8 *fira_frame_twr_get_aoa_info(u8 *p, + struct fira_ranging_info *ranging_info, + bool aoa_azimuth_present, + bool aoa_elevation_present, + bool aoa_fom_present) +{ + 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) { + u8 aoa_azimuth_fom = 0, aoa_elevation_fom = 0; + + ranging_info->remote_aoa_fom_present = true; + if (aoa_azimuth_present) + aoa_azimuth_fom = *p++; + if (aoa_elevation_present) + aoa_elevation_fom = *p++; + ranging_info->remote_aoa_azimuth_fom = + (aoa_azimuth_fom > 100) ? 0 : aoa_azimuth_fom; + ranging_info->remote_aoa_elevation_fom = + (aoa_elevation_fom > 100) ? 0 : aoa_elevation_fom; + } + return p; +} + 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) @@ -756,23 +788,8 @@ fira_frame_result_report_fill_ranging_info(struct fira_local *local, 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++; - } + p = fira_frame_twr_get_aoa_info(p, ranging_info, aoa_azimuth_present, + aoa_elevation_present, aoa_fom_present); if (neg_tof_present) { /* When negative ToF is present at end of frame, * ToF read ahead MUST be 0, so, is safe to overwrite */ -- cgit v1.2.3