mac: declare invalid any FOM value superior than 100android-u-qpr3-beta-1_r0.7android-u-qpr3-beta-1_r0.5android-u-qpr3-beta-1_r0.4android-u-qpr3-beta-1_r0.3android-u-qpr3-beta-1_r0.2android-u-qpr3-beta-1_r0.1android-u-qpr2-beta-3_r0.7android-u-qpr2-beta-3_r0.6android-u-qpr2-beta-3_r0.5android-u-qpr2-beta-3_r0.4android-u-qpr2-beta-3_r0.3android-u-qpr2-beta-3_r0.2android-u-qpr2-beta-3.1_r0.7android-u-qpr2-beta-3.1_r0.5android-u-qpr2-beta-3.1_r0.4android-u-qpr2-beta-3.1_r0.3android-u-qpr2-beta-3.1_r0.2android-u-qpr2-beta-3.1_r0.1android-u-qpr2-beta-2_r0.7android-u-qpr2-beta-2_r0.5android-u-qpr2-beta-2_r0.4android-u-qpr2-beta-2_r0.3android-u-qpr2-beta-2_r0.2android-u-qpr2-beta-2_r0.1android-u-qpr2-beta-1_r0.6android-u-qpr2-beta-1_r0.5android-u-qpr2-beta-1_r0.4android-u-qpr2-beta-1_r0.3android-u-qpr2-beta-1_r0.2android-u-qpr2-beta-1_r0.1android-u-qpr1-beta-2_r0.7android-u-qpr1-beta-2_r0.5android-u-qpr1-beta-2_r0.4android-u-qpr1-beta-2_r0.3android-u-qpr1-beta-2_r0.2android-u-qpr1-beta-2_r0.1android-u-qpr1-beta-2.2_r0.6android-u-qpr1-beta-2.2_r0.5android-u-qpr1-beta-2.2_r0.4android-u-qpr1-beta-2.2_r0.3android-u-qpr1-beta-2.2_r0.2android-u-qpr1-beta-2.2_r0.1android-u-qpr1-beta-2.1_r0.7android-u-qpr1-beta-2.1_r0.5android-u-qpr1-beta-2.1_r0.4android-u-qpr1-beta-2.1_r0.3android-u-qpr1-beta-2.1_r0.2android-u-qpr1-beta-2.1_r0.1android-u-qpr1-beta-1_r0.7android-u-qpr1-beta-1_r0.5android-u-qpr1-beta-1_r0.4android-u-qpr1-beta-1_r0.3android-u-qpr1-beta-1_r0.2android-u-qpr1-beta-1_r0.1android-15-dp-1_r0.7android-15-dp-1_r0.5android-15-dp-1_r0.4android-15-dp-1_r0.3android-15-dp-1_r0.2android-15-dp-1_r0.1android-14.0.0_r0.76android-14.0.0_r0.75android-14.0.0_r0.74android-14.0.0_r0.73android-14.0.0_r0.72android-14.0.0_r0.71android-14.0.0_r0.66android-14.0.0_r0.65android-14.0.0_r0.64android-14.0.0_r0.63android-14.0.0_r0.62android-14.0.0_r0.61android-14.0.0_r0.56android-14.0.0_r0.55android-14.0.0_r0.54android-14.0.0_r0.53android-14.0.0_r0.52android-14.0.0_r0.51android-14.0.0_r0.44android-14.0.0_r0.43android-14.0.0_r0.42android-14.0.0_r0.41android-14.0.0_r0.40android-14.0.0_r0.39android-14.0.0_r0.37android-14.0.0_r0.36android-14.0.0_r0.35android-14.0.0_r0.34android-14.0.0_r0.33android-14.0.0_r0.31android-14.0.0_r0.25android-14.0.0_r0.23android-14.0.0_r0.22android-14.0.0_r0.21android-14.0.0_r0.19android-gs-tangorpro-5.10-u-qpr1-beta2android-gs-tangorpro-5.10-android15-dpandroid-gs-tangorpro-5.10-android14-qpr3-betaandroid-gs-tangorpro-5.10-android14-qpr2-betaandroid-gs-tangorpro-5.10-android14-qpr2android-gs-tangorpro-5.10-android14-qpr1-betaandroid-gs-tangorpro-5.10-android14-qpr1android-gs-raviole-5.10-u-qpr1-beta2android-gs-raviole-5.10-android15-dpandroid-gs-raviole-5.10-android14-qpr3-betaandroid-gs-raviole-5.10-android14-qpr2-betaandroid-gs-raviole-5.10-android14-qpr2android-gs-raviole-5.10-android14-qpr1-betaandroid-gs-raviole-5.10-android14-qpr1android-gs-pantah-5.10-u-qpr1-beta2android-gs-pantah-5.10-android15-dpandroid-gs-pantah-5.10-android14-qpr3-betaandroid-gs-pantah-5.10-android14-qpr2-betaandroid-gs-pantah-5.10-android14-qpr2android-gs-pantah-5.10-android14-qpr1-betaandroid-gs-pantah-5.10-android14-qpr1android-gs-lynx-5.10-u-qpr1-beta2android-gs-lynx-5.10-android15-dpandroid-gs-lynx-5.10-android14-qpr3-betaandroid-gs-lynx-5.10-android14-qpr2-betaandroid-gs-lynx-5.10-android14-qpr2android-gs-lynx-5.10-android14-qpr1-betaandroid-gs-lynx-5.10-android14-qpr1android-gs-felix-5.10-u-qpr1-beta2android-gs-felix-5.10-android15-dpandroid-gs-felix-5.10-android14-qpr3-betaandroid-gs-felix-5.10-android14-qpr2-betaandroid-gs-felix-5.10-android14-qpr2android-gs-felix-5.10-android14-qpr1-betaandroid-gs-felix-5.10-android14-qpr1android-gs-bluejay-5.10-u-qpr1-beta2android-gs-bluejay-5.10-android15-dpandroid-gs-bluejay-5.10-android14-qpr3-betaandroid-gs-bluejay-5.10-android14-qpr2-betaandroid-gs-bluejay-5.10-android14-qpr2android-gs-bluejay-5.10-android14-qpr1-betaandroid-gs-bluejay-5.10-android14-qpr1

Bug: 242879002
Change-Id: I4974cb866d844e6a5d86638bf284ac16f2662b65
Signed-off-by: Clément Viel <clement.viel@qorvo.com>

1 files 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 */