1// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
  2/*
  3 * Copyright (C) 2022 Intel Corporation
  4 */
  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	else
 23		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 SKBs
 39	 * in the queue, they did not get a time sync notification and are
 40	 * 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 the
140	 * 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}