From: Viktor Barna <viktor.barna@xxxxxxxxxx>
(Part of the split. Please, take a look at the cover letter for more
details).
Signed-off-by: Viktor Barna <viktor.barna@xxxxxxxxxx>
---
drivers/net/wireless/celeno/cl8k/twt.c | 455 +++++++++++++++++++++++++
1 file changed, 455 insertions(+)
create mode 100644 drivers/net/wireless/celeno/cl8k/twt.c
diff --git a/drivers/net/wireless/celeno/cl8k/twt.c b/drivers/net/wireless/celeno/cl8k/twt.c
new file mode 100644
index 000000000000..3a601c1a3fb7
--- /dev/null
+++ b/drivers/net/wireless/celeno/cl8k/twt.c
@@ -0,0 +1,455 @@
+// SPDX-License-Identifier: MIT
+/* Copyright(c) 2019-2021, Celeno Communications Ltd. */
+
+#include "twt.h"
+#include "twt_frame.h"
+#include "fw/msg_tx.h"
+#include "sta.h"
+#include "utils/utils.h"
+
+#define CL_TWT_CONST_TARGET_WAKE_TIME_OFFSET_US 500000
+#define WIFI_AWARE_TARGET_WAKE_TIME_MASK ~0xFULL
+
+struct cl_next_start_time {
+ struct cl_twt_session_db *session;
+ u64 time;
+};
+
+static struct cl_twt_session_db *cl_twt_get_free_session(struct cl_hw *cl_hw)
+{
+ u8 i, max_sessions = cl_hw->conf->ce_twt_max_sessions;
+ struct cl_twt_session_db *session;
+
+ for (i = 0, session = &cl_hw->twt_db.cl_twt_sessions[0];
+ i < max_sessions;
+ i++, session++)
+ if (!session->cl_sta)
+ return session;
+
+ return NULL;
+}
+
+static void cl_twt_remove_session_by_params(struct cl_hw *cl_hw,
+ struct cl_sta *cl_sta,
+ u8 flow_id,
+ bool send_teardown_req)
+{
+ u8 i, handled_sessions = 0, num_sessions = cl_hw->twt_db.num_sessions;
+ u8 max_sessions = cl_hw->conf->ce_twt_max_sessions;
+ struct cl_twt_session_db *session;
+
+ /* If flow_id == CL_TWT_FLOW_ID_INVALID then we'd like to remove all the sessions of STA */
+ bool remove_all_flows = (flow_id == CL_TWT_FLOW_ID_ALL);
+
+ for (i = 0, session = &cl_hw->twt_db.cl_twt_sessions[0];
+ (i < max_sessions) && (handled_sessions < num_sessions);
+ i++, session++) {
+ if (session->cl_sta == cl_sta &&
+ (remove_all_flows ||
+ session->twt_setup.req_type.fields.flow_id == flow_id)) {
+ cl_twt_remove_session(cl_hw, session, send_teardown_req);
+
+ if (!remove_all_flows)
+ break;
+
+ handled_sessions++;
+ }
+ }
+}
+
+static void cl_twt_remove_all_sta_sessions(struct cl_hw *cl_hw,
+ struct cl_sta *cl_sta,
+ bool send_teardown_req)
+{
+
+ if (send_teardown_req)
+ cl_twt_frame_send_individual_teardown_request(cl_hw, cl_sta, CL_TWT_FLOW_ID_ALL);
+
+ cl_twt_remove_session_by_params(cl_hw, cl_sta, CL_TWT_FLOW_ID_ALL, false);
+}
+
+static u64 cl_twt_get_next_start_time(struct cl_hw *cl_hw,
+ struct cl_twt_session_db *session,
+ u64 tsf)
+{
+ struct ieee80211_twt_individual_elem *elem = &session->twt_setup;
+ u64 start_time = elem->target_wake_time;
+ u64 interval = cl_twt_get_wake_interval_us(elem);
+ u64 remainder = 0;
+ u64 intervals_from_start_to_tsf = div64_u64_rem(tsf - start_time, interval, &remainder);
+ u64 next_time = 0;
+
+ if (remainder)
+ /* We add 1 to intervals_from_start_to_tsf to get next_time > tsf */
+ next_time = ((intervals_from_start_to_tsf + 1) * interval + start_time);
+ else
+ /* No remainder means that the next start time equals exactly to tsf */
+ next_time = tsf;
+
+ return next_time;
+}
+
+static void cl_twt_get_next_start_times_sorted_array(struct cl_hw *cl_hw,
+ u64 tsf,
+ struct cl_next_start_time *next_start_times)
+{
+ u64 next_time;
+ u8 i, j, handled_sessions = 0, num_sessions = cl_hw->twt_db.num_sessions;
+ u8 max_sessions = cl_hw->conf->ce_twt_max_sessions;
+ u8 bytes_to_move = 0;
+ struct cl_twt_session_db *session;
+
+ /* Calculate the next TWT time of all sessions and save them in a sorted array. */
+ for (i = 0, session = &cl_hw->twt_db.cl_twt_sessions[0];
+ i < max_sessions && handled_sessions < num_sessions;
+ i++, session++) {
+ if (!session->cl_sta)
+ continue;
+
+ /* Calculate the current session's next start time */
+ next_time = cl_twt_get_next_start_time(cl_hw, session, tsf);
+
+ /* Find the correct position in the sorted array */
+ for (j = 0;
+ (j < handled_sessions) && (next_start_times[j].time <= next_time);
+ j++)
+ ;
+
+ /* Make room for the current session's next start time */
+ bytes_to_move = (handled_sessions - j) * sizeof(next_start_times[0]);
+
+ if (bytes_to_move)
+ memmove(&next_start_times[j + 1], &next_start_times[j],
+ bytes_to_move);
+
+ /* Insert the current session's next start time to the array */
+ next_start_times[j].session = session;
+ next_start_times[j].time = next_time;
+ handled_sessions++;
+ }
+}
+
+bool cl_twt_is_enabled(struct cl_hw *cl_hw)
+{
+ return cl_hw->conf->ce_twt_en;
+}
+
+void cl_twt_init(struct cl_hw *cl_hw)
+{
+ if (!cl_twt_is_enabled(cl_hw))
+ return;
+
+ memset(&cl_hw->twt_db, 0, sizeof(cl_hw->twt_db));
+}
+
+void cl_twt_close(struct cl_hw *cl_hw)
+{
+ u8 i, max_sessions = cl_hw->conf->ce_twt_max_sessions;
+ struct cl_twt_session_db *session;
+
+ if (!cl_hw->twt_db.num_sessions)
+ return;
+
+ /* Remove all sessions */
+ for (i = 0, session = &cl_hw->twt_db.cl_twt_sessions[0];
+ (i < max_sessions) && (cl_hw->twt_db.num_sessions);
+ i++, session++) {
+ if (!session->cl_sta)
+ continue;
+
+ cl_twt_remove_all_sta_sessions(cl_hw, session->cl_sta, false);
+ }
+}
+
+struct cl_twt_session_db *
+cl_twt_get_session(struct cl_hw *cl_hw, struct cl_sta *cl_sta, u8 flow_id)
+{
+ u8 i, max_sessions = cl_hw->conf->ce_twt_max_sessions;
+ struct cl_twt_session_db *session;
+
+ for (i = 0, session = &cl_hw->twt_db.cl_twt_sessions[0];
+ i < max_sessions;
+ i++, session++)
+ if (session->cl_sta == cl_sta &&
+ session->twt_setup.req_type.fields.flow_id == flow_id)
+ return session;
+
+ return NULL;
+}
+
+int cl_twt_update_session(struct cl_hw *cl_hw,
+ struct cl_sta *cl_sta,
+ struct ieee80211_twt_individual_elem *twt_elem,
+ struct cl_twt_session_db *session)
+{
+ session->cl_sta = cl_sta;
+ session->twt_setup = *twt_elem;
+
+ return 0;
+}
+
+int cl_twt_add_session(struct cl_hw *cl_hw,
+ struct cl_sta *cl_sta,
+ struct ieee80211_twt_individual_elem *twt_elem,
+ struct cl_twt_session_db **session)
+{
+ struct cl_twt_session_db *session_db = cl_twt_get_free_session(cl_hw);
+
+ if (!session_db) {
+ cl_dbg_err(cl_hw, "There are no available TWT sessions\n");
+ return -1;
+ }
+
+ cl_twt_update_session(cl_hw, cl_sta, twt_elem, session_db);
+ cl_hw->twt_db.num_sessions++;
+ *session = session_db;
+
+ return 0;
+}
+
+void cl_twt_remove_session(struct cl_hw *cl_hw,
+ struct cl_twt_session_db *session,
+ bool send_teardown_req)
+{
+ /* Send a TWT teardown frame */
+ if (send_teardown_req) {
+ u8 flow_id = session->twt_setup.req_type.fields.flow_id;
+
+ cl_twt_frame_send_individual_teardown_request(cl_hw, session->cl_sta, flow_id);
+ }
+
+ session->cl_sta = NULL;
+ memset(&session->twt_setup, 0, sizeof(session->twt_setup));
+ cl_hw->twt_db.num_sessions--;
+}
+
+int cl_twt_teardown_individual_sesseion(struct cl_hw *cl_hw,
+ struct cl_sta *cl_sta,
+ u8 flow_id,
+ bool send_teardown_req)
+{
+ struct cl_twt_session_db *session;
+ struct mm_twt_teardown_req req_msg = {
+ .sta_idx = cl_sta->sta_idx,
+ .twt_flow_id = flow_id
+ };
+
+ /* There are no sessions */
+ if (!cl_hw->twt_db.num_sessions)
+ return -1;
+
+ /* Remove the session(s) from the db */
+ if (flow_id == CL_TWT_FLOW_ID_ALL) {
+ cl_twt_remove_all_sta_sessions(cl_hw, cl_sta, send_teardown_req);
+ } else {
+ /* Check if the session exists */
+ session = cl_twt_get_session(cl_hw, cl_sta, flow_id);
+
+ if (!session) {
+ cl_dbg_err(cl_hw, "Cannot handle the TWT teardown request "
+ "since no such session was found: "
+ "sta_idx %u, flow_id %u\n",
+ cl_sta->sta_idx, flow_id);
+ return -1;
+ }
+
+ cl_twt_remove_session(cl_hw, session, send_teardown_req);
+ }
+
+ /* Send a msg to fw */
+ cl_msg_tx_twt_teardown(cl_hw, &req_msg);
+ return 0;
+}
+
+void cl_twt_sta_remove(struct cl_hw *cl_hw,
+ struct cl_sta *cl_sta)
+{
+ struct mm_twt_teardown_req req_msg = {
+ .sta_idx = cl_sta->sta_idx,
+ .twt_flow_id = CL_TWT_FLOW_ID_ALL
+ };
+
+ if (!cl_twt_is_enabled(cl_hw))
+ return;
+
+ /* Remove all sessions of STA from the db */
+ cl_twt_remove_all_sta_sessions(cl_hw, cl_sta, false);
+
+ /* Send a msg to fw */
+ cl_msg_tx_twt_teardown(cl_hw, &req_msg);
+}
+
+u64 cl_twt_get_wake_interval_us(struct ieee80211_twt_individual_elem *elem)
+{
+ u8 exponent = elem->req_type.fields.wake_interval_exponent;
+ u64 mantissa = elem->wake_interval_mantissa;
+
+ return (mantissa << exponent);
+}
+
+u32 cl_twt_get_min_wake_time_us(struct ieee80211_twt_individual_elem *elem)
+{
+ u8 duration_unit_exp = (elem->control.fields.wake_duration_unit << 1);
+ u32 wake_time = elem->min_wake_duration;
+
+ /* Convert to us (1 unit = 256us/1024us) */
+ return (wake_time << (8 + duration_unit_exp));
+}
+
+void cl_twt_set_target_wake_time(struct cl_hw *cl_hw,
+ struct ieee80211_twt_individual_elem *elem)
+{
+ /*
+ * Read TSF and add it an offset to make sure we set a future TSF.
+ * Make sure that tsf mod 16 = 0 to support WiFi aware requirement.
+ */
+ u64 tsf = (cl_get_tsf_u64(cl_hw) + CL_TWT_CONST_TARGET_WAKE_TIME_OFFSET_US) &
+ WIFI_AWARE_TARGET_WAKE_TIME_MASK;
+ u64 start_time = tsf;
+ struct cl_next_start_time sorted_next_start_times[CL_TWT_MAX_SESSIONS] = { { 0 } };
+ u8 i, num_sessions = cl_hw->twt_db.num_sessions;
+
+ if (cl_hw->twt_db.num_sessions > 0) {
+ u32 min_wake_time = cl_twt_get_min_wake_time_us(elem);
+
+ cl_twt_get_next_start_times_sorted_array(cl_hw, tsf, sorted_next_start_times);
+
+ /* Find the closest time for the new session */
+
+ for (i = 0; i < num_sessions; i++) {
+ struct cl_twt_session_db *curr_session =
+ sorted_next_start_times[i].session;
+ u32 curr_min_wake_time =
+ cl_twt_get_min_wake_time_us(&curr_session->twt_setup);
+ u64 curr_next_time = sorted_next_start_times[i].time;
+ u64 next_next_time;
+
+ /*
+ * We set the new session's start time such that the new TWT window
+ * would start right after the min wake time of the last session since
+ * we couldn't find a closer time for it.
+ */
+
+ if (i == num_sessions - 1) {
+ start_time = curr_next_time + curr_min_wake_time;
+ break;
+ }
+
+ next_next_time = sorted_next_start_times[i + 1].time;
+
+ /*
+ * We can put the new session between sessions i and i+1 without
+ * overlapping.
+ * The TWT windows would not overlap only if the intervals are a
+ * multiply of each other.
+ */
+
+ if (next_next_time - curr_next_time >=
+ curr_min_wake_time + min_wake_time) {
+ start_time = curr_next_time + curr_min_wake_time;
+ break;
+ }
+ }
+ }
+
+ elem->target_wake_time = start_time;
+}
+
+void cl_twt_set_min_wake_duration(struct cl_hw *cl_hw,
+ struct ieee80211_twt_individual_elem *elem,
+ u32 duration_us)
+{
+ u16 duration_in_units_of_256_us = (duration_us >> 8);
+
+ if (duration_in_units_of_256_us <= U8_MAX) {
+ elem->min_wake_duration = duration_in_units_of_256_us;
+ elem->control.fields.wake_duration_unit = 0;
+ } else {
+ /* Convert to units of 1024us */
+ duration_in_units_of_256_us >>= 2;
+ elem->min_wake_duration = duration_in_units_of_256_us;
+ elem->control.fields.wake_duration_unit = 1;
+ }
+
+ if (duration_in_units_of_256_us > U8_MAX)
+ cl_dbg_err(cl_hw, "Invalid min wake duration %u! exceeds 8 bits\n", duration_us);
+
+ cl_dbg_info(cl_hw, "Set TWT min wake duration to %u us: "
+ "duration val - %u, duration unit - %u us\n",
+ duration_us, elem->min_wake_duration,
+ (elem->control.fields.wake_duration_unit ? 1024 : 256));
+}
+
+void cl_twt_set_interval(struct cl_hw *cl_hw,
+ struct ieee80211_twt_individual_elem *elem,
+ u64 interval_us)
+{
+ u8 exponent;
+ u16 mantissa;
+
+ if (!interval_us)
+ return;
+
+ exponent = ffs((u32)interval_us) - 1;
+
+ elem->req_type.fields.wake_interval_exponent = exponent;
+ mantissa = (interval_us >> exponent);
+ elem->wake_interval_mantissa = mantissa;
+
+ if (mantissa > U16_MAX)
+ cl_dbg_err(cl_hw, "Invalid interval! mantissa exceeds 16 bits\n");
+
+ cl_dbg_info(cl_hw, "Set TWT interval to %llu us: exponent %u, mantissa %u\n",
+ interval_us, elem->req_type.fields.wake_interval_exponent,
+ elem->wake_interval_mantissa);
+}
+
+int cl_twt_handle_individual_setup_request(struct cl_hw *cl_hw,
+ struct cl_sta *cl_sta,
+ struct cl_ieee80211_mgmt *request)
+{
+ struct mm_twt_setup_req req_msg;
+ struct cl_twt_session_db *session = NULL;
+
+ /* Max sessions reached */
+ if (cl_hw->twt_db.num_sessions >= cl_hw->conf->ce_twt_max_sessions)
+ return -1;
+
+ /* Check the validity of the request */
+ if (!cl_twt_frame_is_individual_setup_request_valid(cl_hw, request)) {
+ cl_dbg_err(cl_hw, "TWT individual request is invalid!\n");
+ return -1;
+ }
+
+ /* Send a TWT response and add a new TWT session to the db */
+ if (cl_twt_frame_send_individual_setup_response(cl_hw, cl_sta, request, &session)) {
+ cl_dbg_err(cl_hw, "TWT response failed!\n");
+ return -1;
+ }
+
+ /* Fill and send a msg to fw */
+ req_msg.sta_idx = session->cl_sta->sta_idx;
+ req_msg.twt_flow_id = session->twt_setup.req_type.fields.flow_id;
+ req_msg.min_wake_duration_us = cl_twt_get_min_wake_time_us(&session->twt_setup);
+ req_msg.twt_interval_us = cl_twt_get_wake_interval_us(&session->twt_setup);
+ req_msg.twt_start_time_tsf = session->twt_setup.target_wake_time;
+ req_msg.announced = !(session->twt_setup.req_type.fields.flow_type);
+ req_msg.triggered = session->twt_setup.req_type.fields.trigger;
+
+ cl_msg_tx_twt_setup(cl_hw, &req_msg);
+ return 0;
+}
+
+int cl_twt_handle_individual_teardown_request(struct cl_hw *cl_hw,
+ struct cl_sta *cl_sta,
+ struct cl_ieee80211_mgmt *request)
+{
+ u8 flow_id = request->u.action.u.twt_individual_teardown.flow_id;
+ bool all_twt = request->u.action.u.twt_individual_teardown.teardown_all_twt;
+
+ if (all_twt)
+ flow_id = CL_TWT_FLOW_ID_ALL;
+
+ cl_twt_teardown_individual_sesseion(cl_hw, cl_sta, flow_id, false);
+ return 0;
+}
--
2.30.0
________________________________
The information transmitted is intended only for the person or entity to which it is addressed and may contain confidential and/or privileged material. Any retransmission, dissemination, copying or other use of, or taking of any action in reliance upon this information is prohibited. If you received this in error, please contact the sender and delete the material from any computer. Nothing contained herein shall be deemed as a representation, warranty or a commitment by Celeno. No warranties are expressed or implied, including, but not limited to, any implied warranties of non-infringement, merchantability and fitness for a particular purpose.
________________________________
