qcacld-3.0: Add support for roaming in case of NUD failure

Add support for roaming in case the driver detects a nud failure. The STA then would roam to another BSSID, else if the roaming gets failed the STA would remain connected to the same BSSID as connected before. Change-Id: Idbc99b0ce2f9cacd97564dd8cf9892120958eda2 CRs-Fixed: 2461675
2019-05-29 19:41:25 +05:30
--- a/components/blacklist_mgr/core/inc/wlan_blm_core.h
+++ b/components/blacklist_mgr/core/inc/wlan_blm_core.h
@@ -121,6 +121,7 @@ struct blm_reject_ap_timestamp {
 * @bad_bssid_counter: It represent how many times data stall happened.
 * @ap_timestamp: Ap timestamp.
 * @reject_ap_type: what is the type of rejection for the AP (avoid, black etc.)
+ * @connect_timestamp: Timestamp when the STA got connected with this BSSID
 */
 struct blm_reject_ap {
 	qdf_list_node_t node;
@@ -139,6 +140,7 @@ struct blm_reject_ap {
 		};
 		uint8_t reject_ap_type;
 	};
+	qdf_time_t connect_timestamp;
 };

 /**
@@ -197,6 +199,25 @@ blm_add_userspace_black_list(struct wlan_objmgr_pdev *pdev,
 			     struct qdf_mac_addr *bssid_black_list,
 			     uint8_t num_of_bssid);

+/**
+ * blm_update_bssid_connect_params() - Inform the BLM about connect/disconnect
+ * with the current AP.
+ * @pdev: pdev object
+ * @bssid: BSSID of the AP
+ * @con_state: Connection stae (connected/disconnected)
+ *
+ * This API will inform the BLM about the state with the AP so that if the AP
+ * is selected, and the connection went through, and the connection did not
+ * face any data stall till the bad bssid reset timer, BLM can remove the
+ * AP from the reject ap list maintained by it.
+ *
+ * Return: None
+ */
+void
+blm_update_bssid_connect_params(struct wlan_objmgr_pdev *pdev,
+				struct qdf_mac_addr bssid,
+				enum blm_connection_state con_state);
+
 /**
 * blm_delete_reject_ap_list() - Clear away BSSID and destroy the reject ap list
 * @blm_ctx: blacklist manager pdev priv object
--- a/components/blacklist_mgr/core/src/wlan_blm_core.c
+++ b/components/blacklist_mgr/core/src/wlan_blm_core.c
@@ -300,6 +300,8 @@ blm_handle_avoid_list(struct blm_reject_ap *entry,
 	blm_debug("Added %pM to avoid list type %d, counter %d",
 		  entry->bssid.bytes, ap_info->reject_ap_type,
 		  entry->bad_bssid_counter);
+
+	entry->connect_timestamp = qdf_mc_timer_get_system_time();
 }

 static void
@@ -454,7 +456,14 @@ blm_is_oldest_entry(enum blm_reject_ap_type list_type,
 {
 	switch (list_type) {
 	case DRIVER_RSSI_REJECT_TYPE:
-		if (cur_node_delta < oldest_node_delta)
+		/*
+		 * For RSSI reject type, the lowest retry delay has to be found
+		 * out hence if oldest_node_delta is 0, mean this is the first
+		 * entry and thus return true, If oldest_node_delta is non
+		 * zero, compare the delta and return true if the cur entry
+		 * has lower retry delta.
+		 */
+		if (!oldest_node_delta || (cur_node_delta < oldest_node_delta))
 			return true;
 		break;
 	case USERSPACE_AVOID_TYPE:
@@ -483,17 +492,6 @@ blm_try_delete_bssid_in_list(qdf_list_t *reject_ap_list,
 	qdf_time_t oldest_node_delta = 0;
 	qdf_time_t cur_node_delta = 0;

-	/*
-	 * For RSSI reject type, the lowest retry delay has to be found out,
-	 * hence for reference oldest node delta should be max, and then the
-	 * first entry entry would always be less than oldest entry delta. For
-	 * every other case the delta is the current timestamp minus the time
-	 * when the AP was added, hence it has to be maximum, so a greater than
-	 * check has to be there, so the oldest node delta should be minimum.
-	 */
-	if (list_type == DRIVER_RSSI_REJECT_TYPE)
-		oldest_node_delta = 0xFFFFFFFFFFFFFFFF;
-
 	qdf_list_peek_front(reject_ap_list, &cur_node);

 	while (cur_node) {
@@ -641,7 +639,7 @@ blm_send_reject_ap_list_to_fw(struct wlan_objmgr_pdev *pdev,
 			      struct blm_config *cfg)
 {
 	QDF_STATUS status;
-	struct reject_ap_params reject_params;
+	struct reject_ap_params reject_params = {0};

 	reject_params.bssid_list =
 			qdf_mem_malloc(sizeof(*reject_params.bssid_list) *
@@ -699,7 +697,7 @@ blm_add_bssid_to_reject_list(struct wlan_objmgr_pdev *pdev,
 	blm_ctx = blm_get_pdev_obj(pdev);
 	blm_psoc_obj = blm_get_psoc_obj(wlan_pdev_get_psoc(pdev));

-	if (!blm_ctx || blm_psoc_obj) {
+	if (!blm_ctx || !blm_psoc_obj) {
 		blm_err("blm_ctx or blm_psoc_obj is NULL");
 		return QDF_STATUS_E_INVAL;
 	}
@@ -893,7 +891,7 @@ blm_get_bssid_reject_list(struct wlan_objmgr_pdev *pdev,
 	blm_ctx = blm_get_pdev_obj(pdev);
 	blm_psoc_obj = blm_get_psoc_obj(wlan_pdev_get_psoc(pdev));

-	if (!blm_ctx || blm_psoc_obj) {
+	if (!blm_ctx || !blm_psoc_obj) {
 		blm_err("blm_ctx or blm_psoc_obj is NULL");
 		return 0;
 	}
@@ -912,3 +910,77 @@ blm_get_bssid_reject_list(struct wlan_objmgr_pdev *pdev,

 	return num_of_reject_bssid;
 }
+
+void
+blm_update_bssid_connect_params(struct wlan_objmgr_pdev *pdev,
+				struct qdf_mac_addr bssid,
+				enum blm_connection_state con_state)
+{
+	struct blm_pdev_priv_obj *blm_ctx;
+	struct blm_psoc_priv_obj *blm_psoc_obj;
+	qdf_list_node_t *cur_node = NULL, *next_node = NULL;
+	QDF_STATUS status;
+	struct blm_reject_ap *blm_entry = NULL;
+	qdf_time_t connection_age = 0;
+	bool entry_found = false;
+
+	blm_ctx = blm_get_pdev_obj(pdev);
+	blm_psoc_obj = blm_get_psoc_obj(wlan_pdev_get_psoc(pdev));
+
+	if (!blm_ctx || !blm_psoc_obj) {
+		blm_err("blm_ctx or blm_psoc_obj is NULL");
+		return;
+	}
+
+	status = qdf_mutex_acquire(&blm_ctx->reject_ap_list_lock);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		blm_err("failed to acquire reject_ap_list_lock");
+		return;
+	}
+
+	qdf_list_peek_front(&blm_ctx->reject_ap_list, &cur_node);
+
+	while (cur_node) {
+		qdf_list_peek_next(&blm_ctx->reject_ap_list, cur_node,
+				   &next_node);
+		blm_entry = qdf_container_of(cur_node, struct blm_reject_ap,
+					     node);
+
+		if (!qdf_mem_cmp(blm_entry->bssid.bytes, bssid.bytes,
+				 QDF_MAC_ADDR_SIZE)) {
+			blm_debug("%pM present in BLM reject list, updating connect info",
+				  blm_entry->bssid.bytes);
+			entry_found = true;
+			break;
+		}
+		cur_node = next_node;
+		next_node = NULL;
+	}
+
+	/* This means that the BSSID was not added in the reject list of BLM */
+	if (!entry_found) {
+		qdf_mutex_release(&blm_ctx->reject_ap_list_lock);
+		return;
+	}
+	switch (con_state) {
+	case BLM_AP_CONNECTED:
+		blm_entry->connect_timestamp = qdf_mc_timer_get_system_time();
+		break;
+	case BLM_AP_DISCONNECTED:
+		connection_age = qdf_mc_timer_get_system_time() -
+						blm_entry->connect_timestamp;
+		if (connection_age >
+		    blm_psoc_obj->blm_cfg.bad_bssid_counter_reset_time) {
+			blm_debug("Bad Bssid timer expired, removed %pM from list",
+				  blm_entry->bssid.bytes);
+			qdf_list_remove_node(&blm_ctx->reject_ap_list,
+					     &blm_entry->node);
+			qdf_mem_free(blm_entry);
+		}
+		break;
+	default:
+		blm_debug("Invalid AP connection state recevied %d", con_state);
+	};
+
+	qdf_mutex_release(&blm_ctx->reject_ap_list_lock);
+}
--- a/components/blacklist_mgr/dispatcher/inc/wlan_blm_public_struct.h
+++ b/components/blacklist_mgr/dispatcher/inc/wlan_blm_public_struct.h
@@ -56,6 +56,16 @@ enum blm_reject_ap_type {
 	DRIVER_MONITOR_TYPE =      5
 };

+/**
+ * enum blm_connection_state - State with AP (Connected, Disconnected)
+ * @BLM_AP_CONNECTED: Connected with the AP
+ * @BLM_AP_DISCONNECTED: Disconnected with the AP
+ */
+enum blm_connection_state {
+	BLM_AP_CONNECTED,
+	BLM_AP_DISCONNECTED,
+};
+
 /**
 * struct reject_ap_config_params - Structure to send reject ap list to FW
 * @bssid: BSSID of the AP
--- a/components/blacklist_mgr/dispatcher/inc/wlan_blm_ucfg_api.h
+++ b/components/blacklist_mgr/dispatcher/inc/wlan_blm_ucfg_api.h
@@ -83,6 +83,25 @@ ucfg_blm_add_userspace_black_list(struct wlan_objmgr_pdev *pdev,
 				  struct qdf_mac_addr *bssid_black_list,
 				  uint8_t num_of_bssid);

+/**
+ * ucfg_blm_update_bssid_connect_params() - Inform the BLM about connect or
+ * disconnect with the current AP.
+ * @pdev: pdev object
+ * @bssid: BSSID of the AP
+ * @con_state: Connection stae (connected/disconnected)
+ *
+ * This API will inform the BLM about the state with the AP so that if the AP
+ * is selected, and the connection went through, and the connection did not
+ * face any data stall till the bad bssid reset timer, BLM can remove the
+ * AP from the reject ap list maintained by it.
+ *
+ * Return: None
+ */
+void
+ucfg_blm_update_bssid_connect_params(struct wlan_objmgr_pdev *pdev,
+				     struct qdf_mac_addr bssid,
+				     enum blm_connection_state con_state);
+
 /**
 * ucfg_blm_add_bssid_to_reject_list() - Add BSSID to the specific reject list.
 * @pdev: Pdev object
@@ -137,5 +156,13 @@ ucfg_blm_add_userspace_black_list(struct wlan_objmgr_pdev *pdev,
 {
 	return QDF_STATUS_SUCCESS;
 }
+
+static inline void
+ucfg_blm_update_bssid_connect_params(struct wlan_objmgr_pdev *pdev,
+				     struct qdf_mac_addr bssid,
+				     enum blm_connection_state con_state)
+{
+}
+
 #endif
 #endif /* _WLAN_BLM_UCFG_H_ */
--- a/components/blacklist_mgr/dispatcher/src/wlan_blm_ucfg_api.c
+++ b/components/blacklist_mgr/dispatcher/src/wlan_blm_ucfg_api.c
@@ -130,3 +130,11 @@ ucfg_blm_add_userspace_black_list(struct wlan_objmgr_pdev *pdev,
 	return blm_add_userspace_black_list(pdev, bssid_black_list,
 					    num_of_bssid);
 }
+
+void
+ucfg_blm_update_bssid_connect_params(struct wlan_objmgr_pdev *pdev,
+				     struct qdf_mac_addr bssid,
+				     enum blm_connection_state con_state)
+{
+	blm_update_bssid_connect_params(pdev, bssid, con_state);
+}
--- a/components/mlme/core/inc/wlan_mlme_main.h
+++ b/components/mlme/core/inc/wlan_mlme_main.h
@@ -75,6 +75,7 @@ enum vdev_assoc_type {
 * @dynamic_cfg: current configuration of nss, chains for vdev.
 * @ini_cfg: Max configuration of nss, chains supported for vdev.
 * @sta_dynamic_oce_value: Dyanmic oce flags value for sta
+ * @roam_invoke_params: Roam invoke params
 */
 struct mlme_legacy_priv {
 	bool chan_switch_in_progress;
@@ -86,6 +87,7 @@ struct mlme_legacy_priv {
 	struct wlan_mlme_nss_chains dynamic_cfg;
 	struct wlan_mlme_nss_chains ini_cfg;
 	uint8_t sta_dynamic_oce_value;
+	struct mlme_roam_after_data_stall roam_invoke_params;
 };

 #ifndef CRYPTO_SET_KEY_CONVERGED
@@ -200,6 +202,15 @@ struct wlan_mlme_nss_chains *mlme_get_dynamic_vdev_config(
 struct wlan_mlme_nss_chains *mlme_get_ini_vdev_config(
 					struct wlan_objmgr_vdev *vdev);

+/**
+ * mlme_get_roam_invoke_params() - get the roam invoke params
+ * @vdev: vdev pointer
+ *
+ * Return: pointer to the vdev roam invoke config structure
+ */
+struct mlme_roam_after_data_stall *
+mlme_get_roam_invoke_params(struct wlan_objmgr_vdev *vdev);
+
 /**
 * mlme_psoc_object_created_notification(): mlme psoc create handler
 * @psoc: psoc which is going to created by objmgr
--- a/components/mlme/core/src/wlan_mlme_main.c
+++ b/components/mlme/core/src/wlan_mlme_main.c
@@ -78,6 +78,21 @@ struct wlan_mlme_nss_chains *mlme_get_ini_vdev_config(
 	return &mlme_priv->ini_cfg;
 }

+struct mlme_roam_after_data_stall *
+mlme_get_roam_invoke_params(struct wlan_objmgr_vdev *vdev)
+{
+	struct vdev_mlme_obj *vdev_mlme;
+	struct mlme_legacy_priv *mlme_priv;
+
+	vdev_mlme = wlan_vdev_mlme_get_cmpt_obj(vdev);
+	if (!vdev_mlme)
+		return NULL;
+
+	mlme_priv = vdev_mlme->ext_vdev_ptr;
+
+	return &mlme_priv->roam_invoke_params;
+}
+
 uint8_t *mlme_get_dynamic_oce_flags(struct wlan_objmgr_vdev *vdev)
 {
 	struct vdev_mlme_obj *vdev_mlme;
--- a/components/mlme/dispatcher/inc/wlan_mlme_api.h
+++ b/components/mlme/dispatcher/inc/wlan_mlme_api.h
@@ -62,6 +62,17 @@ QDF_STATUS wlan_mlme_set_cfg_str(uint8_t *src, struct mlme_cfg_str *dst_cfg_str,
 QDF_STATUS wlan_mlme_get_edca_params(struct wlan_mlme_edca_params *edca_params,
 				     uint8_t *data, enum e_edca_type edca_ac);

+/**
+ * wlan_mlme_update_cfg_with_tgt_caps() - Update mlme cfg with tgt caps
+ * @psoc: pointer to psoc object
+ * @tgt_caps:  Pointer to the mlme related capability structure
+ *
+ * Return: None
+ */
+void
+wlan_mlme_update_cfg_with_tgt_caps(struct wlan_objmgr_psoc *psoc,
+				   struct mlme_tgt_caps *tgt_caps);
+
 /*
 * mlme_get_wep_key() - get the wep key to process during auth frame
 * @vdev: VDEV object for which the wep key is being requested
--- a/components/mlme/dispatcher/inc/wlan_mlme_public_struct.h
+++ b/components/mlme/dispatcher/inc/wlan_mlme_public_struct.h
@@ -189,6 +189,26 @@ struct wlan_mlme_dot11_mode {
 	enum mlme_dot11_mode dot11_mode;
 };

+/**
+ * enum roam_invoke_source_entity - Source of invoking roam invoke command.
+ * @USERSPACE_INITIATED: Userspace (supplicant)
+ * @CONNECTION_MGR_INITIATED: connection mgr initiated.
+ */
+enum roam_invoke_source_entity {
+	USERSPACE_INITIATED,
+	CONNECTION_MGR_INITIATED,
+};
+
+/**
+ * struct mlme_roam_after_data_stall - roam invoke entity params
+ * @roam_invoke_in_progress: is roaming already in progress.
+ * @source: source of the roam invoke command.
+ */
+struct mlme_roam_after_data_stall {
+	bool roam_invoke_in_progress;
+	enum roam_invoke_source_entity source;
+};
+
 /**
 * struct mlme_edca_ac_vi - cwmin, cwmax and  aifs value for edca_ac_vi
 *
@@ -865,6 +885,17 @@ struct wlan_mlme_chain_cfg {
 	uint8_t max_rx_chains_5g;
 };

+/**
+ * struct mlme_tgt_caps - mlme related capability coming from target (FW)
+ * @data_stall_recovery_fw_support: does target supports data stall recovery.
+ *
+ * Add all the mlme-tgt related capablities here, and the public API would fill
+ * the related capability in the required mlme cfg structure.
+ */
+struct mlme_tgt_caps {
+	bool data_stall_recovery_fw_support;
+};
+
 /**
 * struct wlan_mlme_rates - RATES related config items
 * @cfpPeriod: cfp period info
@@ -1029,6 +1060,7 @@ struct wlan_mlme_chainmask {
 * @enabled_11d: enable 11d flag
 * @enable_beacon_reception_stats: enable beacon reception stats
 * @enable_remove_time_stamp_sync_cmd: Enable remove time stamp sync cmd
+ * @data_stall_recovery_fw_support: whether FW supports Data stall recovery.
 * @enable_change_channel_bandwidth: enable/disable change channel bw in mission
 * mode
 */
@@ -1059,6 +1091,7 @@ struct wlan_mlme_generic {
 	bool enable_deauth_to_disassoc_map;
 	bool enable_beacon_reception_stats;
 	bool enable_remove_time_stamp_sync_cmd;
+	bool data_stall_recovery_fw_support;
 	bool enable_change_channel_bandwidth;
 };

--- a/components/mlme/dispatcher/src/wlan_mlme_api.c
+++ b/components/mlme/dispatcher/src/wlan_mlme_api.c
@@ -440,6 +440,22 @@ QDF_STATUS wlan_mlme_get_tx_chainmask_1ss(struct wlan_objmgr_psoc *psoc,
 	return QDF_STATUS_SUCCESS;
 }

+void
+wlan_mlme_update_cfg_with_tgt_caps(struct wlan_objmgr_psoc *psoc,
+				   struct mlme_tgt_caps *tgt_caps)
+{
+	struct wlan_mlme_psoc_obj *mlme_obj;
+
+	mlme_obj = mlme_get_psoc_obj(psoc);
+	if (!mlme_obj)
+		return;
+
+	/* Update the mlme cfg according to the tgt capability received */
+
+	mlme_obj->cfg.gen.data_stall_recovery_fw_support =
+				tgt_caps->data_stall_recovery_fw_support;
+}
+
 #ifdef WLAN_FEATURE_11AX
 QDF_STATUS wlan_mlme_cfg_get_he_ul_mumimo(struct wlan_objmgr_psoc *psoc,
 					  uint32_t *value)
--- a/core/hdd/src/wlan_hdd_assoc.c
+++ b/core/hdd/src/wlan_hdd_assoc.c
@@ -68,6 +68,8 @@
 #include "wlan_hdd_nud_tracking.h"
 #include <wlan_cfg80211_crypto.h>
 #include <wlan_crypto_global_api.h>
+#include "wlan_blm_ucfg_api.h"
+
 /* These are needed to recognize WPA and RSN suite types */
 #define HDD_WPA_OUI_SIZE 4
 #define HDD_RSN_OUI_SIZE 4
@@ -1798,6 +1800,12 @@ static QDF_STATUS hdd_dis_connect_handler(struct hdd_adapter *adapter,
 		ucfg_p2p_status_disconnect(adapter->vdev);
 	}

+	/* Inform BLM about the disconnection with the AP */
+	if (adapter->device_mode == QDF_STA_MODE)
+		ucfg_blm_update_bssid_connect_params(hdd_ctx->pdev,
+						     sta_ctx->conn_info.bssid,
+						     BLM_AP_DISCONNECTED);
+
 	hdd_wmm_adapter_clear(adapter);
 	mac_handle = hdd_ctx->mac_handle;
 	sme_ft_reset(mac_handle, adapter->vdev_id);
@@ -3324,6 +3332,11 @@ hdd_association_completion_handler(struct hdd_adapter *adapter,
 		qdf_mem_zero(&adapter->hdd_stats.hdd_pmf_stats,
 			     sizeof(adapter->hdd_stats.hdd_pmf_stats));
 #endif
+		if (adapter->device_mode == QDF_STA_MODE)
+			ucfg_blm_update_bssid_connect_params(hdd_ctx->pdev,
+							     roam_info->bssid,
+							     BLM_AP_CONNECTED);
+
 		policy_mgr_check_n_start_opportunistic_timer(hdd_ctx->psoc);
 		hdd_debug("check for SAP restart");
 		policy_mgr_check_concurrent_intf_and_restart_sap(
--- a/core/hdd/src/wlan_hdd_main.c
+++ b/core/hdd/src/wlan_hdd_main.c
@@ -324,6 +324,7 @@ static const struct category_info cinfo[MAX_SUPPORTED_CATEGORY] = {
 	[QDF_MODULE_ID_NAN] = {QDF_TRACE_LEVEL_ALL},
 	[QDF_MODULE_ID_CP_STATS] = {QDF_TRACE_LEVEL_ALL},
 	[QDF_MODULE_ID_INTEROP_ISSUES_AP] = {QDF_TRACE_LEVEL_ALL},
+	[QDF_MODULE_ID_BLACKLIST_MGR] = {QDF_TRACE_LEVEL_ALL},
 };

 struct notifier_block hdd_netdev_notifier;
--- a/core/hdd/src/wlan_hdd_nud_tracking.c
+++ b/core/hdd/src/wlan_hdd_nud_tracking.c
@@ -22,6 +22,7 @@

 #include "osif_sync.h"
 #include "wlan_hdd_main.h"
+#include "wlan_blm_ucfg_api.h"

 void hdd_nud_set_gateway_addr(struct hdd_adapter *adapter,
 			      struct qdf_mac_addr gw_mac_addr)
@@ -205,6 +206,63 @@ static void hdd_nud_set_tracking(struct hdd_adapter *adapter,
 	adapter->nud_tracking.is_gw_rx_pkt_track_enabled = capture_enabled;
 }

+static void
+hdd_handle_nud_fail_sta(struct hdd_context *hdd_ctx,
+			struct hdd_adapter *adapter)
+{
+	struct reject_ap_info ap_info;
+
+	qdf_mutex_acquire(&adapter->disconnection_status_lock);
+	if (adapter->disconnection_in_progress) {
+		qdf_mutex_release(&adapter->disconnection_status_lock);
+		hdd_debug("Disconnect is in progress");
+		return;
+	}
+	qdf_mutex_release(&adapter->disconnection_status_lock);
+
+	if (hdd_is_roaming_in_progress(hdd_ctx)) {
+		hdd_debug("Roaming already in progress, cannot trigger roam.");
+		return;
+	}
+
+	hdd_debug("nud fail detected, try roaming to better BSSID, vdev id: %d",
+		  adapter->vdev_id);
+
+	ap_info.bssid = adapter->mac_addr;
+	ap_info.reject_ap_type = DRIVER_AVOID_TYPE;
+	ucfg_blm_add_bssid_to_reject_list(hdd_ctx->pdev, &ap_info);
+
+	if (roaming_offload_enabled(hdd_ctx))
+		sme_roam_invoke_nud_fail(hdd_ctx->mac_handle,
+					 adapter->vdev_id);
+}
+
+static void
+hdd_handle_nud_fail_non_sta(struct hdd_adapter *adapter)
+{
+	int status;
+
+	qdf_mutex_acquire(&adapter->disconnection_status_lock);
+	if (adapter->disconnection_in_progress) {
+		qdf_mutex_release(&adapter->disconnection_status_lock);
+		hdd_debug("Disconnect is in progress");
+		return;
+	}
+
+	adapter->disconnection_in_progress = true;
+	qdf_mutex_release(&adapter->disconnection_status_lock);
+
+	hdd_debug("Disconnecting vdev with vdev id: %d",
+		  adapter->vdev_id);
+	/* Issue Disconnect */
+	status = wlan_hdd_disconnect(adapter, eCSR_DISCONNECT_REASON_DEAUTH);
+	if (0 != status) {
+		hdd_err("wlan_hdd_disconnect failed, status: %d",
+			status);
+		hdd_set_disconnect_status(adapter, false);
+	}
+}
+
 /**
 * __hdd_nud_failure_work() - work for nud event
 * @adapter: Pointer to hdd_adapter
@@ -240,23 +298,11 @@ static void __hdd_nud_failure_work(struct hdd_adapter *adapter)
 		return;
 	}

-	qdf_mutex_acquire(&adapter->disconnection_status_lock);
-	if (adapter->disconnection_in_progress) {
-		qdf_mutex_release(&adapter->disconnection_status_lock);
-		hdd_debug("Disconnect is already in progress");
+	if (adapter->device_mode == QDF_STA_MODE) {
+		hdd_handle_nud_fail_sta(hdd_ctx, adapter);
 		return;
 	}
-	adapter->disconnection_in_progress = true;
-	qdf_mutex_release(&adapter->disconnection_status_lock);
-
-	hdd_debug("Disconnecting STA with session id: %d",
-		  adapter->vdev_id);
-	/* Issue Disconnect */
-	status = wlan_hdd_disconnect(adapter, eCSR_DISCONNECT_REASON_DEAUTH);
-	if (0 != status) {
-		hdd_err("wlan_hdd_disconnect failed, status: %d", status);
-		hdd_set_disconnect_status(adapter, false);
-	}
+	hdd_handle_nud_fail_non_sta(adapter);

 	hdd_exit();
 }
--- a/core/sme/inc/sme_api.h
+++ b/core/sme/inc/sme_api.h
@@ -2013,6 +2013,16 @@ QDF_STATUS sme_fast_reassoc(mac_handle_t mac_handle,
 			    struct csr_roam_profile *profile,
 			    const tSirMacAddr bssid, int channel,
 			    uint8_t vdev_id, const tSirMacAddr connected_bssid);
+
+/**
+ * sme_roam_invoke_nud_fail() - invokes REASSOC command to best available bssid
+ * @mac_handle: handle returned by mac_open
+ * @vdev_id: vdev id
+ *
+ * Return: QDF_STATUS
+ */
+QDF_STATUS sme_roam_invoke_nud_fail(mac_handle_t mac_handle, uint8_t vdev_id);
+
 #else
 static inline
 QDF_STATUS sme_fast_reassoc(mac_handle_t mac_handle,
@@ -2022,6 +2032,13 @@ QDF_STATUS sme_fast_reassoc(mac_handle_t mac_handle,
 {
 	return QDF_STATUS_SUCCESS;
 }
+
+static inline
+QDF_STATUS sme_roam_invoke_nud_fail(mac_handle_t mac_handle, uint8_t vdev_id)
+{
+	return QDF_STATUS_SUCCESS;
+}
+
 #endif
 /**
 * sme_congestion_register_callback() - registers congestion callback
--- a/core/sme/src/common/sme_api.c
+++ b/core/sme/src/common/sme_api.c
@@ -13813,18 +13813,22 @@ free_scan_flter:
 }

 #ifdef WLAN_FEATURE_ROAM_OFFLOAD
-QDF_STATUS sme_fast_reassoc(mac_handle_t mac_handle,
-			    struct csr_roam_profile *profile,
-			    const tSirMacAddr bssid, int channel,
-			    uint8_t vdev_id, const tSirMacAddr connected_bssid)
+QDF_STATUS sme_roam_invoke_nud_fail(mac_handle_t mac_handle, uint8_t vdev_id)
 {
-	QDF_STATUS status;
-	struct wma_roam_invoke_cmd *fastreassoc;
+	struct wma_roam_invoke_cmd *roam_invoke_params;
 	struct scheduler_msg msg = {0};
+	QDF_STATUS status;
+	struct wlan_objmgr_vdev *vdev;
 	struct mac_context *mac_ctx = MAC_CONTEXT(mac_handle);
+	struct mlme_roam_after_data_stall *vdev_roam_params;
 	struct csr_roam_session *session;
 	struct csr_roam_profile *roam_profile;

+	if (!mac_ctx->mlme_cfg->gen.data_stall_recovery_fw_support) {
+		sme_debug("FW does not support data stall recovery, aborting roam invoke");
+		return QDF_STATUS_E_NOSUPPORT;
+	}
+
 	session = CSR_GET_SESSION(mac_ctx, vdev_id);
 	if (!session || !session->pCurRoamProfile) {
 		sme_err("session %d not found", vdev_id);
@@ -13837,10 +13841,114 @@ QDF_STATUS sme_fast_reassoc(mac_handle_t mac_handle,
 			  roam_profile->driver_disabled_roaming);
 		return QDF_STATUS_E_FAILURE;
 	}
-	fastreassoc = qdf_mem_malloc(sizeof(*fastreassoc));
-	if (!fastreassoc)
-		return QDF_STATUS_E_NOMEM;

+	vdev = wlan_objmgr_get_vdev_by_id_from_psoc(mac_ctx->psoc, vdev_id,
+						    WLAN_LEGACY_SME_ID);
+
+	if (!vdev) {
+		sme_err("vdev is NULL, aborting roam invoke");
+		return QDF_STATUS_E_NULL_VALUE;
+	}
+
+	vdev_roam_params = mlme_get_roam_invoke_params(vdev);
+
+	if (!vdev_roam_params) {
+		sme_err("Invalid vdev roam params, aborting roam invoke");
+		wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
+		return QDF_STATUS_E_NULL_VALUE;
+	}
+
+	if (vdev_roam_params->roam_invoke_in_progress) {
+		sme_debug("Roaming in progress set by source = %d, aborting this roam invoke",
+			  vdev_roam_params->source);
+		wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
+		return QDF_STATUS_E_BUSY;
+	}
+
+	roam_invoke_params = qdf_mem_malloc(sizeof(*roam_invoke_params));
+	if (!roam_invoke_params) {
+		wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
+		return QDF_STATUS_E_NOMEM;
+	}
+	roam_invoke_params->vdev_id = vdev_id;
+	/* Set forced roaming as true so that FW scans all ch, and connect */
+	roam_invoke_params->forced_roaming = true;
+
+	msg.type = eWNI_SME_ROAM_INVOKE;
+	msg.reserved = 0;
+	msg.bodyptr = roam_invoke_params;
+	status = scheduler_post_message(QDF_MODULE_ID_SME,
+					QDF_MODULE_ID_PE,
+					QDF_MODULE_ID_PE, &msg);
+	if (QDF_IS_STATUS_ERROR(status)) {
+		sme_err("Not able to post ROAM_INVOKE_CMD message to PE");
+		qdf_mem_free(roam_invoke_params);
+	} else {
+		vdev_roam_params->roam_invoke_in_progress = true;
+		vdev_roam_params->source = CONNECTION_MGR_INITIATED;
+		sme_debug("Trigger roaming for vdev id %d source = CONNECTION_MGR_INITIATED",
+			  session->sessionId);
+	}
+	wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
+
+	return status;
+}
+
+QDF_STATUS sme_fast_reassoc(mac_handle_t mac_handle,
+			    struct csr_roam_profile *profile,
+			    const tSirMacAddr bssid, int channel,
+			    uint8_t vdev_id, const tSirMacAddr connected_bssid)
+{
+	QDF_STATUS status;
+	struct wma_roam_invoke_cmd *fastreassoc;
+	struct scheduler_msg msg = {0};
+	struct mac_context *mac_ctx = MAC_CONTEXT(mac_handle);
+	struct csr_roam_session *session;
+	struct csr_roam_profile *roam_profile;
+	struct wlan_objmgr_vdev *vdev;
+	struct mlme_roam_after_data_stall *vdev_roam_params;
+
+	session = CSR_GET_SESSION(mac_ctx, vdev_id);
+	if (!session || !session->pCurRoamProfile) {
+		sme_err("session %d not found", vdev_id);
+		return QDF_STATUS_E_FAILURE;
+	}
+
+	roam_profile = session->pCurRoamProfile;
+	if (roam_profile->driver_disabled_roaming) {
+		sme_debug("roaming status in driver %d",
+			  roam_profile->driver_disabled_roaming);
+		return QDF_STATUS_E_FAILURE;
+	}
+
+	vdev = wlan_objmgr_get_vdev_by_id_from_psoc(mac_ctx->psoc, vdev_id,
+						    WLAN_LEGACY_SME_ID);
+
+	if (!vdev) {
+		sme_err("vdev is NULL, aborting roam invoke");
+		return QDF_STATUS_E_NULL_VALUE;
+	}
+
+	vdev_roam_params = mlme_get_roam_invoke_params(vdev);
+
+	if (!vdev_roam_params) {
+		sme_err("Invalid vdev roam params, aborting roam invoke");
+		wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
+		return QDF_STATUS_E_NULL_VALUE;
+	}
+
+	if (vdev_roam_params->roam_invoke_in_progress) {
+		sme_debug("Roaming in progress set by source = %d, aborting this roam invoke",
+			  vdev_roam_params->source);
+		wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
+		return QDF_STATUS_E_FAILURE;
+	}
+
+	fastreassoc = qdf_mem_malloc(sizeof(*fastreassoc));
+	if (!fastreassoc) {
+		wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
+		return QDF_STATUS_E_NOMEM;
+	}
 	/* if both are same then set the flag */
 	if (!qdf_mem_cmp(connected_bssid, bssid, ETH_ALEN)) {
 		fastreassoc->is_same_bssid = true;
@@ -13861,13 +13969,18 @@ QDF_STATUS sme_fast_reassoc(mac_handle_t mac_handle,

 	if (!channel) {
 		sme_err("channel retrieval from BSS desc fails!");
+		qdf_mem_free(fastreassoc->frame_buf);
+		fastreassoc->frame_buf = NULL;
+		fastreassoc->frame_len = 0;
 		qdf_mem_free(fastreassoc);
+		wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
 		return QDF_STATUS_E_FAULT;
 	}

 	fastreassoc->channel = channel;
 	if (QDF_STATUS_SUCCESS != status) {
 		sme_warn("sme_get_beacon_frm failed");
+		qdf_mem_free(fastreassoc->frame_buf);
 		fastreassoc->frame_buf = NULL;
 		fastreassoc->frame_len = 0;
 	}
@@ -13888,11 +14001,20 @@ QDF_STATUS sme_fast_reassoc(mac_handle_t mac_handle,
 	status = scheduler_post_message(QDF_MODULE_ID_SME,
 					QDF_MODULE_ID_PE,
 					QDF_MODULE_ID_PE, &msg);
-	if (QDF_STATUS_SUCCESS != status) {
+	if (QDF_IS_STATUS_ERROR(status)) {
 		sme_err("Not able to post ROAM_INVOKE_CMD message to PE");
+		qdf_mem_free(fastreassoc->frame_buf);
+		fastreassoc->frame_buf = NULL;
+		fastreassoc->frame_len = 0;
 		qdf_mem_free(fastreassoc);
+	} else {
+		vdev_roam_params->roam_invoke_in_progress = true;
+		vdev_roam_params->source = USERSPACE_INITIATED;
+		sme_debug("Trigger roaming for vdev id %d source = USERSPACE_INITIATED",
+			  session->sessionId);
 	}

+	wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
 	return status;
 }
 #endif
--- a/core/sme/src/csr/csr_api_roam.c
+++ b/core/sme/src/csr/csr_api_roam.c
@@ -21611,13 +21611,35 @@ static QDF_STATUS csr_process_roam_sync_callback(struct mac_context *mac_ctx,
 	struct ht_profile *src_profile = NULL;
 	tCsrRoamHTProfile *dst_profile = NULL;
 #endif
+	struct wlan_objmgr_vdev *vdev;
+	struct mlme_roam_after_data_stall *vdev_roam_params;
+
+	vdev = wlan_objmgr_get_vdev_by_id_from_psoc(mac_ctx->psoc, session_id,
+						    WLAN_LEGACY_SME_ID);
+
+	if (!vdev) {
+		sme_err("vdev is NULL, aborting roam invoke");
+		return QDF_STATUS_E_NULL_VALUE;
+	}
+
+	vdev_roam_params = mlme_get_roam_invoke_params(vdev);
+
+	if (!vdev_roam_params) {
+		sme_err("Invalid vdev roam params, aborting roam invoke");
+		status = QDF_STATUS_E_NULL_VALUE;
+		goto end;
+	}

 	if (!session) {
 		sme_err("LFR3: Session not found");
-		return QDF_STATUS_E_FAILURE;
+		status = QDF_STATUS_E_NULL_VALUE;
+		goto end;
 	}

-	sme_debug("LFR3: reason: %d", reason);
+	sme_debug("LFR3: reason: %d roaming in progress %d, source %d", reason,
+		  vdev_roam_params->roam_invoke_in_progress,
+		  vdev_roam_params->source);
+
 	switch (reason) {
 	case SIR_ROAMING_DEREGISTER_STA:
 		/*
@@ -21631,11 +21653,12 @@ static QDF_STATUS csr_process_roam_sync_callback(struct mac_context *mac_ctx,
 			QDF_TRACE(QDF_MODULE_ID_SME, QDF_TRACE_LEVEL_DEBUG,
 				FL("LFR3: Session not in connected state or disconnect is in progress %d"),
 				session->discon_in_progress);
-			return QDF_STATUS_E_FAILURE;
+			status = QDF_STATUS_E_FAILURE;
+			goto end;
 		}
 		csr_roam_call_callback(mac_ctx, session_id, NULL, 0,
 				eCSR_ROAM_FT_START, eCSR_ROAM_RESULT_SUCCESS);
-		return status;
+		goto end;
 	case SIR_ROAMING_START:
 		csr_roam_roaming_offload_timer_action(mac_ctx,
 				CSR_ROAMING_OFFLOAD_TIMEOUT_PERIOD, session_id,
@@ -21644,23 +21667,28 @@ static QDF_STATUS csr_process_roam_sync_callback(struct mac_context *mac_ctx,
 				eCSR_ROAM_START, eCSR_ROAM_RESULT_SUCCESS);
 		wlan_abort_scan(mac_ctx->pdev, INVAL_PDEV_ID,
 				session_id, INVAL_SCAN_ID, false);
-		return status;
+		goto end;
 	case SIR_ROAMING_ABORT:
 		csr_roam_roaming_offload_timer_action(mac_ctx,
 				0, session_id, ROAMING_OFFLOAD_TIMER_STOP);
 		csr_roam_call_callback(mac_ctx, session_id, NULL, 0,
 				eCSR_ROAM_ABORT, eCSR_ROAM_RESULT_SUCCESS);
-		return status;
+		vdev_roam_params->roam_invoke_in_progress = false;
+		goto end;
 	case SIR_ROAM_SYNCH_NAPI_OFF:
 		csr_roam_call_callback(mac_ctx, session_id, NULL, 0,
 				eCSR_ROAM_NAPI_OFF, eCSR_ROAM_RESULT_SUCCESS);
-		return status;
+		goto end;
 	case SIR_ROAMING_INVOKE_FAIL:
-		/* Userspace roam request failed, disconnect with current AP */
-		sme_debug("LFR3: roam invoke from user-space fail, dis cur AP");
-		csr_roam_disconnect(mac_ctx, session_id,
-				    eCSR_DISCONNECT_REASON_DEAUTH);
-		return status;
+		if (vdev_roam_params->source == USERSPACE_INITIATED) {
+			/* Userspace roam req fail, disconnect with AP */
+			sme_debug("LFR3: roam invoke from user-space fail, dis cur AP");
+			csr_roam_disconnect(mac_ctx, session_id,
+					    eCSR_DISCONNECT_REASON_DEAUTH);
+		}
+		sme_debug("Roaming Failed, clearing roam invoke in progress");
+		vdev_roam_params->roam_invoke_in_progress = false;
+		goto end;
 	case SIR_ROAM_SYNCH_PROPAGATION:
 		break;
 	case SIR_ROAM_SYNCH_COMPLETE:
@@ -21703,10 +21731,13 @@ static QDF_STATUS csr_process_roam_sync_callback(struct mac_context *mac_ctx,
 		csr_roam_call_callback(mac_ctx, session_id, NULL, 0,
 				       eCSR_ROAM_SYNCH_COMPLETE,
 				       eCSR_ROAM_RESULT_SUCCESS);
-		return status;
+		vdev_roam_params->roam_invoke_in_progress = false;
+		goto end;
 	default:
 		sme_debug("LFR3: callback reason %d", reason);
-		return QDF_STATUS_E_FAILURE;
+		vdev_roam_params->roam_invoke_in_progress = false;
+		status = QDF_STATUS_E_FAILURE;
+		goto end;
 	}
 	session->roam_synch_in_progress = true;
 	session->roam_synch_data = roam_synch_data;
@@ -21715,7 +21746,7 @@ static QDF_STATUS csr_process_roam_sync_callback(struct mac_context *mac_ctx,
 	if (!QDF_IS_STATUS_SUCCESS(status)) {
 		sme_err("LFR3: fail to parse IEs");
 		session->roam_synch_in_progress = false;
-		return status;
+		goto end;
 	}

 	conn_profile = &session->connectedProfile;
@@ -21731,7 +21762,8 @@ static QDF_STATUS csr_process_roam_sync_callback(struct mac_context *mac_ctx,
 	if (!roam_info) {
 		session->roam_synch_in_progress = false;
 		qdf_mem_free(ies_local);
-		return QDF_STATUS_E_NOMEM;
+		status = QDF_STATUS_E_NOMEM;
+		goto end;
 	}
 	csr_rso_save_ap_to_scan_cache(mac_ctx, roam_synch_data, bss_desc);
 	roam_info->sessionId = session_id;
@@ -21835,7 +21867,8 @@ static QDF_STATUS csr_process_roam_sync_callback(struct mac_context *mac_ctx,
 		if (roam_info)
 			qdf_mem_free(roam_info);
 		qdf_mem_free(ies_local);
-		return QDF_STATUS_E_NOMEM;
+		status = QDF_STATUS_E_NOMEM;
+		goto end;
 	}
 	qdf_mem_copy(roam_info->pbFrames,
 			(uint8_t *)roam_synch_data +
@@ -21996,6 +22029,9 @@ static QDF_STATUS csr_process_roam_sync_callback(struct mac_context *mac_ctx,
 	qdf_mem_free(roam_info);
 	qdf_mem_free(ies_local);

+end:
+	wlan_objmgr_vdev_release_ref(vdev, WLAN_LEGACY_SME_ID);
+
 	return status;
 }

--- a/core/wma/inc/wma.h
+++ b/core/wma/inc/wma.h
@@ -1600,6 +1600,7 @@ enum uapsd_up {
 * @frame_len: frame length, includs mac header, fixed params and ies
 * @frame_buf: buffer contaning probe response or beacon
 * @is_same_bssid: flag to indicate if roaming is requested for same bssid
+ * @forced_roaming: Roaming to be done without giving bssid, and channel.
 */
 struct wma_roam_invoke_cmd {
 	uint32_t vdev_id;
@@ -1608,6 +1609,7 @@ struct wma_roam_invoke_cmd {
 	uint32_t frame_len;
 	uint8_t *frame_buf;
 	uint8_t is_same_bssid;
+	bool forced_roaming;
 };

 /**
--- a/core/wma/src/wma_main.c
+++ b/core/wma/src/wma_main.c
@@ -5610,6 +5610,19 @@ wma_fill_chain_cfg(struct target_psoc_info *tgt_hdl,
 		mac_ctx->fw_chain_cfg.max_rx_chains_5g = num_chain;
 }

+static void wma_update_mlme_related_tgt_caps(struct wlan_objmgr_psoc *psoc,
+					     struct wmi_unified *wmi_handle)
+{
+	struct mlme_tgt_caps mlme_tgt_cfg;
+
+	mlme_tgt_cfg.data_stall_recovery_fw_support =
+		wmi_service_enabled(wmi_handle,
+				    wmi_service_data_stall_recovery_support);
+
+	/* Call this at last only after filling all the tgt caps */
+	wlan_mlme_update_cfg_with_tgt_caps(psoc, &mlme_tgt_cfg);
+}
+
 /**
 * wma_update_hdd_cfg() - update HDD config
 * @wma_handle: wma handle
@@ -5649,6 +5662,7 @@ static int wma_update_hdd_cfg(tp_wma_handle wma_handle)
 		return -EINVAL;
 	}

+	wma_update_mlme_related_tgt_caps(wma_handle->psoc, wmi_handle);
 	qdf_mem_zero(&tgt_cfg, sizeof(struct wma_tgt_cfg));

 	tgt_cfg.sub_20_support = wma_handle->sub_20_support;