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
iwl_mld_init_time_sync(struct iwl_mld * mld,u32 protocols,const u8 * addr)12 static 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
iwl_mld_time_sync_fw_config(struct iwl_mld * mld)29 int 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
iwl_mld_time_sync_config(struct iwl_mld * mld,const u8 * addr,u32 protocols)52 int 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
iwl_mld_deinit_time_sync(struct iwl_mld * mld)84 void 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
iwl_mld_time_sync_frame(struct iwl_mld * mld,struct sk_buff * skb,u8 * addr)97 bool 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
iwl_mld_is_skb_match(struct sk_buff * skb,u8 * addr,u8 dialog_token)114 static 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
iwl_mld_time_sync_find_skb(struct iwl_mld * mld,u8 * addr,u8 dialog_token)132 static 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
iwl_mld_get_64_bit(__le32 high,__le32 low)166 static 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
iwl_mld_handle_time_msmt_notif(struct iwl_mld * mld,struct iwl_rx_packet * pkt)171 void 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
iwl_mld_handle_time_sync_confirm_notif(struct iwl_mld * mld,struct iwl_rx_packet * pkt)206 void 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 }
241