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