ath10k: improve vdev map handling

Check vdev map has space before calling ffs,
fix invalid cleanup in failure to create vdev
case.

Open-code the BIT() logic since BIT does not properly
handle 64-bit bitfields and future patches will make
use of larger bitfields.
Signed-off-by: Ben Greear <greearb@candelatech.com>
Signed-off-by: Kalle Valo <kvalo@qca.qualcomm.com>

drivers/net/wireless/ath/ath10k/mac.c

@@ -597,14 +597,14 @@ static int ath10k_monitor_vdev_create(struct ath10k *ar)
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	bit = ffs(ar->free_vdev_map);
-	if (bit == 0) {
+	if (ar->free_vdev_map == 0) {
 		ath10k_warn("failed to find free vdev id for monitor vdev\n");
 		return -ENOMEM;
 	}
 
+	bit = ffs(ar->free_vdev_map);
+
 	ar->monitor_vdev_id = bit - 1;
-	ar->free_vdev_map &= ~(1 << ar->monitor_vdev_id);
 
 	ret = ath10k_wmi_vdev_create(ar, ar->monitor_vdev_id,
 				     WMI_VDEV_TYPE_MONITOR,
@@ -612,20 +612,14 @@ static int ath10k_monitor_vdev_create(struct ath10k *ar)
 	if (ret) {
 		ath10k_warn("failed to request monitor vdev %i creation: %d\n",
 			    ar->monitor_vdev_id, ret);
-		goto vdev_fail;
+		return ret;
 	}
 
+	ar->free_vdev_map &= ~(1 << ar->monitor_vdev_id);
 	ath10k_dbg(ATH10K_DBG_MAC, "mac monitor vdev %d created\n",
 		   ar->monitor_vdev_id);
 
 	return 0;
-
-vdev_fail:
-	/*
-	 * Restore the ID to the global map.
-	 */
-	ar->free_vdev_map |= 1 << (ar->monitor_vdev_id);
-	return ret;
 }
 
 static int ath10k_monitor_vdev_delete(struct ath10k *ar)
@@ -641,7 +635,7 @@ static int ath10k_monitor_vdev_delete(struct ath10k *ar)
 		return ret;
 	}
 
-	ar->free_vdev_map |= 1 << (ar->monitor_vdev_id);
+	ar->free_vdev_map |= 1 << ar->monitor_vdev_id;
 
 	ath10k_dbg(ATH10K_DBG_MAC, "mac monitor vdev %d deleted\n",
 		   ar->monitor_vdev_id);
@@ -2780,11 +2774,12 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	INIT_WORK(&arvif->wep_key_work, ath10k_tx_wep_key_work);
 	INIT_LIST_HEAD(&arvif->list);
 
-	bit = ffs(ar->free_vdev_map);
-	if (bit == 0) {
+	if (ar->free_vdev_map == 0) {
+		ath10k_warn("Free vdev map is empty, no more interfaces allowed.\n");
 		ret = -EBUSY;
 		goto err;
 	}
+	bit = ffs(ar->free_vdev_map);
 
 	arvif->vdev_id = bit - 1;
 	arvif->vdev_subtype = WMI_VDEV_SUBTYPE_NONE;
@@ -2827,7 +2822,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		goto err;
 	}
 
-	ar->free_vdev_map &= ~BIT(arvif->vdev_id);
+	ar->free_vdev_map &= ~(1 << arvif->vdev_id);
 	list_add(&arvif->list, &ar->arvifs);
 
 	vdev_param = ar->wmi.vdev_param->def_keyid;
@@ -2920,7 +2915,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 
 err_vdev_delete:
 	ath10k_wmi_vdev_delete(ar, arvif->vdev_id);
-	ar->free_vdev_map &= ~BIT(arvif->vdev_id);
+	ar->free_vdev_map |= 1 << arvif->vdev_id;
 	list_del(&arvif->list);
 
 err:
@@ -2956,7 +2951,7 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 		ath10k_warn("failed to stop spectral for vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 
-	ar->free_vdev_map |= 1 << (arvif->vdev_id);
+	ar->free_vdev_map |= 1 << arvif->vdev_id;
 	list_del(&arvif->list);
 
 	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {