174 lines · c
1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause2/*3 * Copyright (C) 2022 Intel Corporation4 */5 6#include "mvm.h"7#include "time-sync.h"8#include <linux/ieee80211.h>9 10void iwl_mvm_init_time_sync(struct iwl_time_sync_data *data)11{12 skb_queue_head_init(&data->frame_list);13}14 15static bool iwl_mvm_is_skb_match(struct sk_buff *skb, u8 *addr, u8 dialog_token)16{17 struct ieee80211_mgmt *mgmt = (void *)skb->data;18 u8 skb_dialog_token;19 20 if (ieee80211_is_timing_measurement(skb))21 skb_dialog_token = mgmt->u.action.u.wnm_timing_msr.dialog_token;22 else23 skb_dialog_token = mgmt->u.action.u.ftm.dialog_token;24 25 if ((ether_addr_equal(mgmt->sa, addr) ||26 ether_addr_equal(mgmt->da, addr)) &&27 skb_dialog_token == dialog_token)28 return true;29 30 return false;31}32 33static struct sk_buff *iwl_mvm_time_sync_find_skb(struct iwl_mvm *mvm, u8 *addr,34 u8 dialog_token)35{36 struct sk_buff *skb;37 38 /* The queue is expected to have only one SKB. If there are other SKBs39 * in the queue, they did not get a time sync notification and are40 * probably obsolete by now, so drop them.41 */42 while ((skb = skb_dequeue(&mvm->time_sync.frame_list))) {43 if (iwl_mvm_is_skb_match(skb, addr, dialog_token))44 break;45 46 kfree_skb(skb);47 skb = NULL;48 }49 50 return skb;51}52 53static u64 iwl_mvm_get_64_bit(__le32 high, __le32 low)54{55 return ((u64)le32_to_cpu(high) << 32) | le32_to_cpu(low);56}57 58void iwl_mvm_time_sync_msmt_event(struct iwl_mvm *mvm,59 struct iwl_rx_cmd_buffer *rxb)60{61 struct iwl_rx_packet *pkt = rxb_addr(rxb);62 struct iwl_time_msmt_notify *notif = (void *)pkt->data;63 struct ieee80211_rx_status *rx_status;64 struct skb_shared_hwtstamps *shwt;65 u64 ts_10ns;66 struct sk_buff *skb =67 iwl_mvm_time_sync_find_skb(mvm, notif->peer_addr,68 le32_to_cpu(notif->dialog_token));69 u64 adj_time;70 71 if (!skb) {72 IWL_DEBUG_INFO(mvm, "Time sync event but no pending skb\n");73 return;74 }75 76 ts_10ns = iwl_mvm_get_64_bit(notif->t2_hi, notif->t2_lo);77 adj_time = iwl_mvm_ptp_get_adj_time(mvm, ts_10ns * 10);78 shwt = skb_hwtstamps(skb);79 shwt->hwtstamp = ktime_set(0, adj_time);80 81 ts_10ns = iwl_mvm_get_64_bit(notif->t3_hi, notif->t3_lo);82 adj_time = iwl_mvm_ptp_get_adj_time(mvm, ts_10ns * 10);83 rx_status = IEEE80211_SKB_RXCB(skb);84 rx_status->ack_tx_hwtstamp = ktime_set(0, adj_time);85 86 IWL_DEBUG_INFO(mvm,87 "Time sync: RX event - report frame t2=%llu t3=%llu\n",88 ktime_to_ns(shwt->hwtstamp),89 ktime_to_ns(rx_status->ack_tx_hwtstamp));90 ieee80211_rx_napi(mvm->hw, NULL, skb, NULL);91}92 93void iwl_mvm_time_sync_msmt_confirm_event(struct iwl_mvm *mvm,94 struct iwl_rx_cmd_buffer *rxb)95{96 struct iwl_rx_packet *pkt = rxb_addr(rxb);97 struct iwl_time_msmt_cfm_notify *notif = (void *)pkt->data;98 struct ieee80211_tx_status status = {};99 struct skb_shared_hwtstamps *shwt;100 u64 ts_10ns, adj_time;101 102 status.skb =103 iwl_mvm_time_sync_find_skb(mvm, notif->peer_addr,104 le32_to_cpu(notif->dialog_token));105 106 if (!status.skb) {107 IWL_DEBUG_INFO(mvm, "Time sync confirm but no pending skb\n");108 return;109 }110 111 ts_10ns = iwl_mvm_get_64_bit(notif->t1_hi, notif->t1_lo);112 adj_time = iwl_mvm_ptp_get_adj_time(mvm, ts_10ns * 10);113 shwt = skb_hwtstamps(status.skb);114 shwt->hwtstamp = ktime_set(0, adj_time);115 116 ts_10ns = iwl_mvm_get_64_bit(notif->t4_hi, notif->t4_lo);117 adj_time = iwl_mvm_ptp_get_adj_time(mvm, ts_10ns * 10);118 status.info = IEEE80211_SKB_CB(status.skb);119 status.ack_hwtstamp = ktime_set(0, adj_time);120 121 IWL_DEBUG_INFO(mvm,122 "Time sync: TX event - report frame t1=%llu t4=%llu\n",123 ktime_to_ns(shwt->hwtstamp),124 ktime_to_ns(status.ack_hwtstamp));125 ieee80211_tx_status_ext(mvm->hw, &status);126}127 128int iwl_mvm_time_sync_config(struct iwl_mvm *mvm, const u8 *addr, u32 protocols)129{130 struct iwl_time_sync_cfg_cmd cmd = {};131 int err;132 133 lockdep_assert_held(&mvm->mutex);134 135 if (!fw_has_capa(&mvm->fw->ucode_capa,136 IWL_UCODE_TLV_CAPA_TIME_SYNC_BOTH_FTM_TM))137 return -EINVAL;138 139 /* The fw only supports one peer. We do allow reconfiguration of the140 * same peer for cases of fw reset etc.141 */142 if (mvm->time_sync.active &&143 !ether_addr_equal(addr, mvm->time_sync.peer_addr)) {144 IWL_DEBUG_INFO(mvm, "Time sync: reject config for peer: %pM\n",145 addr);146 return -ENOBUFS;147 }148 149 if (protocols & ~(IWL_TIME_SYNC_PROTOCOL_TM |150 IWL_TIME_SYNC_PROTOCOL_FTM))151 return -EINVAL;152 153 cmd.protocols = cpu_to_le32(protocols);154 155 ether_addr_copy(cmd.peer_addr, addr);156 157 err = iwl_mvm_send_cmd_pdu(mvm,158 WIDE_ID(DATA_PATH_GROUP,159 WNM_80211V_TIMING_MEASUREMENT_CONFIG_CMD),160 0, sizeof(cmd), &cmd);161 if (err) {162 IWL_ERR(mvm, "Failed to send time sync cfg cmd: %d\n", err);163 } else {164 mvm->time_sync.active = protocols != 0;165 ether_addr_copy(mvm->time_sync.peer_addr, addr);166 IWL_DEBUG_INFO(mvm, "Time sync: set peer addr=%pM\n", addr);167 }168 169 if (!mvm->time_sync.active)170 skb_queue_purge(&mvm->time_sync.frame_list);171 172 return err;173}174