158 lines
3.2 KiB
C
158 lines
3.2 KiB
C
/*
|
|
* Copyright (c) 2024 BayLibre SAS
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <zephyr/logging/log.h>
|
|
LOG_MODULE_REGISTER(net_ptp_btca, CONFIG_PTP_LOG_LEVEL);
|
|
|
|
#include <string.h>
|
|
|
|
#include "btca.h"
|
|
#include "clock.h"
|
|
#include "port.h"
|
|
|
|
#define A_BETTER (1)
|
|
#define A_BETTER_TOPOLOGY (2)
|
|
#define B_BETTER (-1)
|
|
#define B_BETTER_TOPOLOGY (-2)
|
|
|
|
static int btca_port_id_cmp(const struct ptp_port_id *p1, const struct ptp_port_id *p2)
|
|
{
|
|
int diff = memcmp(&p1->clk_id, &p2->clk_id, sizeof(p1->clk_id));
|
|
|
|
if (diff == 0) {
|
|
diff = p1->port_number - p2->port_number;
|
|
}
|
|
|
|
return diff;
|
|
}
|
|
|
|
static int btca_ds_cmp2(const struct ptp_dataset *a, const struct ptp_dataset *b)
|
|
{
|
|
int diff;
|
|
|
|
if (b->steps_rm + 1 < a->steps_rm) {
|
|
return B_BETTER;
|
|
}
|
|
if (a->steps_rm + 1 < b->steps_rm) {
|
|
return A_BETTER;
|
|
}
|
|
if (a->steps_rm > b->steps_rm) {
|
|
diff = btca_port_id_cmp(&a->receiver, &a->sender);
|
|
if (diff > 0) {
|
|
return B_BETTER_TOPOLOGY;
|
|
}
|
|
if (diff < 0) {
|
|
return B_BETTER;
|
|
}
|
|
/* error-1 */
|
|
return 0;
|
|
}
|
|
if (a->steps_rm < b->steps_rm) {
|
|
diff = btca_port_id_cmp(&b->receiver, &b->sender);
|
|
if (diff > 0) {
|
|
return A_BETTER_TOPOLOGY;
|
|
}
|
|
if (diff < 0) {
|
|
return A_BETTER;
|
|
}
|
|
/* error-1 */
|
|
return 0;
|
|
}
|
|
|
|
diff = btca_port_id_cmp(&a->sender, &b->sender);
|
|
if (diff > 0) {
|
|
return B_BETTER_TOPOLOGY;
|
|
}
|
|
if (diff < 0) {
|
|
return A_BETTER_TOPOLOGY;
|
|
}
|
|
|
|
if (a->receiver.port_number > b->receiver.port_number) {
|
|
return B_BETTER_TOPOLOGY;
|
|
}
|
|
if (a->receiver.port_number > b->receiver.port_number) {
|
|
return A_BETTER_TOPOLOGY;
|
|
}
|
|
/* error-2 */
|
|
return 0;
|
|
}
|
|
|
|
int ptp_btca_ds_cmp(const struct ptp_dataset *a, const struct ptp_dataset *b)
|
|
{
|
|
if (a == b) {
|
|
return 0;
|
|
}
|
|
if (a && !b) {
|
|
return A_BETTER;
|
|
}
|
|
if (!a && b) {
|
|
return B_BETTER;
|
|
}
|
|
|
|
int id_diff = memcmp(&a->clk_id, &b->clk_id, sizeof(a->clk_id));
|
|
|
|
if (id_diff == 0) {
|
|
return btca_ds_cmp2(a, b);
|
|
}
|
|
if (a->priority1 > b->priority1) {
|
|
return B_BETTER;
|
|
}
|
|
if (a->clk_quality.class > b->clk_quality.class) {
|
|
return B_BETTER;
|
|
}
|
|
if (a->clk_quality.accuracy > b->clk_quality.accuracy) {
|
|
return B_BETTER;
|
|
}
|
|
if (a->clk_quality.offset_scaled_log_variance > b->clk_quality.offset_scaled_log_variance) {
|
|
return B_BETTER;
|
|
}
|
|
if (a->priority2 > b->priority2) {
|
|
return B_BETTER;
|
|
}
|
|
|
|
return id_diff < 0 ? A_BETTER : B_BETTER;
|
|
}
|
|
|
|
enum ptp_port_state ptp_btca_state_decision(struct ptp_port *port)
|
|
{
|
|
const struct ptp_foreign_tt_clock *clk_best_foreign = ptp_clock_best_time_transmitter();
|
|
const struct ptp_dataset *clk_default, *clk_best, *port_best;
|
|
|
|
clk_default = ptp_clock_ds();
|
|
clk_best = ptp_clock_best_foreign_ds();
|
|
port_best = ptp_port_best_foreign_ds(port);
|
|
|
|
if (!port_best && ptp_port_state(port) == PTP_PS_LISTENING) {
|
|
return PTP_PS_LISTENING;
|
|
}
|
|
|
|
if (clk_default->clk_quality.class <= 127) {
|
|
if (ptp_btca_ds_cmp(clk_default, port_best) > 0) {
|
|
/* M1 */
|
|
return PTP_PS_GRAND_MASTER;
|
|
}
|
|
/* P1 */
|
|
return PTP_PS_PASSIVE;
|
|
}
|
|
|
|
if (ptp_btca_ds_cmp(clk_default, clk_best) > 0) {
|
|
/* M2 */
|
|
return PTP_PS_GRAND_MASTER;
|
|
}
|
|
|
|
if (clk_best_foreign->port == port) {
|
|
/* S1 */
|
|
return PTP_PS_TIME_RECEIVER;
|
|
}
|
|
|
|
if (ptp_btca_ds_cmp(clk_best, port_best) == A_BETTER_TOPOLOGY) {
|
|
/* P2 */
|
|
return PTP_PS_PASSIVE;
|
|
}
|
|
/* M3 */
|
|
return PTP_PS_TIME_TRANSMITTER;
|
|
}
|