Progress in Mac.c file

mac.c

/*
 * Copyright (c) 2024 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */
#include <string.h>
#include <stdlib.h>
#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <nrf_modem_dect_phy.h>
#include <modem/nrf_modem_lib.h>
#include <zephyr/drivers/hwinfo.h>
#include "config.h"
#include "mac.h"
#include "utils.h"

#define MAC_REFRESH_STATUS_RATE_MS 5000

LOG_MODULE_REGISTER(mac);

BUILD_ASSERT(CONFIG_CARRIER, "Carrier must be configured according to local regulations");

/* Header type 1, due to endianness the order is different than in the specification. */
struct phy_ctrl_field_common
{
    uint32_t packet_length : 4;
    uint32_t packet_length_type : 1;
    uint32_t header_format : 3;
    uint32_t short_network_id : 8;
    uint32_t transmitter_id_hi : 8;
    uint32_t transmitter_id_lo : 8;
    uint32_t df_mcs : 3;
    uint32_t reserved : 1;
    uint32_t transmit_power : 4;
    uint32_t pad : 24;
};

#pragma pack(push, 1)
struct beacon_packet
{
    uint64_t reference_time; /** Reference time in ticks from FT device */
    uint32_t handshake_slot;
};
#pragma pack(pop)

/* Dect PHY config parameters. */
static struct nrf_modem_dect_phy_config_params dect_phy_config_params = {
    .band_group_index = ((CONFIG_CARRIER >= 525 && CONFIG_CARRIER <= 551)) ? 1 : 0,
    .harq_rx_process_count = 4,
    .harq_rx_expiry_time_us = 5000000,
};

static bool is_exit;
static uint16_t device_id;
static struct beacon_packet rx_beacon_packet;

/* Semaphore to synchronize modem calls. */
K_SEM_DEFINE(operation_sem, 0, 1);

K_SEM_DEFINE(deinit_sem, 0, 1);

static mac_layer *internal_instance = NULL;

static int mac_init_modem(void);
static void mac_dect_phy_event_handler(const struct nrf_modem_dect_phy_event *evt);
static void mac_gateway_start(void);
static void mac_node_start(void);
static int mac_is_slot(uint64_t current_modem_time, int slot_number);
static void mac_receive_beacon_reference_time(void *data);
static uint64_t mac_get_ticks_per_slot(uint64_t frame_duration_ms, uint64_t slots_per_frame);
static uint64_t mac_get_node_synced_time(uint64_t current_modem_time);
static uint64_t mac_calculate_start_time(uint64_t reference_time, uint32_t frame_offset, uint8_t slot_idx);
static int mac_shutdown_modem(void);

/**
 * @brief Singleton initialization for the mac_layer instance,
 * if the instance was already initialized then
 * it returns its pointer.
 */
mac_layer *mac_get_instance()
{
    if (internal_instance == NULL)
    {
        internal_instance = malloc(sizeof(mac_layer));
        internal_instance->modem_time = 0;
        internal_instance->modem_time_beacon_ref = 0;
        internal_instance->modem_time_synced = 0;
        internal_instance->node_reference_time = 0;
        internal_instance->tx_counter_value = 0;
        internal_instance->rx_counter_value = 0;
        internal_instance->tx_start_time = 0;
        return internal_instance;
    }

    return internal_instance;
}

void mac_set_role(mac_layer *instance, const char selected_role)
{
    if (internal_instance == NULL)
    {
        LOG_ERR("Intance not initialized, initialize the instance first.\n");
        return;
    }

    instance->selected_role = selected_role;
}

/**
 * @brief Allows to add an event listener for the mac layer.
 *
 * @param event_name the name of the event the current available
 * choices are MAC_RECEIVE, MAC_TRANSMIT_EVENT
 */
void mac_add_event_listener(int event_type, receive_packet_event_handler event_handler)
{
    if (internal_instance == NULL)
    {
        LOG_ERR("Intance not initialized, initialize the instance first.\n");
        return;
    }

    switch (event_type)
    {
        case MAC_EVENT_RECEIVE:
            internal_instance->receive_event_handler = event_handler;
            break;
        case MAC_EVENT_TRANSMIT:
            internal_instance->transmit_event_handler = event_handler;
            break;
        default:
            break;
    }
}

int mac_init_layer(mac_layer *instance)
{
    int err;

    err = mac_init_modem();
    if (err != 0)
    {
        return err;
    }

    if (instance->selected_role != MAC_ROLE_GATEWAY && instance->selected_role != MAC_ROLE_NODE)
    {
        LOG_ERR("(!) No role for this device was assigned\n");
        return 1;
    }

    switch (instance->selected_role)
    {
        case MAC_ROLE_GATEWAY:
            LOG_INF("\n*** Gateway Role (FT) Starting broadcasting ***\n");
            mac_gateway_start();
            break;
        case MAC_ROLE_NODE:
            LOG_INF("\nNode Role (PT) Starting scanning\n");
            mac_node_start();
            break;
        default:
            LOG_INF("\n");
            break;
    }

    err = mac_shutdown_modem();

    if (err != 0)
    {
        return err;
    }

    return 0;
}

/* Send operation. */
int mac_transmit(uint64_t start_time, void *data)
{
    int err;
    uint32_t tx_handle = 0;

    struct phy_ctrl_field_common header = {
        .header_format = 0x0,
        .packet_length_type = 0x0,
        .packet_length = 0x01,
        .short_network_id = (CONFIG_NETWORK_ID & 0xff),
        .transmitter_id_hi = (device_id >> 8),
        .transmitter_id_lo = (device_id & 0xff),
        .transmit_power = CONFIG_TX_POWER,
        .reserved = 0,
        .df_mcs = CONFIG_MCS,
    };

    struct nrf_modem_dect_phy_tx_params tx_op_params = {
        .start_time = start_time,
        .handle = tx_handle,
        .network_id = CONFIG_NETWORK_ID,
        .phy_type = 0,
        .lbt_rssi_threshold_max = 0,
        .carrier = CONFIG_CARRIER,
        .lbt_period = NRF_MODEM_DECT_LBT_PERIOD_MAX,
        .phy_header = (union nrf_modem_dect_phy_hdr *)&header,
        .data = data,
        .data_size = sizeof(data),
    };

    err = nrf_modem_dect_phy_tx(&tx_op_params);
    if (err != 0)
    {
        return err;
    }

    return 0;
}

/* Receive operation. */
int mac_receive(void)
{
    int err;
    int rx_handle = 1;

    uint64_t ticks_per_frame = mac_convert_ms_to_ticks(MAC_FRAME_DURATION_MS);

    struct nrf_modem_dect_phy_rx_params rx_op_params = {
        .start_time = 0,
        .handle = rx_handle,
        .network_id = CONFIG_NETWORK_ID,
        .mode = NRF_MODEM_DECT_PHY_RX_MODE_CONTINUOUS,
        .rssi_interval = NRF_MODEM_DECT_PHY_RSSI_INTERVAL_OFF,
        .link_id = NRF_MODEM_DECT_PHY_LINK_UNSPECIFIED,
        .rssi_level = -60,
        .carrier = CONFIG_CARRIER,
        .duration = ticks_per_frame,
        .filter.short_network_id = CONFIG_NETWORK_ID & 0xff,
        .filter.is_short_network_id_used = 1,
        /* listen for everything (broadcast mode used) */
        .filter.receiver_identity = 0,
    };

    err = nrf_modem_dect_phy_rx(&rx_op_params);
    if (err != 0)
    {
        return err;
    }

    return 0;
}
int mac_get_bytes_sent(mac_layer *instance)
{
    return instance->bytes_sent;
}

int mac_get_bytes_received(mac_layer *instance)
{
    return instance->bytes_received;
}

void mac_destroy(mac_layer *instance)
{
    free(instance);
}

static int mac_init_modem()
{
    /** Initialize */
    int err;

    LOG_INF("*** Dect NR+ MAC Layer started ***");
    err = nrf_modem_lib_init();
    if (err)
    {
        LOG_ERR("modem init failed, err %d", err);
        return err;
    }

    err = nrf_modem_dect_phy_event_handler_set(mac_dect_phy_event_handler);
    if (err)
    {
        LOG_ERR("nrf_modem_dect_phy_event_handler_set failed, err %d", err);
        return err;
    }

    err = nrf_modem_dect_phy_init();
    if (err)
    {
        LOG_ERR("nrf_modem_dect_phy_init failed, err %d", err);
        return err;
    }

    k_sem_take(&operation_sem, K_FOREVER);
    if (is_exit)
    {
        return -EIO;
    }

    err = nrf_modem_dect_phy_configure(&dect_phy_config_params);
    if (err)
    {
        LOG_ERR("nrf_modem_dect_phy_configure failed, err %d", err);
        return err;
    }

    k_sem_take(&operation_sem, K_FOREVER);
    if (is_exit)
    {
        return -EIO;
    }

    err = nrf_modem_dect_phy_activate(NRF_MODEM_DECT_PHY_RADIO_MODE_LOW_LATENCY);
    if (err)
    {
        LOG_ERR("nrf_modem_dect_phy_activate failed, err %d", err);
        return err;
    }

    k_sem_take(&operation_sem, K_FOREVER);
    if (is_exit)
    {
        return -EIO;
    }

    hwinfo_get_device_id((void *)&device_id, sizeof(device_id));

    LOG_INF("Dect NR+ PHY initialized, device ID: %d", device_id);

    err = nrf_modem_dect_phy_capability_get();
    if (err)
    {
        LOG_ERR("nrf_modem_dect_phy_capability_get failed, err %d", err);
    }

    return 0;
}

static void mac_gateway_start(void)
{
    int err;
    uint64_t start_modem_time = internal_instance->modem_time;

    internal_instance->tx_counter_value = 0;

    while (1)
    {
        /** Gets modem time */
        nrf_modem_dect_phy_time_get();

        uint64_t tx_start_time = mac_calculate_start_time(internal_instance->modem_time, 1, 0);
        internal_instance->tx_start_time = tx_start_time;

        uint64_t modem_time_diff = internal_instance->modem_time - start_modem_time;

        if (modem_time_diff >= mac_convert_ms_to_ticks(MAC_REFRESH_STATUS_RATE_MS))
        {
            mac_display_state(internal_instance);
            start_modem_time = internal_instance->modem_time;
        }

        /** Slot 0 */
        struct beacon_packet beacon;
        memset(&beacon, 0, sizeof(struct beacon_packet)); /** Initializes all struct fields to 0 */
        beacon.reference_time = internal_instance->modem_time;
        beacon.handshake_slot = 1;

        err = mac_transmit(0, &beacon);

        if (err != 0)
        {
            LOG_ERR("Transmission failed, err %d", err);
            return;
        }

        internal_instance->tx_counter_value++;

        /* Wait for TX operation to complete. */
        k_sem_take(&operation_sem, K_FOREVER);
        
    }
}

static void mac_node_start(void)
{
    int err;
    memset(&rx_beacon_packet, 0, sizeof(rx_beacon_packet)); /** Initializes all struct fields to 0 */

    uint64_t start_modem_time = internal_instance->modem_time_synced;
    
    internal_instance->rx_counter_value = 0;

    while (1)
    {
        /** Gets modem time */
        nrf_modem_dect_phy_time_get();

        uint64_t modem_time_diff = internal_instance->modem_time_synced - start_modem_time;
        
        if (modem_time_diff >= mac_convert_ms_to_ticks(MAC_REFRESH_STATUS_RATE_MS))
        {
            mac_display_state(internal_instance);
            start_modem_time = internal_instance->modem_time_synced;
        }

        /** Receiving messages at the start of every frame */
        err = mac_receive();
        if (err)
        {
            LOG_ERR("Reception failed, err %d", err);
            return;
        }
        internal_instance->rx_counter_value++;

        k_sem_take(&operation_sem, K_FOREVER);

        /** Received reference time */
        if (rx_beacon_packet.reference_time == 0)
        {
            continue;
        }

        /** Transmit data */
        uint64_t reference_time = mac_convert_ms_to_ticks(rx_beacon_packet.reference_time);
        uint64_t start_time = mac_calculate_start_time(reference_time, 1, 0);
        
        /** Running the transmit data event */
        internal_instance->transmit_event_handler((void*) &start_time);

        /* Wait for TX operation to complete. */
        k_sem_take(&operation_sem, K_FOREVER);
    }
}

static int mac_is_slot(uint64_t current_modem_time, int slot_number)
{
    uint64_t ticks_per_slot = mac_get_ticks_per_slot(MAC_FRAME_DURATION_MS, MAC_SLOTS_PER_FRAME);
    int module_op = current_modem_time % (mac_convert_ms_to_ticks(MAC_FRAME_DURATION_MS) + (slot_number * ticks_per_slot));
    return module_op == 0 ? 1 : 0;
}

/**
 * @brief Get the start time using a reference time (like beacon time for scheduling),
 * a frame offset (a frame in the future to transmit) and an slot_index (0-23) to transmit
 * on a certain slot out of the 24 available.
 *
 * @param reference_time a reference time like rx beacon modem time for scheduling.
 * @param frame_offset a frame in the future to transmit (min 1 for continuous transmission or reception)
 * @param slot_idx the slot where the the device wants to transmit or receive it should be between 0 and 24.
 * @return uint64_t in ticks.
 */
static uint64_t mac_calculate_start_time(uint64_t reference_time, uint32_t frame_offset, uint8_t slot_idx)
{
    if (slot_idx >= MAC_SLOTS_PER_FRAME)
    {
        LOG_ERR("%s: Error, slots above %d, the number of slots should be lower than this", __func__, MAC_SLOTS_PER_FRAME);
        return 0;
    }

    uint64_t ticks_per_frame = mac_convert_ms_to_ticks(MAC_FRAME_DURATION_MS);
    uint64_t ticks_per_slot = mac_get_ticks_per_slot(MAC_FRAME_DURATION_MS, MAC_SLOTS_PER_FRAME);

    uint64_t target_frame_start = reference_time + (frame_offset * ticks_per_frame);
    uint64_t final_start_time = target_frame_start + (slot_idx * ticks_per_slot);
    return final_start_time;
}

static uint64_t mac_get_ticks_per_slot(uint64_t frame_duration_ms, uint64_t slots_per_frame)
{
    return mac_convert_ms_to_ticks(frame_duration_ms) / slots_per_frame;
}

static uint64_t mac_get_node_synced_time(uint64_t current_modem_time)
{
    int64_t signed_node_reference_time = internal_instance->node_reference_time;
    int64_t signed_modem_beacon_ref = internal_instance->modem_time_beacon_ref;
    int64_t time_diff = signed_node_reference_time - signed_modem_beacon_ref;
    
    return current_modem_time + time_diff;
}

static void mac_receive_beacon_reference_time(void *data)
{
    internal_instance->receive_event_handler(data);

    struct beacon_packet *received_beacon_packet = (struct beacon_packet *) data;

    if (received_beacon_packet->reference_time != 0)
    {
        rx_beacon_packet = *received_beacon_packet;
        internal_instance->node_reference_time = received_beacon_packet->reference_time;
        internal_instance->modem_time_beacon_ref = internal_instance->modem_time;
    }
}

static int mac_shutdown_modem()
{
    int err;
    LOG_INF("Shutting down");

    err = nrf_modem_dect_phy_deactivate();
    if (err)
    {
        LOG_ERR("nrf_modem_dect_phy_deactivate failed, err %d", err);
        return err;
    }

    k_sem_take(&deinit_sem, K_FOREVER);

    err = nrf_modem_dect_phy_deinit();
    if (err)
    {
        LOG_ERR("nrf_modem_dect_phy_deinit() failed, err %d", err);
        return err;
    }

    k_sem_take(&deinit_sem, K_FOREVER);

    err = nrf_modem_lib_shutdown();
    if (err)
    {
        LOG_ERR("nrf_modem_lib_shutdown() failed, err %d", err);
        return err;
    }

    LOG_INF("Bye!");

    return 0;
}

/** -- Events -- */

static void on_init(const struct nrf_modem_dect_phy_init_event *evt)
{
    if (evt->err)
    {
        LOG_ERR("Init failed, err %d", evt->err);
        is_exit = true;
        return;
    }

    k_sem_give(&operation_sem);
}

static void on_deinit(const struct nrf_modem_dect_phy_deinit_event *evt)
{
    if (evt->err)
    {
        LOG_ERR("Deinit failed, err %d", evt->err);
        return;
    }

    k_sem_give(&deinit_sem);
}

static void on_activate(const struct nrf_modem_dect_phy_activate_event *evt)
{
    if (evt->err)
    {
        LOG_ERR("Activate failed, err %d", evt->err);
        is_exit = true;
        return;
    }

    k_sem_give(&operation_sem);
}

static void on_deactivate(const struct nrf_modem_dect_phy_deactivate_event *evt)
{

    if (evt->err)
    {
        LOG_ERR("Deactivate failed, err %d", evt->err);
        return;
    }

    k_sem_give(&deinit_sem);
}

static void on_configure(const struct nrf_modem_dect_phy_configure_event *evt)
{
    if (evt->err)
    {
        LOG_ERR("Configure failed, err %d", evt->err);
        return;
    }

    k_sem_give(&operation_sem);
}

/* Callback after link configuration operation. */
static void on_link_config(const struct nrf_modem_dect_phy_link_config_event *evt)
{
    LOG_DBG("link_config cb time %" PRIu64 " status %d", internal_instance->modem_time, evt->err);
}

static void on_radio_config(const struct nrf_modem_dect_phy_radio_config_event *evt)
{
    if (evt->err)
    {
        LOG_ERR("Radio config failed, err %d", evt->err);
        return;
    }

    k_sem_give(&operation_sem);
}

/* Callback after capability get operation. */
static void on_capability_get(const struct nrf_modem_dect_phy_capability_get_event *evt)
{
    LOG_DBG("capability_get cb time %" PRIu64 " status %d", internal_instance->modem_time, evt->err);
}

static void on_bands_get(const struct nrf_modem_dect_phy_band_get_event *evt)
{
    LOG_DBG("bands_get cb status %d", evt->err);
}

static void on_latency_info_get(const struct nrf_modem_dect_phy_latency_info_event *evt)
{
    LOG_DBG("latency_info_get cb status %d", evt->err);
}

/* Callback after time query operation. */
static void on_time_get(const struct nrf_modem_dect_phy_time_get_event *evt)
{
    LOG_DBG("time_get cb time %" PRIu64 " status %d", internal_instance->modem_time, evt->err);
}

static void on_cancel(const struct nrf_modem_dect_phy_cancel_event *evt)
{
    LOG_DBG("on_cancel cb status %d", evt->err);
    k_sem_give(&operation_sem);
}

/* Operation complete notification. */
static void on_op_complete(const struct nrf_modem_dect_phy_op_complete_event *evt)
{
    LOG_DBG("op_complete cb time %" PRIu64 " status %d", internal_instance->modem_time, evt->err);
    k_sem_give(&operation_sem);
}

/* Physical Control Channel reception notification. */
static void on_pcc(const struct nrf_modem_dect_phy_pcc_event *evt)
{
    LOG_DBG("Received header from device ID %d",
            evt->hdr.hdr_type_1.transmitter_id_hi << 8 | evt->hdr.hdr_type_1.transmitter_id_lo);
}

/* Physical Control Channel CRC error notification. */
static void on_pcc_crc_err(const struct nrf_modem_dect_phy_pcc_crc_failure_event *evt)
{
    LOG_DBG("pcc_crc_err cb time %" PRIu64 "", internal_instance->modem_time);
}

/* Physical Data Channel reception notification. */
static void on_pdc(const struct nrf_modem_dect_phy_pdc_event *evt)
{
    /* Received RSSI value is in fixed precision format Q14.1 */
    LOG_DBG("Received data (RSSI: %d.%d): %u bytes",
            (evt->rssi_2 / 2), (evt->rssi_2 & 0b1) * 5, sizeof(evt->data));

    mac_receive_beacon_reference_time(evt->data);
}

/* Physical Data Channel CRC error notification. */
static void on_pdc_crc_err(const struct nrf_modem_dect_phy_pdc_crc_failure_event *evt)
{
    LOG_DBG("pdc_crc_err cb time %" PRIu64 "", internal_instance->modem_time);
}

/* RSSI measurement result notification. */
static void on_rssi(const struct nrf_modem_dect_phy_rssi_event *evt)
{
    LOG_DBG("rssi cb time %" PRIu64 " carrier %d", internal_instance->modem_time, evt->carrier);
}

static void on_stf_cover_seq_control(const struct nrf_modem_dect_phy_stf_control_event *evt)
{
    LOG_WRN("Unexpectedly in %s\n", (__func__));
}

static void mac_dect_phy_event_handler(const struct nrf_modem_dect_phy_event *evt)
{
    internal_instance->modem_time = evt->time;
    internal_instance->modem_time_synced = mac_get_node_synced_time(internal_instance->modem_time);

    switch (evt->id)
    {
    case NRF_MODEM_DECT_PHY_EVT_INIT:
        on_init(&evt->init);
        break;
    case NRF_MODEM_DECT_PHY_EVT_DEINIT:
        on_deinit(&evt->deinit);
        break;
    case NRF_MODEM_DECT_PHY_EVT_ACTIVATE:
        on_activate(&evt->activate);
        break;
    case NRF_MODEM_DECT_PHY_EVT_DEACTIVATE:
        on_deactivate(&evt->deactivate);
        break;
    case NRF_MODEM_DECT_PHY_EVT_CONFIGURE:
        on_configure(&evt->configure);
        break;
    case NRF_MODEM_DECT_PHY_EVT_RADIO_CONFIG:
        on_radio_config(&evt->radio_config);
        break;
    case NRF_MODEM_DECT_PHY_EVT_COMPLETED:
        on_op_complete(&evt->op_complete);
        break;
    case NRF_MODEM_DECT_PHY_EVT_CANCELED:
        on_cancel(&evt->cancel);
        break;
    case NRF_MODEM_DECT_PHY_EVT_RSSI:
        on_rssi(&evt->rssi);
        break;
    case NRF_MODEM_DECT_PHY_EVT_PCC:
        on_pcc(&evt->pcc);
        break;
    case NRF_MODEM_DECT_PHY_EVT_PCC_ERROR:
        on_pcc_crc_err(&evt->pcc_crc_err);
        break;
    case NRF_MODEM_DECT_PHY_EVT_PDC:
        on_pdc(&evt->pdc);
        break;
    case NRF_MODEM_DECT_PHY_EVT_PDC_ERROR:
        on_pdc_crc_err(&evt->pdc_crc_err);
        break;
    case NRF_MODEM_DECT_PHY_EVT_TIME:
        on_time_get(&evt->time_get);
        break;
    case NRF_MODEM_DECT_PHY_EVT_CAPABILITY:
        on_capability_get(&evt->capability_get);
        break;
    case NRF_MODEM_DECT_PHY_EVT_BANDS:
        on_bands_get(&evt->band_get);
        break;
    case NRF_MODEM_DECT_PHY_EVT_LATENCY:
        on_latency_info_get(&evt->latency_get);
        break;
    case NRF_MODEM_DECT_PHY_EVT_LINK_CONFIG:
        on_link_config(&evt->link_config);
        break;
    case NRF_MODEM_DECT_PHY_EVT_STF_CONFIG:
        on_stf_cover_seq_control(&evt->stf_cover_seq_control);
        break;
    }
}