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