// SPDX-License-Identifier: GPL-2.0-only
/*
 * mac80211_hwsim_nan - NAN software simulation for mac80211_hwsim
 * Copyright (C) 2025-2026 Intel Corporation
 */

#include <net/cfg80211.h>
#include "mac80211_hwsim_i.h"

/* Defined as the lower 23 bits being zero */
#define DW0_TSF_MASK		GENMASK(22, 0)

/* DWs are repeated every 512 TUs */
#define DWST_TU			512
#define DWST_TSF_MASK		(ieee80211_tu_to_usec(DWST_TU) - 1)

#define SLOT_TU			16
#define SLOT_TSF_MASK		(ieee80211_tu_to_usec(DWST_TU) - 1)

/* The 2.4 GHz DW is at the start, the 5 GHz is in slot 8 (after 128 TUs) */
#define DW_5G_OFFSET_TU		128

#define SLOT_24GHZ_DW		0
#define SLOT_5GHZ_DW		(DW_5G_OFFSET_TU / SLOT_TU)

/* The special DW0 happens every 16 DWSTs (8192 TUs) */
static_assert(16 * DWST_TU * 1024 == 8192 * 1024);
static_assert(DW0_TSF_MASK + 1 == 8192 * 1024);

/* warmup phase should be 120 seconds, which is approximately 225 DWSTs */
#define NAN_WARMUP_DWST		225

#define NAN_RSSI_CLOSE (-60)
#define NAN_RSSI_MIDDLE (-75)

/* Quiet time at the end of each slot where TX is suppressed */
#define NAN_CHAN_SWITCH_TIME_US		256

struct hwsim_nan_sta_iter_ctx {
	struct ieee80211_hw *hw;
	bool can_tx;
};

static void mac80211_hwsim_nan_resume_txqs(struct mac80211_hwsim_data *data);

static u64 hwsim_nan_get_timer_tsf(struct mac80211_hwsim_data *data)
{
	ktime_t expires = hrtimer_get_expires(&data->nan.slot_timer);

	return mac80211_hwsim_boottime_to_tsf(data, expires);
}

static u8 hwsim_nan_slot_from_tsf(u64 tsf)
{
	return (tsf & DWST_TSF_MASK) / ieee80211_tu_to_usec(SLOT_TU);
}

static u64 hwsim_nan_encode_master_rank(u8 master_pref, u8 random_factor,
					const u8 *addr)
{
	return ((u64)master_pref << 56) +
		((u64)random_factor << 48) +
		((u64)addr[5] << 40) +
		((u64)addr[4] << 32) +
		((u64)addr[3] << 24) +
		((u64)addr[2] << 16) +
		((u64)addr[1] << 8) +
		((u64)addr[0] << 0);
}

static u64 hwsim_nan_get_master_rank(struct mac80211_hwsim_data *data)
{
	u8 master_pref = 0;
	u8 random_factor = 0;

	if (data->nan.phase == MAC80211_HWSIM_NAN_PHASE_UP) {
		master_pref = data->nan.master_pref;
		random_factor = data->nan.random_factor;
	}

	return hwsim_nan_encode_master_rank(master_pref, random_factor,
					    data->nan.device_vif->addr);
}

static bool mac80211_hwsim_nan_is_dw_slot(struct mac80211_hwsim_data *data,
					  u8 slot)
{
	return slot == SLOT_24GHZ_DW ||
		(slot == SLOT_5GHZ_DW &&
		 (data->nan.bands & BIT(NL80211_BAND_5GHZ)));
}

static bool
hwsim_nan_rx_chandef_compatible(struct mac80211_hwsim_data *data, u8 slot,
				struct ieee80211_channel *rx_chan, u8 rx_bw)
{
	static const int bw_to_mhz[] = {
		[RATE_INFO_BW_20] = 20, [RATE_INFO_BW_40] = 40,
		[RATE_INFO_BW_80] = 80, [RATE_INFO_BW_160] = 160,
	};
	struct cfg80211_chan_def sched_chandef;
	int rx_mhz, sched_mhz;

	scoped_guard(spinlock_bh, &data->nan.state_lock)
		sched_chandef = data->nan.local_sched[slot];

	if (!sched_chandef.chan ||
	    sched_chandef.chan->center_freq != rx_chan->center_freq)
		return false;

	if (rx_bw >= ARRAY_SIZE(bw_to_mhz) ||!bw_to_mhz[rx_bw])
		return false;

	rx_mhz = bw_to_mhz[rx_bw];
	sched_mhz = cfg80211_chandef_get_width(&sched_chandef);

	/* Accept RX at narrower or equal bandwidth */
	return rx_mhz <= sched_mhz;
}

static bool hwsim_nan_peer_present_in_dw(struct hwsim_sta_priv *sp, u64 tsf)
{
	u8 slot = hwsim_nan_slot_from_tsf(tsf);
	u8 cdw = 0;
	u8 dw_index, wake_interval;
	u16 committed_dw;

	scoped_guard(spinlock_bh, &sp->nan_sched.lock)
		committed_dw = sp->nan_sched.committed_dw;

	/* If peer doesn't advertise committed DW, assume presence in
	 * all 2.4 GHz DW slots
	 */
	if (!committed_dw)
		return slot == SLOT_24GHZ_DW;

	/* Get DW index (0-15) within the 16-DWST DW0 cycle */
	dw_index = (tsf / ieee80211_tu_to_usec(DWST_TU)) & 0xf;

	/* Extract CDW for the appropriate band (spec Table 80) */
	if (slot == SLOT_24GHZ_DW)
		cdw = committed_dw & 0x7;
	else if (slot == SLOT_5GHZ_DW)
		cdw = (committed_dw >> 3) & 0x7;

	if (cdw == 0)
		return false;

	/* Peer wakes every 2^(cdw-1) DWs: 1, 2, 4, 8, or 16 */
	wake_interval = 1 << (cdw - 1);

	return (dw_index % wake_interval) == 0;
}

static bool
hwsim_nan_peer_present_in_faw(struct hwsim_sta_priv *sp,
			      struct mac80211_hwsim_data *data, u8 slot)
{
	struct cfg80211_chan_def local_chandef;

	scoped_guard(spinlock_bh, &data->nan.state_lock)
		local_chandef = data->nan.local_sched[slot];

	if (!local_chandef.chan)
		return false;

	scoped_guard(spinlock_bh, &sp->nan_sched.lock) {
		for (int i = 0; i < CFG80211_NAN_MAX_PEER_MAPS; i++) {
			struct cfg80211_chan_def *peer_chandef;

			if (sp->nan_sched.maps[i].map_id ==
			    CFG80211_NAN_INVALID_MAP_ID)
				continue;

			peer_chandef = &sp->nan_sched.maps[i].chans[slot];
			if (!peer_chandef->chan)
				continue;

			if (cfg80211_chandef_compatible(&local_chandef,
							peer_chandef))
				return true;
		}
	}

	return false;
}

static void
mac80211_hwsim_nan_schedule_slot(struct mac80211_hwsim_data *data, u8 slot,
				 bool discontinuity)
{
	u64 tsf;

	if (!discontinuity)
		tsf = hwsim_nan_get_timer_tsf(data);
	else
		tsf = mac80211_hwsim_get_tsf(data->hw, data->nan.device_vif);

	/* Only called by mac80211_hwsim_nan_dw_timer from softirq context */
	lockdep_assert_in_softirq();

	tsf &= ~DWST_TSF_MASK;
	tsf += ieee80211_tu_to_usec(slot * SLOT_TU);

	hrtimer_set_expires(&data->nan.slot_timer,
			    mac80211_hwsim_tsf_to_boottime(data, tsf));
}

void mac80211_hwsim_nan_rx(struct ieee80211_hw *hw,
			   struct sk_buff *skb)
{
	struct mac80211_hwsim_data *data = hw->priv;
	const struct ieee80211_mgmt *mgmt = (void *)skb->data;
	struct element *nan_elem = (void *)mgmt->u.beacon.variable;
	struct ieee80211_nan_anchor_master_info *ami = NULL;
	const struct ieee80211_nan_attr *nan_attr;
	struct ieee80211_rx_status rx_status;
	bool joined_cluster = false;
	bool adopt_tsf = false;
	bool is_sync_beacon;
	bool is_same_cluster;
	u64 master_rank = 0;
	ssize_t data_len;
	u8 slot;

	/* Need a NAN vendor element at the start */
	if (skb->len < (offsetofend(struct ieee80211_mgmt, u.beacon) + 6) ||
	    !ieee80211_is_beacon(mgmt->frame_control))
		return;

	data_len = skb->len - offsetofend(struct ieee80211_mgmt, u.beacon);

	/* Copy the RX status to add a MAC timestamp if needed */
	memcpy(&rx_status, IEEE80211_SKB_RXCB(skb),
	       sizeof(struct ieee80211_rx_status));

	/* And deal with the lack of mac time stamp */
	if ((rx_status.flag & RX_FLAG_MACTIME) != RX_FLAG_MACTIME_START) {
		u64 tsf = mac80211_hwsim_get_tsf(hw, data->nan.device_vif);;

		/* In that case there should be no timestamp */
		WARN_ON_ONCE(rx_status.flag & RX_FLAG_MACTIME);

		/* No mac timestamp, set current TSF for the frame end */
		rx_status.flag |= RX_FLAG_MACTIME_END;
		rx_status.mactime = tsf;

		/* And translate to the start for the rest of the code */
		rx_status.mactime =
			ieee80211_calculate_rx_timestamp(hw, &rx_status,
							 skb->len, 0);
		rx_status.flag &= ~RX_FLAG_MACTIME;
		rx_status.flag |= RX_FLAG_MACTIME_START;

		/* Match mac80211_hwsim_nan_receive, see comment there */
		slot = hwsim_nan_slot_from_tsf(tsf + 128);
	} else {
		slot = hwsim_nan_slot_from_tsf(rx_status.mactime);
	}

	/*
	 * (overly) simplify things, only track 2.4 GHz here. Also, ignore
	 * frames outside of the 2.4 GHz DW slot, unless in the initial SCAN
	 * phase.
	 */
	if ((slot != SLOT_24GHZ_DW &&
	     data->nan.phase != MAC80211_HWSIM_NAN_PHASE_SCAN) ||
	    rx_status.freq != 2437)
		return;

	/* Just ignore low RSSI beacons that we cannot sync to */
	if (rx_status.signal < NAN_RSSI_MIDDLE)
		return;

	/* Needs to be a valid NAN cluster ID in A3 */
	if (get_unaligned_be32(mgmt->bssid) != ((WLAN_OUI_WFA << 8) | 0x01))
		return;

	/* We are only interested in NAN beacons */
	if (nan_elem->id != WLAN_EID_VENDOR_SPECIFIC ||
	    nan_elem->datalen < 4 ||
	    get_unaligned_be32(nan_elem->data) !=
	    (WLAN_OUI_WFA << 8 | WLAN_OUI_TYPE_WFA_NAN))
		return;

	u8 *nan_defragmented __free(kfree) = kzalloc(data_len, GFP_ATOMIC);
	if (!nan_defragmented)
		return;

	data_len = cfg80211_defragment_element(nan_elem,
					       mgmt->u.beacon.variable,
					       data_len,
					       nan_defragmented, data_len,
					       WLAN_EID_FRAGMENT);

	if (data_len < 0)
		return;

	/* Assume it is a synchronization beacon if beacon_int is 512 TUs */
	is_sync_beacon = le16_to_cpu(mgmt->u.beacon.beacon_int) == DWST_TU;
	is_same_cluster = ether_addr_equal(mgmt->bssid, data->nan.cluster_id);

	for_each_nan_attr(nan_attr, nan_defragmented + 4, data_len - 4) {
		if (nan_attr->attr == NAN_ATTR_MASTER_INDICATION &&
		    le16_to_cpu(nan_attr->length) >=
		    sizeof(struct ieee80211_nan_master_indication)) {
			struct ieee80211_nan_master_indication *mi =
				(void *)nan_attr->data;

			master_rank =
				hwsim_nan_encode_master_rank(mi->master_pref,
							     mi->random_factor,
							     mgmt->sa);
		}

		if (nan_attr->attr == NAN_ATTR_CLUSTER_INFO &&
		    le16_to_cpu(nan_attr->length) >=
		    sizeof(struct ieee80211_nan_anchor_master_info)) {
			ami = (void *)nan_attr->data;

			/*
			 * The AMBTT should be set to the beacon timestamp when
			 * the sender is the anchor master. We can simply
			 * modify the structure because we created a copy when
			 * defragmenting the NAN element.
			 */
			if (ami->hop_count == 0)
				ami->ambtt = cpu_to_le32(
					le64_to_cpu(mgmt->u.beacon.timestamp));
		}
	}

	/* Do the rest of the processing under lock */
	spin_lock_bh(&data->nan.state_lock);

	/*
	 * sync beacon should be discarded if the master rank is the same
	 * and the AMBTT is older than 16 * 512 TUs compared to our own TSF.
	 *
	 * Subtract the AMBTT from the lowered TSF. If the AMBTT is older
	 * (smaller) then the calculation will not underflow.
	 */
	if (is_sync_beacon && ami &&
	    ami->master_rank == data->nan.current_ami.master_rank &&
	    (((u32)rx_status.mactime -
	      ieee80211_tu_to_usec(16 * 512)) -
	     le32_to_cpu(ami->ambtt)) < 0x8000000) {
		wiphy_dbg(hw->wiphy,
			  "NAN: ignoring sync beacon with old AMBTT\n");
		is_sync_beacon = false;
	}

	if (is_same_cluster && is_sync_beacon &&
	    master_rank > hwsim_nan_get_master_rank(data)) {
		if (rx_status.signal > NAN_RSSI_CLOSE)
			data->nan.master_transition_score += 3;
		else
			data->nan.master_transition_score += 1;
	}

	if (is_same_cluster && is_sync_beacon && ami &&
	    ((ami->master_rank == data->nan.current_ami.master_rank &&
	      ami->hop_count < data->nan.current_ami.hop_count) ||
	     (master_rank > hwsim_nan_get_master_rank(data) &&
	      ami->hop_count == data->nan.current_ami.hop_count))) {
		if (rx_status.signal > NAN_RSSI_CLOSE)
			data->nan.sync_transition_score += 3;
		else
			data->nan.sync_transition_score += 1;
	}

	/*
	 * Decide on TSF adjustments before updating any other state
	 */
	if (is_same_cluster && is_sync_beacon && ami &&
	    data->nan.current_ami.hop_count != 0) {
		if (le64_to_cpu(ami->master_rank) >
		    le64_to_cpu(data->nan.current_ami.master_rank) &&
		    ami->master_rank != data->nan.last_ami.master_rank)
			adopt_tsf = true;

		if (le64_to_cpu(ami->master_rank) >
		    le64_to_cpu(data->nan.current_ami.master_rank) &&
		    ami->master_rank == data->nan.last_ami.master_rank &&
		    le32_to_cpu(ami->ambtt) >
		    le32_to_cpu(data->nan.last_ami.ambtt))
			adopt_tsf = true;

		if (le64_to_cpu(ami->master_rank) <
		    le64_to_cpu(data->nan.current_ami.master_rank) &&
		    le64_to_cpu(ami->master_rank) >
		    hwsim_nan_get_master_rank(data) &&
		    ether_addr_equal(ami->master_addr,
				     data->nan.current_ami.master_addr))
			adopt_tsf = true;

		if (ami->master_rank == data->nan.current_ami.master_rank &&
		    le32_to_cpu(ami->ambtt) >
		    le32_to_cpu(data->nan.current_ami.ambtt))
			adopt_tsf = true;

		/* Anchor Master case is handled below */
	}

	/*
	 * NAN Cluster merging
	 */
	if (!is_same_cluster && ami) {
		u64 curr_amr;
		u64 own_cg;
		u64 frame_amr;
		u64 cg;

		/* Shifted down by 19 bits compared to spec */
		frame_amr = le64_to_cpu(ami->master_rank);
		cg = (u64)ami->master_pref << (64 - 19);
		cg += le64_to_cpu(mgmt->u.beacon.timestamp) >> 19;

		curr_amr = le64_to_cpu(data->nan.current_ami.master_rank);
		own_cg = (u64)data->nan.current_ami.master_pref << (64 - 19);
		own_cg += rx_status.mactime >> 19;

		/*
		 * Check if the cluster shall be joined
		 *
		 * When in the "scan" phase, just join immediately.
		 */
		if (cg > own_cg ||
		    (cg == own_cg && frame_amr > curr_amr) ||
		    data->nan.phase == MAC80211_HWSIM_NAN_PHASE_SCAN) {
			/* Avoid a state transition */
			data->nan.master_transition_score = 0;
			data->nan.sync_transition_score = 0;

			/*
			 * NOTE: The spec says we should TX sync beacons on the
			 * old schedule after joining. We do not implement this.
			 */

			wiphy_dbg(hw->wiphy, "NAN: joining cluster %pM\n",
				  mgmt->bssid);

			joined_cluster = true;
			adopt_tsf = true;

			memcpy(&data->nan.last_ami, &data->nan.current_ami,
			       sizeof(data->nan.last_ami));
			memcpy(&data->nan.current_ami, ami,
			       sizeof(data->nan.last_ami));
			data->nan.current_ami.hop_count += 1;

			memcpy(data->nan.cluster_id, mgmt->bssid, ETH_ALEN);

			/*
			 * Assume we are UP if we joined a cluster.
			 *
			 * If the other anchor master is still in the warmup
			 * phase, then we may temporarily become the anchor
			 * master until it sets its own master preference to
			 * be non-zero.
			 */
			data->nan.phase = MAC80211_HWSIM_NAN_PHASE_UP;
			data->nan.random_factor_valid_dwst = 0;
		}
	}

	/*
	 * Anchor master selection
	 */
	/* We are not anchor master */
	if (is_same_cluster && is_sync_beacon && ami &&
	    data->nan.current_ami.hop_count != 0) {
		if (le64_to_cpu(data->nan.current_ami.master_rank) <
		    le64_to_cpu(ami->master_rank)) {
			if (ami->master_rank == data->nan.last_ami.master_rank &&
			    le32_to_cpu(ami->ambtt) <=
			    le32_to_cpu(data->nan.last_ami.ambtt)) {
				/* disregard frame */
			} else {
				memcpy(&data->nan.last_ami,
				       &data->nan.current_ami,
				       sizeof(data->nan.last_ami));
				memcpy(&data->nan.current_ami, ami,
				       sizeof(data->nan.last_ami));
				data->nan.current_ami.hop_count += 1;
			}
		}

		if (le64_to_cpu(data->nan.current_ami.master_rank) >
		    le64_to_cpu(ami->master_rank)) {
			if (!ether_addr_equal(data->nan.current_ami.master_addr,
					      ami->master_addr)) {
				/* disregard frame */
			} else {
				u64 amr = hwsim_nan_get_master_rank(data);

				if (amr > le64_to_cpu(ami->master_rank)) {
					/* assume ourselves as anchor master */
					wiphy_dbg(hw->wiphy,
						  "NAN: assume anchor master role\n");
					data->nan.current_ami.master_rank =
						cpu_to_le64(amr);
					data->nan.current_ami.hop_count = 0;
					memset(&data->nan.last_ami, 0,
					       sizeof(data->nan.last_ami));
					data->nan.last_ami.ambtt =
						data->nan.current_ami.ambtt;
					data->nan.current_ami.ambtt = 0;
				} else {
					memcpy(&data->nan.last_ami,
					       &data->nan.current_ami,
					       sizeof(data->nan.last_ami));
					memcpy(&data->nan.current_ami, ami,
					       sizeof(data->nan.last_ami));
					data->nan.current_ami.hop_count += 1;
				}
			}
		}

		if (data->nan.current_ami.master_rank == ami->master_rank) {
			if (le32_to_cpu(data->nan.current_ami.ambtt) <
			    le32_to_cpu(ami->ambtt)) {
				data->nan.current_ami.ambtt = ami->ambtt;
			}

			if (data->nan.current_ami.hop_count >
			    ami->hop_count + 1) {
				data->nan.current_ami.hop_count =
					ami->hop_count + 1;
			}
		}
	}

	/* We are anchor master */
	if (is_same_cluster && is_sync_beacon && ami &&
	    data->nan.current_ami.hop_count == 0) {
		WARN_ON_ONCE(!ether_addr_equal(data->nan.current_ami.master_addr,
					       data->nan.device_vif->addr));

		if (le64_to_cpu(ami->master_rank) <
		    le64_to_cpu(data->nan.current_ami.master_rank) ||
		    ether_addr_equal(ami->master_addr,
				     data->nan.current_ami.master_addr)) {
			/* disregard */
		} else {
			wiphy_dbg(hw->wiphy, "NAN: lost anchor master role\n");
			adopt_tsf = true;
			memcpy(&data->nan.last_ami, &data->nan.current_ami,
			       sizeof(data->nan.last_ami));
			memcpy(&data->nan.current_ami, ami,
			       sizeof(data->nan.last_ami));
			data->nan.current_ami.hop_count += 1;
		}
	}

	if (adopt_tsf && !data->nan.tsf_adjusted) {
		int threshold = 5;
		s64 adjustment;

		/* Timestamp is likely inaccurate (and late) in this case */
		if (!(IEEE80211_SKB_RXCB(skb)->flag & RX_FLAG_MACTIME))
			threshold = 128;

		adjustment =
			le64_to_cpu(mgmt->u.beacon.timestamp) -
			ieee80211_calculate_rx_timestamp(hw, &rx_status,
							 skb->len, 24);

		scoped_guard(spinlock_bh, &data->tsf_offset_lock) {
			if (adjustment < -threshold || adjustment > threshold) {
				if (adjustment < -(s64)ieee80211_tu_to_usec(4) ||
				    adjustment > (s64)ieee80211_tu_to_usec(4))
					data->nan.tsf_discontinuity = true;

				wiphy_debug(hw->wiphy,
					    "NAN: Adjusting TSF by +/- %d us or more: %lld us (discontinuity: %d, from: %pM, old offset: %lld)\n",
					    threshold, adjustment,
					    data->nan.tsf_discontinuity, mgmt->sa,
					    data->tsf_offset);
			} else {
				/* smooth things out a little bit */
				adjustment /= 2;
			}

			/*
			 * Do the TSF adjustment
			 * The flag prevents further adjustments until the next
			 * 2.4 GHz DW starts to avoid race conditions for
			 * in-flight packets.
			 */
			data->nan.tsf_adjusted = true;
			data->tsf_offset += adjustment;
		}
	}

	spin_unlock_bh(&data->nan.state_lock);

	if (joined_cluster)
		ieee80211_nan_cluster_joined(data->nan.device_vif,
					     data->nan.cluster_id, false,
					     GFP_ATOMIC);
}

static void
mac80211_hwsim_nan_exec_state_transitions(struct mac80211_hwsim_data *data)
{
	bool notify_join = false;

	/*
	 * Handle NAN role and state transitions at the end of the DW period
	 * in accordance to Wi-Fi Aware version 4.0 section 3.3.7 point 2, i.e.
	 * end of 5 GHz DW if enabled else at the end of the 2.4 GHz DW.
	 */

	spin_lock(&data->nan.state_lock);

	/* Handle role transitions, Wi-Fi Aware version 4.0 section 3.3.6  */
	if (data->nan.master_transition_score < 3)
		data->nan.role = MAC80211_HWSIM_NAN_ROLE_MASTER;
	else if (data->nan.role == MAC80211_HWSIM_NAN_ROLE_MASTER &&
		 data->nan.master_transition_score >= 3)
		data->nan.role = MAC80211_HWSIM_NAN_ROLE_SYNC;
	else if (data->nan.role == MAC80211_HWSIM_NAN_ROLE_SYNC &&
		 data->nan.sync_transition_score >= 3)
		data->nan.role = MAC80211_HWSIM_NAN_ROLE_NON_SYNC;
	else if (data->nan.role == MAC80211_HWSIM_NAN_ROLE_NON_SYNC &&
		 data->nan.sync_transition_score < 3)
		data->nan.role = MAC80211_HWSIM_NAN_ROLE_SYNC;

	/*
	 * The discovery beacon timer will stop automatically. Make sure it is
	 * running if we are master. Do not bother with a proper alignment it
	 * will sync itself to the TSF after the first TX.
	 */
	if (data->nan.role == MAC80211_HWSIM_NAN_ROLE_MASTER &&
	    !hrtimer_active(&data->nan.discovery_beacon_timer))
		hrtimer_start(&data->nan.discovery_beacon_timer,
			      ns_to_ktime(10 * NSEC_PER_USEC),
			      HRTIMER_MODE_REL_SOFT);

	data->nan.master_transition_score = 0;
	data->nan.sync_transition_score = 0;

	if (data->nan.random_factor_valid_dwst == 0) {
		u64 amr;

		if (data->nan.phase == MAC80211_HWSIM_NAN_PHASE_SCAN) {
			data->nan.phase = MAC80211_HWSIM_NAN_PHASE_WARMUP;
			data->nan.random_factor_valid_dwst = NAN_WARMUP_DWST;

			notify_join = true;
		} else {
			data->nan.phase = MAC80211_HWSIM_NAN_PHASE_UP;
			data->nan.random_factor_valid_dwst =
				get_random_u32_inclusive(120, 240);
			data->nan.random_factor = get_random_u8();
		}

		amr = hwsim_nan_get_master_rank(data);

		if (data->nan.current_ami.hop_count == 0) {
			/* Update if we are already anchor master */
			data->nan.current_ami.master_rank = cpu_to_le64(amr);
		} else if (le64_to_cpu(data->nan.current_ami.master_rank) < amr) {
			/* assume role if we have a higher rank */
			wiphy_dbg(data->hw->wiphy,
				  "NAN: assume anchor master role\n");
			data->nan.current_ami.master_rank = cpu_to_le64(amr);
			data->nan.current_ami.hop_count = 0;
			memset(&data->nan.last_ami, 0,
			       sizeof(data->nan.last_ami));
			data->nan.last_ami.ambtt = data->nan.current_ami.ambtt;
			data->nan.current_ami.ambtt = 0;
		}
	} else {
		data->nan.random_factor_valid_dwst--;
	}

	spin_unlock(&data->nan.state_lock);

	if (notify_join)
		ieee80211_nan_cluster_joined(data->nan.device_vif,
					     data->nan.cluster_id, true,
					     GFP_ATOMIC);
}

static void
mac80211_hwsim_nan_tx_beacon(struct mac80211_hwsim_data *data,
			     bool is_discovery,
			     struct ieee80211_channel *channel)
{
	struct ieee80211_vendor_ie nan_ie = {
		.element_id = WLAN_EID_VENDOR_SPECIFIC,
		.len = 27 - 2,
		.oui = { u32_get_bits(WLAN_OUI_WFA, 0xff0000),
			 u32_get_bits(WLAN_OUI_WFA, 0xff00),
			 u32_get_bits(WLAN_OUI_WFA, 0xff) },
		.oui_type = WLAN_OUI_TYPE_WFA_NAN,
	};
	size_t alloc_size =
		IEEE80211_TX_STATUS_HEADROOM +
		offsetofend(struct ieee80211_mgmt, u.beacon) +
		27 /* size of NAN vendor element */;
	struct ieee80211_nan_master_indication master_indication;
	struct ieee80211_nan_attr nan_attr;
	struct ieee80211_mgmt *mgmt;
	struct sk_buff *skb;

	/*
	 * TODO: Should the configured vendor elements or NAN attributes be
	 * included in some of these beacons?
	 */

	skb = alloc_skb(alloc_size, GFP_ATOMIC);
	if (!skb)
		return;

	spin_lock(&data->nan.state_lock);

	skb_reserve(skb, IEEE80211_TX_STATUS_HEADROOM);
	mgmt = skb_put(skb, offsetofend(struct ieee80211_mgmt, u.beacon));

	memset(mgmt, 0, offsetofend(struct ieee80211_mgmt, u.beacon));
	memcpy(mgmt->sa, data->nan.device_vif->addr, ETH_ALEN);
	memset(mgmt->da, 0xff, ETH_ALEN);
	memcpy(mgmt->bssid, data->nan.cluster_id, ETH_ALEN);

	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
					  IEEE80211_STYPE_BEACON);
	mgmt->u.beacon.beacon_int = cpu_to_le16(is_discovery ? 100 : DWST_TU);
	mgmt->u.beacon.capab_info =
		cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT_TIME |
			    WLAN_CAPABILITY_SHORT_PREAMBLE);

	/* FIXME: set these to saner values? */
	mgmt->duration = 0;
	mgmt->seq_ctrl = 0;

	/* Put the NAN element */
	skb_put_data(skb, &nan_ie, sizeof(nan_ie));

	nan_attr.attr = NAN_ATTR_MASTER_INDICATION;
	nan_attr.length = cpu_to_le16(sizeof(master_indication));
	if (data->nan.phase == MAC80211_HWSIM_NAN_PHASE_UP) {
		master_indication.master_pref = data->nan.master_pref;
		master_indication.random_factor = data->nan.random_factor;
	} else {
		master_indication.master_pref = 0;
		master_indication.random_factor = 0;
	}

	skb_put_data(skb, &nan_attr, sizeof(nan_attr));
	skb_put_data(skb, &master_indication, sizeof(master_indication));

	nan_attr.attr = NAN_ATTR_CLUSTER_INFO;
	nan_attr.length = cpu_to_le16(sizeof(data->nan.current_ami));
	skb_put_data(skb, &nan_attr, sizeof(nan_attr));
	skb_put_data(skb, &data->nan.current_ami,
		     sizeof(data->nan.current_ami));

	spin_unlock(&data->nan.state_lock);

	mac80211_hwsim_tx_frame(data->hw, skb, channel);
}

enum hrtimer_restart
mac80211_hwsim_nan_slot_timer(struct hrtimer *timer)
{
	struct mac80211_hwsim_data *data =
		container_of(timer, struct mac80211_hwsim_data,
			     nan.slot_timer);
	struct ieee80211_hw *hw = data->hw;
	struct ieee80211_channel *notify_dw_chan = NULL;
	struct ieee80211_channel *beacon_sync_chan = NULL;
	u64 tsf = hwsim_nan_get_timer_tsf(data);
	u8 slot = hwsim_nan_slot_from_tsf(tsf);
	bool dwst_of_dw0 = false;
	bool dw_end = false;
	bool tx_sync_beacon;

	if (!data->nan.device_vif)
		return HRTIMER_NORESTART;

	if ((tsf & DW0_TSF_MASK & ~DWST_TSF_MASK) == 0)
		dwst_of_dw0 = true;


	scoped_guard(spinlock, &data->nan.state_lock) {
		if (data->nan.tsf_discontinuity) {
			data->nan.tsf_discontinuity = false;

			mac80211_hwsim_nan_schedule_slot(data, 32, true);

			return HRTIMER_RESTART;
		}

		if (slot == SLOT_24GHZ_DW)
			data->nan.tsf_adjusted = false;

		tx_sync_beacon =
			data->nan.phase != MAC80211_HWSIM_NAN_PHASE_SCAN &&
			data->nan.role != MAC80211_HWSIM_NAN_ROLE_NON_SYNC;
	}

	switch (slot) {
	case SLOT_24GHZ_DW:
		wiphy_dbg(data->hw->wiphy, "Start of 2.4 GHz DW, is DW0=%d\n",
			  dwst_of_dw0);
		beacon_sync_chan = ieee80211_get_channel(hw->wiphy, 2437);
		break;

	case SLOT_24GHZ_DW + 1:
		if (!(data->nan.bands & BIT(NL80211_BAND_5GHZ))) {
			notify_dw_chan = ieee80211_get_channel(hw->wiphy, 2437);
			dw_end = true;
		} else {
			notify_dw_chan = ieee80211_get_channel(hw->wiphy, 5745);
		}
		break;

	case SLOT_5GHZ_DW:
		if (data->nan.bands & BIT(NL80211_BAND_5GHZ)) {
			wiphy_dbg(data->hw->wiphy, "Start of 5 GHz DW\n");
			beacon_sync_chan =
				ieee80211_get_channel(hw->wiphy, 5745);
		}
		break;

	case SLOT_5GHZ_DW + 1:
		if (data->nan.bands & BIT(NL80211_BAND_5GHZ)) {
			notify_dw_chan =
				ieee80211_get_channel(hw->wiphy, 2437);
			dw_end = true;
		}
		break;
	}

	/* TODO: This does not implement DW contention mitigation */
	if (beacon_sync_chan && tx_sync_beacon)
		mac80211_hwsim_nan_tx_beacon(data, false, beacon_sync_chan);

	if (dw_end)
		mac80211_hwsim_nan_exec_state_transitions(data);

	if (data->nan.notify_dw && notify_dw_chan) {
		struct wireless_dev *wdev =
			ieee80211_vif_to_wdev(data->nan.device_vif);

		cfg80211_next_nan_dw_notif(wdev, notify_dw_chan, GFP_ATOMIC);
	}

	mac80211_hwsim_nan_resume_txqs(data);

	mac80211_hwsim_nan_schedule_slot(data, slot + 1, false);

	return HRTIMER_RESTART;
}

enum hrtimer_restart
mac80211_hwsim_nan_discovery_beacon_timer(struct hrtimer *timer)
{
	struct mac80211_hwsim_data *data =
		container_of(timer, struct mac80211_hwsim_data,
			     nan.discovery_beacon_timer);
	u32 remainder;
	u64 tsf_now;
	u64 tbtt;

	if (!data->nan.device_vif)
		return HRTIMER_NORESTART;

	scoped_guard(spinlock, &data->nan.state_lock) {
		if (data->nan.phase == MAC80211_HWSIM_NAN_PHASE_SCAN ||
		    data->nan.role != MAC80211_HWSIM_NAN_ROLE_MASTER)
			return HRTIMER_NORESTART;
	}

	mac80211_hwsim_nan_tx_beacon(
		data, true, ieee80211_get_channel(data->hw->wiphy, 2437));

	if (data->nan.bands & BIT(NL80211_BAND_5GHZ))
		mac80211_hwsim_nan_tx_beacon(
			data, true,
			ieee80211_get_channel(data->hw->wiphy, 5745));

	/* Read the TSF from the current time in case of adjustments */
	tsf_now = mac80211_hwsim_get_tsf(data->hw, data->nan.device_vif);

	/* Wrap value to be after the next TBTT */
	tbtt = tsf_now + ieee80211_tu_to_usec(100);

	/* Round TBTT down to the correct time */
	div_u64_rem(tbtt, ieee80211_tu_to_usec(100), &remainder);
	tbtt = tbtt - remainder;

	hrtimer_set_expires(&data->nan.discovery_beacon_timer,
			    mac80211_hwsim_tsf_to_boottime(data, tbtt));

	return HRTIMER_RESTART;
}

int mac80211_hwsim_nan_start(struct ieee80211_hw *hw,
			     struct ieee80211_vif *vif,
			     struct cfg80211_nan_conf *conf)
{
	struct mac80211_hwsim_data *data = hw->priv;

	if (vif->type != NL80211_IFTYPE_NAN)
		return -EINVAL;

	if (data->nan.device_vif)
		return -EALREADY;

	/* set this before starting the timer, as preemption might occur */
	data->nan.device_vif = vif;
	data->nan.bands = conf->bands;

	scoped_guard(spinlock_bh, &data->nan.state_lock) {
		/* Start in the "scan" phase and stay there for a little bit */
		data->nan.phase = MAC80211_HWSIM_NAN_PHASE_SCAN;
		data->nan.random_factor_valid_dwst = 1;
		data->nan.random_factor = 0;
		data->nan.master_pref = conf->master_pref;
		data->nan.role = MAC80211_HWSIM_NAN_ROLE_MASTER;
		memset(&data->nan.current_ami, 0,
		       sizeof(data->nan.current_ami));
		memset(&data->nan.last_ami, 0, sizeof(data->nan.last_ami));
		data->nan.current_ami.master_rank =
			cpu_to_le64(hwsim_nan_get_master_rank(data));
	}

	/* Just run this "soon" and start in a random schedule position */
	hrtimer_start(&data->nan.slot_timer,
		      ns_to_ktime(10 * NSEC_PER_USEC),
		      HRTIMER_MODE_REL_SOFT);

	ether_addr_copy(data->nan.cluster_id, conf->cluster_id);

	data->nan.notify_dw = conf->enable_dw_notification;

	return 0;
}

int mac80211_hwsim_nan_stop(struct ieee80211_hw *hw,
			    struct ieee80211_vif *vif)
{
	struct mac80211_hwsim_data *data = hw->priv;

	if (vif->type != NL80211_IFTYPE_NAN || !data->nan.device_vif ||
	    data->nan.device_vif != vif)
		return -EINVAL;

	hrtimer_cancel(&data->nan.slot_timer);
	hrtimer_cancel(&data->nan.resume_txqs_timer);
	hrtimer_cancel(&data->nan.discovery_beacon_timer);
	data->nan.device_vif = NULL;

	return 0;
}

int mac80211_hwsim_nan_change_config(struct ieee80211_hw *hw,
				     struct ieee80211_vif *vif,
				     struct cfg80211_nan_conf *conf,
				     u32 changes)
{
	struct mac80211_hwsim_data *data = hw->priv;

	if (vif->type != NL80211_IFTYPE_NAN)
		return -EINVAL;

	if (!data->nan.device_vif)
		return -EINVAL;

	wiphy_debug(hw->wiphy, "nan_config_changed: changes=0x%x\n", changes);

	/* Handle only the changes we care about for simulation purposes */
	if (changes & CFG80211_NAN_CONF_CHANGED_BANDS)
		data->nan.bands = conf->bands;

	if (changes & CFG80211_NAN_CONF_CHANGED_CONFIG)
		data->nan.notify_dw = conf->enable_dw_notification;

	if (changes & CFG80211_NAN_CONF_CHANGED_PREF) {
		scoped_guard(spinlock_bh, &data->nan.state_lock)
			data->nan.master_pref = conf->master_pref;
	}

	return 0;
}

static void hwsim_nan_can_sta_transmit(void *_ctx, struct ieee80211_sta *sta)
{
	struct hwsim_nan_sta_iter_ctx *ctx = _ctx;

	if (ctx->can_tx)
		return;

	for (int i = 0; i < ARRAY_SIZE(sta->txq); i++) {
		struct ieee80211_txq *txq = sta->txq[i];

		if (!txq)
			continue;

		if (txq->vif->type != NL80211_IFTYPE_NAN &&
		    txq->vif->type != NL80211_IFTYPE_NAN_DATA)
			return;

		if (mac80211_hwsim_nan_txq_transmitting(ctx->hw, txq)) {
			ctx->can_tx = true;
			return;
		}
	}
}

static void mac80211_hwsim_nan_resume_txqs(struct mac80211_hwsim_data *data)
{
	u64 tsf = mac80211_hwsim_get_tsf(data->hw, data->nan.device_vif);
	u8 slot = hwsim_nan_slot_from_tsf(tsf);
	bool is_dw_slot = mac80211_hwsim_nan_is_dw_slot(data, slot);
	struct hwsim_nan_sta_iter_ctx ctx = {
		.hw = data->hw,
		.can_tx = false,
	};
	u32 timeout_ns;

	/* Outside DW, require local FAW schedule to proceed */
	if (!is_dw_slot) {
		scoped_guard(spinlock_bh, &data->nan.state_lock) {
			if (!data->nan.local_sched[slot].chan)
				return;
		}
	}

	guard(rcu)();

	/* Check if management queue can transmit */
	if (mac80211_hwsim_nan_txq_transmitting(data->hw,
						data->nan.device_vif->txq_mgmt))
		goto resume_txqs_timer;

	/* Check if any STA queue can transmit */
	ieee80211_iterate_stations_atomic(data->hw,
					  hwsim_nan_can_sta_transmit,
					  &ctx);

	if (!ctx.can_tx)
		return;

resume_txqs_timer:
	/*
	 * Wait a bit and also randomize things so that not everyone is TXing
	 * at the same time. Each slot is 16 TU long, this waits between 100 us
	 * and 5 ms before starting to TX (unless a new frame arrives).
	 */
	timeout_ns = get_random_u32_inclusive(100 * NSEC_PER_USEC,
					      5 * NSEC_PER_MSEC);

	hrtimer_start(&data->nan.resume_txqs_timer,
		      ns_to_ktime(timeout_ns),
		      HRTIMER_MODE_REL_SOFT);
}

static void hwsim_nan_wake_sta_iter(void *_data, struct ieee80211_sta *sta)
{
	struct ieee80211_hw *hw = _data;

	for (int i = 0; i < ARRAY_SIZE(sta->txq); i++) {
		struct ieee80211_txq *txq = sta->txq[i];

		if (!txq)
			continue;

		/* exit early if non-NAN */
		if (txq->vif->type != NL80211_IFTYPE_NAN &&
		    txq->vif->type != NL80211_IFTYPE_NAN_DATA)
			return;

		if (mac80211_hwsim_nan_txq_transmitting(hw, txq))
			ieee80211_hwsim_wake_tx_queue(hw, txq);
	}
}

enum hrtimer_restart
mac80211_hwsim_nan_resume_txqs_timer(struct hrtimer *timer)
{
	struct mac80211_hwsim_data *data =
		container_of(timer, struct mac80211_hwsim_data,
			     nan.resume_txqs_timer);

	guard(rcu)();

	/* Wake TX queue for management frames on the NAN device interface */
	if (mac80211_hwsim_nan_txq_transmitting(data->hw,
						data->nan.device_vif->txq_mgmt))
		ieee80211_hwsim_wake_tx_queue(data->hw,
					      data->nan.device_vif->txq_mgmt);

	/* Wake TX queues for all stations */
	ieee80211_iterate_stations_atomic(data->hw,
					  hwsim_nan_wake_sta_iter,
					  data->hw);

	return HRTIMER_NORESTART;
}

bool mac80211_hwsim_nan_txq_transmitting(struct ieee80211_hw *hw,
					 struct ieee80211_txq *txq)
{
	struct mac80211_hwsim_data *data = hw->priv;
	struct ieee80211_sta *nmi_sta;
	struct hwsim_sta_priv *sp;
	bool is_dw_slot;
	u64 tsf;
	u8 slot;

	if (WARN_ON_ONCE(!data->nan.device_vif))
		return true;

	tsf = mac80211_hwsim_get_tsf(hw, data->nan.device_vif);
	slot = hwsim_nan_slot_from_tsf(tsf);

	/* Enforce a maximum channel switch time and guard against TX delays */
	if (slot != hwsim_nan_slot_from_tsf(tsf + NAN_CHAN_SWITCH_TIME_US))
		return false;

	is_dw_slot = mac80211_hwsim_nan_is_dw_slot(data, slot);

	/* Non-STA TXQ: allow management frames during DW */
	if (!txq->sta)
		return is_dw_slot;

	/* STA TXQ: need peer schedule for availability check */
	nmi_sta = rcu_dereference(txq->sta->nmi) ?: txq->sta;
	sp = (void *)nmi_sta->drv_priv;

	/* DW slot: NDI can TX only mgmt but not worth checking,
	 * NMI checks peer's committed DW
	 */
	if (is_dw_slot) {
		if (txq->vif->type == NL80211_IFTYPE_NAN_DATA)
			return false;
		return hwsim_nan_peer_present_in_dw(sp, tsf);
	}

	/* FAW slot: verify local schedule and peer availability */
	return hwsim_nan_peer_present_in_faw(sp, data, slot);
}

void mac80211_hwsim_nan_get_tx_chandef(struct ieee80211_hw *hw,
				       struct cfg80211_chan_def *chandef)
{
	struct mac80211_hwsim_data *data = hw->priv;
	u64 tsf = mac80211_hwsim_get_tsf(data->hw, data->nan.device_vif);
	u8 slot = hwsim_nan_slot_from_tsf(tsf);

	/* DW slots are always 20 MHz */
	if (slot == SLOT_24GHZ_DW) {
		cfg80211_chandef_create(chandef,
					ieee80211_get_channel(hw->wiphy, 2437),
					NL80211_CHAN_NO_HT);
		return;
	}

	if (slot == SLOT_5GHZ_DW && data->nan.bands & BIT(NL80211_BAND_5GHZ)) {
		cfg80211_chandef_create(chandef,
					ieee80211_get_channel(hw->wiphy, 5745),
					NL80211_CHAN_NO_HT);
		return;
	}

	/* FAW slot: copy local schedule for this slot */
	scoped_guard(spinlock_bh, &data->nan.state_lock)
		*chandef = data->nan.local_sched[slot];
}

bool mac80211_hwsim_nan_receive(struct ieee80211_hw *hw,
				struct ieee80211_channel *channel,
				struct ieee80211_rx_status *rx_status)
{
	struct mac80211_hwsim_data *data = hw->priv;
	u8 slot;

	if (WARN_ON_ONCE(!data->nan.device_vif))
		return false;

	if (data->nan.phase == MAC80211_HWSIM_NAN_PHASE_SCAN)
		return channel->center_freq == 2437;

	if (rx_status->flag & RX_FLAG_MACTIME) {
		slot = hwsim_nan_slot_from_tsf(rx_status->mactime);
	} else {
		u64 tsf;

		/*
		 * This is not perfect, but that should be fine.
		 *
		 * Assume the frame might be a bit early in relation to our
		 * own TSF. This is largely because the TSF sync is going to be
		 * pretty bad when the frame was RXed via NL and the beacon as
		 * well as RX timestamps are not accurate.
		 */
		tsf = mac80211_hwsim_get_tsf(data->hw, data->nan.device_vif);
		slot = hwsim_nan_slot_from_tsf(tsf + 128);
	}

	if (slot == SLOT_24GHZ_DW && channel->center_freq == 2437)
		return true;

	if (slot == SLOT_5GHZ_DW && data->nan.bands & BIT(NL80211_BAND_5GHZ) &&
	    channel->center_freq == 5745)
		return true;

	/* Accept frames during FAW slots if chandef is compatible */
	return hwsim_nan_rx_chandef_compatible(data, slot, channel,
					       rx_status->bw);
}

void mac80211_hwsim_nan_local_sched_changed(struct ieee80211_hw *hw,
					    struct ieee80211_vif *vif)
{
	struct mac80211_hwsim_data *data = hw->priv;
	struct ieee80211_nan_channel **slots = vif->cfg.nan_schedule;

	if (WARN_ON(vif->type != NL80211_IFTYPE_NAN))
		return;

	spin_lock_bh(&data->nan.state_lock);

	for (int i = 0; i < ARRAY_SIZE(data->nan.local_sched); i++) {
		struct ieee80211_chanctx_conf *chanctx;

		if (!slots[i] || IS_ERR(slots[i])) {
			memset(&data->nan.local_sched[i], 0,
			       sizeof(data->nan.local_sched[i]));
			continue;
		}

		chanctx = slots[i]->chanctx_conf;
		if (!chanctx) {
			memset(&data->nan.local_sched[i], 0,
			       sizeof(data->nan.local_sched[i]));
			continue;
		}

		data->nan.local_sched[i] = chanctx->def;
	}

	spin_unlock_bh(&data->nan.state_lock);
}

int mac80211_hwsim_nan_peer_sched_changed(struct ieee80211_hw *hw,
					  struct ieee80211_sta *sta)
{
	struct hwsim_sta_priv *sp = (void *)sta->drv_priv;
	struct ieee80211_nan_peer_sched *sched = sta->nan_sched;

	spin_lock_bh(&sp->nan_sched.lock);

	/* Clear existing schedule */
	sp->nan_sched.committed_dw = 0;
	for (int i = 0; i < CFG80211_NAN_MAX_PEER_MAPS; i++) {
		sp->nan_sched.maps[i].map_id = CFG80211_NAN_INVALID_MAP_ID;
		memset(sp->nan_sched.maps[i].chans, 0,
		       sizeof(sp->nan_sched.maps[i].chans));
	}

	if (!sched)
		goto out;

	sp->nan_sched.committed_dw = sched->committed_dw;

	for (int i = 0; i < CFG80211_NAN_MAX_PEER_MAPS; i++) {
		struct ieee80211_nan_peer_map *map = &sched->maps[i];

		if (map->map_id == CFG80211_NAN_INVALID_MAP_ID)
			continue;

		sp->nan_sched.maps[i].map_id = map->map_id;

		for (int j = 0; j < CFG80211_NAN_SCHED_NUM_TIME_SLOTS; j++) {
			struct ieee80211_nan_channel *peer_chan =
				map->slots[j];

			if (peer_chan && peer_chan->chanreq.oper.chan)
				sp->nan_sched.maps[i].chans[j] =
					peer_chan->chanreq.oper;
			else
				memset(&sp->nan_sched.maps[i].chans[j], 0,
				       sizeof(sp->nan_sched.maps[i].chans[j]));
		}
	}

out:
	spin_unlock_bh(&sp->nan_sched.lock);
	return 0;
}
