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