Skip to content

Commit a81144b

Browse files
datdenkiknietdatdenkikniet
committed
Add timesync example
1 parent d2d39ec commit a81144b

File tree

4 files changed

+624
-1
lines changed

4 files changed

+624
-1
lines changed

‎CHANGELOG.md

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
## Unreleased
2-
* Update to `smoltcp` v0.9 [#71](https://github.com/stm32-rs/stm32-eth/pull/71).
2+
* Update to `smoltcp` v0.9 ([#71](https://github.com/stm32-rs/stm32-eth/pull/71))
3+
* Add `timesync-server` and `timesync-client` examples ([#72](https://github.com/stm32-rs/stm32-eth/pull/72))
34

45
## [0.4.1](https://github.com/stm32-rs/stm32-eth/tree/v0.4.1)
56
* Fix a bug when caching timestamps in the TX path ([#73](https://github.com/stm32-rs/stm32-eth/pull/73))

‎Cargo.toml

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -94,6 +94,16 @@ required-features = [ "defmt" , "smoltcp-phy" ]
9494
name = "rtic-timestamp"
9595
required-features = [ "defmt", "ptp" ]
9696

97+
[[example]]
98+
name = "timesync-client"
99+
path = "./examples/timesync/client.rs"
100+
required-features = [ "defmt", "ptp" ]
101+
102+
[[example]]
103+
name = "timesync-server"
104+
path = "./examples/timesync/server.rs"
105+
required-features = [ "defmt", "ptp" ]
106+
97107
[profile.release]
98108
debug = 2
99109
lto = true

‎examples/timesync/client.rs

Lines changed: 366 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,366 @@
1+
#![no_std]
2+
#![no_main]
3+
4+
//! For build and run instructions, see README.md
5+
//!
6+
//! # The `timesync` example
7+
//!
8+
//! A very basic PTP-based time synchronization example built using RTIC.
9+
//!
10+
//! For this example to do anything, it is necessary that the server (`timesync/server.rs`) is
11+
//! already running and that broadcast ethernet traffic can flow between the two nodes.
12+
//!
13+
//! # Notes on running this example
14+
//!
15+
//! When using the internal oscillator of an STM32, accurately syncing is very difficult, as the
16+
//! frequency drift and error between two instances of that clock is difficult to accurately
17+
//! compensate for with the very limited protocol that this example uses.
18+
//!
19+
//! If a more accurate High Speed External oscillator is used, the rate of time and global time
20+
//! on the two nodes can be synchronized to within a few tenths of PPMs.
21+
//!
22+
//! To activate the HSE configuration for the examples, set the `STM32_ETH_EXAMPLE_HSE`
23+
//! environment variable to `oscillator` or `bypass` when compiling examples.
24+
//!
25+
//! # Monitoring the accuracy of synchronization
26+
//!
27+
//! Generally, the output displayed by the client with `DEFMT_LOG=info` is quite accurate.
28+
//!
29+
//! However, if you wish to observe the difference externally the PPS pin can be used. In
30+
//! these examples, it is always enabled. By default, it will run on PB5. If you wish to use
31+
//! the alternate PPS pin (PG8), set the `STM32_ETH_EXAMPLE_PPS_PIN` environment variable
32+
//! to `alternate` when compiling the examples.
33+
//!
34+
//! # What does this example do?
35+
//!
36+
//! The nodes will exchange messages according to the following protocol:
37+
//! 1. Client sends empty message 0x0 to initiate an exchange
38+
//! 2. Server sends empty message 0x01, (client records RX time t1')
39+
//! 3. Server sends message 0x02 with TX time of #2 (client records TX time t1)
40+
//! 4. Client sends empty message 0x03 (client records TX time t2)
41+
//! 5. Server sends message 0x04 with RX time of #4 (client records RX time t2')
42+
//!
43+
//! Once the exchange is finished, the client will do the following:
44+
//! 1. If the calculated time difference is larger than 200 microseconds, the current local time is
45+
//! set to the received value.
46+
//! 2. If the difference is smaller than or equal to 200 microseconds, the PTP addend value is
47+
//! updated to compensate for the observed difference using a basic PI integrator.
48+
//!
49+
//! See the [PTP article on Wikipedia] for more information on how/why this works.
50+
//!
51+
//! [PTP article on Wikipedia]: https://en.wikipedia.org/wiki/Precision_Time_Protocol#Synchronization
52+
use core::ops::Neg;
53+
54+
use defmt_rtt as _;
55+
use panic_probe as _;
56+
use stm32_eth::ptp::Timestamp;
57+
58+
#[path = "../common.rs"]
59+
mod common;
60+
61+
/// Calculate the time offset, as laid out by the [PTP article on wikipedia].
62+
///
63+
/// [PTP article on wikipedia]: https://en.wikipedia.org/wiki/Precision_Time_Protocol#Synchronization
64+
pub fn calculate_offset(
65+
t1: Timestamp,
66+
t1_prim: Timestamp,
67+
t2: Timestamp,
68+
t2_prim: Timestamp,
69+
) -> Timestamp {
70+
let double_offset = t1_prim - t1 - t2_prim + t2;
71+
72+
let raw = double_offset.raw();
73+
let offset = (raw / 2).neg();
74+
let offset = Timestamp::new_raw(offset);
75+
76+
offset
77+
}
78+
79+
#[rtic::app(device = stm32_eth::stm32, dispatchers = [SPI1])]
80+
mod app {
81+
82+
use crate::common::EthernetPhy;
83+
84+
use ieee802_3_miim::{phy::PhySpeed, Phy};
85+
use systick_monotonic::Systick;
86+
87+
use stm32_eth::{
88+
dma::{EthernetDMA, PacketId, RxRingEntry, TxRingEntry},
89+
mac::Speed,
90+
ptp::{EthernetPTP, Timestamp},
91+
Parts,
92+
};
93+
94+
const BROADCAST: [u8; 6] = [0xFF; 6];
95+
#[allow(dead_code)]
96+
const CLIENT_ADDR: [u8; 6] = [0x80, 0x00, 0xDE, 0xAD, 0xBE, 0xEF];
97+
const SERVER_ADDR: [u8; 6] = [0x80, 0x00, 0xDE, 0xAD, 0xBE, 0xFF];
98+
const ETHER_TYPE: [u8; 2] = [0xFF, 0xFF];
99+
100+
#[local]
101+
struct Local {
102+
start_addend: u32,
103+
}
104+
105+
#[shared]
106+
struct Shared {
107+
dma: EthernetDMA<'static, 'static>,
108+
ptp: EthernetPTP,
109+
}
110+
111+
#[monotonic(binds = SysTick, default = true)]
112+
type Monotonic = Systick<1000>;
113+
114+
#[init(local = [
115+
rx_ring: [RxRingEntry; 2] = [RxRingEntry::new(),RxRingEntry::new()],
116+
tx_ring: [TxRingEntry; 2] = [TxRingEntry::new(),TxRingEntry::new()],
117+
])]
118+
fn init(cx: init::Context) -> (Shared, Local, init::Monotonics) {
119+
defmt::info!("Pre-init");
120+
let core = cx.core;
121+
let p = cx.device;
122+
123+
let rx_ring = cx.local.rx_ring;
124+
let tx_ring = cx.local.tx_ring;
125+
126+
let (clocks, gpio, ethernet) = crate::common::setup_peripherals(p);
127+
let mono = Systick::new(core.SYST, clocks.hclk().raw());
128+
129+
defmt::info!("Setting up pins");
130+
let (pins, mdio, mdc, pps) = crate::common::setup_pins(gpio);
131+
132+
defmt::info!("Configuring ethernet");
133+
134+
let Parts { dma, mac, mut ptp } =
135+
stm32_eth::new_with_mii(ethernet, rx_ring, tx_ring, clocks, pins, mdio, mdc).unwrap();
136+
137+
// Enable PPS, and set its frequency to 1024 Hz
138+
#[cfg(not(feature = "stm32f107"))]
139+
ptp.set_pps_freq(10);
140+
ptp.enable_pps(pps);
141+
142+
let start_addend = ptp.addend();
143+
144+
defmt::info!("Enabling interrupts");
145+
dma.enable_interrupt();
146+
147+
match EthernetPhy::from_miim(mac, 0) {
148+
Ok(mut phy) => {
149+
defmt::info!(
150+
"Resetting PHY as an extra step. Type: {}",
151+
phy.ident_string()
152+
);
153+
154+
phy.phy_init();
155+
156+
defmt::info!("Waiting for link up.");
157+
158+
while !phy.phy_link_up() {}
159+
160+
defmt::info!("Link up.");
161+
162+
if let Some(speed) = phy.speed().map(|s| match s {
163+
PhySpeed::HalfDuplexBase10T => Speed::HalfDuplexBase10T,
164+
PhySpeed::FullDuplexBase10T => Speed::FullDuplexBase10T,
165+
PhySpeed::HalfDuplexBase100Tx => Speed::HalfDuplexBase100Tx,
166+
PhySpeed::FullDuplexBase100Tx => Speed::FullDuplexBase100Tx,
167+
}) {
168+
phy.get_miim().set_speed(speed);
169+
defmt::info!("Detected link speed: {}", speed);
170+
} else {
171+
defmt::warn!("Failed to detect link speed.");
172+
}
173+
}
174+
Err(_) => {
175+
defmt::info!("Not resetting unsupported PHY. Cannot detect link speed.");
176+
}
177+
};
178+
179+
runner::spawn().ok();
180+
181+
(
182+
Shared { dma, ptp },
183+
Local { start_addend },
184+
init::Monotonics(mono),
185+
)
186+
}
187+
188+
#[task(shared = [dma, ptp], local = [addend_integrator: f32 = 0.0, start_addend, packet_id: u32 = 0])]
189+
fn runner(cx: runner::Context) {
190+
use fugit::ExtU64;
191+
192+
runner::spawn_after(100.millis()).ok();
193+
194+
let start = monotonics::now();
195+
196+
let (mut dma, mut ptp) = (cx.shared.dma, cx.shared.ptp);
197+
198+
let (addend_integrator, start_addend, packet_id) = (
199+
cx.local.addend_integrator,
200+
*cx.local.start_addend,
201+
cx.local.packet_id,
202+
);
203+
204+
let mut buf = [0u8; 128];
205+
206+
macro_rules! recv {
207+
() => {
208+
loop {
209+
if monotonics::now() - 500u64.millis() > start {
210+
return;
211+
}
212+
213+
let res = dma.lock(|dma| {
214+
if let Ok(rx_packet) = dma.recv_next(None) {
215+
if rx_packet.len() > 14
216+
&& &rx_packet[6..12] == &SERVER_ADDR
217+
&& &rx_packet[12..14] == &ETHER_TYPE
218+
{
219+
if let Some(timestamp) = rx_packet.timestamp() {
220+
let data_len = rx_packet.len() - 14;
221+
buf[..data_len].copy_from_slice(&rx_packet[14..14 + data_len]);
222+
Ok((&buf[..data_len], timestamp))
223+
} else {
224+
Err(true)
225+
}
226+
} else {
227+
Err(false)
228+
}
229+
} else {
230+
Err(false)
231+
}
232+
});
233+
234+
if let Ok(res) = res {
235+
break res;
236+
} else if let Err(true) = res {
237+
return;
238+
}
239+
}
240+
};
241+
}
242+
243+
macro_rules! send {
244+
($data:expr) => {{
245+
let current_id = PacketId(*packet_id);
246+
*packet_id += 1;
247+
let current_clone = current_id.clone();
248+
dma.lock(|dma| {
249+
dma.send(14 + $data.len(), Some(current_clone), |buf| {
250+
buf[0..6].copy_from_slice(&BROADCAST);
251+
buf[6..12].copy_from_slice(&CLIENT_ADDR);
252+
buf[12..14].copy_from_slice(&ETHER_TYPE);
253+
buf[14..14 + $data.len()].copy_from_slice($data);
254+
})
255+
.unwrap();
256+
});
257+
258+
loop {
259+
if monotonics::now() - 500u64.millis() > start {
260+
return;
261+
}
262+
263+
let current_id = current_id.clone();
264+
if let Ok(timestamp) = dma.lock(|dma| dma.get_timestamp_for_id(current_id)) {
265+
break timestamp;
266+
}
267+
}
268+
}};
269+
}
270+
271+
use core::convert::TryInto;
272+
273+
defmt::trace!("Step 1");
274+
send!(&[0x00]);
275+
276+
defmt::trace!("Step 2");
277+
let (buf, t1_prim) = recv!();
278+
279+
if !buf.starts_with(&[0x01]) {
280+
defmt::error!("Expected message 0x01, got {}", buf);
281+
return;
282+
}
283+
284+
defmt::trace!("Step 3");
285+
let (buf, _) = recv!();
286+
287+
if !buf.starts_with(&[0x02]) {
288+
defmt::error!("Expected message 0x02, got {}", buf);
289+
return;
290+
}
291+
292+
let t1 = Timestamp::new_raw(i64::from_le_bytes(
293+
buf[1..9].try_into().expect("Infallible"),
294+
));
295+
296+
defmt::trace!("Step 4");
297+
let t2 = send!(&[0x03]);
298+
299+
defmt::trace!("Step 5");
300+
let (buf, _) = recv!();
301+
302+
if !buf.starts_with(&[0x04]) {
303+
defmt::error!("Expected message 0x04, got {}", buf);
304+
return;
305+
}
306+
307+
let t2_prim = Timestamp::new_raw(i64::from_le_bytes(
308+
buf[1..9].try_into().expect("Infallible"),
309+
));
310+
311+
let offset = crate::calculate_offset(t1, t1_prim, t2, t2_prim);
312+
313+
ptp.lock(|ptp| {
314+
let now = ptp.get_time();
315+
if offset.seconds() > 0 || offset.nanos() > 200_000 {
316+
*addend_integrator = 0.0;
317+
defmt::info!("Updating time. Offset {} ", offset);
318+
let updated_time = now + offset;
319+
ptp.set_time(updated_time);
320+
} else {
321+
let mut offset_nanos = offset.nanos() as i64;
322+
if offset.is_negative() {
323+
offset_nanos *= -1;
324+
}
325+
326+
let error = (offset_nanos * start_addend as i64) / 1_000_000_000;
327+
*addend_integrator += error as f32 / 500.;
328+
329+
defmt::info!(
330+
"Error: {}. Integrator: {}, Offset: {} ns",
331+
error,
332+
addend_integrator,
333+
offset_nanos
334+
);
335+
336+
defmt::debug!(
337+
"DATA: {}, {}, {}, {}",
338+
error,
339+
addend_integrator,
340+
offset_nanos,
341+
now
342+
);
343+
344+
let new_addend =
345+
(start_addend as i64 + error / 4 + (*addend_integrator as i64)) as u32;
346+
ptp.set_addend(new_addend);
347+
}
348+
});
349+
350+
defmt::debug!(
351+
"The exchange took {} ms",
352+
(monotonics::now() - start).to_millis()
353+
);
354+
}
355+
356+
#[task(binds = ETH, shared = [dma, ptp], priority = 2)]
357+
fn eth_interrupt(cx: eth_interrupt::Context) {
358+
let (dma, ptp) = (cx.shared.dma, cx.shared.ptp);
359+
(dma, ptp).lock(|dma, _ptp| {
360+
dma.interrupt_handler();
361+
362+
#[cfg(not(feature = "stm32f107"))]
363+
_ptp.interrupt_handler();
364+
})
365+
}
366+
}

0 commit comments

Comments
 (0)