From 6b8b07741af5958c2d1b96cdea9a749d4aae6769 Mon Sep 17 00:00:00 2001
From: Janusz Dziedzic <janusz.dziedzic@tieto.com>
Date: Thu, 8 Jan 2015 12:48:36 +0100
Subject: [PATCH] ibss/mesh: Enable HT40 if supported

Setup HT40+/HT40- if supported by driver.

Signed-off-by: Janusz Dziedzic <janusz.dziedzic@tieto.com>
---
 wpa_supplicant/wpa_supplicant.c | 103 +++++++++++++++++++++++++++++++-
 1 file changed, 102 insertions(+), 1 deletion(-)

diff --git a/wpa_supplicant/wpa_supplicant.c b/wpa_supplicant/wpa_supplicant.c
index 6cad068b2..8a77909b9 100644
--- a/wpa_supplicant/wpa_supplicant.c
+++ b/wpa_supplicant/wpa_supplicant.c
@@ -33,6 +33,7 @@
 #include "rsn_supp/pmksa_cache.h"
 #include "common/wpa_ctrl.h"
 #include "common/ieee802_11_defs.h"
+#include "common/hw_features_common.h"
 #include "p2p/p2p.h"
 #include "blacklist.h"
 #include "wpas_glue.h"
@@ -1655,8 +1656,12 @@ static void ibss_mesh_setup_freq(struct wpa_supplicant *wpa_s,
 {
 	enum hostapd_hw_mode hw_mode;
 	struct hostapd_hw_modes *mode = NULL;
+	int ht40plus[] = { 36, 44, 52, 60, 100, 108, 116, 124, 132, 149, 157,
+			   184, 192 };
+	struct hostapd_channel_data *pri_chan = NULL, *sec_chan = NULL;
 	u8 channel;
-	int i;
+	int i, chan_idx, ht40 = -1, res;
+	unsigned int j;
 
 	freq->freq = ssid->frequency;
 
@@ -1677,6 +1682,102 @@ static void ibss_mesh_setup_freq(struct wpa_supplicant *wpa_s,
 		return;
 
 	freq->ht_enabled = ht_supported(mode);
+	if (!freq->ht_enabled)
+		return;
+
+	/* Setup higher BW only for 5 GHz */
+	if (mode->mode != HOSTAPD_MODE_IEEE80211A)
+		return;
+
+	for (chan_idx = 0; chan_idx < mode->num_channels; chan_idx++) {
+		pri_chan = &mode->channels[chan_idx];
+		if (pri_chan->chan == channel)
+			break;
+		pri_chan = NULL;
+	}
+	if (!pri_chan)
+		return;
+
+	/* Check primary channel flags */
+	if (pri_chan->flag & (HOSTAPD_CHAN_DISABLED | HOSTAPD_CHAN_NO_IR))
+		return;
+
+	/* Check/setup HT40+/HT40- */
+	for (j = 0; j < ARRAY_SIZE(ht40plus); j++) {
+		if (ht40plus[j] == channel) {
+			ht40 = 1;
+			break;
+		}
+	}
+
+	/* Find secondary channel */
+	for (i = 0; i < mode->num_channels; i++) {
+		sec_chan = &mode->channels[i];
+		if (sec_chan->chan == channel + ht40 * 4)
+			break;
+		sec_chan = NULL;
+	}
+	if (!sec_chan)
+		return;
+
+	/* Check secondary channel flags */
+	if (sec_chan->flag & (HOSTAPD_CHAN_DISABLED | HOSTAPD_CHAN_NO_IR))
+		return;
+
+	freq->channel = pri_chan->chan;
+
+	switch (ht40) {
+	case -1:
+		if (!(pri_chan->flag & HOSTAPD_CHAN_HT40MINUS))
+			return;
+		freq->sec_channel_offset = -1;
+		break;
+	case 1:
+		if (!(pri_chan->flag & HOSTAPD_CHAN_HT40PLUS))
+			return;
+		freq->sec_channel_offset = 1;
+		break;
+	default:
+		break;
+	}
+
+	if (freq->sec_channel_offset) {
+		struct wpa_scan_results *scan_res;
+
+		scan_res = wpa_supplicant_get_scan_results(wpa_s, NULL, 0);
+		if (scan_res == NULL) {
+			/* Back to HT20 */
+			freq->sec_channel_offset = 0;
+			return;
+		}
+
+		res = check_40mhz_5g(mode, scan_res, pri_chan->chan,
+				     sec_chan->chan);
+		switch (res) {
+		case 0:
+			/* Back to HT20 */
+			freq->sec_channel_offset = 0;
+			break;
+		case 1:
+			/* Configuration allowed */
+			break;
+		case 2:
+			/* Switch pri/sec channels */
+			freq->freq = hw_get_freq(mode, sec_chan->chan);
+			freq->sec_channel_offset = -freq->sec_channel_offset;
+			freq->channel = sec_chan->chan;
+			break;
+		default:
+			freq->sec_channel_offset = 0;
+			break;
+		}
+
+		wpa_scan_results_free(scan_res);
+	}
+
+	wpa_printf(MSG_DEBUG,
+		   "IBSS/mesh: setup freq channel %d, sec_channel_offset %d",
+		   freq->channel, freq->sec_channel_offset);
 }