Temporary workaround for relaying probe_req frame at wmediumd am: c920afba2c am: 4f82d8fd22

Original change: https://android-review.googlesource.com/c/platform/external/wmediumd/+/1921697

Change-Id: Iffde4b03e69d6b2721168eb4fb5d735bb736e2dc

4 files changed, 74 insertions, 0 deletions

diff --git a/wmediumd/ieee80211.h b/wmediumd/ieee80211.h
index f3deb4b..ef122f3 100644
--- a/wmediumd/ieee80211.h
+++ b/wmediumd/ieee80211.h
@@ -40,6 +40,7 @@
 #define FTYPE_DATA		0x08
 
 #define STYPE_QOS_DATA		0x80
+#define STYPE_PROBE_REQ		0x40
 
 #define QOS_CTL_TAG1D_MASK	0x07
 
@@ -50,6 +51,10 @@ enum ieee80211_ac_number {
 	IEEE80211_AC_BK		= 3,
 };
 
+enum ieee80211_eid {
+	WLAN_EID_VENDOR_SPECIFIC = 221,
+};
+
 static const enum ieee80211_ac_number ieee802_1d_to_ac[8] = {
 	IEEE80211_AC_BE,
 	IEEE80211_AC_BK,
@@ -71,4 +76,10 @@ struct ieee80211_hdr {
 	unsigned char addr4[ETH_ALEN];
 };
 
+struct ieee80211_element {
+	unsigned char id;
+	unsigned char datalen;
+	unsigned char data[];
+} __attribute__((packed));
+
 #endif /* IEEE80211_H_ */
diff --git a/wmediumd/per.c b/wmediumd/per.c
index 1f89a77..8be3548 100644
--- a/wmediumd/per.c
+++ b/wmediumd/per.c
@@ -271,6 +271,11 @@ int read_per_file(struct wmediumd *ctx, const char *file_name)
 	return EXIT_SUCCESS;
 }
 
+int get_max_index(void)
+{
+	return rate_len - 1;
+}
+
 int index_to_rate(size_t index, u32 freq)
 {
 	if (freq > 5000)
diff --git a/wmediumd/wmediumd.c b/wmediumd/wmediumd.c
index 15535cd..9c1eeeb 100644
--- a/wmediumd/wmediumd.c
+++ b/wmediumd/wmediumd.c
@@ -134,6 +134,54 @@ static inline bool frame_is_data_qos(struct frame *frame)
 		(FTYPE_DATA | STYPE_QOS_DATA);
 }
 
+static inline bool frame_is_probe_req(struct frame *frame)
+{
+	struct ieee80211_hdr *hdr = (void *)frame->data;
+
+	return (hdr->frame_control[0] & (FCTL_FTYPE | STYPE_PROBE_REQ)) ==
+		(FTYPE_MGMT | STYPE_PROBE_REQ);
+}
+
+
+static inline bool frame_has_zero_rates(const struct frame *frame)
+{
+	for (int i = 0; i < frame->tx_rates_count; i++) {
+		if (frame->tx_rates[i].idx < 0)
+			break;
+
+		if (frame->tx_rates[i].count > 0) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+static inline void fill_tx_rates(struct frame *frame)
+{
+	if (frame->tx_rates_count <= 0) {
+		return;
+	}
+
+	int max_index = get_max_index();
+
+	/* Starting from OFDM rate (See per.c#rateset) */
+	const int basic_rate_start = 4; /* 6 mbps */
+
+	int i;
+	int rate_count = min(max_index - basic_rate_start + 1, frame->tx_rates_count);
+
+	for (i = 0; i < rate_count; i++) {
+		frame->tx_rates[i].idx = basic_rate_start + rate_count - i - 1;
+		frame->tx_rates[i].count = 4;
+	}
+
+	for (; i < frame->tx_rates_count; i++) {
+		frame->tx_rates[i].idx = -1;
+		frame->tx_rates[i].count = 0;
+	}
+}
+
 static inline u8 *frame_get_qos_ctl(struct frame *frame)
 {
 	struct ieee80211_hdr *hdr = (void *)frame->data;
@@ -417,6 +465,15 @@ static void queue_frame(struct wmediumd *ctx, struct station *station,
 
 	noack = is_multicast_ether_addr(dest);
 
+	/*
+	 * TODO(b/211353765) Remove this when fundamenal solution is applied
+	 *
+	 *   Temporary workaround for relaying probe_req frame.
+	 */
+	if (frame_is_probe_req(frame) && frame_has_zero_rates(frame)) {
+		fill_tx_rates(frame);
+	}
+
 	double choice = drand48();
 
 	for (i = 0; i < frame->tx_rates_count && !is_acked; i++) {
diff --git a/wmediumd/wmediumd.h b/wmediumd/wmediumd.h
index dca4f02..3c9818d 100644
--- a/wmediumd/wmediumd.h
+++ b/wmediumd/wmediumd.h
@@ -280,5 +280,6 @@ int read_per_file(struct wmediumd *ctx, const char *file_name);
 int w_logf(struct wmediumd *ctx, u8 level, const char *format, ...);
 int w_flogf(struct wmediumd *ctx, u8 level, FILE *stream, const char *format, ...);
 int index_to_rate(size_t index, u32 freq);
+int get_max_index(void);
 
 #endif /* WMEDIUMD_H_ */