1 /*
2 * Copyright (c) 2024 BayLibre SAS
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_ptp_btca, CONFIG_PTP_LOG_LEVEL);
9
10 #include <string.h>
11
12 #include "btca.h"
13 #include "clock.h"
14 #include "port.h"
15
16 #define A_BETTER (1)
17 #define A_BETTER_TOPOLOGY (2)
18 #define B_BETTER (-1)
19 #define B_BETTER_TOPOLOGY (-2)
20
btca_port_id_cmp(const struct ptp_port_id * p1,const struct ptp_port_id * p2)21 static int btca_port_id_cmp(const struct ptp_port_id *p1, const struct ptp_port_id *p2)
22 {
23 int diff = memcmp(&p1->clk_id, &p2->clk_id, sizeof(p1->clk_id));
24
25 if (diff == 0) {
26 diff = p1->port_number - p2->port_number;
27 }
28
29 return diff;
30 }
31
btca_ds_cmp2(const struct ptp_dataset * a,const struct ptp_dataset * b)32 static int btca_ds_cmp2(const struct ptp_dataset *a, const struct ptp_dataset *b)
33 {
34 int diff;
35
36 if (b->steps_rm + 1 < a->steps_rm) {
37 return B_BETTER;
38 }
39 if (a->steps_rm + 1 < b->steps_rm) {
40 return A_BETTER;
41 }
42 if (a->steps_rm > b->steps_rm) {
43 diff = btca_port_id_cmp(&a->receiver, &a->sender);
44 if (diff > 0) {
45 return B_BETTER_TOPOLOGY;
46 }
47 if (diff < 0) {
48 return B_BETTER;
49 }
50 /* error-1 */
51 return 0;
52 }
53 if (a->steps_rm < b->steps_rm) {
54 diff = btca_port_id_cmp(&b->receiver, &b->sender);
55 if (diff > 0) {
56 return A_BETTER_TOPOLOGY;
57 }
58 if (diff < 0) {
59 return A_BETTER;
60 }
61 /* error-1 */
62 return 0;
63 }
64
65 diff = btca_port_id_cmp(&a->sender, &b->sender);
66 if (diff > 0) {
67 return B_BETTER_TOPOLOGY;
68 }
69 if (diff < 0) {
70 return A_BETTER_TOPOLOGY;
71 }
72
73 if (a->receiver.port_number > b->receiver.port_number) {
74 return B_BETTER_TOPOLOGY;
75 }
76 if (a->receiver.port_number > b->receiver.port_number) {
77 return A_BETTER_TOPOLOGY;
78 }
79 /* error-2 */
80 return 0;
81 }
82
ptp_btca_ds_cmp(const struct ptp_dataset * a,const struct ptp_dataset * b)83 int ptp_btca_ds_cmp(const struct ptp_dataset *a, const struct ptp_dataset *b)
84 {
85 if (a == b) {
86 return 0;
87 }
88 if (a && !b) {
89 return A_BETTER;
90 }
91 if (!a && b) {
92 return B_BETTER;
93 }
94
95 int id_diff = memcmp(&a->clk_id, &b->clk_id, sizeof(a->clk_id));
96
97 if (id_diff == 0) {
98 return btca_ds_cmp2(a, b);
99 }
100 if (a->priority1 > b->priority1) {
101 return B_BETTER;
102 }
103 if (a->clk_quality.class > b->clk_quality.class) {
104 return B_BETTER;
105 }
106 if (a->clk_quality.accuracy > b->clk_quality.accuracy) {
107 return B_BETTER;
108 }
109 if (a->clk_quality.offset_scaled_log_variance > b->clk_quality.offset_scaled_log_variance) {
110 return B_BETTER;
111 }
112 if (a->priority2 > b->priority2) {
113 return B_BETTER;
114 }
115
116 return id_diff < 0 ? A_BETTER : B_BETTER;
117 }
118
ptp_btca_state_decision(struct ptp_port * port)119 enum ptp_port_state ptp_btca_state_decision(struct ptp_port *port)
120 {
121 const struct ptp_foreign_tt_clock *clk_best_foreign = ptp_clock_best_time_transmitter();
122 const struct ptp_dataset *clk_default, *clk_best, *port_best;
123
124 clk_default = ptp_clock_ds();
125 clk_best = ptp_clock_best_foreign_ds();
126 port_best = ptp_port_best_foreign_ds(port);
127
128 if (!port_best && ptp_port_state(port) == PTP_PS_LISTENING) {
129 return PTP_PS_LISTENING;
130 }
131
132 if (clk_default->clk_quality.class <= 127) {
133 if (ptp_btca_ds_cmp(clk_default, port_best) > 0) {
134 /* M1 */
135 return PTP_PS_GRAND_MASTER;
136 }
137 /* P1 */
138 return PTP_PS_PASSIVE;
139 }
140
141 if (ptp_btca_ds_cmp(clk_default, clk_best) > 0) {
142 /* M2 */
143 return PTP_PS_GRAND_MASTER;
144 }
145
146 if (clk_best_foreign->port == port) {
147 /* S1 */
148 return PTP_PS_TIME_RECEIVER;
149 }
150
151 if (ptp_btca_ds_cmp(clk_best, port_best) == A_BETTER_TOPOLOGY) {
152 /* P2 */
153 return PTP_PS_PASSIVE;
154 }
155 /* M3 */
156 return PTP_PS_TIME_TRANSMITTER;
157 }
158
