| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
|
| 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 |
} |