Google Git
Sign in
android / kernel / google-modules / uwb / f4943a3bc6a9af2588d4a8e88451c4da665d99b2 / . / mac / fira_access.c
blob: ac7c50a5b3b288301c755ce5c162df1cfffedc2b [file] [log] [blame]
/*
* This file is part of the UWB stack for linux.
*
* 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.
*
* You may choose to use this software under the terms of the GPLv2 License,
* version 2 ("GPLv2"), as published by the Free Software Foundation.
* You should have received a copy of the GPLv2 along with this program. If
* not, see <http://www.gnu.org/licenses/>.
*
* This program is distributed under the GPLv2 in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GPLv2 for more
* details.
*
* If you cannot meet the requirements of the GPLv2, you may not use this
* software for any purpose without first obtaining a commercial license from
* Qorvo. Please contact Qorvo to inquire about licensing terms.
*/
#include "fira_round_hopping_sequence.h"
#include "fira_access.h"
#include "fira_session.h"
#include "fira_frame.h"
#include "fira_trace.h"
#include "fira_sts.h"
#include <asm/unaligned.h>
#include <linux/string.h>
#include <linux/ieee802154.h>
#include <linux/math64.h>
#include <linux/limits.h>
#include <linux/errno.h>
#include <net/mcps802154_frame.h>
#include "warn_return.h"
#define FIRA_STS_FOM_THRESHOLD 153
#define FIRA_RSSI_MAX 0xff
/**
* sat_fp() - Saturate the range of fixed-point
* @x: fixed-point value on s32.
*
* Return: value saturate to s16 range
*/
static s16 sat_fp(s32 x)
{
if (x > S16_MAX)
return S16_MAX;
else if (x < S16_MIN)
return S16_MIN;
else
return x;
}
/**
* map_q11_to_2pi() - Map a Fixed Point angle to an signed 16 bit interger
* @x: angle as Q11 fixed_point value in range [-PI, PI]
*
* Return: the angle mapped to [INT16_MIN, INT16_MAX]
*/
static s16 map_q11_to_2pi(s16 x)
{
const s16 pi = 6434; /* Same as round(M_PI * (1 << 11)). */
s32 temp = (s32)x * S16_MAX;
temp /= pi;
return sat_fp(temp);
}
/**
* fira_access_setup_frame() - Fill an access frame from a FiRa slot.
* @local: FiRa context.
* @session: Session.
* @frame: Access frame.
* @sts_params: Where to store STS parameters.
* @slot: Corresponding slot.
* @frame_dtu: frame transmission or reception date.
* @is_tx: true for TX.
*/
static void fira_access_setup_frame(struct fira_local *local,
struct fira_session *session,
struct mcps802154_access_frame *frame,
struct mcps802154_sts_params *sts_params,
const struct fira_slot *slot, u32 frame_dtu,
bool is_tx)
{
const struct fira_session_params *params = &session->params;
struct mcps802154_access *access = &local->access;
struct mcps802154_sts_params *sts_params_for_access = NULL;
int rframe_config = session->params.rframe_config;
const struct fira_measurement_sequence_step *current_ms_step =
fira_session_get_meas_seq_step(session);
bool is_rframe = slot->message_id <= FIRA_MESSAGE_ID_RFRAME_MAX;
bool is_last_rframe = slot->message_id == FIRA_MESSAGE_ID_RANGING_FINAL;
bool is_first_frame = slot->message_id == FIRA_MESSAGE_ID_CONTROL;
bool request_rssi = session->params.report_rssi;
if (is_rframe) {
fira_sts_get_sts_params(session, slot->index, sts_params->v,
sizeof(sts_params->v), sts_params->key,
sizeof(sts_params->key));
sts_params->n_segs = params->number_of_sts_segments;
sts_params->seg_len =
params->sts_length == FIRA_STS_LENGTH_128 ?
128 :
params->sts_length == FIRA_STS_LENGTH_32 ? 32 :
64;
sts_params->sp2_tx_gap_4chips = 0;
sts_params->sp2_rx_gap_4chips[0] = 0;
sts_params->sp2_rx_gap_4chips[1] = 0;
sts_params->sp2_rx_gap_4chips[2] = 0;
sts_params->sp2_rx_gap_4chips[3] = 0;
sts_params_for_access = sts_params;
}
if (is_tx) {
u8 flags = MCPS802154_TX_FRAME_CONFIG_TIMESTAMP_DTU;
/* Add a small margin to the Tx timestamps. */
if (!is_first_frame)
frame_dtu += FIRA_TX_MARGIN_US *
local->llhw->dtu_freq_hz / 1000000;
if (is_rframe) {
struct fira_ranging_info *ranging_info;
ranging_info =
&local->ranging_info[slot->ranging_index];
ranging_info->timestamps_rctu[slot->message_id] =
mcps802154_tx_timestamp_dtu_to_rmarker_rctu(
local->llhw, frame_dtu,
access->hrp_uwb_params, access->channel,
slot->tx_ant_set);
flags |= MCPS802154_TX_FRAME_CONFIG_RANGING;
if (rframe_config == FIRA_RFRAME_CONFIG_SP3)
flags |= MCPS802154_TX_FRAME_CONFIG_SP3;
else
flags |= MCPS802154_TX_FRAME_CONFIG_SP1;
if (!is_last_rframe)
flags |=
MCPS802154_TX_FRAME_CONFIG_KEEP_RANGING_CLOCK;
} else if (is_first_frame) {
flags |= MCPS802154_TX_FRAME_CONFIG_RANGING_ROUND;
}
*frame = (struct mcps802154_access_frame){
.is_tx = true,
.tx_frame_config = {
.timestamp_dtu = frame_dtu,
.flags = flags,
.ant_set_id = slot->tx_ant_set,
},
.sts_params = sts_params_for_access,
};
} else {
u8 flags = MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU;
u16 request = 0;
if (request_rssi)
request |= MCPS802154_RX_FRAME_INFO_RSSI;
if (is_rframe) {
flags |= MCPS802154_RX_FRAME_CONFIG_RANGING;
if (rframe_config == FIRA_RFRAME_CONFIG_SP3)
flags |= MCPS802154_RX_FRAME_CONFIG_SP3;
else
flags |= MCPS802154_RX_FRAME_CONFIG_SP1;
if (!is_last_rframe)
flags |=
MCPS802154_RX_FRAME_CONFIG_KEEP_RANGING_CLOCK;
request |= MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU |
MCPS802154_RX_FRAME_INFO_RANGING_STS_FOM;
if (current_ms_step->type !=
FIRA_MEASUREMENT_TYPE_RANGE) {
flags |=
MCPS802154_RX_FRAME_CONFIG_RANGING_PDOA;
request |=
MCPS802154_RX_FRAME_INFO_RANGING_PDOA |
MCPS802154_RX_FRAME_INFO_RANGING_PDOA_FOM;
}
if (params->ranging_round_usage ==
FIRA_RANGING_ROUND_USAGE_SSTWR &&
session->params.device_type ==
FIRA_DEVICE_TYPE_CONTROLEE)
request |=
MCPS802154_RX_FRAME_INFO_RANGING_OFFSET;
}
*frame = (struct mcps802154_access_frame){
.is_tx = false,
.rx = {
.frame_config = {
.timestamp_dtu = frame_dtu,
.flags = flags,
.ant_set_id = slot->rx_ant_set,
},
.frame_info_flags_request = request,
},
.sts_params = sts_params_for_access,
};
}
}
static void fira_controlee_resync(struct fira_session *session,
u32 phy_sts_index, u32 timestamp_dtu)
{
u32 block_idx, round_idx, slot_idx;
int block_start_timestamp_dtu;
const struct fira_session_params *params = &session->params;
fira_sts_convert_phy_sts_idx_to_time_indexes(
session, phy_sts_index, &block_idx, &round_idx, &slot_idx);
block_start_timestamp_dtu =
timestamp_dtu -
(round_idx * session->params.round_duration_slots + slot_idx) *
params->slot_duration_dtu;
/* Update the session. */
session->block_start_dtu = block_start_timestamp_dtu;
session->block_index = block_idx;
session->round_index = round_idx;
session->controlee.synchronised = true;
session->controlee.next_round_index_valid = false;
session->controlee.block_index_sync = block_idx;
}
static bool fira_rx_sts_good(struct fira_local *local,
const struct mcps802154_rx_frame_info *info)
{
int i;
if (!(info->flags & MCPS802154_RX_FRAME_INFO_RANGING_STS_FOM))
return false;
for (i = 0; i < MCPS802154_STS_N_SEGS_MAX; i++) {
if (info->ranging_sts_fom[i] < FIRA_STS_FOM_THRESHOLD)
return false;
}
return true;
}
static void fira_ranging_info_set_status(struct fira_ranging_info *ranging_info,
enum fira_ranging_status status,
u8 slot_index)
{
/* Report first error. */
if (ranging_info->status)
return;
ranging_info->status = status;
ranging_info->slot_index = slot_index;
}
static void
fira_diagnostic_rssis(const struct mcps802154_rx_measurement_info *info,
struct fira_diagnostic *diagnostic)
{
if (info->flags & MCPS802154_RX_MEASUREMENTS_RSSIS) {
int max = max(MCPS802154_RSSIS_N_MAX - 1, info->n_rssis);
int i;
for (i = 0; i < max; i++)
diagnostic->rssis_q1[i] = info->rssis_q1[i];
diagnostic->n_rssis = i;
}
}
static void
fira_diagnostic_aoas(const struct mcps802154_rx_measurement_info *info,
struct fira_diagnostic *diagnostic)
{
int max = max(MCPS802154_RX_AOA_MEASUREMENTS_MAX - 1, info->n_aoas);
int i;
for (i = 0; i < max; i++)
diagnostic->aoas[i] = info->aoas[i];
diagnostic->n_aoas = info->n_aoas;
}
static struct mcps802154_rx_cir *
fira_diagnostic_cirs_alloc(const struct mcps802154_rx_measurement_info *info)
{
const struct mcps802154_rx_cir_sample_window *si;
struct mcps802154_rx_cir_sample_window *so;
struct mcps802154_rx_cir *cirs;
int i;
int j;
cirs = kmalloc(info->n_cirs * sizeof(struct mcps802154_rx_cir),
GFP_KERNEL);
if (!cirs)
return NULL;
for (i = 0; i < info->n_cirs; i++) {
so = &cirs[i].sample_window;
si = &info->cirs[i].sample_window;
so->samples =
kmalloc(si->n_samples * si->sizeof_sample, GFP_KERNEL);
if (!so->samples)
goto failed;
}
return cirs;
failed:
for (j = 0; j < i; j++)
kfree(cirs[j].sample_window.samples);
kfree(cirs);
return NULL;
}
static void
fira_diagnostic_cirs_copy(const struct mcps802154_rx_measurement_info *info,
struct fira_diagnostic *diagnostic)
{
int i;
for (i = 0; i < info->n_cirs; i++) {
struct mcps802154_rx_cir *cir_in;
struct mcps802154_rx_cir *cir_out;
struct mcps802154_rx_cir_sample_window *si;
struct mcps802154_rx_cir_sample_window *so;
cir_out = &diagnostic->cirs[i];
cir_in = &info->cirs[i];
so = &cir_out->sample_window;
si = &cir_in->sample_window;
cir_out->fp_index = cir_in->fp_index;
cir_out->fp_snr = cir_in->fp_snr;
cir_out->fp_ns_q6 = cir_in->fp_ns_q6;
cir_out->pp_index = cir_in->pp_index;
cir_out->pp_snr = cir_in->pp_snr;
cir_out->pp_ns_q6 = cir_in->pp_ns_q6;
cir_out->fp_sample_offset = cir_in->fp_sample_offset;
so->n_samples = si->n_samples;
so->sizeof_sample = si->sizeof_sample;
memcpy(so->samples, si->samples,
si->n_samples * si->sizeof_sample);
}
diagnostic->n_cirs = i;
}
static void
fira_diagnostic_cirs(const struct mcps802154_rx_measurement_info *info,
struct fira_diagnostic *diagnostic)
{
if (info->flags & MCPS802154_RX_MEASUREMENTS_CIRS) {
diagnostic->cirs = fira_diagnostic_cirs_alloc(info);
if (diagnostic->cirs)
fira_diagnostic_cirs_copy(info, diagnostic);
else
diagnostic->n_cirs = 0;
}
}
static void fira_diagnostic(struct fira_local *local,
struct fira_session *session, void *rx_ctx,
int slot_idx)
{
const struct fira_session_params *params = &session->params;
struct fira_diagnostic *diagnostic = &local->diagnostics[slot_idx];
struct mcps802154_rx_measurement_info info = {};
int r;
WARN_ON(diagnostic->cirs);
if (params->diagnostic_report_flags &
FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_RSSIS)
info.flags |= MCPS802154_RX_MEASUREMENTS_RSSIS;
if (params->diagnostic_report_flags &
FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_CIRS)
info.flags |= MCPS802154_RX_MEASUREMENTS_CIRS;
if (!info.flags)
return;
r = mcps802154_rx_get_measurement(local->llhw, rx_ctx, &info);
if (r)
return;
fira_diagnostic_rssis(&info, diagnostic);
fira_diagnostic_cirs(&info, diagnostic);
}
static void fira_diagnostic_free(struct fira_local *local)
{
int i;
for (i = 0; i < local->access.n_frames; i++) {
struct fira_diagnostic *diagnostic = &local->diagnostics[i];
int j;
for (j = 0; j < diagnostic->n_cirs; j++)
kfree(diagnostic->cirs[j].sample_window.samples);
kfree(diagnostic->cirs);
diagnostic->cirs = NULL;
diagnostic->n_cirs = 0;
}
}
static void fira_rx_frame_ranging(struct fira_local *local,
const struct fira_slot *slot,
struct sk_buff *skb,
const struct mcps802154_rx_frame_info *info)
{
const struct fira_session *session = local->current_session;
const struct fira_session_params *params = &session->params;
struct fira_ranging_info *ranging_info =
&local->ranging_info[slot->ranging_index];
struct mcps802154_ie_get_context ie_get = {};
bool pdoa_info_present;
if (!fira_rx_sts_good(local, info)) {
fira_ranging_info_set_status(
ranging_info, FIRA_STATUS_RANGING_RX_PHY_STS_FAILED,
slot->index);
return;
}
if (!(info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_RCTU)) {
fira_ranging_info_set_status(
ranging_info, FIRA_STATUS_RANGING_RX_PHY_TOA_FAILED,
slot->index);
return;
}
ranging_info->timestamps_rctu[slot->message_id] = info->timestamp_rctu;
pdoa_info_present = info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA;
if (pdoa_info_present) {
struct fira_local_aoa_info *local_aoa;
bool pdoa_fom_info_present =
info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA_FOM;
s16 local_pdoa_q11 = 0;
s16 local_aoa_q11 = 0;
const struct fira_measurement_sequence_step *current_step =
fira_session_get_meas_seq_step(session);
struct mcps802154_rx_measurement_info meas_info = {};
int r;
meas_info.flags |= MCPS802154_RX_MEASUREMENTS_AOAS;
r = mcps802154_rx_get_measurement(
local->llhw, ranging_info->rx_ctx, &meas_info);
if (!r && meas_info.flags & MCPS802154_RX_MEASUREMENTS_AOAS &&
meas_info.n_aoas) {
struct fira_diagnostic *diagnostic =
&local->diagnostics[slot->index];
/* TODO: Find which aoas index to use. */
local_pdoa_q11 = meas_info.aoas[0].pdoa_rad_q11;
local_aoa_q11 = meas_info.aoas[0].aoa_rad_q11;
if (params->diagnostic_report_flags &
FIRA_RANGING_DIAGNOSTICS_FRAME_REPORT_AOAS)
fira_diagnostic_aoas(&meas_info, diagnostic);
}
switch (current_step->type) {
case FIRA_MEASUREMENT_TYPE_AOA:
local_aoa = &ranging_info->local_aoa;
break;
case FIRA_MEASUREMENT_TYPE_AOA_AZIMUTH:
local_aoa = &ranging_info->local_aoa_azimuth;
break;
case FIRA_MEASUREMENT_TYPE_AOA_ELEVATION:
local_aoa = &ranging_info->local_aoa_elevation;
break;
case FIRA_MEASUREMENT_TYPE_AOA_AZIMUTH_ELEVATION:
local_aoa = (slot->message_id ==
FIRA_MESSAGE_ID_RANGING_FINAL) ?
&ranging_info->local_aoa_elevation :
&ranging_info->local_aoa_azimuth;
break;
default: /* LCOV_EXCL_START */
local_aoa = NULL;
/* LCOV_EXCL_STOP */
}
/* LCOV_EXCL_START */
if (local_aoa) {
/* LCOV_EXCL_STOP */
local_aoa->present = true;
local_aoa->rx_ant_set = slot->rx_ant_set;
local_aoa->pdoa_2pi = map_q11_to_2pi(local_pdoa_q11);
local_aoa->aoa_2pi = map_q11_to_2pi(local_aoa_q11);
/* LCOV_EXCL_START */
/* FoM is always expected when PDoA present. */
if (pdoa_fom_info_present)
/* LCOV_EXCL_STOP */
local_aoa->aoa_fom = info->ranging_pdoa_fom;
}
}
if (info->flags & MCPS802154_RX_FRAME_INFO_RANGING_OFFSET) {
ranging_info->clock_offset_q26 =
div64_s64((s64)info->ranging_offset_rctu << 26,
info->ranging_tracking_interval_rctu);
ranging_info->clock_offset_present = true;
}
if (skb) {
if (fira_frame_header_check_decrypt(local, slot, skb,
&ie_get) ||
!fira_frame_rframe_payload_check(local, slot, skb,
&ie_get)) {
fira_ranging_info_set_status(
ranging_info,
FIRA_STATUS_RANGING_RX_MAC_IE_DEC_FAILED,
slot->index);
}
}
}
static void fira_rx_frame_control(struct fira_local *local,
const struct fira_slot *slot,
struct sk_buff *skb,
const struct mcps802154_rx_frame_info *info)
{
struct mcps802154_access *access = &local->access;
struct fira_ranging_info *ri =
&local->ranging_info[slot->ranging_index];
struct mcps802154_ie_get_context ie_get = {};
const struct fira_session_params *params = NULL;
struct fira_session *session;
int header_len;
__le16 src_short_addr;
int last_slot_index = 0;
int offset_in_access_duration_dtu;
int left_duration_dtu;
unsigned n_slots;
u32 phy_sts_index;
u8 *header;
int r;
if (!(info->flags & MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU)) {
fira_ranging_info_set_status(
ri, FIRA_STATUS_RANGING_RX_PHY_DEC_FAILED, slot->index);
return;
}
offset_in_access_duration_dtu =
info->timestamp_dtu - access->timestamp_dtu;
/* Read the header to capture the session context. */
header = skb->data;
session = fira_rx_frame_control_header_check(local, slot, skb, &ie_get,
&phy_sts_index);
if (!session)
goto failed;
fira_controlee_resync(session, phy_sts_index, info->timestamp_dtu);
params = &session->params;
ri->rx_ctx = session->rx_ctx[0];
header_len = skb->data - header;
src_short_addr = slot->controller_tx ? local->dst_short_addr :
slot->controlee->short_addr;
/* Continue to decode the frame. */
r = fira_sts_decrypt_frame(session, skb, header_len, src_short_addr,
slot->index);
if (r)
goto failed;
r = fira_frame_control_payload_check(local, skb, &ie_get, &n_slots,
&session->stop_inband,
&session->block_stride_len);
if (!r)
goto failed;
left_duration_dtu =
access->duration_dtu - offset_in_access_duration_dtu;
if (left_duration_dtu < n_slots * params->slot_duration_dtu ||
session->stop_inband) {
n_slots = 1;
} else {
int i;
for (i = 1; i < n_slots; i++) {
const struct fira_slot *slot = &local->slots[i];
struct mcps802154_access_frame *frame =
&local->frames[i];
struct mcps802154_sts_params *sts_params =
&local->sts_params[i];
bool is_tx;
u32 frame_dtu;
is_tx = !slot->controller_tx;
frame_dtu = info->timestamp_dtu +
params->slot_duration_dtu * slot->index;
last_slot_index = slot->index;
fira_access_setup_frame(local, session, frame,
sts_params, slot, frame_dtu,
is_tx);
}
}
/* Trace the new (or not) session context and slots received. */
trace_region_fira_rx_frame_control(local, session, left_duration_dtu,
n_slots);
/* Update the access. */
access->duration_dtu =
offset_in_access_duration_dtu +
(last_slot_index + 1) * params->slot_duration_dtu;
access->n_frames = n_slots;
return;
failed:
params = &local->current_session->params;
access->duration_dtu =
offset_in_access_duration_dtu + params->slot_duration_dtu;
fira_ranging_info_set_status(
ri, FIRA_STATUS_RANGING_RX_MAC_IE_DEC_FAILED, slot->index);
}
static void fira_rx_frame_measurement_report(
struct fira_local *local, const struct fira_slot *slot,
struct sk_buff *skb, const struct mcps802154_rx_frame_info *info)
{
struct fira_ranging_info *ranging_info =
&local->ranging_info[slot->ranging_index];
struct mcps802154_ie_get_context ie_get = {};
if (fira_frame_header_check_decrypt(local, slot, skb, &ie_get))
goto failed;
if (!fira_frame_measurement_report_payload_check(local, slot, skb,
&ie_get))
goto failed;
return;
failed:
fira_ranging_info_set_status(ranging_info,
FIRA_STATUS_RANGING_RX_MAC_IE_DEC_FAILED,
slot->index);
}
static void
fira_rx_frame_result_report(struct fira_local *local,
const struct fira_slot *slot, struct sk_buff *skb,
const struct mcps802154_rx_frame_info *info)
{
struct fira_ranging_info *ranging_info =
&local->ranging_info[slot->ranging_index];
struct mcps802154_ie_get_context ie_get = {};
if (fira_frame_header_check_decrypt(local, slot, skb, &ie_get))
goto failed;
if (!fira_frame_result_report_payload_check(local, slot, skb, &ie_get))
goto failed;
return;
failed:
fira_ranging_info_set_status(ranging_info,
FIRA_STATUS_RANGING_RX_MAC_IE_DEC_FAILED,
slot->index);
}
static bool fira_do_process_rx_frame(enum mcps802154_rx_error_type error,
struct fira_ranging_info *ranging_info,
u8 slot_index)
{
enum fira_ranging_status status = FIRA_STATUS_RANGING_INTERNAL_ERROR;
switch (error) {
case MCPS802154_RX_ERROR_NONE:
return true;
case MCPS802154_RX_ERROR_SFD_TIMEOUT:
case MCPS802154_RX_ERROR_TIMEOUT:
case MCPS802154_RX_ERROR_HPDWARN:
status = FIRA_STATUS_RANGING_RX_TIMEOUT;
break;
case MCPS802154_RX_ERROR_FILTERED:
case MCPS802154_RX_ERROR_BAD_CKSUM:
status = FIRA_STATUS_RANGING_RX_MAC_DEC_FAILED;
break;
case MCPS802154_RX_ERROR_UNCORRECTABLE:
case MCPS802154_RX_ERROR_OTHER:
case MCPS802154_RX_ERROR_PHR_DECODE:
status = FIRA_STATUS_RANGING_RX_PHY_DEC_FAILED;
break;
}
fira_ranging_info_set_status(ranging_info, status, slot_index);
return false;
}
static void fira_rx_frame(struct mcps802154_access *access, int frame_idx,
struct sk_buff *skb,
const struct mcps802154_rx_frame_info *info,
enum mcps802154_rx_error_type error)
{
struct fira_local *local = access_to_local(access);
const struct fira_session_params *params;
const struct fira_slot *slot = &local->slots[frame_idx];
struct fira_ranging_info *ri =
&local->ranging_info[slot->ranging_index];
/* Don't initialize session before rx_frame_control. */
struct fira_session *session;
trace_region_fira_rx_frame(local->current_session, slot->message_id,
error);
if (info && info->flags & MCPS802154_RX_FRAME_INFO_RSSI) {
ri->rx_rssis[ri->n_rx_rssis++] =
info->rssi < FIRA_RSSI_MAX ? info->rssi : FIRA_RSSI_MAX;
}
if (fira_do_process_rx_frame(error, ri, slot->index)) {
switch (slot->message_id) {
case FIRA_MESSAGE_ID_RANGING_INITIATION:
case FIRA_MESSAGE_ID_RANGING_RESPONSE:
case FIRA_MESSAGE_ID_RANGING_FINAL:
fira_rx_frame_ranging(local, slot, skb, info);
break;
case FIRA_MESSAGE_ID_CONTROL:
fira_rx_frame_control(local, slot, skb, info);
break;
case FIRA_MESSAGE_ID_MEASUREMENT_REPORT:
fira_rx_frame_measurement_report(local, slot, skb,
info);
break;
case FIRA_MESSAGE_ID_RESULT_REPORT:
fira_rx_frame_result_report(local, slot, skb, info);
break;
case FIRA_MESSAGE_ID_CONTROL_UPDATE:
break;
default:
WARN_UNREACHABLE_DEFAULT();
}
}
/* Current session can change after call of rx_frame_control function. */
session = local->current_session;
session->last_access_timestamp_dtu = access->timestamp_dtu;
params = &session->params;
kfree_skb(skb);
fira_diagnostic(local, session, ri->rx_ctx, slot->index);
/*
* Controlee: Stop round on error.
* Controller: Stop when all ranging fails.
*/
/*
* TODO:
* The usage of ri->status is hidden in function called.
* The reason of the end of access is not limpid.
*/
if (ri->status != FIRA_STATUS_RANGING_SUCCESS) {
if (params->device_type == FIRA_DEVICE_TYPE_CONTROLEE ||
(slot->message_id <= FIRA_MESSAGE_ID_RFRAME_MAX &&
--local->n_ranging_valid == 0))
access->n_frames = frame_idx + 1;
}
}
static struct sk_buff *fira_tx_get_frame(struct mcps802154_access *access,
int frame_idx)
{
struct fira_local *local = access_to_local(access);
struct fira_session *session = local->current_session;
const struct fira_session_params *params = &session->params;
const struct fira_slot *slot = &local->slots[frame_idx];
struct sk_buff *skb;
int header_len;
trace_region_fira_tx_get_frame(session, slot->message_id);
if (params->rframe_config == FIRA_RFRAME_CONFIG_SP3 &&
slot->message_id <= FIRA_MESSAGE_ID_RFRAME_MAX)
return NULL;
skb = mcps802154_frame_alloc(local->llhw, IEEE802154_MTU, GFP_KERNEL);
if (!skb)
return NULL;
fira_frame_header_put(local, slot, skb);
switch (slot->message_id) {
case FIRA_MESSAGE_ID_RANGING_INITIATION:
case FIRA_MESSAGE_ID_RANGING_RESPONSE:
fira_frame_rframe_payload_put(local, skb);
break;
case FIRA_MESSAGE_ID_RANGING_FINAL:
break;
case FIRA_MESSAGE_ID_CONTROL:
fira_frame_control_payload_put(local, slot, skb);
break;
case FIRA_MESSAGE_ID_MEASUREMENT_REPORT:
fira_frame_measurement_report_payload_put(local, slot, skb);
break;
case FIRA_MESSAGE_ID_RESULT_REPORT:
fira_frame_result_report_payload_put(local, slot, skb);
break;
case FIRA_MESSAGE_ID_CONTROL_UPDATE:
break;
default: /* LCOV_EXCL_START */
kfree_skb(skb);
WARN_UNREACHABLE_DEFAULT();
return NULL;
/* LCOV_EXCL_STOP */
}
header_len = mcps802154_ie_put_end(skb, false);
WARN_ON(header_len < 0);
if (fira_sts_encrypt_frame(local->current_session, skb, header_len,
local->src_short_addr, slot->index)) {
kfree_skb(skb);
return NULL;
}
return skb;
}
static void fira_tx_return(struct mcps802154_access *access, int frame_idx,
struct sk_buff *skb,
enum mcps802154_access_tx_return_reason reason)
{
struct fira_local *local = access_to_local(access);
struct fira_session *session = local->current_session;
int i;
kfree_skb(skb);
/* Error on TX. */
trace_region_fira_tx_return(session, reason);
if (reason == MCPS802154_ACCESS_TX_RETURN_REASON_CANCEL) {
for (i = 0; i < local->n_ranging_info; i++) {
local->ranging_info[i].status =
FIRA_STATUS_RANGING_TX_FAILED;
}
}
}
static void fira_access_done(struct mcps802154_access *access, bool error)
{
struct fira_local *local = access_to_local(access);
struct fira_session *session = local->current_session;
u32 timestamp_dtu = access->timestamp_dtu;
trace_region_fira_access_done(local, session, access->duration_dtu,
error);
fira_session_fsm_access_done(local, session, error);
fira_diagnostic_free(local);
if (!error)
/* No access are infinite normally. */
timestamp_dtu += access->duration_dtu;
/*
* Must be call after FSM access done, because
* shared resource in local are used.
*/
fira_check_all_missed_ranging(local, session, timestamp_dtu);
}
static __le16 fira_access_set_short_address(struct fira_local *local,
const struct fira_session *session,
struct mcps802154_access *access)
{
const struct fira_session_params *params = &session->params;
__le16 src_short_addr = mcps802154_get_short_addr(local->llhw);
if (params->short_addr != IEEE802154_ADDR_SHORT_BROADCAST &&
src_short_addr != params->short_addr) {
access->hw_addr_filt = (struct ieee802154_hw_addr_filt){
.short_addr = params->short_addr,
};
access->hw_addr_filt_changed = IEEE802154_AFILT_SADDR_CHANGED;
return params->short_addr;
}
access->hw_addr_filt_changed = 0;
return src_short_addr;
}
static struct mcps802154_access_ops fira_controller_access_ops = {
.common = {
.access_done = fira_access_done,
},
.rx_frame = fira_rx_frame,
.tx_get_frame = fira_tx_get_frame,
.tx_return = fira_tx_return,
};
int fira_session_get_slot_count(const struct fira_session *session)
{
const struct fira_session_params *params = &session->params;
int nb_controlee = fira_session_controlees_running_count(session);
/* Control frame. */
int slot_count = 1;
if (nb_controlee) {
/* Ranging initiation frame. */
slot_count++;
/* Ranging response frame(s). */
slot_count += nb_controlee;
/* Ranging final frame. */
if (params->ranging_round_usage ==
FIRA_RANGING_ROUND_USAGE_DSTWR)
slot_count++;
/* Measurement report frame. */
slot_count++;
/* Result report frame(s). */
slot_count += nb_controlee;
}
return slot_count;
}
struct mcps802154_access *
fira_get_access_controller(struct fira_local *local,
const struct fira_session_demand *fsd)
{
struct fira_session *session = local->current_session;
const struct fira_session_params *params = &session->params;
const struct fira_measurement_sequence_step *step =
fira_session_get_meas_seq_step(session);
struct mcps802154_access *access = &local->access;
struct mcps802154_access_frame *frame;
struct mcps802154_sts_params *sts_params;
struct fira_ranging_info *ri;
struct fira_slot *s;
u32 frame_dtu;
int index = 0;
int i;
struct fira_controlee *controlee;
trace_region_fira_get_access_controller(local, session, fsd);
/* Update local context (shared memory used by all sessions). */
local->src_short_addr =
fira_access_set_short_address(local, session, access);
local->dst_short_addr =
session->n_current_controlees == 1 ?
list_first_entry(&session->current_controlees,
struct fira_controlee, entry)
->short_addr :
IEEE802154_ADDR_SHORT_BROADCAST;
/* Update session. */
session->last_access_timestamp_dtu = fsd->timestamp_dtu;
session->block_start_dtu = fsd->block_start_dtu;
session->block_index += fsd->add_blocks;
session->block_stride_len = params->block_stride_len;
session->round_index = fsd->round_index;
session->controller.next_block_index =
session->block_index + session->block_stride_len + 1;
session->next_round_index =
params->round_hopping ?
fira_round_hopping_sequence_get(
session, session->controller.next_block_index) :
0;
/* Build number of controlee which are stopped. */
local->n_stopped_controlees = 0;
list_for_each_entry (controlee, &session->current_controlees, entry) {
if (controlee->state == FIRA_CONTROLEE_STATE_STOPPING ||
controlee->state == FIRA_CONTROLEE_STATE_DELETING)
local->stopped_controlees[local->n_stopped_controlees++] =
controlee->short_addr;
}
/* Build number of controlee which are running. */
local->n_ranging_valid =
session->n_current_controlees - local->n_stopped_controlees;
/* Reset 'n' ranging info to store info related to controlees. */
local->n_ranging_info = local->n_ranging_valid;
ri = local->ranging_info;
memset(ri, 0,
local->n_ranging_valid * sizeof(struct fira_ranging_info));
/* Prepare control message slot for fira_rx_frame. */
s = local->slots;
s->index = index++;
s->controller_tx = true;
s->ranging_index = 0;
s->tx_ant_set = step->tx_ant_set_nonranging;
s->message_id = FIRA_MESSAGE_ID_CONTROL;
s->controlee = NULL;
s++;
/* Prepare other slots. */
if (local->n_ranging_info) {
s->index = index++;
s->controller_tx = true;
s->ranging_index = 0;
s->tx_ant_set = step->tx_ant_set_ranging;
s->message_id = FIRA_MESSAGE_ID_RANGING_INITIATION;
s->controlee = NULL;
s++;
i = 0;
list_for_each_entry (controlee, &session->current_controlees,
entry) {
if (!fira_session_controlee_active(controlee))
continue;
ri->short_addr = controlee->short_addr;
ri->rx_ctx = session->rx_ctx[i];
/* Requested in fira_report_aoa function. */
ri++;
s->index = index++;
s->controller_tx = false;
s->ranging_index = i++;
s->rx_ant_set = fira_session_get_rx_ant_set(
session, FIRA_MESSAGE_ID_RANGING_RESPONSE);
s->message_id = FIRA_MESSAGE_ID_RANGING_RESPONSE;
s->controlee = controlee;
s++;
}
if (params->ranging_round_usage ==
FIRA_RANGING_ROUND_USAGE_DSTWR) {
s->index = index++;
s->controller_tx = true;
s->ranging_index = 0;
s->tx_ant_set = step->tx_ant_set_ranging;
s->message_id = FIRA_MESSAGE_ID_RANGING_FINAL;
s->controlee = NULL;
s++;
}
s->index = index++;
s->controller_tx = true;
s->ranging_index = 0;
s->tx_ant_set = step->tx_ant_set_nonranging;
s->message_id = FIRA_MESSAGE_ID_MEASUREMENT_REPORT;
s->controlee = NULL;
s++;
if (params->result_report_phase) {
i = 0;
list_for_each_entry (controlee,
&session->current_controlees,
entry) {
if (!fira_session_controlee_active(controlee))
continue;
s->index = index++;
s->controller_tx = false;
s->ranging_index = i++;
s->rx_ant_set = step->rx_ant_set_nonranging;
s->message_id = FIRA_MESSAGE_ID_RESULT_REPORT;
s->controlee = controlee;
s++;
}
}
}
/* Configure frames for fproc. */
frame_dtu = fsd->timestamp_dtu;
for (i = 0; i < index; i++) {
s = &local->slots[i];
frame = &local->frames[i];
sts_params = &local->sts_params[i];
fira_access_setup_frame(local, session, frame, sts_params, s,
frame_dtu, s->controller_tx);
frame_dtu += params->slot_duration_dtu;
}
/*
* Configure the access.
* 'duration_dtu' can be decrease on reception error.
*/
access->ops = &fira_controller_access_ops;
access->timestamp_dtu = fsd->timestamp_dtu;
access->duration_dtu = frame_dtu - fsd->timestamp_dtu;
access->n_frames = index;
return access;
}
static struct mcps802154_access_ops fira_controlee_access_ops = {
.common = {
.access_done = fira_access_done,
},
.rx_frame = fira_rx_frame,
.tx_get_frame = fira_tx_get_frame,
.tx_return = fira_tx_return,
};
struct mcps802154_access *
fira_get_access_controlee(struct fira_local *local,
const struct fira_session_demand *fsd)
{
/*
* \ Important:
* '-.__.-' It's almost forbidden to update session
* /oo |--.--,--,--. content for a controlee. Because, the
* \_.-'._i__i__i_.' session can change on control frame received.
* """""""""
*/
struct fira_session *session = local->current_session;
const struct fira_session_params *params = &session->params;
struct mcps802154_access *access = &local->access;
struct mcps802154_access_frame *frame;
struct fira_ranging_info *ri;
struct fira_slot *s;
u16 request = MCPS802154_RX_FRAME_INFO_TIMESTAMP_DTU;
trace_region_fira_get_access_controlee(local, session, fsd);
/* Update local context (shared memory used by all sessions). */
local->src_short_addr =
fira_access_set_short_address(local, session, access);
local->dst_short_addr = params->controller_short_addr;
local->n_stopped_controlees = 0;
/*
* Update session.
* Updated values are used in case of bad reception (timeout/error).
* Otherwise on good reception, many are override.
* by the controlee resync function.
*/
session->block_start_dtu = fsd->block_start_dtu;
session->block_index += fsd->add_blocks;
/* Reset one ranging info to store info related to the controller. */
local->n_ranging_info = 1;
ri = local->ranging_info;
memset(ri, 0, sizeof(struct fira_ranging_info));
/*
* Warning:
* - short_addr is used when rx control have an error.
* - Be careful to not initialize too much in ri, because
* session can change on rx control.
*/
ri->short_addr = params->controller_short_addr;
/* Prepare control message slot for fira_rx_frame. */
s = local->slots;
s->index = 0;
s->controller_tx = true;
s->ranging_index = 0;
s->rx_ant_set =
fira_session_get_meas_seq_step(session)->rx_ant_set_nonranging;
s->message_id = FIRA_MESSAGE_ID_CONTROL;
s->controlee = NULL;
/* Configure frames for fproc. */
if (params->report_rssi)
request |= MCPS802154_RX_FRAME_INFO_RSSI;
frame = local->frames;
*frame = (struct mcps802154_access_frame){
.is_tx = false,
.rx = {
.frame_config = {
.timestamp_dtu = fsd->timestamp_dtu,
.timeout_dtu = fsd->rx_timeout_dtu,
.flags = MCPS802154_RX_FRAME_CONFIG_RANGING_ROUND |
MCPS802154_RX_FRAME_CONFIG_TIMESTAMP_DTU,
.ant_set_id = s->rx_ant_set,
},
.frame_info_flags_request = request,
},
};
/*
* Configure the access.
* 'duration_dtu' will be overridden on control frame reception.
*/
access->ops = &fira_controlee_access_ops;
access->timestamp_dtu = fsd->timestamp_dtu;
access->duration_dtu = fsd->max_duration_dtu;
access->n_frames = 1;
return access;
}