ptp_core.c

Go to the documentation of this file.

/* (C) András Wiesner, 2020-2022 */
 
#include "ptp_core.h"
 
#include <math.h>
#include <stdint.h>
#include <string.h>
 
#include "common.h"
#include "event.h"
#include "logging.h"
#include "master.h"
#include "slave.h"
 
#include "bmca.h"
#include "cli_cmds.h"
#include "clock_utils.h"
#include "format_utils.h"
#include "msg_utils.h"
#include "network_stack_driver.h"
#include "ptp_defs.h"
#include "ptp_types.h"
#include "settings_interface.h"
#include "stats.h"
#include "task_ptp.h"
#include "timeutils.h"
 
#include <flexptp_options.h>
 
#include "minmax.h"
 
#include <stdlib.h>
 
// global state
PtpCoreState gPtpCoreState;
#define S (gPtpCoreState)
 
const TimestampI zeroTs = {0, 0}; // a zero timestamp
 
// --------------------------------------
 
static void ptp_common_init(void) {
 
    // seed the randomizer
    srand(S.hwoptions.clockIdentity);
 
    // reset options
    nsToTsI(&S.hwoptions.offset, PTP_DEFAULT_SERVO_OFFSET);
 
    // initialize hardware
#ifdef PTP_ADDEND_INTERFACE
    PTP_HW_INIT(PTP_INCREMENT_NSEC, (uint32_t)PTP_ADDEND_INIT);
#elif defined(PTP_HLT_INTERFACE)
    PTP_HW_INIT();
#endif
 
    // get the hardware address
    uint8_t hwa[6];
    ptp_nsd_get_interface_address(hwa);
 
    // create clock identity
    ptp_create_clock_identity(hwa);
 
    // initialize controller
    PTP_SERVO_INIT();
}
 
// initialize PTP module
void ptp_init(void) {
#ifdef CLI_REG_CMD
    // register cli commands
    ptp_register_cli_commands();
#endif // CLI_REG_CMD
 
    // clear the timer
    S.ticks = 0;
 
    /* ---- COMMON ----- */
    ptp_common_init();
 
    /* ----- SBMC ------ */
    ptp_bmca_init();
 
    /* ----- SLAVE ----- */
    ptp_slave_init();
 
    /* ---- MASTER --- */
    ptp_master_init();
 
    // ---------------------
 
    ptp_reset(); // reset all PTP systems
 
    // dispatch INIT_DONE event
    PTP_IUEV(PTP_UEV_INIT_DONE);
}
 
// deinit PTP module
void ptp_deinit() {
    /* ---- COMMON ----- */
#ifdef CLI_REG_CMD
    // remove cli commands
    ptp_remove_cli_commands();
#endif // CLI_REG_CMD
 
    // deinitialize controller
    PTP_SERVO_DEINIT();
 
    /* ----- SBMC ------ */
    ptp_bmca_destroy();
 
    /* ----- SLAVE ----- */
    ptp_slave_destroy();
 
    /* ---- MASTER --- */
    ptp_master_destroy();
}
 
// reset PTP subsystem
static void ptp_core_reset() {
    // pause the heartbeat timer
    ptp_stop_heartbeat_tmr();
 
    /* ---- COMMON ---- */
    memset(&S.network, 0, sizeof(PtpNetworkState)); // network state
 
    // reinitialize the Network Stack Driver
    NsdInitSettings nsdInit = {
        ptp_get_transport_type(),
        ptp_get_delay_mechanism(),
        {}, {}
    };
    memcpy(nsdInit.primary_p2p_8023_dest, S.profile.primary_p2p_8023_destination, 6);
    memcpy(nsdInit.pdelay_p2p_8023_dest, S.profile.pdelay_p2p_8023_destination, 6);
 
    ptp_nsd_init(&nsdInit);
 
    // reset statistics
    ptp_clear_stats();
 
    // reset common functionality
    ptp_common_reset();
 
    /* ---- SBMC ----- */
    ptp_bmca_reset();
 
    /* ---- SLAVE ---- */
    ptp_slave_reset();
 
    /* ---- MASTER --- */
    ptp_master_reset();
 
    // ------------------------
 
    // resume the heartbeat timer
    ptp_start_heartbeat_tmr();
 
    // dispatch RESET event
    PTP_IUEV(PTP_UEV_RESET_DONE);
}
 
// packet processing
void ptp_process_packet(RawPtpMessage *pRawMsg) {
    PtpHeader header;
 
    // header readout
    ptp_extract_header(&header, pRawMsg->data);
 
    // consider only messages in our domain
    if (header.domainNumber != S.profile.domainNumber ||
        header.transportSpecific != S.profile.transportSpecific) {
        return;
    }
 
    // process Announce messages and halt further processing
    PtpMessageType mt = header.messageType;
    if (mt == PTP_MT_Announce) {
        PtpMasterProperties newMstProp;
        ptp_extract_announce_message(&newMstProp, pRawMsg->data);
        ptp_handle_announce_msg(&newMstProp, &header);
        PTP_IUEV(PTP_UEV_ANNOUNCE_RECVED); // dispatch ANNOUNCE_RECVED event
        return;
    }
 
    // PDelay_Req messages should always be processed
    if ((header.messageType == PTP_MT_PDelay_Req) && (S.profile.delayMechanism == PTP_DM_P2P)) {
        PTP_IUEV(PTP_UEV_PDELAY_REQ_RECVED); // dispatch PDELAY_REQ_RECVED event
        ptp_send_pdelay_resp(pRawMsg);       // sent the PDelay_Resp message
        PTP_IUEV(PTP_UEV_PDELAY_RESP_SENT);  // dispatch PDELAY_RESP_SENT event
        return;
    }
 
    // if operating in slave mode
    PtpBmcaFsmState bmcaState = S.bmca.state;
    if (bmcaState == PTP_BMCA_SLAVE) {
        ptp_slave_process_message(pRawMsg, &header);
        return;
    } else if (bmcaState == PTP_BMCA_MASTER) { // if operating in Master mode
        ptp_master_process_message(pRawMsg, &header);
        return;
    }
}
 
void ptp_process_event(const PtpCoreEvent *event) {
    switch (event->code) {
    case PTP_CEV_HEARTBEAT: { // heartbeat event
        S.ticks++;
        ptp_bmca_tick();
        ptp_slave_tick();
        ptp_master_tick();
    } break;
    case PTP_CEV_BMCA_STATE_CHANGED: {
        PtpBmcaFsmState bmcaState = event->w.w;
        if (bmcaState == PTP_BMCA_SLAVE) {
            ptp_slave_enable();
        } else if (bmcaState == PTP_BMCA_MASTER) {
            ptp_master_enable();
        } else {
            ptp_slave_disable();
            ptp_master_disable();
        }
 
        // dispatch BMCA_STATE_CHANGED event
        PTP_IUEV(PTP_UEV_BMCA_STATE_CHANGED);
    } break;
    case PTP_CEV_RESET: {
        ptp_core_reset();
    } break;
    default:
        break;
    }
}
 
// -----------------------------------------------
 
uint32_t ptp_get_tick() {
    return S.ticks;
}
 
void ptp_reset() {
    PtpCoreEvent event = { .code = PTP_CEV_RESET, .w = 0, .dw = 0 };
    ptp_event_enqueue(&event);
}
 
// -----------------------------------------------
 
void ptp_set_sync_callback(PtpSyncCallback syncCb) {
    S.slave.syncCb = syncCb;
}
 
void ptp_set_user_event_callback(PtpUserEventCallback userEventCb) {
    S.userEventCb = userEventCb;
}