time_sync.c

  1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
  2/*
  3 * Copyright (C) 2025 Intel Corporation
  4 */
  5
  6#include "mld.h"
  7#include "hcmd.h"
  8#include "ptp.h"
  9#include "time_sync.h"
 10#include <linux/ieee80211.h>
 11
 12static int iwl_mld_init_time_sync(struct iwl_mld *mld, u32 protocols,
 13				  const u8 *addr)
 14{
 15	struct iwl_mld_time_sync_data *time_sync = kzalloc(sizeof(*time_sync),
 16							   GFP_KERNEL);
 17
 18	if (!time_sync)
 19		return -ENOMEM;
 20
 21	time_sync->active_protocols = protocols;
 22	ether_addr_copy(time_sync->peer_addr, addr);
 23	skb_queue_head_init(&time_sync->frame_list);
 24	rcu_assign_pointer(mld->time_sync, time_sync);
 25
 26	return 0;
 27}
 28
 29int iwl_mld_time_sync_fw_config(struct iwl_mld *mld)
 30{
 31	struct iwl_time_sync_cfg_cmd cmd = {};
 32	struct iwl_mld_time_sync_data *time_sync;
 33	int err;
 34
 35	time_sync = wiphy_dereference(mld->wiphy, mld->time_sync);
 36	if (!time_sync)
 37		return -EINVAL;
 38
 39	cmd.protocols = cpu_to_le32(time_sync->active_protocols);
 40	ether_addr_copy(cmd.peer_addr, time_sync->peer_addr);
 41
 42	err = iwl_mld_send_cmd_pdu(mld,
 43				   WIDE_ID(DATA_PATH_GROUP,
 44					   WNM_80211V_TIMING_MEASUREMENT_CONFIG_CMD),
 45				   &cmd);
 46	if (err)
 47		IWL_ERR(mld, "Failed to send time sync cfg cmd: %d\n", err);
 48
 49	return err;
 50}
 51
 52int iwl_mld_time_sync_config(struct iwl_mld *mld, const u8 *addr, u32 protocols)
 53{
 54	struct iwl_mld_time_sync_data *time_sync;
 55	int err;
 56
 57	time_sync = wiphy_dereference(mld->wiphy, mld->time_sync);
 58
 59	/* The fw only supports one peer. We do allow reconfiguration of the
 60	 * same peer for cases of fw reset etc.
 61	 */
 62	if (time_sync && time_sync->active_protocols &&
 63	    !ether_addr_equal(addr, time_sync->peer_addr)) {
 64		IWL_DEBUG_INFO(mld, "Time sync: reject config for peer: %pM\n",
 65			       addr);
 66		return -ENOBUFS;
 67	}
 68
 69	if (protocols & ~(IWL_TIME_SYNC_PROTOCOL_TM |
 70			  IWL_TIME_SYNC_PROTOCOL_FTM))
 71		return -EINVAL;
 72
 73	IWL_DEBUG_INFO(mld, "Time sync: set peer addr=%pM\n", addr);
 74
 75	iwl_mld_deinit_time_sync(mld);
 76	err = iwl_mld_init_time_sync(mld, protocols, addr);
 77	if (err)
 78		return err;
 79
 80	err = iwl_mld_time_sync_fw_config(mld);
 81	return err;
 82}
 83
 84void iwl_mld_deinit_time_sync(struct iwl_mld *mld)
 85{
 86	struct iwl_mld_time_sync_data *time_sync =
 87		wiphy_dereference(mld->wiphy, mld->time_sync);
 88
 89	if (!time_sync)
 90		return;
 91
 92	RCU_INIT_POINTER(mld->time_sync, NULL);
 93	skb_queue_purge(&time_sync->frame_list);
 94	kfree_rcu(time_sync, rcu_head);
 95}
 96
 97bool iwl_mld_time_sync_frame(struct iwl_mld *mld, struct sk_buff *skb, u8 *addr)
 98{
 99	struct iwl_mld_time_sync_data *time_sync;
100
101	rcu_read_lock();
102	time_sync = rcu_dereference(mld->time_sync);
103	if (time_sync && ether_addr_equal(time_sync->peer_addr, addr) &&
104	    (ieee80211_is_timing_measurement(skb) || ieee80211_is_ftm(skb))) {
105		skb_queue_tail(&time_sync->frame_list, skb);
106		rcu_read_unlock();
107		return true;
108	}
109	rcu_read_unlock();
110
111	return false;
112}
113
114static bool iwl_mld_is_skb_match(struct sk_buff *skb, u8 *addr, u8 dialog_token)
115{
116	struct ieee80211_mgmt *mgmt = (void *)skb->data;
117	u8 skb_dialog_token;
118
119	if (ieee80211_is_timing_measurement(skb))
120		skb_dialog_token = mgmt->u.action.u.wnm_timing_msr.dialog_token;
121	else
122		skb_dialog_token = mgmt->u.action.u.ftm.dialog_token;
123
124	if ((ether_addr_equal(mgmt->sa, addr) ||
125	     ether_addr_equal(mgmt->da, addr)) &&
126	    skb_dialog_token == dialog_token)
127		return true;
128
129	return false;
130}
131
132static struct sk_buff *iwl_mld_time_sync_find_skb(struct iwl_mld *mld, u8 *addr,
133						  u8 dialog_token)
134{
135	struct iwl_mld_time_sync_data *time_sync;
136	struct sk_buff *skb;
137
138	rcu_read_lock();
139
140	time_sync = rcu_dereference(mld->time_sync);
141	if (IWL_FW_CHECK(mld, !time_sync,
142			 "Time sync notification but time sync is not initialized\n")) {
143		rcu_read_unlock();
144		return NULL;
145	}
146
147	/* The notifications are expected to arrive in the same order of the
148	 * frames. If the incoming notification doesn't match the first SKB
149	 * in the queue, it means there was no time sync notification for this
150	 * SKB and it can be dropped.
151	 */
152	while ((skb = skb_dequeue(&time_sync->frame_list))) {
153		if (iwl_mld_is_skb_match(skb, addr, dialog_token))
154			break;
155
156		kfree_skb(skb);
157		skb = NULL;
158		IWL_DEBUG_DROP(mld,
159			       "Time sync: drop SKB without matching notification\n");
160	}
161	rcu_read_unlock();
162
163	return skb;
164}
165
166static u64 iwl_mld_get_64_bit(__le32 high, __le32 low)
167{
168	return ((u64)le32_to_cpu(high) << 32) | le32_to_cpu(low);
169}
170
171void iwl_mld_handle_time_msmt_notif(struct iwl_mld *mld,
172				    struct iwl_rx_packet *pkt)
173{
174	struct ptp_data *data = &mld->ptp_data;
175	struct iwl_time_msmt_notify *notif = (void *)pkt->data;
176	struct ieee80211_rx_status *rx_status;
177	struct skb_shared_hwtstamps *shwt;
178	u64 ts_10ns;
179	struct sk_buff *skb =
180		iwl_mld_time_sync_find_skb(mld, notif->peer_addr,
181					   le32_to_cpu(notif->dialog_token));
182	u64 adj_time;
183
184	if (IWL_FW_CHECK(mld, !skb, "Time sync event but no pending skb\n"))
185		return;
186
187	spin_lock_bh(&data->lock);
188	ts_10ns = iwl_mld_get_64_bit(notif->t2_hi, notif->t2_lo);
189	adj_time = iwl_mld_ptp_get_adj_time(mld, ts_10ns * 10);
190	shwt = skb_hwtstamps(skb);
191	shwt->hwtstamp = ktime_set(0, adj_time);
192
193	ts_10ns = iwl_mld_get_64_bit(notif->t3_hi, notif->t3_lo);
194	adj_time = iwl_mld_ptp_get_adj_time(mld, ts_10ns * 10);
195	rx_status = IEEE80211_SKB_RXCB(skb);
196	rx_status->ack_tx_hwtstamp = ktime_set(0, adj_time);
197	spin_unlock_bh(&data->lock);
198
199	IWL_DEBUG_INFO(mld,
200		       "Time sync: RX event - report frame t2=%llu t3=%llu\n",
201		       ktime_to_ns(shwt->hwtstamp),
202		       ktime_to_ns(rx_status->ack_tx_hwtstamp));
203	ieee80211_rx_napi(mld->hw, NULL, skb, NULL);
204}
205
206void iwl_mld_handle_time_sync_confirm_notif(struct iwl_mld *mld,
207					    struct iwl_rx_packet *pkt)
208{
209	struct ptp_data *data = &mld->ptp_data;
210	struct iwl_time_msmt_cfm_notify *notif = (void *)pkt->data;
211	struct ieee80211_tx_status status = {};
212	struct skb_shared_hwtstamps *shwt;
213	u64 ts_10ns, adj_time;
214
215	status.skb =
216		iwl_mld_time_sync_find_skb(mld, notif->peer_addr,
217					   le32_to_cpu(notif->dialog_token));
218
219	if (IWL_FW_CHECK(mld, !status.skb,
220			 "Time sync confirm but no pending skb\n"))
221		return;
222
223	spin_lock_bh(&data->lock);
224	ts_10ns = iwl_mld_get_64_bit(notif->t1_hi, notif->t1_lo);
225	adj_time = iwl_mld_ptp_get_adj_time(mld, ts_10ns * 10);
226	shwt = skb_hwtstamps(status.skb);
227	shwt->hwtstamp = ktime_set(0, adj_time);
228
229	ts_10ns = iwl_mld_get_64_bit(notif->t4_hi, notif->t4_lo);
230	adj_time = iwl_mld_ptp_get_adj_time(mld, ts_10ns * 10);
231	status.info = IEEE80211_SKB_CB(status.skb);
232	status.ack_hwtstamp = ktime_set(0, adj_time);
233	spin_unlock_bh(&data->lock);
234
235	IWL_DEBUG_INFO(mld,
236		       "Time sync: TX event - report frame t1=%llu t4=%llu\n",
237		       ktime_to_ns(shwt->hwtstamp),
238		       ktime_to_ns(status.ack_hwtstamp));
239	ieee80211_tx_status_ext(mld->hw, &status);
240}