Skip to content

Commit 2657102

Browse files
andomaandoma
committed
Extract clock servo from ethernet drivers into shared clock_realtime abstraction
Introduce clock_realtime_class vtable (set_time, get_time, adj_time) and a protocol-agnostic clock_servo (PI filter) that works for PTP, NTP, or any time sync source. The servo accepts an update_interval parameter to scale the integrator for different poll frequencies. This eliminates ~75 lines of duplicated servo code between stm32f4_eth.c and stm32h7_eth.c. Each driver now just implements the three vtable methods for its hardware registers.
1 parent 58531a8 commit 2657102

12 files changed

Lines changed: 352 additions & 206 deletions

File tree

‎include/mios/clock_realtime.h‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
#pragma once
2+
3+
#include <stdint.h>
4+
5+
typedef struct clock_realtime clock_realtime_t;
6+
7+
typedef struct clock_realtime_class {
8+
const char *name;
9+
void (*set_time)(clock_realtime_t *clk, int64_t nsec);
10+
int64_t (*get_time)(clock_realtime_t *clk);
11+
void (*adj_time)(clock_realtime_t *clk, int32_t ppb);
12+
} clock_realtime_class_t;
13+
14+
struct clock_realtime {
15+
const clock_realtime_class_t *clk_class;
16+
};

‎src/net/ether.h‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -59,9 +59,6 @@ typedef struct ether_netif {
5959

6060
#ifdef ENABLE_NET_PTP
6161
ptp_ether_state_t eni_ptp;
62-
63-
void (*eni_adjust_mac_clock)(struct ether_netif *eni, int64_t offset);
64-
6562
#endif
6663

6764
} ether_netif_t;

‎src/net/ptp.c‎

Lines changed: 11 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -69,8 +69,8 @@ calc(ether_netif_t *eni)
6969
const int64_t one_way_delay = (d_sm + d_ms) / 2;
7070
const int64_t offset = (d_sm - d_ms) / 2;
7171

72-
if(eni->eni_adjust_mac_clock != NULL)
73-
eni->eni_adjust_mac_clock(eni, offset);
72+
if(pes->pes_clock.clk_class != NULL)
73+
clock_servo_adjust(&pes->pes_servo, offset, 1);
7474

7575
pes->pes_offset = offset;
7676
pes->pes_one_way_delay = one_way_delay;
@@ -201,12 +201,18 @@ ptpv2_input(ether_netif_t *eni, pbuf_t *pb, pbuf_timestamp_t *pt,
201201
}
202202

203203

204-
void
204+
int
205205
ptp_print_info(stream_t *st, struct ether_netif *eni)
206206
{
207207
const ptp_ether_state_t *pes = &eni->eni_ptp;
208+
if(!pes->pes_servo.cs_synchronized) {
209+
stprintf(st, "PTP: not synchronized\n");
210+
return 0;
211+
}
208212
stprintf(st, "PTP Offset:%"PRId64" ns Delay:%d ns\n", pes->pes_offset,
209213
pes->pes_one_way_delay);
210-
stprintf(st, "Downstream delay %"PRId64" ns\n", pes->pes_t1_cf >> 16);
211-
stprintf(st, "Upstream delay %"PRId64" ns\n", pes->pes_t4_cf >> 16);
214+
stprintf(st, " Downstream delay %"PRId64" ns\n", pes->pes_t1_cf >> 16);
215+
stprintf(st, " Upstream delay %"PRId64" ns\n", pes->pes_t4_cf >> 16);
216+
clock_servo_print(&pes->pes_servo, st);
217+
return 1;
212218
}

‎src/net/ptp.h‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,9 @@
33
#include <stddef.h>
44
#include <stdint.h>
55

6+
#include <mios/clock_realtime.h>
7+
#include "util/clock_servo.h"
8+
69
struct pbuf;
710
struct pbuf_timestamp;
811
struct ether_netif;
@@ -35,6 +38,10 @@ typedef struct ptp_ether_state {
3538
int64_t pes_offset;
3639
int pes_one_way_delay;
3740

41+
clock_realtime_t pes_clock;
42+
clock_servo_t pes_servo;
43+
3844
} ptp_ether_state_t;
3945

40-
void ptp_print_info(struct stream *st, struct ether_netif *eni);
46+
// Returns 1 if synchronized
47+
int ptp_print_info(struct stream *st, struct ether_netif *eni);

‎src/platform/stm32f4/stm32f4_eth.c‎

Lines changed: 66 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,14 @@
1616
#include <mios/mios.h>
1717
#include <mios/eventlog.h>
1818
#include <mios/sys.h>
19+
#include <mios/type_macros.h>
1920

2021
#include <util/crc32.h>
2122

2223
#include "irq.h"
2324

25+
#define MAC_NAME "stm32f4-eth"
26+
2427
#define DMA_BUFFER_PAD 2
2528

2629
#define ETH_TX_RING_SIZE 16
@@ -132,7 +135,6 @@ typedef struct stm32f4_eth {
132135
timer_t se_periodic;
133136

134137
#ifdef ENABLE_NET_PTP
135-
int64_t se_accumulated_drift_ppb;
136138
uint32_t se_addend;
137139
int32_t se_ppb_scale_mult;
138140
int32_t se_ppb_scale_div;
@@ -195,14 +197,12 @@ stm32f4_eth_print_info(struct device *dev, struct stream *st)
195197
strtbl(mactxfcstatus, (macdbg >> 17) & 3),
196198
macdbg & (1 << 16) ? "MMTEA " : "");
197199
#ifdef ENABLE_NET_PTP
198-
stprintf(st, "MAC time: %u.%u\n",
199-
reg_rd(ETH_PTPTSHR),
200-
reg_rd(ETH_PTPTSLR));
201-
ptp_print_info(st, &se->se_eni);
200+
if(ptp_print_info(st, &se->se_eni)) {
201+
stprintf(st, " Hardware time: %u.%u\n",
202+
reg_rd(ETH_PTPTSHR),
203+
reg_rd(ETH_PTPTSLR));
204+
}
202205
#endif
203-
204-
// if(se->se_ethphy_class && se->se_ethphy_class->print_info)
205-
// se->se_ethphy_class->print_info(st, &stm32f4_eth_mdio, se);
206206
}
207207

208208

@@ -534,75 +534,82 @@ stm32f4_periodic(void *opaque, uint64_t now)
534534

535535
#ifdef ENABLE_NET_PTP
536536

537-
#define PTP_STEP_THRESHOLD_NS 100000000LL // 10 ms
538-
#define MAX_ADJ_PPB 100000
539-
#define SERVO_KP_SHIFT 2
540-
#define SERVO_KI_SHIFT 7
537+
static void
538+
stm32f4_clock_set_time(clock_realtime_t *clk, int64_t nsec)
539+
{
540+
reg_wr(ETH_PTPTSHUR, nsec / 1000000000);
541+
reg_wr(ETH_PTPTSLUR, nsec % 1000000000);
542+
reg_set_bit(ETH_PTPTSCR, 2);
543+
while(reg_get_bit(ETH_PTPTSCR, 2));
544+
}
541545

542-
void
543-
ptp_clock_slew(stm32f4_eth_t *se, int64_t offset_ns)
546+
static int64_t
547+
stm32f4_clock_get_time(clock_realtime_t *clk)
544548
{
545-
int64_t adj_p = offset_ns >> SERVO_KP_SHIFT;
546-
se->se_accumulated_drift_ppb += (offset_ns >> SERVO_KI_SHIFT);
549+
while(1) {
550+
uint32_t s = reg_rd(ETH_PTPTSHR);
551+
uint32_t ns = reg_rd(ETH_PTPTSLR);
552+
if(s == reg_rd(ETH_PTPTSHR)) {
553+
return (int64_t)s * 1000000000 + ns;
554+
}
555+
}
556+
}
547557

548-
// Anti-windup clamping
549-
if(se->se_accumulated_drift_ppb > MAX_ADJ_PPB)
550-
se->se_accumulated_drift_ppb = MAX_ADJ_PPB;
551-
else if(se->se_accumulated_drift_ppb < -MAX_ADJ_PPB)
552-
se->se_accumulated_drift_ppb = -MAX_ADJ_PPB;
558+
static void
559+
stm32f4_clock_adj_time(clock_realtime_t *clk, int32_t ppb)
560+
{
561+
stm32f4_eth_t *se =
562+
container_of(clk, stm32f4_eth_t, se_eni.eni_ptp.pes_clock);
553563

554-
int64_t total_ppb = adj_p + se->se_accumulated_drift_ppb;
555564
int32_t addend_adjustment =
556-
(total_ppb * se->se_ppb_scale_mult) / se->se_ppb_scale_div;
565+
((int64_t)ppb * se->se_ppb_scale_mult) / se->se_ppb_scale_div;
557566

558567
uint32_t final_addend = se->se_addend + addend_adjustment;
559568
reg_wr(ETH_PTPTSAR, final_addend);
560569
reg_set_bit(ETH_PTPTSCR, 5);
561570
}
562571

563-
564-
int64_t
565-
stm32f4_get_current_mac_time(void)
566-
{
567-
while(1) {
568-
const uint32_t cur_s = reg_rd(ETH_PTPTSHR);
569-
const uint32_t cur_ns = reg_rd(ETH_PTPTSLR);
570-
if(cur_s == reg_rd(ETH_PTPTSHR)) {
571-
return (int64_t)cur_s * 1000000000 + cur_ns;
572-
}
573-
}
574-
}
575-
572+
static const clock_realtime_class_t stm32f4_clock_realtime_class = {
573+
.name = MAC_NAME,
574+
.set_time = stm32f4_clock_set_time,
575+
.get_time = stm32f4_clock_get_time,
576+
.adj_time = stm32f4_clock_adj_time,
577+
};
576578

577579
static void
578-
ptp_clock_step(stm32f4_eth_t *se, int64_t offset_ns)
580+
stm32f4_ptp_init(stm32f4_eth_t *se)
579581
{
580-
int64_t cur_t = stm32f4_get_current_mac_time();
581-
int64_t ch = cur_t + offset_ns;
582+
const unsigned int ahb_freq = CPU_SYSTICK_RVR;
582583

583-
reg_wr(ETH_PTPTSHUR, ch / 1000000000);
584-
reg_wr(ETH_PTPTSLUR, ch % 1000000000);
585-
reg_set_bit(ETH_PTPTSCR, 2);
586-
while(reg_get_bit(ETH_PTPTSCR, 2));
584+
uint32_t ssinc = (1000000000 / ahb_freq) + 1;
587585

588-
se->se_accumulated_drift_ppb = 0; // Reset clock servo integrator
586+
uint64_t numerator = 1000000000ULL;
587+
uint64_t denominator = ahb_freq * ssinc;
588+
se->se_addend = (numerator << 32) / denominator;
589589

590-
evlog(LOG_DEBUG, "%s: Step adjust %lld", se->se_eni.eni_ni.ni_dev.d_name,
591-
offset_ns);
592-
}
590+
se->se_ppb_scale_mult = se->se_addend / 1000;
591+
se->se_ppb_scale_div = 1000000;
593592

593+
reg_wr(ETH_PTPTSCR,
594+
(1 << 11) | // PTPv2 ethernet
595+
(1 << 10) | // Enable timestamping for PTPv2
596+
(1 << 9) | // Rollover ns at 999999999
597+
(1 << 1) | // Fine mode
598+
(1 << 0) | // Enable
599+
0);
594600

595-
static void
596-
stm32f4_eth_set_clock(struct ether_netif *eni, int64_t offset_ns)
597-
{
598-
stm32f4_eth_t *se = (stm32f4_eth_t *)eni;
599-
int64_t abs_offset = (offset_ns < 0) ? -offset_ns : offset_ns;
601+
reg_wr(ETH_PTPSSIR, ssinc);
602+
reg_wr(ETH_PTPTSHUR, 0);
603+
reg_wr(ETH_PTPTSLUR, 0);
604+
reg_set_bit(ETH_PTPTSCR, 2);
600605

601-
if(abs_offset > PTP_STEP_THRESHOLD_NS) {
602-
ptp_clock_step(se, offset_ns);
603-
} else {
604-
ptp_clock_slew(se, offset_ns);
605-
}
606+
reg_wr(ETH_PTPTSAR, se->se_addend);
607+
reg_set_bit(ETH_PTPTSCR, 5);
608+
while(reg_get_bit(ETH_PTPTSCR, 5));
609+
610+
se->se_eni.eni_ptp.pes_clock.clk_class = &stm32f4_clock_realtime_class;
611+
clock_servo_init(&se->se_eni.eni_ptp.pes_servo,
612+
&se->se_eni.eni_ptp.pes_clock);
606613
}
607614

608615
#endif
@@ -657,40 +664,7 @@ stm32f4_thread(stm32f4_eth_t *se, gpio_t phyrst,
657664
usleep(10);
658665

659666
#ifdef ENABLE_NET_PTP
660-
if(flags & STM32F4_ETH_ENABLE_PTP_TIMESTAMPING) {
661-
662-
const unsigned int ahb_freq = CPU_SYSTICK_RVR;
663-
664-
uint32_t ssinc = (1000000000 / ahb_freq) + 1;
665-
666-
uint64_t numerator = 1000000000ULL;
667-
uint64_t denominator = ahb_freq * ssinc;
668-
se->se_addend = (numerator << 32) / denominator;
669-
670-
se->se_ppb_scale_mult = se->se_addend / 1000;
671-
se->se_ppb_scale_div = 1000000;
672-
673-
reg_wr(ETH_PTPTSCR,
674-
(1 << 11) | // PTPv2 ethernet
675-
(1 << 10) | // Enable timestamping for PTPv2
676-
(1 << 9) | // Rollover ns at 999999999
677-
(1 << 1) | // Fine mode
678-
(1 << 0) | // Enable
679-
0);
680-
681-
reg_wr(ETH_PTPSSIR, ssinc);
682-
reg_wr(ETH_PTPTSHUR, 0);
683-
reg_wr(ETH_PTPTSLUR, 0);
684-
reg_set_bit(ETH_PTPTSCR, 2);
685-
686-
reg_wr(ETH_PTPTSAR, se->se_addend);
687-
reg_set_bit(ETH_PTPTSCR, 5);
688-
while(reg_get_bit(ETH_PTPTSCR, 5));
689-
690-
se->se_eni.eni_adjust_mac_clock = stm32f4_eth_set_clock;
691-
}
692-
693-
667+
stm32f4_ptp_init(se);
694668
#endif
695669

696670
reg_set_bit(ETH_MACCR, 13); // Receive-Own-Disable
@@ -765,7 +739,7 @@ stm32f4_eth_set_duplex(ether_netif_t *eni, int full)
765739

766740
static const ethmac_device_class_t stm32f4_eth_device_class = {
767741
.dc = {
768-
.dc_class_name = "stm32f4 Ethernet NIC",
742+
.dc_class_name = MAC_NAME,
769743
.dc_print_info = stm32f4_eth_print_info,
770744
},
771745

‎src/platform/stm32f4/stm32f4_eth.h‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,6 @@
44
#include <mios/ethphy.h>
55

66

7-
#ifdef ENABLE_NET_PTP
8-
#define STM32F4_ETH_ENABLE_PTP_TIMESTAMPING 0x1
9-
#endif
10-
117
void stm32f4_eth_init(gpio_t phyrst, const uint8_t *gpios, size_t gpio_count,
128
int phy_addr,
139
ethphy_mode_t mode, uint32_t flags);

‎src/platform/stm32h7-nucleo144/stm32h7-nucleo144.c‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -74,8 +74,7 @@ platform_init_late(void)
7474
stm32h7_eth_init(GPIO_UNUSED,
7575
nucleo144_eth_gpios,
7676
ARRAYSIZE(nucleo144_eth_gpios),
77-
0, ETHPHY_MODE_RMII,
78-
STM32H7_ETH_ENABLE_PTP_TIMESTAMPING);
77+
0, ETHPHY_MODE_RMII, 0);
7978

8079
// ETH PPS OUT
8180
// gpio_conf_af(GPIO_PB(5), 11, GPIO_PUSH_PULL, GPIO_SPEED_HIGH, GPIO_PULL_UP);

0 commit comments

Comments
 (0)