1 files changed, 351 insertions, 222 deletions

diff --git a/mac/fira_region.c b/mac/fira_region.c
index 7924c6f..9e4bf53 100644
--- a/mac/fira_region.c
+++ b/mac/fira_region.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the UWB stack for linux.
  *
- * Copyright (c) 2020-2021 Qorvo US, Inc.
+ * Copyright (c) 2020-2022 Qorvo US, Inc.
  *
  * This software is provided under the GNU General Public License, version 2
  * (GPLv2), as well as under a Qorvo commercial license.
@@ -22,10 +22,8 @@
  */
 
 #include <linux/slab.h>
-#include <linux/errno.h>
-#include <linux/math64.h>
-
 #include <linux/netdevice.h>
+#include <linux/errno.h>
 
 #include <net/mcps802154_schedule.h>
 #include <net/fira_region_nl.h>
@@ -34,10 +32,27 @@
 #include "fira_region_call.h"
 #include "fira_access.h"
 #include "fira_session.h"
-
+#include "fira_crypto.h"
 #include "warn_return.h"
 
 static struct mcps802154_region_ops fira_region_ops;
+static bool do_crypto_selftest_on_module_init;
+
+static void fira_report_event(struct work_struct *work)
+{
+	struct fira_local *local =
+		container_of(work, struct fira_local, report_work);
+	struct sk_buff *skb;
+	int r;
+
+	while (!skb_queue_empty(&local->report_queue)) {
+		skb = skb_dequeue(&local->report_queue);
+		r = mcps802154_region_event(local->llhw, skb);
+		if (r == -ECONNREFUSED)
+			/* TODO stop. */
+			;
+	}
+}
 
 static struct mcps802154_region *fira_open(struct mcps802154_llhw *llhw)
 {
@@ -48,17 +63,28 @@ static struct mcps802154_region *fira_open(struct mcps802154_llhw *llhw)
 		return NULL;
 	local->llhw = llhw;
 	local->region.ops = &fira_region_ops;
-	local->current_session = NULL;
 	INIT_LIST_HEAD(&local->inactive_sessions);
 	INIT_LIST_HEAD(&local->active_sessions);
+	skb_queue_head_init(&local->report_queue);
+	INIT_WORK(&local->report_work, fira_report_event);
+	/* FIXME: Hack to simplify unit test, which is borderline. */
 	local->block_duration_rx_margin_ppm = UWB_BLOCK_DURATION_MARGIN_PPM;
+	fira_crypto_init(NULL);
 	return &local->region;
 }
 
 static void fira_close(struct mcps802154_region *region)
 {
 	struct fira_local *local = region_to_local(region);
+	struct fira_session *session, *s;
+
+	list_for_each_entry_safe (session, s, &local->inactive_sessions,
+				  entry) {
+		fira_session_free(local, session);
+	}
 
+	cancel_work_sync(&local->report_work);
+	skb_queue_purge(&local->report_queue);
 	kfree_sensitive(local);
 }
 
@@ -68,8 +94,8 @@ static void fira_notify_stop(struct mcps802154_region *region)
 	struct fira_session *session, *s;
 
 	list_for_each_entry_safe (session, s, &local->active_sessions, entry) {
-		session->stop_request = true;
-		fira_session_access_done(local, session, false);
+		fira_session_stop_controlees(session);
+		fira_session_fsm_stop(local, session);
 	}
 }
 
@@ -86,236 +112,300 @@ static int fira_call(struct mcps802154_region *region, u32 call_id,
 	}
 }
 
-static int fira_get_demand(struct mcps802154_region *region,
-			   u32 next_timestamp_dtu,
-			   struct mcps802154_region_demand *demand)
+/**
+ * fira_session_init_block_start_dtu() - Build the first block start dtu.
+ * @local: FiRa context.
+ * @session: Session context.
+ * @timestamp_dtu: First access opportunity.
+ */
+static void fira_session_init_block_start_dtu(struct fira_local *local,
+					      struct fira_session *session,
+					      u32 timestamp_dtu)
 {
-	struct fira_local *local = region_to_local(region);
-	struct fira_session *session;
-
-	session = fira_session_next(
-		local, next_timestamp_dtu + local->llhw->anticip_dtu, 0);
-
-	if (session) {
-		fira_session_get_demand(local, session, demand);
-		demand->max_duration_dtu = session->last_access_duration_dtu;
-		local->current_session = session;
-		return 1;
+	if (!session->block_start_valid) {
+		const struct fira_session_params *params = &session->params;
+		int dtu_freq_khz = local->llhw->dtu_freq_hz / 1000;
+
+		session->block_start_valid = true;
+		session->block_start_dtu =
+			timestamp_dtu +
+			params->initiation_time_ms * dtu_freq_khz;
+		session->next_access_timestamp_dtu = session->block_start_dtu;
 	}
-	return 0;
-}
-
-static int fira_report_local_aoa(struct sk_buff *msg,
-				 const struct fira_local_aoa_info *info)
-{
-#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_AOA_ATTR_##x
-	if (nla_put_u8(msg, A(RX_ANTENNA_SET), info->rx_ant_set))
-		goto nla_put_failure;
-	if (nla_put_s16(msg, A(AOA_2PI), info->aoa_2pi))
-		goto nla_put_failure;
-	if (nla_put_s16(msg, A(PDOA_2PI), info->pdoa_2pi))
-		goto nla_put_failure;
-	if (nla_put_u8(msg, A(AOA_FOM), info->aoa_fom))
-		goto nla_put_failure;
-#undef A
-	return 0;
-nla_put_failure:
-	return -EMSGSIZE;
 }
 
-static int fira_report_measurement(struct fira_local *local,
-				   struct sk_buff *msg,
-				   const struct fira_ranging_info *ranging_info)
+/**
+ * fira_get_next_session() - Find the next session which should have the
+ * access.
+ * @local: FiRa context.
+ * @next_timestamp_dtu: Next access opportunity.
+ * @max_duration_dtu: Max duration of the next access opportunity.
+ * @adopted_demand: Output updated related to next session returned.
+ *
+ * Return: Pointer to the next session which should have the access.
+ */
+static struct fira_session *
+fira_get_next_session(struct fira_local *local, u32 next_timestamp_dtu,
+		      int max_duration_dtu,
+		      struct fira_session_demand *adopted_demand)
 {
-	const struct fira_session *session = local->current_session;
-	struct nlattr *aoa;
-#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_ATTR_##x
-
-	if (nla_put_u16(msg, A(SHORT_ADDR), ranging_info->short_addr) ||
-	    nla_put_u8(msg, A(STATUS), ranging_info->status))
-		goto nla_put_failure;
-
-	if (ranging_info->status) {
-		if (nla_put_u8(msg, A(SLOT_INDEX), ranging_info->slot_index))
-			goto nla_put_failure;
-		return 0;
-	}
+	struct fira_session *adopted_session = NULL;
+	struct fira_session *session;
+	bool is_candidate_unsync, is_adopted_unsync;
+	int max_unsync_duration_dtu = max_duration_dtu;
+	int r;
 
-	if (ranging_info->tof_present) {
-		static const s64 speed_of_light_mm_per_s = 299702547000ull;
-		s32 distance_mm = div64_s64(
-			ranging_info->tof_rctu * speed_of_light_mm_per_s,
-			(s64)local->llhw->dtu_freq_hz * local->llhw->dtu_rctu);
-		if (nla_put_s32(msg, A(DISTANCE_MM), distance_mm))
-			goto nla_put_failure;
+	/*
+	 * Little cheat to start all sessions as initiation_time_ms is a
+	 * delay, and not a absolute time. This is the only function
+	 * which change the session content.
+	 */
+	list_for_each_entry (session, &local->active_sessions, entry) {
+		fira_session_init_block_start_dtu(local, session,
+						  next_timestamp_dtu);
 	}
 
-	if (ranging_info->local_aoa.present) {
-		aoa = nla_nest_start(msg, A(LOCAL_AOA));
-		if (!aoa)
-			goto nla_put_failure;
-		if (fira_report_local_aoa(msg, &ranging_info->local_aoa))
-			goto nla_put_failure;
-		nla_nest_end(msg, aoa);
-	}
+	/*
+	 * Reminder: active_sessions list is sorted by session->priority
+	 * from highest priority to lowest priority.
+	 */
+	list_for_each_entry (session, &local->active_sessions, entry) {
+		/*
+		 * Welcome in sessions election!
+		 *
+		 * First, the candidate session will provide its wish in
+		 * 'candidate_demand'.
+		 * And then the candidate will be compared with the adopted
+		 * session. The "best" will be become or stay the adopted
+		 * session.
+		 * So the session election will process candidate after
+		 * candidate, to find the most appropriate session.
+		 */
+		struct fira_session_demand candidate_demand;
+
+		/*
+		 * Sessions with lower priority are not allowed to overlap
+		 * the adopted session. But a lower priority can start and
+		 * stop before the session adopted.
+		 */
+		if (adopted_session && adopted_session->params.priority !=
+					       session->params.priority) {
+			/* Is there some time left? */
+			if (is_before_dtu(next_timestamp_dtu,
+					  adopted_demand->timestamp_dtu))
+				/*
+				 * Limit max duration for session with lower
+				 * priority to not overlap sessions which have
+				 * an higher priority.
+				 */
+				max_duration_dtu =
+					adopted_demand->timestamp_dtu -
+					next_timestamp_dtu;
+			else
+				/* No more time left. */
+				break;
+			if (!max_unsync_duration_dtu ||
+			    max_unsync_duration_dtu > max_duration_dtu)
+				max_unsync_duration_dtu = max_duration_dtu;
+		}
 
-	if (ranging_info->local_aoa_azimuth.present) {
-		aoa = nla_nest_start(msg, A(LOCAL_AOA_AZIMUTH));
-		if (!aoa)
-			goto nla_put_failure;
-		if (fira_report_local_aoa(msg,
-					  &ranging_info->local_aoa_azimuth))
-			goto nla_put_failure;
-		nla_nest_end(msg, aoa);
-	}
-	if (ranging_info->local_aoa_elevation.present) {
-		aoa = nla_nest_start(msg, A(LOCAL_AOA_ELEVATION));
-		if (!aoa)
-			goto nla_put_failure;
-		if (fira_report_local_aoa(msg,
-					  &ranging_info->local_aoa_elevation))
-			goto nla_put_failure;
-		nla_nest_end(msg, aoa);
-	}
-	if (ranging_info->remote_aoa_azimuth_present) {
-		if (nla_put_s16(msg, A(REMOTE_AOA_AZIMUTH_2PI),
-				ranging_info->remote_aoa_azimuth_2pi))
-			goto nla_put_failure;
-		if (ranging_info->remote_aoa_fom_present) {
-			if (nla_put_u8(msg, A(REMOTE_AOA_AZIMUTH_FOM),
-				       ranging_info->remote_aoa_azimuth_fom))
-				goto nla_put_failure;
+		is_candidate_unsync = session->params.device_type ==
+					      FIRA_DEVICE_TYPE_CONTROLEE &&
+				      !session->controlee.synchronised;
+		/* Retrieve the wish of the session candidate. */
+		r = fira_session_fsm_get_demand(
+			local, session, next_timestamp_dtu,
+			is_candidate_unsync ? max_unsync_duration_dtu :
+					      max_duration_dtu,
+			&candidate_demand);
+		/* When 'r' is one, the session have a demand. */
+		if (r != 1)
+			/* The session doesn't have a demand. */
+			continue;
+
+		/*
+		 * If there is no adopted session, the candidate is the
+		 * adopted session.
+		 */
+		if (!adopted_session)
+			goto candidate_adopted;
+		/*
+		 * Is session finish before the adopted session ?
+		 * adopted_demand |              [-----]
+		 * candidate      |   [------]
+		 *              --+-----------------------> Time
+		 */
+		if (is_before_dtu(candidate_demand.timestamp_dtu +
+					  candidate_demand.max_duration_dtu,
+				  adopted_demand->timestamp_dtu))
+			/*
+			 * Candidate is adopted and replace the
+			 * previous one.
+			 */
+			goto candidate_adopted;
+		/*
+		 * Is session start after the adopted session ?
+		 * adopted_demand |   [------]
+		 * candidate      |             [--------]
+		 *              --+----------------------> Time
+		 */
+		if (is_before_dtu(adopted_demand->timestamp_dtu +
+					  adopted_demand->max_duration_dtu,
+				  candidate_demand.timestamp_dtu))
+			/* Candidate is not adopted. */
+			continue;
+		/*
+		 * The candidate session have an overlap with the adopted
+		 * session. Try the negotiation first to find an agreement
+		 * about the access usage.
+		 *
+		 * But take care, synchronized session have a better
+		 * eloquence in case of negotiation failure with an
+		 * unsynchronized session.
+		 */
+		is_adopted_unsync = adopted_session->params.device_type ==
+					    FIRA_DEVICE_TYPE_CONTROLEE &&
+				    !adopted_session->controlee.synchronised;
+		/*
+		 * The candidate session have an overlap with the adopted
+		 * session.
+		 *
+		 * adopted_demand |   [------]
+		 * candidate      |       [--------]
+		 *              --+----------------------> Time
+		 *
+		 * Request a duration reduction to the adopted session.
+		 */
+		if (is_adopted_unsync &&
+		    !is_before_dtu(candidate_demand.timestamp_dtu,
+				   adopted_demand->timestamp_dtu)) {
+			int limit_duration_dtu =
+				candidate_demand.timestamp_dtu -
+				adopted_demand->timestamp_dtu;
+			struct fira_session_demand tmp;
+
+			if (limit_duration_dtu)
+				/* Ask to reduce the duration. */
+				r = fira_session_fsm_get_demand(
+					local, adopted_session,
+					next_timestamp_dtu, limit_duration_dtu,
+					&tmp);
+			else
+				/* Both sessions start at same time. */
+				r = 0;
+			if (r == 1) {
+				/*
+				 * The adopted session accept to
+				 * reduction its max duration.
+				 */
+				*adopted_demand = tmp;
+				max_unsync_duration_dtu = limit_duration_dtu;
+				continue;
+			}
+			if (!is_candidate_unsync)
+				/*
+				 * In this corrupted world, synchronized
+				 * session have better relation.
+				 */
+				goto candidate_adopted;
 		}
-	}
-	if (ranging_info->remote_aoa_elevation_present) {
-		if (nla_put_s16(msg, A(REMOTE_AOA_ELEVATION_PI),
-				ranging_info->remote_aoa_elevation_pi))
-			goto nla_put_failure;
-		if (ranging_info->remote_aoa_fom_present) {
-			if (nla_put_u8(msg, A(REMOTE_AOA_ELEVATION_FOM),
-				       ranging_info->remote_aoa_elevation_fom))
-				goto nla_put_failure;
+		/*
+		 * The candidate session have an overlap with the adopted
+		 * session.
+		 *
+		 * adopted_demand |       [-----]
+		 * candidate      |  [------]
+		 *              --+----------------------> Time
+		 *
+		 * Request a duration reduction to the candidate session.
+		 */
+		if (is_candidate_unsync &&
+		    !is_before_dtu(adopted_demand->timestamp_dtu,
+				   candidate_demand.timestamp_dtu)) {
+			int limit_duration_dtu = adopted_demand->timestamp_dtu -
+						 candidate_demand.timestamp_dtu;
+			struct fira_session_demand tmp;
+
+			if (limit_duration_dtu)
+				/* Ask to reduce the duration. */
+				r = fira_session_fsm_get_demand(
+					local, session, next_timestamp_dtu,
+					limit_duration_dtu, &tmp);
+			else
+				/* Both sessions start at same time. */
+				r = 0;
+			if (r == 1) {
+				/*
+				 * The candidate session accept to
+				 * reduction its max duration.
+				 */
+				adopted_session = session;
+				*adopted_demand = tmp;
+				max_unsync_duration_dtu = limit_duration_dtu;
+				continue;
+			}
+			if (!is_adopted_unsync)
+				/*
+				 * In this corrupted world, synchronized
+				 * session have better relation.
+				 */
+				continue;
 		}
-	}
-	if (ranging_info->data_payload_len > 0) {
-		if (nla_put(msg, A(DATA_PAYLOAD_RECV),
-			    ranging_info->data_payload_len,
-			    ranging_info->data_payload))
-			goto nla_put_failure;
-	}
-	if (session->data_payload_seq_sent > 0) {
-		if (nla_put_u32(msg, A(DATA_PAYLOAD_SEQ_SENT),
-				session->data_payload_seq_sent))
-			goto nla_put_failure;
-	}
 
-#undef A
-	return 0;
-nla_put_failure:
-	return -EMSGSIZE;
+		/*
+		 * Finally, negotiation between adopted and candidate fails.
+		 * One of the session will probably have ranging not done.
+		 * Choose the session which have the oldest access.
+		 */
+		if (is_before_dtu(session->last_access_timestamp_dtu,
+				  adopted_session->last_access_timestamp_dtu))
+			goto candidate_adopted;
+
+		/* Candidate is not adopted. */
+		continue;
+
+	candidate_adopted:
+		adopted_session = session;
+		*adopted_demand = candidate_demand;
+	}
+	return adopted_session;
 }
 
-static int fira_report_measurement_stopped_controlee(struct fira_local *local,
-						     struct sk_buff *msg,
-						     __le16 short_addr)
+static struct mcps802154_access *
+fira_get_access(struct mcps802154_region *region, u32 next_timestamp_dtu,
+		int next_in_region_dtu, int region_duration_dtu)
 {
-#define A(x) FIRA_RANGING_DATA_MEASUREMENTS_ATTR_##x
-
-	if (nla_put_u16(msg, A(SHORT_ADDR), short_addr) ||
-	    nla_put_u8(msg, A(STOPPED), 1))
-		goto nla_put_failure;
-
-#undef A
-	return 0;
-nla_put_failure:
-	return -EMSGSIZE;
+	struct fira_local *local = region_to_local(region);
+	/* 'fsd' acronyms is FiRa Session Demand. */
+	struct fira_session_demand fsd;
+	struct fira_session *session;
+	int max_duration_dtu =
+		region_duration_dtu ? region_duration_dtu - next_in_region_dtu :
+				      0;
+
+	session = fira_get_next_session(local, next_timestamp_dtu,
+					max_duration_dtu, &fsd);
+	if (session)
+		return fira_session_fsm_get_access(local, session, &fsd);
+	return NULL;
 }
 
-void fira_report(struct fira_local *local, struct fira_session *session,
-		 bool add_measurements)
+static int fira_get_demand(struct mcps802154_region *region,
+			   u32 next_timestamp_dtu,
+			   struct mcps802154_region_demand *next_demand)
 {
-	struct sk_buff *msg;
-	struct nlattr *data, *measurements, *measurement;
-	int ranging_interval_ms, i, r;
-	bool stop_completed;
-
-	msg = mcps802154_region_event_alloc_skb(local->llhw, &local->region,
-						FIRA_CALL_SESSION_NOTIFICATION,
-						session->event_portid,
-						NLMSG_DEFAULT_SIZE, GFP_KERNEL);
-	if (!msg)
-		return;
-
-	if (nla_put_u32(msg, FIRA_CALL_ATTR_SESSION_ID, session->id))
-		goto nla_put_failure;
-
-	data = nla_nest_start(msg, FIRA_CALL_ATTR_RANGING_DATA);
-	if (!data)
-		goto nla_put_failure;
-
-	ranging_interval_ms = session->params.block_duration_dtu *
-			      (session->block_stride_len + 1) /
-			      (local->llhw->dtu_freq_hz / 1000);
-	if (nla_put_u32(msg, FIRA_RANGING_DATA_ATTR_BLOCK_INDEX,
-			session->block_index) ||
-	    nla_put_u32(msg, FIRA_RANGING_DATA_ATTR_RANGING_INTERVAL_MS,
-			ranging_interval_ms))
-		goto nla_put_failure;
-
-	stop_completed = (session->max_number_of_measurements_reached ||
-			  session->stop_request) &&
-			 !(session->controlee_management_flags &
-			   FIRA_SESSION_CONTROLEE_MANAGEMENT_FLAG_STOP);
-	if (stop_completed || session->stop_inband ||
-	    session->stop_no_response) {
-		enum fira_ranging_data_attrs_stopped_values stopped_value =
-			stop_completed ?
-				FIRA_RANGING_DATA_ATTR_STOPPED_REQUEST :
-				session->stop_inband ?
-				FIRA_RANGING_DATA_ATTR_STOPPED_IN_BAND :
-				FIRA_RANGING_DATA_ATTR_STOPPED_NO_RESPONSE;
-
-		if (nla_put_u8(msg, FIRA_RANGING_DATA_ATTR_STOPPED,
-			       stopped_value))
-			goto nla_put_failure;
-	}
-
-	if (add_measurements && (local->n_ranging_info +
-				 local->n_stopped_controlees_short_addr) != 0) {
-		measurements = nla_nest_start(
-			msg, FIRA_RANGING_DATA_ATTR_MEASUREMENTS);
-		if (!measurements)
-			goto nla_put_failure;
-
-		for (i = 0; i < local->n_ranging_info; i++) {
-			measurement = nla_nest_start(msg, 1);
-			if (fira_report_measurement(local, msg,
-						    &local->ranging_info[i]))
-				goto nla_put_failure;
-			nla_nest_end(msg, measurement);
-		}
-
-		for (i = 0; i < local->n_stopped_controlees_short_addr; i++) {
-			measurement = nla_nest_start(msg, 1);
-			if (fira_report_measurement_stopped_controlee(
-				    local, msg,
-				    local->stopped_controlees_short_addr[i]))
-				goto nla_put_failure;
-			nla_nest_end(msg, measurement);
-		}
+	struct fira_local *local = region_to_local(region);
+	/* 'fsd' for FiRa Session Demand. */
+	struct fira_session_demand fsd;
+	struct fira_session *session;
 
-		nla_nest_end(msg, measurements);
+	session = fira_get_next_session(local, next_timestamp_dtu, 0, &fsd);
+	if (session) {
+		next_demand->timestamp_dtu = fsd.timestamp_dtu;
+		next_demand->max_duration_dtu = fsd.max_duration_dtu;
+		return 1;
 	}
-
-	nla_nest_end(msg, data);
-
-	r = mcps802154_region_event(local->llhw, msg);
-	if (r == -ECONNREFUSED)
-		/* TODO stop. */
-		;
-	return;
-nla_put_failure:
-	kfree_skb(msg);
+	return 0;
 }
 
 static struct mcps802154_region_ops fira_region_ops = {
@@ -329,12 +419,50 @@ static struct mcps802154_region_ops fira_region_ops = {
 	.get_demand = fira_get_demand,
 };
 
-int __init fira_region_init(void)
+struct fira_session *fira_get_session_by_session_id(struct fira_local *local,
+						    u32 session_id)
 {
-	int r;
+	struct fira_session *session;
+
+	list_for_each_entry (session, &local->active_sessions, entry) {
+		if (session->id == session_id)
+			return session;
+	}
+	list_for_each_entry (session, &local->inactive_sessions, entry) {
+		if (session->id == session_id)
+			return session;
+	}
+	return NULL;
+}
 
-	r = fira_crypto_test();
-	WARN_RETURN(r);
+void fira_check_all_missed_ranging(struct fira_local *local,
+				   const struct fira_session *recent_session,
+				   u32 timestamp_dtu)
+{
+	struct fira_session *session, *s;
+
+	/*
+	 * Process sessions with safe function, as the session FSM can leave
+	 * the active list for many stop reasons.
+	 */
+	list_for_each_entry_safe (session, s, &local->active_sessions, entry) {
+		if (recent_session == session)
+			continue;
+		/*
+		 * Does the session started during the access of
+		 * recent_session?
+		 */
+		if (!session->block_start_valid)
+			continue;
+		fira_session_fsm_check_missed_ranging(local, session,
+						      timestamp_dtu);
+	}
+}
+
+int __init fira_region_init(void)
+{
+	if (do_crypto_selftest_on_module_init)
+		WARN_RETURN(fira_crypto_test());
 
 	return mcps802154_region_register(&fira_region_ops);
 }
@@ -344,6 +472,7 @@ void __exit fira_region_exit(void)
 	mcps802154_region_unregister(&fira_region_ops);
 }
 
+module_param_named(crypto_selftest, do_crypto_selftest_on_module_init, bool, 0644);
 module_init(fira_region_init);
 module_exit(fira_region_exit);