drivers/net/wireless/bcmdhd_1363/dhd_bandsteer.c - nest-learning-thermostat/6.0/linux-imx-4.9.88 - Git at Google

 /*
  * Band Steering logic
  *
  * Feature by which dualband capable PEERs will be
  * forced move on 5GHz interface
  *
  * Copyright (C) 1999-2017, Broadcom Corporation
  *
  *      Unless you and Broadcom execute a separate written software license
  * agreement governing use of this software, this software is licensed to you
  * under the terms of the GNU General Public License version 2 (the "GPL"),
  * available at http://www.broadcom.com/licenses/GPLv2.php, with the
  * following added to such license:
  *
  *      As a special exception, the copyright holders of this software give you
  * permission to link this software with independent modules, and to copy and
  * distribute the resulting executable under terms of your choice, provided that
  * you also meet, for each linked independent module, the terms and conditions of
  * the license of that module.  An independent module is a module which is not
  * derived from this software.  The special exception does not apply to any
  * modifications of the software.
  *
  *      Notwithstanding the above, under no circumstances may you combine this
  * software in any way with any other Broadcom software provided under a license
  * other than the GPL, without Broadcom's express prior written consent.
  *
  * <<Broadcom-WL-IPTag/Open:>>
  *
  * $ Copyright Cypress Semiconductor $
  *
  * $Id: dhd_bandsteer.c 664149 2017-03-07 13:29:02Z $
  */

 #include <typedefs.h>
 #include <linuxver.h>
 #include <osl.h>
 #include <linux/kernel.h>
 #include <linux/time.h>
 #include <bcmutils.h>
 #include <net/cfg80211.h>
 #include <net/rtnetlink.h>
 #include <wl_cfg80211.h>
 #include <wl_android.h>
 #include <wldev_common.h>
 #include <dhd_linux_wq.h>
 #include <dhd_cfg80211.h>
 #include <dhd_bandsteer.h>
 #include <dhd_dbg.h>

 /* defines */
 /* BANDSTEER STATE MACHINE STATES */
 #define DHD_BANDSTEER_START			0x0001
 #define DHD_BANDSTEER_WNM_FRAME_SEND		0x0002
 #define DHD_BANDSTEER_WNM_FRAME_RETRY		0x0004
 #define DHD_BANDSTEER_WNM_FRAME_SENT		0x0008
 #define DHD_BANDSTEER_TRIAL_DONE		0x0080

 #define DHD_BANDSTEER_ON_PROCESS_MASK (DHD_BANDSTEER_START | DHD_BANDSTEER_WNM_FRAME_SEND \
 	| DHD_BANDSTEER_WNM_FRAME_RETRY | DHD_BANDSTEER_TRIAL_DONE)

 #define DHD_BANDSTEER_WNM_FRAME_MAXRETRY 3
 #define DHD_BANDSTEER_WNM_FRAME_DELAY 1000
 #define MAX_NUM_OF_ASSOCLIST    64
 #define DHD_BANDSTEER_MAXIFACES 2

 #define CHANNEL_IS_5G(channel)  (((channel >= 36) && (channel <= 165)) ? \
 true : false)

 /* ********************** Function declaration *********************** */
 static s32
 dhd_bandsteer_addmac_to_monitorlist(dhd_bandsteer_context_t *dhd_bandsteer_cntx,  uint8 *mac_addr);
 static s32
 dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac, int all);
 static dhd_bandsteer_mac_entry_t*
 dhd_bandsteer_look_for_match(dhd_bandsteer_context_t *dhd_bandsteer_cntx, uint8 *mac_addr);
 static s32
 dhd_bandsteer_tx_wnm_actframe(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac);
 static void
 dhd_bandsteer_add_to_black_list(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac);

 extern int
 wl_android_set_ap_mac_list(struct net_device *dev, int macmode, struct maclist *maclist);

 /* ********************** Function declartion ends ****************** */

 static void
 dhd_bandsteer_add_timer(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac, unsigned long expires)
 {
 	dhd_bandsteer_mac->dhd_bandsteer_timer.expires = jiffies + msecs_to_jiffies(expires);
 	add_timer(&dhd_bandsteer_mac->dhd_bandsteer_timer);
 }

 static void
 dhd_bandsteer_delete_timer(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac)
 {
 	del_timer(&dhd_bandsteer_mac->dhd_bandsteer_timer);
 }

 /*
  * Idea used to call same callback everytime you conifigure timer
  * based on the status of the mac entry next step will be taken
  */
 static void
 dhd_bandsteer_state_machine(ulong arg)
 {
 	dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac = (dhd_bandsteer_mac_entry_t *)arg;

 	if (dhd_bandsteer_mac == NULL) {
 		DHD_ERROR(("%s: dhd_bandsteer_mac is null\n", __FUNCTION__));
 		return;
 	}

 	DHD_TRACE(("%s: Peer STA BandSteer status 0x%x", __FUNCTION__,
 			dhd_bandsteer_mac->dhd_bandsteer_status));

 	switch (dhd_bandsteer_mac->dhd_bandsteer_status) {
 	case DHD_BANDSTEER_START:
 	case DHD_BANDSTEER_WNM_FRAME_RETRY:
 		dhd_bandsteer_mac->dhd_bandsteer_status = DHD_BANDSTEER_WNM_FRAME_SEND;
 		dhd_bandsteer_schedule_work_on_timeout(dhd_bandsteer_mac);
 		break;

 	case  DHD_BANDSTEER_WNM_FRAME_SEND:
 		dhd_bandsteer_tx_wnm_actframe(dhd_bandsteer_mac);
 		if (dhd_bandsteer_mac->wnm_frame_counter < DHD_BANDSTEER_WNM_FRAME_MAXRETRY) {
 			/* Sending out WNM action frame as soon as assoc indication recieved */
 			dhd_bandsteer_mac->dhd_bandsteer_status = DHD_BANDSTEER_WNM_FRAME_RETRY;
 		}
 		else {
 			dhd_bandsteer_mac->dhd_bandsteer_status = DHD_BANDSTEER_TRIAL_DONE;
 		}
 		break;
 	case DHD_BANDSTEER_TRIAL_DONE:
 		dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac, 0);
 		break;
 	}
 	return;
 }

 void
 dhd_bandsteer_workqueue_wrapper(void *handle, void *event_info, u8 event)
 {
 	dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac = (dhd_bandsteer_mac_entry_t *)event_info;

 	if (event != DHD_WQ_WORK_BANDSTEER_STEP_MOVE) {
 		DHD_ERROR(("%s: unexpected event \n", __FUNCTION__));
 		return;
 	}

 	dhd_bandsteer_state_machine((ulong)dhd_bandsteer_mac);
 }

 /*
  *	This API create and initilize an entry into list which need to be processed later
  */
 static s32
 dhd_bandsteer_addmac_to_monitorlist(dhd_bandsteer_context_t *dhd_bandsteer_cntx,  uint8 *mac_addr)
 {
 	dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac;

 	dhd_bandsteer_mac = kzalloc(sizeof(dhd_bandsteer_mac_entry_t), GFP_KERNEL);
 	if (unlikely(!dhd_bandsteer_mac)) {
 		DHD_ERROR(("%s: alloc failed\n", __FUNCTION__));
 		return BCME_NOMEM;
 	}
 	INIT_LIST_HEAD(&dhd_bandsteer_mac->list);
 	/* pointer dhd_bandsteer_cntx for future use */
 	dhd_bandsteer_mac->dhd_bandsteer_cntx = dhd_bandsteer_cntx;

 	dhd_bandsteer_mac->dhd_bandsteer_status = DHD_BANDSTEER_START;

 	memcpy(&dhd_bandsteer_mac->mac_addr.octet, mac_addr, ETHER_ADDR_LEN);

 	/* Configure timer for 20 Sec */
 	init_timer(&dhd_bandsteer_mac->dhd_bandsteer_timer);
 	dhd_bandsteer_mac->dhd_bandsteer_timer.data = (ulong)dhd_bandsteer_mac;
 	dhd_bandsteer_mac->dhd_bandsteer_timer.function = dhd_bandsteer_state_machine;
 	dhd_bandsteer_mac->wnm_frame_counter = 0;

 	/* Add new entry into the list */
 	list_add_tail(&dhd_bandsteer_mac->list,
 			&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list);

 	DHD_TRACE(("%s: " MACDBG " added into list \n", __FUNCTION__,
 			MAC2STRDBG(dhd_bandsteer_mac->mac_addr.octet)));
 	/* This can be tweaked more */
 	dhd_bandsteer_add_timer(dhd_bandsteer_mac, DHD_BANDSTEER_WNM_FRAME_DELAY);

 	return BCME_OK;
 }

 /*
  * This function removes one or all mac entry from the list
  */
 static s32
 dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac, int all)
 {
 	dhd_bandsteer_context_t *dhd_bandsteer_cntx;
 	dhd_bandsteer_mac_entry_t *curr, *next;

 	DHD_INFO(("%s: entered \n", __FUNCTION__));
 	/* TODO:probably these sanity lines can be removed */
 	if (dhd_bandsteer_mac == NULL) {
 		DHD_ERROR(("%s: dhd_bandsteer_mac is NULL\n", __FUNCTION__));
 		return BCME_ERROR;
 	}

 	dhd_bandsteer_cntx = dhd_bandsteer_mac->dhd_bandsteer_cntx;

 	list_for_each_entry_safe(curr, next,
 			&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list, list) {
 		if ((curr == dhd_bandsteer_mac) || all) {
 			DHD_ERROR(("%s: " MACDBG " deleted from list \n", __FUNCTION__,
 					MAC2STRDBG(dhd_bandsteer_mac->mac_addr.octet)));
 			list_del(&dhd_bandsteer_mac->list);
 			dhd_bandsteer_delete_timer(dhd_bandsteer_mac);
 			kfree(dhd_bandsteer_mac);
 			if (!all)
 				break;
 		}
 	}
 	return BCME_OK;
 }

 /*
  * Logic to find corresponding node in list based given mac address
  * Returns null if entry not seen
  */
 static dhd_bandsteer_mac_entry_t*
 dhd_bandsteer_look_for_match(dhd_bandsteer_context_t *dhd_bandsteer_cntx, uint8 *mac_addr)
 {
 	dhd_bandsteer_mac_entry_t *curr = NULL, *next = NULL;

 	list_for_each_entry_safe(curr, next,
 			&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list, list) {
 		if (memcmp(&curr->mac_addr.octet, mac_addr, ETHER_ADDR_LEN) == 0) {
 			return curr;
 		}
 	}
 	return NULL;
 }

 /*
  * This API will send wnm action frame also configure timeout timer
  */
 static s32
 dhd_bandsteer_tx_wnm_actframe(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac)
 {
 	dhd_bandsteer_context_t *dhd_bandsteer_cntx = dhd_bandsteer_mac->dhd_bandsteer_cntx;
 	wl_action_frame_t *action_frame = NULL;
 	wl_af_params_t *af_params = NULL;
 	char *smbuf = NULL;
 	int error = BCME_ERROR;
 	uint8 *bp;
 	dhd_bandsteer_iface_info_t *if_info_5g, *if_info_2g;

 	if_info_5g = &dhd_bandsteer_cntx->bsd_ifaces[dhd_bandsteer_cntx->ifidx_5g];
 	if_info_2g = &dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g];

 	smbuf = kzalloc(WLC_IOCTL_MAXLEN, GFP_KERNEL);
 	if (smbuf == NULL) {
 		DHD_ERROR(("%s: failed to allocated memory %d bytes\n", __FUNCTION__,
 				WLC_IOCTL_MAXLEN));
 		goto send_action_frame_out;
 	}

 	af_params = (wl_af_params_t *) kzalloc(WL_WIFI_AF_PARAMS_SIZE, GFP_KERNEL);
 	if (af_params == NULL) {
 		DHD_ERROR(("%s: unable to allocate frame\n", __FUNCTION__));
 		goto send_action_frame_out;
 	}

 	af_params->channel = if_info_2g->channel;
 	af_params->dwell_time = -1;
 	memcpy(&af_params->BSSID, &dhd_bandsteer_mac->mac_addr.octet, ETHER_ADDR_LEN);
 	action_frame = &af_params->action_frame;

 	action_frame->packetId = 0;
 	memcpy(&action_frame->da, &dhd_bandsteer_mac->mac_addr.octet, ETHER_ADDR_LEN);

 	dhd_bandsteer_mac->wnm_frame_counter++;
 	bp = (uint8 *)action_frame->data;
 	*bp++ = 0xa; /* Category */
 	*bp++ = 0x7; /* Action ID */
 	*bp++ = dhd_bandsteer_mac->wnm_frame_counter; /* Dialog Token */
 	*bp++ = 0x1; /* Request mode */
 	*bp++ = 0x0; /* disassociation timer has two bytes */
 	*bp++ = 0x0;
 	*bp++ = 0x0; /* Validity interval */
 	*bp++ = 0x34; /* Element ID */
 	*bp++ = 0xd; /* Len */
 	memcpy(bp, if_info_5g->macaddr.octet, ETHER_ADDR_LEN);
 	bp += ETHER_ADDR_LEN;
 	bp +=4; /* Skip BSSID info 4 bytes in size */
 	*bp++ = 0x7d; /* Operating class */
 	*bp++ = if_info_5g->channel; /* Channel number */
 	*bp = 0x0; /* Phy Type */

 	action_frame->len = (bp - (uint8 *)&action_frame->data) + 1;

 	error = wldev_iovar_setbuf(if_info_2g->ndev, "actframe", af_params,
 			sizeof(wl_af_params_t), smbuf, WLC_IOCTL_MAXLEN, NULL);
 	if (error) {
 		DHD_ERROR(("Failed to set action frame, error=%d\n", error));
 		goto send_action_frame_out;
 	}
 	DHD_TRACE(("%s: BSS Trans Req frame sent to " MACDBG " try %d\n", __FUNCTION__,
 			MAC2STRDBG(dhd_bandsteer_mac->mac_addr.octet),
 			dhd_bandsteer_mac->wnm_frame_counter));

 send_action_frame_out:
 	/* Re-schedule the timer */
 	dhd_bandsteer_add_timer(dhd_bandsteer_mac, DHD_BANDSTEER_WNM_FRAME_DELAY);
 	if (af_params)
 		kfree(af_params);

 	if (smbuf)
 		kfree(smbuf);

 	if (error)
 		return BCME_ERROR;

 	return BCME_OK;
 }

 /*
  * Call dhd_bandsteer_remove_mac_from_list()
  * Add into black list of corresponding interace at present 2.4
  * Uses existing IOVAR calls
  */
 static void
 dhd_bandsteer_add_to_black_list(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac)
 {
 	dhd_bandsteer_context_t *dhd_bandsteer_cntx = dhd_bandsteer_mac->dhd_bandsteer_cntx;
 	dhd_bandsteer_iface_info_t *if_info_2g;
 	int err;
 	int macmode = MACLIST_MODE_DENY;
 	struct maclist *maclist;
 	uint8 *pmaclist_ea;
 	uint8 mac_buf[MAX_NUM_OF_ASSOCLIST *
 		sizeof(struct ether_addr) + sizeof(uint)] = {0};

 	if_info_2g = &dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g];

 	/* Black listing */
 	DHD_INFO(("%s: Black listing " MACDBG " on 2GHz IF\n", __FUNCTION__,
 			MAC2STRDBG(dhd_bandsteer_mac->mac_addr.octet)));

 	/* Get current black list */
 	if ((err = wldev_ioctl(if_info_2g->ndev, WLC_GET_MACLIST, mac_buf,
 			sizeof(mac_buf), false)) != 0) {
 		DHD_ERROR(("%s: WLC_GET_MACLIST error=%d\n", __FUNCTION__, err));
 	}

 	maclist = (struct maclist *)mac_buf;
 	pmaclist_ea = (uint8*) mac_buf +
 			(sizeof(struct ether_addr) * maclist->count) + sizeof(uint);
 	maclist->count++;

 	memcpy(pmaclist_ea, &dhd_bandsteer_mac->mac_addr.octet, ETHER_ADDR_LEN);

 	if ((err = wldev_iovar_setint(if_info_2g->ndev, "probresp_mac_filter", 1)) != BCME_OK) {
 		DHD_ERROR(("%s: failed to set probe response filter err %d\n", __FUNCTION__, err));
 	}

 	if ((err = wldev_ioctl(if_info_2g->ndev, WLC_SET_MACMODE, &macmode,
 			sizeof(macmode), true)) != 0) {
 		DHD_ERROR(("%s: WLC_SET_MACMODE error=%d\n", __FUNCTION__, err));
 	}

 	/* set the MAC filter list */
 	if ((err = wldev_ioctl(if_info_2g->ndev, WLC_SET_MACLIST, maclist,
 			sizeof(int) + sizeof(struct ether_addr) * maclist->count, true)) != 0) {
 		DHD_ERROR(("%s: WLC_SET_MACLIST error=%d\n", __FUNCTION__, err));
 	}
 }

 /*
  * Check if mac association on 2.4 G
  * If on 2.4
  *      * Ignore if we have already run Bandsteer cycle for this
  *	* Add PEER STA mac monitor_list
  *	* Send BSS transition request frame
  * Else
  *	* We recieved assoc on 5g from Mac we forced to move onto 5G
  */
 s32
 dhd_bandsteer_trigger_bandsteer(struct net_device *ndev, uint8 *mac_addr)
 {
 	struct wireless_dev *__wdev = (struct wireless_dev *)(ndev)->ieee80211_ptr;
 	struct bcm_cfg80211 *cfg = (struct bcm_cfg80211 *)wiphy_priv(__wdev->wiphy);
 	struct net_device *netdev_5g = NULL;
 	dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac = NULL;
 	dhd_bandsteer_context_t *dhd_bandsteer_cntx = NULL;

 	DHD_TRACE(("%s: Start band-steer procedure for " MACDBG "\n", __FUNCTION__,
 			MAC2STRDBG(mac_addr)));

 	if (cfg == NULL) {
 		DHD_ERROR(("%s: bcmcfg is null\n", __FUNCTION__));
 		return BCME_ERROR;
 	}
 	if (cfg->dhd_bandsteer_cntx == NULL) {
 		DHD_ERROR(("%s: Band Steering not enabled\n", __FUNCTION__));
 		return BCME_ERROR;
 	}

 	dhd_bandsteer_cntx = cfg->dhd_bandsteer_cntx;

 	netdev_5g = dhd_bandsteer_cntx->bsd_ifaces[dhd_bandsteer_cntx->ifidx_5g].ndev;
 	dhd_bandsteer_mac = dhd_bandsteer_look_for_match(dhd_bandsteer_cntx, mac_addr);
 	if (dhd_bandsteer_mac == NULL) {
 		/*
 		 * This STA entry not found in list check if  bandsteer is already done for this
 		 * Check if this on 2.4/5Ghz
 		 */
 		if (ndev == netdev_5g) {
 			/* Ignore as device aleady connectd to 5G */
 			DHD_INFO(("%s: " MACDBG " is on 5GHz interface\n", __FUNCTION__,
 			 MAC2STRDBG(mac_addr)));
 			return BCME_OK;
 		}
 		else {
 			dhd_bandsteer_addmac_to_monitorlist(dhd_bandsteer_cntx, mac_addr);
 			/*
 			 * TODO: Time for us to enable PROB_REQ MSG
 			 */
 		}
 	}
 	else {
 		/*
 		 * Start post connect process as bandsteer is successful  for this entry
 		 */
 		if (ndev  == netdev_5g) {
 			DHD_INFO(("%s: Band Steer for " MACDBG " successful\n", __FUNCTION__,
 					MAC2STRDBG(mac_addr)));
 			dhd_bandsteer_add_to_black_list(dhd_bandsteer_mac);
 			dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac, 0);
 			/* Probabaly add this mac into black list */
 		}
 	}
 	return BCME_OK;
 }


 s32
 dhd_bandsteer_module_init(struct net_device *ndev)
 {
 	/* Initialize */
 	dhd_bandsteer_context_t *dhd_bandsteer_cntx = NULL;
 	struct channel_info ci;
 	uint8 ifidx;
 	struct wireless_dev *__wdev = (struct wireless_dev *)(ndev)->ieee80211_ptr;
 	struct bcm_cfg80211 *cfg = (struct bcm_cfg80211 *)wiphy_priv(__wdev->wiphy);
 	int err;

 	DHD_INFO(("%s: entered\n", __FUNCTION__));

 	if (cfg == NULL) {
 		DHD_ERROR(("%s: bcmcfg is  null\n", __FUNCTION__));
 		return BCME_ERROR;
 	}

 	if (cfg->dhd_bandsteer_cntx != NULL) {
 		DHD_ERROR(("%s: Band Steering already enabled\n", __FUNCTION__));
 		return BCME_ERROR;
 	}

 	dhd_bandsteer_cntx = (dhd_bandsteer_context_t *)kzalloc(sizeof(dhd_bandsteer_context_t),
 			GFP_KERNEL);
 	if (unlikely(!dhd_bandsteer_cntx)) {
 		DHD_ERROR(("%s: dhd_bandsteer_cntx alloc failed\n", __FUNCTION__));
 		return BCME_NOMEM;
 	}

 	if (dhd_bandsteer_get_ifaces(cfg->pub, &dhd_bandsteer_cntx->bsd_ifaces)) {
 		DHD_ERROR(("%s: AP interfaces count != 2", __FUNCTION__));
 		err = BCME_ERROR;
 		goto failed;
 	}

 	for (ifidx = 0; ifidx < DHD_BANDSTEER_MAXIFACES; ifidx++) {
 		err = wldev_iovar_getbuf_bsscfg(dhd_bandsteer_cntx->bsd_ifaces[ifidx].ndev,
 			 "cur_etheraddr", NULL, 0, cfg->ioctl_buf,
 				WLC_IOCTL_SMLEN, 0, &cfg->ioctl_buf_sync);
 		if (err) {
 			DHD_ERROR(("%s: Failed to get mac address\n", __FUNCTION__));
 			goto failed;
 		}

 		memcpy(dhd_bandsteer_cntx->bsd_ifaces[ifidx].macaddr.octet,
 				cfg->ioctl_buf, ETHER_ADDR_LEN);

 		memset(&ci, 0, sizeof(struct channel_info));
 		err = wldev_ioctl(dhd_bandsteer_cntx->bsd_ifaces[ifidx].ndev, WLC_GET_CHANNEL,
 				&ci, sizeof(ci), false);
 		if (err) {
 			DHD_ERROR(("%s: Failed to get channel\n", __FUNCTION__));
 			goto failed;
 		}
 		if (CHANNEL_IS_5G(ci.hw_channel))
 			dhd_bandsteer_cntx->ifidx_5g = ifidx;

 		dhd_bandsteer_cntx->bsd_ifaces[ifidx].channel = ci.hw_channel;
 	}

 	/* Enable software proberesponse on 2GHz interface */
 	if ((err = wldev_iovar_setint(
 			dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g].ndev,
 			"probresp_sw", 1)) != BCME_OK) {
 		DHD_ERROR(("%s: Failed to enable software ProbeRsp err = %d\n", __FUNCTION__, err));
 	}

 	if ((err = wldev_iovar_setint(ndev, "bandsteer", 1)) != BCME_OK) {
 		DHD_ERROR(("%s: Failed to enable bandsteer in FW err = %d\n", __FUNCTION__, err));
 	}


 	INIT_LIST_HEAD(&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list);
 	dhd_bandsteer_cntx->dhd_pub = cfg->pub;
 	cfg->dhd_bandsteer_cntx = (void *) dhd_bandsteer_cntx;
 	return BCME_OK;

 failed:
 	kfree(dhd_bandsteer_cntx);
 	return err;
 }

 s32
 dhd_bandsteer_module_deinit(struct net_device *ndev)
 {
 	dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac = NULL;
 	dhd_bandsteer_context_t *dhd_bandsteer_cntx = NULL;
 	struct wireless_dev *__wdev = (struct wireless_dev *)(ndev)->ieee80211_ptr;
 	struct bcm_cfg80211 *cfg = (struct bcm_cfg80211 *)wiphy_priv(__wdev->wiphy);
 	int macmode = MACLIST_MODE_DISABLED;
 	int err;
 	struct maclist maclist;

 	DHD_INFO(("%s: entered\n", __FUNCTION__));

 	if (cfg == NULL) {
 		DHD_ERROR(("%s: bcmcfg is null\n", __FUNCTION__));
 		return BCME_ERROR;
 	}

 	if (cfg->dhd_bandsteer_cntx == NULL) {
 		DHD_ERROR(("%s: Band Steering not enabled\n", __FUNCTION__));
 		return BCME_ERROR;
 	}

 	dhd_bandsteer_cntx = cfg->dhd_bandsteer_cntx;

 	/* Disable mac filter */
 	if ((err = wldev_ioctl(dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g].ndev,
 			WLC_SET_MACMODE, &macmode, sizeof(macmode), true)) != 0) {
 		DHD_ERROR(("%s: WLC_SET_MACMODE error=%d\n", __FUNCTION__, err));
 	}

 	/* Set the MAC filter list */
 	memset(&maclist, 0, sizeof(struct maclist));
 	if ((err = wldev_ioctl(dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g].ndev,
 			WLC_SET_MACLIST, &maclist, sizeof(struct maclist), true)) != 0) {
 		DHD_ERROR(("%s: WLC_SET_MACLIST error=%d\n", __FUNCTION__, err));
 	}

 	/* Disable software proberesponse on 2GHz interface */
 	if ((err = wldev_iovar_setint(
 			dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g].ndev,
 			"probresp_sw", 0)) != BCME_OK) {
 		DHD_ERROR(("%s: Failed to disable software ProbeRsp err = %d\n",
 				__FUNCTION__, err));
 	}

 	if ((err = wldev_iovar_setint(ndev, "bandsteer", 0)) != BCME_OK) {
 		DHD_ERROR(("%s: Failed to disable bandsteer in FW err = %d\n", __FUNCTION__, err));
 	}

 	/* Get the first element of the list & pass it to remove */
 	if (dhd_bandsteer_cntx->dhd_bandsteer_monitor_list.next  !=
 			&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list) {
 		dhd_bandsteer_mac = (dhd_bandsteer_mac_entry_t *)list_entry(
 				dhd_bandsteer_cntx->dhd_bandsteer_monitor_list.next,
 				dhd_bandsteer_mac_entry_t, list);
 	}

 	if (dhd_bandsteer_mac) {
 		dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac, 1);
 	}

 	kfree(dhd_bandsteer_cntx);
 	cfg->dhd_bandsteer_cntx = NULL;
 	return BCME_OK;
 }
	/*
	* Band Steering logic
	*
	* Feature by which dualband capable PEERs will be
	* forced move on 5GHz interface
	*
	* Copyright (C) 1999-2017, Broadcom Corporation
	*
	* Unless you and Broadcom execute a separate written software license
	* agreement governing use of this software, this software is licensed to you
	* under the terms of the GNU General Public License version 2 (the "GPL"),
	* available at http://www.broadcom.com/licenses/GPLv2.php, with the
	* following added to such license:
	*
	* As a special exception, the copyright holders of this software give you
	* permission to link this software with independent modules, and to copy and
	* distribute the resulting executable under terms of your choice, provided that
	* you also meet, for each linked independent module, the terms and conditions of
	* the license of that module. An independent module is a module which is not
	* derived from this software. The special exception does not apply to any
	* modifications of the software.
	*
	* Notwithstanding the above, under no circumstances may you combine this
	* software in any way with any other Broadcom software provided under a license
	* other than the GPL, without Broadcom's express prior written consent.
	*
	* <<Broadcom-WL-IPTag/Open:>>
	*
	* $ Copyright Cypress Semiconductor $
	*
	* $Id: dhd_bandsteer.c 664149 2017-03-07 13:29:02Z $
	*/

	#include <typedefs.h>
	#include <linuxver.h>
	#include <osl.h>
	#include <linux/kernel.h>
	#include <linux/time.h>
	#include <bcmutils.h>
	#include <net/cfg80211.h>
	#include <net/rtnetlink.h>
	#include <wl_cfg80211.h>
	#include <wl_android.h>
	#include <wldev_common.h>
	#include <dhd_linux_wq.h>
	#include <dhd_cfg80211.h>
	#include <dhd_bandsteer.h>
	#include <dhd_dbg.h>

	/* defines */
	/* BANDSTEER STATE MACHINE STATES */
	#define DHD_BANDSTEER_START 0x0001
	#define DHD_BANDSTEER_WNM_FRAME_SEND 0x0002
	#define DHD_BANDSTEER_WNM_FRAME_RETRY 0x0004
	#define DHD_BANDSTEER_WNM_FRAME_SENT 0x0008
	#define DHD_BANDSTEER_TRIAL_DONE 0x0080

	#define DHD_BANDSTEER_ON_PROCESS_MASK (DHD_BANDSTEER_START \| DHD_BANDSTEER_WNM_FRAME_SEND \
	\| DHD_BANDSTEER_WNM_FRAME_RETRY \| DHD_BANDSTEER_TRIAL_DONE)

	#define DHD_BANDSTEER_WNM_FRAME_MAXRETRY 3
	#define DHD_BANDSTEER_WNM_FRAME_DELAY 1000
	#define MAX_NUM_OF_ASSOCLIST 64
	#define DHD_BANDSTEER_MAXIFACES 2

	#define CHANNEL_IS_5G(channel) (((channel >= 36) && (channel <= 165)) ? \
	true : false)

	/* ******************** Function declaration ********************* */
	static s32
	dhd_bandsteer_addmac_to_monitorlist(dhd_bandsteer_context_t dhd_bandsteer_cntx, uint8 mac_addr);
	static s32
	dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac, int all);
	static dhd_bandsteer_mac_entry_t*
	dhd_bandsteer_look_for_match(dhd_bandsteer_context_t dhd_bandsteer_cntx, uint8 mac_addr);
	static s32
	dhd_bandsteer_tx_wnm_actframe(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac);
	static void
	dhd_bandsteer_add_to_black_list(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac);

	extern int
	wl_android_set_ap_mac_list(struct net_device dev, int macmode, struct maclist maclist);

	/* ******************** Function declartion ends **************** */

	static void
	dhd_bandsteer_add_timer(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac, unsigned long expires)
	{
	dhd_bandsteer_mac->dhd_bandsteer_timer.expires = jiffies + msecs_to_jiffies(expires);
	add_timer(&dhd_bandsteer_mac->dhd_bandsteer_timer);
	}

	static void
	dhd_bandsteer_delete_timer(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac)
	{
	del_timer(&dhd_bandsteer_mac->dhd_bandsteer_timer);
	}

	/*
	* Idea used to call same callback everytime you conifigure timer
	* based on the status of the mac entry next step will be taken
	*/
	static void
	dhd_bandsteer_state_machine(ulong arg)
	{
	dhd_bandsteer_mac_entry_t dhd_bandsteer_mac = (dhd_bandsteer_mac_entry_t )arg;

	if (dhd_bandsteer_mac == NULL) {
	DHD_ERROR(("%s: dhd_bandsteer_mac is null\n", __FUNCTION__));
	return;
	}

	DHD_TRACE(("%s: Peer STA BandSteer status 0x%x", __FUNCTION__,
	dhd_bandsteer_mac->dhd_bandsteer_status));

	switch (dhd_bandsteer_mac->dhd_bandsteer_status) {
	case DHD_BANDSTEER_START:
	case DHD_BANDSTEER_WNM_FRAME_RETRY:
	dhd_bandsteer_mac->dhd_bandsteer_status = DHD_BANDSTEER_WNM_FRAME_SEND;
	dhd_bandsteer_schedule_work_on_timeout(dhd_bandsteer_mac);
	break;

	case DHD_BANDSTEER_WNM_FRAME_SEND:
	dhd_bandsteer_tx_wnm_actframe(dhd_bandsteer_mac);
	if (dhd_bandsteer_mac->wnm_frame_counter < DHD_BANDSTEER_WNM_FRAME_MAXRETRY) {
	/* Sending out WNM action frame as soon as assoc indication recieved */
	dhd_bandsteer_mac->dhd_bandsteer_status = DHD_BANDSTEER_WNM_FRAME_RETRY;
	}
	else {
	dhd_bandsteer_mac->dhd_bandsteer_status = DHD_BANDSTEER_TRIAL_DONE;
	}
	break;
	case DHD_BANDSTEER_TRIAL_DONE:
	dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac, 0);
	break;
	}
	return;
	}

	void
	dhd_bandsteer_workqueue_wrapper(void handle, void event_info, u8 event)
	{
	dhd_bandsteer_mac_entry_t dhd_bandsteer_mac = (dhd_bandsteer_mac_entry_t )event_info;

	if (event != DHD_WQ_WORK_BANDSTEER_STEP_MOVE) {
	DHD_ERROR(("%s: unexpected event \n", __FUNCTION__));
	return;
	}

	dhd_bandsteer_state_machine((ulong)dhd_bandsteer_mac);
	}

	/*
	* This API create and initilize an entry into list which need to be processed later
	*/
	static s32
	dhd_bandsteer_addmac_to_monitorlist(dhd_bandsteer_context_t dhd_bandsteer_cntx, uint8 mac_addr)
	{
	dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac;

	dhd_bandsteer_mac = kzalloc(sizeof(dhd_bandsteer_mac_entry_t), GFP_KERNEL);
	if (unlikely(!dhd_bandsteer_mac)) {
	DHD_ERROR(("%s: alloc failed\n", __FUNCTION__));
	return BCME_NOMEM;
	}
	INIT_LIST_HEAD(&dhd_bandsteer_mac->list);
	/* pointer dhd_bandsteer_cntx for future use */
	dhd_bandsteer_mac->dhd_bandsteer_cntx = dhd_bandsteer_cntx;

	dhd_bandsteer_mac->dhd_bandsteer_status = DHD_BANDSTEER_START;

	memcpy(&dhd_bandsteer_mac->mac_addr.octet, mac_addr, ETHER_ADDR_LEN);

	/* Configure timer for 20 Sec */
	init_timer(&dhd_bandsteer_mac->dhd_bandsteer_timer);
	dhd_bandsteer_mac->dhd_bandsteer_timer.data = (ulong)dhd_bandsteer_mac;
	dhd_bandsteer_mac->dhd_bandsteer_timer.function = dhd_bandsteer_state_machine;
	dhd_bandsteer_mac->wnm_frame_counter = 0;

	/* Add new entry into the list */
	list_add_tail(&dhd_bandsteer_mac->list,
	&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list);

	DHD_TRACE(("%s: " MACDBG " added into list \n", __FUNCTION__,
	MAC2STRDBG(dhd_bandsteer_mac->mac_addr.octet)));
	/* This can be tweaked more */
	dhd_bandsteer_add_timer(dhd_bandsteer_mac, DHD_BANDSTEER_WNM_FRAME_DELAY);

	return BCME_OK;
	}

	/*
	* This function removes one or all mac entry from the list
	*/
	static s32
	dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac, int all)
	{
	dhd_bandsteer_context_t *dhd_bandsteer_cntx;
	dhd_bandsteer_mac_entry_t curr, next;

	DHD_INFO(("%s: entered \n", __FUNCTION__));
	/* TODO:probably these sanity lines can be removed */
	if (dhd_bandsteer_mac == NULL) {
	DHD_ERROR(("%s: dhd_bandsteer_mac is NULL\n", __FUNCTION__));
	return BCME_ERROR;
	}

	dhd_bandsteer_cntx = dhd_bandsteer_mac->dhd_bandsteer_cntx;

	list_for_each_entry_safe(curr, next,
	&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list, list) {
	if ((curr == dhd_bandsteer_mac) \|\| all) {
	DHD_ERROR(("%s: " MACDBG " deleted from list \n", __FUNCTION__,
	MAC2STRDBG(dhd_bandsteer_mac->mac_addr.octet)));
	list_del(&dhd_bandsteer_mac->list);
	dhd_bandsteer_delete_timer(dhd_bandsteer_mac);
	kfree(dhd_bandsteer_mac);
	if (!all)
	break;
	}
	}
	return BCME_OK;
	}

	/*
	* Logic to find corresponding node in list based given mac address
	* Returns null if entry not seen
	*/
	static dhd_bandsteer_mac_entry_t*
	dhd_bandsteer_look_for_match(dhd_bandsteer_context_t dhd_bandsteer_cntx, uint8 mac_addr)
	{
	dhd_bandsteer_mac_entry_t curr = NULL, next = NULL;

	list_for_each_entry_safe(curr, next,
	&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list, list) {
	if (memcmp(&curr->mac_addr.octet, mac_addr, ETHER_ADDR_LEN) == 0) {
	return curr;
	}
	}
	return NULL;
	}

	/*
	* This API will send wnm action frame also configure timeout timer
	*/
	static s32
	dhd_bandsteer_tx_wnm_actframe(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac)
	{
	dhd_bandsteer_context_t *dhd_bandsteer_cntx = dhd_bandsteer_mac->dhd_bandsteer_cntx;
	wl_action_frame_t *action_frame = NULL;
	wl_af_params_t *af_params = NULL;
	char *smbuf = NULL;
	int error = BCME_ERROR;
	uint8 *bp;
	dhd_bandsteer_iface_info_t if_info_5g, if_info_2g;

	if_info_5g = &dhd_bandsteer_cntx->bsd_ifaces[dhd_bandsteer_cntx->ifidx_5g];
	if_info_2g = &dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g];

	smbuf = kzalloc(WLC_IOCTL_MAXLEN, GFP_KERNEL);
	if (smbuf == NULL) {
	DHD_ERROR(("%s: failed to allocated memory %d bytes\n", __FUNCTION__,
	WLC_IOCTL_MAXLEN));
	goto send_action_frame_out;
	}

	af_params = (wl_af_params_t *) kzalloc(WL_WIFI_AF_PARAMS_SIZE, GFP_KERNEL);
	if (af_params == NULL) {
	DHD_ERROR(("%s: unable to allocate frame\n", __FUNCTION__));
	goto send_action_frame_out;
	}

	af_params->channel = if_info_2g->channel;
	af_params->dwell_time = -1;
	memcpy(&af_params->BSSID, &dhd_bandsteer_mac->mac_addr.octet, ETHER_ADDR_LEN);
	action_frame = &af_params->action_frame;

	action_frame->packetId = 0;
	memcpy(&action_frame->da, &dhd_bandsteer_mac->mac_addr.octet, ETHER_ADDR_LEN);

	dhd_bandsteer_mac->wnm_frame_counter++;
	bp = (uint8 *)action_frame->data;
	bp++ = 0xa; / Category */
	bp++ = 0x7; / Action ID */
	bp++ = dhd_bandsteer_mac->wnm_frame_counter; / Dialog Token */
	bp++ = 0x1; / Request mode */
	bp++ = 0x0; / disassociation timer has two bytes */
	*bp++ = 0x0;
	bp++ = 0x0; / Validity interval */
	bp++ = 0x34; / Element ID */
	bp++ = 0xd; / Len */
	memcpy(bp, if_info_5g->macaddr.octet, ETHER_ADDR_LEN);
	bp += ETHER_ADDR_LEN;
	bp +=4; /* Skip BSSID info 4 bytes in size */
	bp++ = 0x7d; / Operating class */
	bp++ = if_info_5g->channel; / Channel number */
	bp = 0x0; / Phy Type */

	action_frame->len = (bp - (uint8 *)&action_frame->data) + 1;

	error = wldev_iovar_setbuf(if_info_2g->ndev, "actframe", af_params,
	sizeof(wl_af_params_t), smbuf, WLC_IOCTL_MAXLEN, NULL);
	if (error) {
	DHD_ERROR(("Failed to set action frame, error=%d\n", error));
	goto send_action_frame_out;
	}
	DHD_TRACE(("%s: BSS Trans Req frame sent to " MACDBG " try %d\n", __FUNCTION__,
	MAC2STRDBG(dhd_bandsteer_mac->mac_addr.octet),
	dhd_bandsteer_mac->wnm_frame_counter));

	send_action_frame_out:
	/* Re-schedule the timer */
	dhd_bandsteer_add_timer(dhd_bandsteer_mac, DHD_BANDSTEER_WNM_FRAME_DELAY);
	if (af_params)
	kfree(af_params);

	if (smbuf)
	kfree(smbuf);

	if (error)
	return BCME_ERROR;

	return BCME_OK;
	}

	/*
	* Call dhd_bandsteer_remove_mac_from_list()
	* Add into black list of corresponding interace at present 2.4
	* Uses existing IOVAR calls
	*/
	static void
	dhd_bandsteer_add_to_black_list(dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac)
	{
	dhd_bandsteer_context_t *dhd_bandsteer_cntx = dhd_bandsteer_mac->dhd_bandsteer_cntx;
	dhd_bandsteer_iface_info_t *if_info_2g;
	int err;
	int macmode = MACLIST_MODE_DENY;
	struct maclist *maclist;
	uint8 *pmaclist_ea;
	uint8 mac_buf[MAX_NUM_OF_ASSOCLIST *
	sizeof(struct ether_addr) + sizeof(uint)] = {0};

	if_info_2g = &dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g];

	/* Black listing */
	DHD_INFO(("%s: Black listing " MACDBG " on 2GHz IF\n", __FUNCTION__,
	MAC2STRDBG(dhd_bandsteer_mac->mac_addr.octet)));

	/* Get current black list */
	if ((err = wldev_ioctl(if_info_2g->ndev, WLC_GET_MACLIST, mac_buf,
	sizeof(mac_buf), false)) != 0) {
	DHD_ERROR(("%s: WLC_GET_MACLIST error=%d\n", __FUNCTION__, err));
	}

	maclist = (struct maclist *)mac_buf;
	pmaclist_ea = (uint8*) mac_buf +
	(sizeof(struct ether_addr) * maclist->count) + sizeof(uint);
	maclist->count++;

	memcpy(pmaclist_ea, &dhd_bandsteer_mac->mac_addr.octet, ETHER_ADDR_LEN);

	if ((err = wldev_iovar_setint(if_info_2g->ndev, "probresp_mac_filter", 1)) != BCME_OK) {
	DHD_ERROR(("%s: failed to set probe response filter err %d\n", __FUNCTION__, err));
	}

	if ((err = wldev_ioctl(if_info_2g->ndev, WLC_SET_MACMODE, &macmode,
	sizeof(macmode), true)) != 0) {
	DHD_ERROR(("%s: WLC_SET_MACMODE error=%d\n", __FUNCTION__, err));
	}

	/* set the MAC filter list */
	if ((err = wldev_ioctl(if_info_2g->ndev, WLC_SET_MACLIST, maclist,
	sizeof(int) + sizeof(struct ether_addr) * maclist->count, true)) != 0) {
	DHD_ERROR(("%s: WLC_SET_MACLIST error=%d\n", __FUNCTION__, err));
	}
	}

	/*
	* Check if mac association on 2.4 G
	* If on 2.4
	* * Ignore if we have already run Bandsteer cycle for this
	* * Add PEER STA mac monitor_list
	* * Send BSS transition request frame
	* Else
	* * We recieved assoc on 5g from Mac we forced to move onto 5G
	*/
	s32
	dhd_bandsteer_trigger_bandsteer(struct net_device ndev, uint8 mac_addr)
	{
	struct wireless_dev __wdev = (struct wireless_dev )(ndev)->ieee80211_ptr;
	struct bcm_cfg80211 cfg = (struct bcm_cfg80211 )wiphy_priv(__wdev->wiphy);
	struct net_device *netdev_5g = NULL;
	dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac = NULL;
	dhd_bandsteer_context_t *dhd_bandsteer_cntx = NULL;

	DHD_TRACE(("%s: Start band-steer procedure for " MACDBG "\n", __FUNCTION__,
	MAC2STRDBG(mac_addr)));

	if (cfg == NULL) {
	DHD_ERROR(("%s: bcmcfg is null\n", __FUNCTION__));
	return BCME_ERROR;
	}
	if (cfg->dhd_bandsteer_cntx == NULL) {
	DHD_ERROR(("%s: Band Steering not enabled\n", __FUNCTION__));
	return BCME_ERROR;
	}

	dhd_bandsteer_cntx = cfg->dhd_bandsteer_cntx;

	netdev_5g = dhd_bandsteer_cntx->bsd_ifaces[dhd_bandsteer_cntx->ifidx_5g].ndev;
	dhd_bandsteer_mac = dhd_bandsteer_look_for_match(dhd_bandsteer_cntx, mac_addr);
	if (dhd_bandsteer_mac == NULL) {
	/*
	* This STA entry not found in list check if bandsteer is already done for this
	* Check if this on 2.4/5Ghz
	*/
	if (ndev == netdev_5g) {
	/* Ignore as device aleady connectd to 5G */
	DHD_INFO(("%s: " MACDBG " is on 5GHz interface\n", __FUNCTION__,
	MAC2STRDBG(mac_addr)));
	return BCME_OK;
	}
	else {
	dhd_bandsteer_addmac_to_monitorlist(dhd_bandsteer_cntx, mac_addr);
	/*
	* TODO: Time for us to enable PROB_REQ MSG
	*/
	}
	}
	else {
	/*
	* Start post connect process as bandsteer is successful for this entry
	*/
	if (ndev == netdev_5g) {
	DHD_INFO(("%s: Band Steer for " MACDBG " successful\n", __FUNCTION__,
	MAC2STRDBG(mac_addr)));
	dhd_bandsteer_add_to_black_list(dhd_bandsteer_mac);
	dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac, 0);
	/* Probabaly add this mac into black list */
	}
	}
	return BCME_OK;
	}



	s32
	dhd_bandsteer_module_init(struct net_device *ndev)
	{
	/* Initialize */
	dhd_bandsteer_context_t *dhd_bandsteer_cntx = NULL;
	struct channel_info ci;
	uint8 ifidx;
	struct wireless_dev __wdev = (struct wireless_dev )(ndev)->ieee80211_ptr;
	struct bcm_cfg80211 cfg = (struct bcm_cfg80211 )wiphy_priv(__wdev->wiphy);
	int err;

	DHD_INFO(("%s: entered\n", __FUNCTION__));

	if (cfg == NULL) {
	DHD_ERROR(("%s: bcmcfg is null\n", __FUNCTION__));
	return BCME_ERROR;
	}

	if (cfg->dhd_bandsteer_cntx != NULL) {
	DHD_ERROR(("%s: Band Steering already enabled\n", __FUNCTION__));
	return BCME_ERROR;
	}

	dhd_bandsteer_cntx = (dhd_bandsteer_context_t *)kzalloc(sizeof(dhd_bandsteer_context_t),
	GFP_KERNEL);
	if (unlikely(!dhd_bandsteer_cntx)) {
	DHD_ERROR(("%s: dhd_bandsteer_cntx alloc failed\n", __FUNCTION__));
	return BCME_NOMEM;
	}

	if (dhd_bandsteer_get_ifaces(cfg->pub, &dhd_bandsteer_cntx->bsd_ifaces)) {
	DHD_ERROR(("%s: AP interfaces count != 2", __FUNCTION__));
	err = BCME_ERROR;
	goto failed;
	}

	for (ifidx = 0; ifidx < DHD_BANDSTEER_MAXIFACES; ifidx++) {
	err = wldev_iovar_getbuf_bsscfg(dhd_bandsteer_cntx->bsd_ifaces[ifidx].ndev,
	"cur_etheraddr", NULL, 0, cfg->ioctl_buf,
	WLC_IOCTL_SMLEN, 0, &cfg->ioctl_buf_sync);
	if (err) {
	DHD_ERROR(("%s: Failed to get mac address\n", __FUNCTION__));
	goto failed;
	}

	memcpy(dhd_bandsteer_cntx->bsd_ifaces[ifidx].macaddr.octet,
	cfg->ioctl_buf, ETHER_ADDR_LEN);

	memset(&ci, 0, sizeof(struct channel_info));
	err = wldev_ioctl(dhd_bandsteer_cntx->bsd_ifaces[ifidx].ndev, WLC_GET_CHANNEL,
	&ci, sizeof(ci), false);
	if (err) {
	DHD_ERROR(("%s: Failed to get channel\n", __FUNCTION__));
	goto failed;
	}
	if (CHANNEL_IS_5G(ci.hw_channel))
	dhd_bandsteer_cntx->ifidx_5g = ifidx;

	dhd_bandsteer_cntx->bsd_ifaces[ifidx].channel = ci.hw_channel;
	}

	/* Enable software proberesponse on 2GHz interface */
	if ((err = wldev_iovar_setint(
	dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g].ndev,
	"probresp_sw", 1)) != BCME_OK) {
	DHD_ERROR(("%s: Failed to enable software ProbeRsp err = %d\n", __FUNCTION__, err));
	}

	if ((err = wldev_iovar_setint(ndev, "bandsteer", 1)) != BCME_OK) {
	DHD_ERROR(("%s: Failed to enable bandsteer in FW err = %d\n", __FUNCTION__, err));
	}


	INIT_LIST_HEAD(&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list);
	dhd_bandsteer_cntx->dhd_pub = cfg->pub;
	cfg->dhd_bandsteer_cntx = (void *) dhd_bandsteer_cntx;
	return BCME_OK;

	failed:
	kfree(dhd_bandsteer_cntx);
	return err;
	}

	s32
	dhd_bandsteer_module_deinit(struct net_device *ndev)
	{
	dhd_bandsteer_mac_entry_t *dhd_bandsteer_mac = NULL;
	dhd_bandsteer_context_t *dhd_bandsteer_cntx = NULL;
	struct wireless_dev __wdev = (struct wireless_dev )(ndev)->ieee80211_ptr;
	struct bcm_cfg80211 cfg = (struct bcm_cfg80211 )wiphy_priv(__wdev->wiphy);
	int macmode = MACLIST_MODE_DISABLED;
	int err;
	struct maclist maclist;

	DHD_INFO(("%s: entered\n", __FUNCTION__));

	if (cfg == NULL) {
	DHD_ERROR(("%s: bcmcfg is null\n", __FUNCTION__));
	return BCME_ERROR;
	}

	if (cfg->dhd_bandsteer_cntx == NULL) {
	DHD_ERROR(("%s: Band Steering not enabled\n", __FUNCTION__));
	return BCME_ERROR;
	}

	dhd_bandsteer_cntx = cfg->dhd_bandsteer_cntx;

	/* Disable mac filter */
	if ((err = wldev_ioctl(dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g].ndev,
	WLC_SET_MACMODE, &macmode, sizeof(macmode), true)) != 0) {
	DHD_ERROR(("%s: WLC_SET_MACMODE error=%d\n", __FUNCTION__, err));
	}

	/* Set the MAC filter list */
	memset(&maclist, 0, sizeof(struct maclist));
	if ((err = wldev_ioctl(dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g].ndev,
	WLC_SET_MACLIST, &maclist, sizeof(struct maclist), true)) != 0) {
	DHD_ERROR(("%s: WLC_SET_MACLIST error=%d\n", __FUNCTION__, err));
	}

	/* Disable software proberesponse on 2GHz interface */
	if ((err = wldev_iovar_setint(
	dhd_bandsteer_cntx->bsd_ifaces[!dhd_bandsteer_cntx->ifidx_5g].ndev,
	"probresp_sw", 0)) != BCME_OK) {
	DHD_ERROR(("%s: Failed to disable software ProbeRsp err = %d\n",
	__FUNCTION__, err));
	}

	if ((err = wldev_iovar_setint(ndev, "bandsteer", 0)) != BCME_OK) {
	DHD_ERROR(("%s: Failed to disable bandsteer in FW err = %d\n", __FUNCTION__, err));
	}

	/* Get the first element of the list & pass it to remove */
	if (dhd_bandsteer_cntx->dhd_bandsteer_monitor_list.next !=
	&dhd_bandsteer_cntx->dhd_bandsteer_monitor_list) {
	dhd_bandsteer_mac = (dhd_bandsteer_mac_entry_t *)list_entry(
	dhd_bandsteer_cntx->dhd_bandsteer_monitor_list.next,
	dhd_bandsteer_mac_entry_t, list);
	}

	if (dhd_bandsteer_mac) {
	dhd_bandsteer_remove_mac_from_list(dhd_bandsteer_mac, 1);
	}

	kfree(dhd_bandsteer_cntx);
	cfg->dhd_bandsteer_cntx = NULL;
	return BCME_OK;
	}