diff --git a/applications/tests/test_load_udp/src/lib.rs b/applications/tests/test_load_udp/src/lib.rs index b8a193331..7ae5ff4fc 100644 --- a/applications/tests/test_load_udp/src/lib.rs +++ b/applications/tests/test_load_udp/src/lib.rs @@ -44,7 +44,7 @@ async fn udp_server(port: u16) { }; let mut socket = - awkernel_async_lib::net::udp::UdpSocket::bind_on_interface(INTERFACE_ID, config).unwrap(); + awkernel_async_lib::net::udp::UdpSocket::bind_on_interface(INTERFACE_ID, &config).unwrap(); const MAX_DATAGRAM_SIZE: usize = 65_507; let mut buf = [0u8; MAX_DATAGRAM_SIZE]; diff --git a/applications/tests/test_network/src/lib.rs b/applications/tests/test_network/src/lib.rs index ccda7e157..2d3f0f0ba 100644 --- a/applications/tests/test_network/src/lib.rs +++ b/applications/tests/test_network/src/lib.rs @@ -15,13 +15,13 @@ const INTERFACE_ID: u64 = 0; // 10.0.2.0/24 is the IP address range of the Qemu's network. const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 64); - // const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(192, 168, 100, 52); // For experiment. +// const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(192, 168, 122, 24); // libvirt // 10.0.2.2 is the IP address of the Qemu's host. const UDP_TCP_DST_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 2); - // const UDP_TCP_DST_ADDR: Ipv4Addr = Ipv4Addr::new(192, 168, 100, 1); // For experiment. +// const UDP_TCP_DST_ADDR: Ipv4Addr = Ipv4Addr::new(192, 168, 122, 1); // libvirt const UDP_DST_PORT: u16 = 26099; const TCP_DST_PORT: u16 = 26099; @@ -117,6 +117,7 @@ async fn ipv4_multicast_recv_test() { break; } Err(UdpSocketError::SendError) => (), + _ => { log::error!("Failed to join the multicast group."); return; diff --git a/applications/tests/test_ntp/Cargo.toml b/applications/tests/test_ntp/Cargo.toml new file mode 100644 index 000000000..7a98adb38 --- /dev/null +++ b/applications/tests/test_ntp/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "test_ntp" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +log = "0.4" + +[dependencies.awkernel_async_lib] +path = "../../../awkernel_async_lib" +default-features = false + +[dependencies.awkernel_lib] +path = "../../../awkernel_lib" +default-features = false diff --git a/applications/tests/test_ntp/src/lib.rs b/applications/tests/test_ntp/src/lib.rs new file mode 100644 index 000000000..631abdc79 --- /dev/null +++ b/applications/tests/test_ntp/src/lib.rs @@ -0,0 +1,139 @@ +#![no_std] + +extern crate alloc; + +use core::{net::Ipv4Addr, time::Duration}; + +use awkernel_async_lib::net::{udp::UdpConfig, IpAddr}; +use awkernel_lib::ntp::{packet::NtpPacket, SignedDuration, SystemClock, SystemTime}; + +// 10.0.2.0/24 is the IP address range of the Qemu's network. +// const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(10, 0, 2, 64); +// const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(192, 168, 100, 52); // For experiment. +const INTERFACE_ADDR: Ipv4Addr = Ipv4Addr::new(192, 168, 122, 24); // libvirt + +// time-a-g.nist.gov +// const NTP_SERVER_ADDR: Ipv4Addr = Ipv4Addr::new(129, 6, 15, 28); + +// Execute this to start a local NTP server on host: +// $ scripts/ntpserver.py 0.0.0.0 26099 +const NTP_SERVER_ADDR: Ipv4Addr = Ipv4Addr::new(192, 168, 122, 1); +const NTP_SERVER_PORT: u16 = 26099; +const INTERFACE_ID: u64 = 0; + +pub async fn run() { + awkernel_lib::net::add_ipv4_addr(INTERFACE_ID, INTERFACE_ADDR, 24); + + awkernel_async_lib::spawn( + "poll time from server".into(), + poll_time_from_server(), + awkernel_async_lib::scheduler::SchedulerType::FIFO, + ) + .await; + + awkernel_async_lib::spawn( + "syncronize with server and adjust the system clock".into(), + synchronize_with_server(), + awkernel_async_lib::scheduler::SchedulerType::FIFO, + ) + .await; + + awkernel_async_lib::spawn( + "get time from kernel".into(), + get_time_from_kernel(), + awkernel_async_lib::scheduler::SchedulerType::FIFO, + ) + .await; + + awkernel_async_lib::spawn( + "set time from kernel".into(), + set_time(), + awkernel_async_lib::scheduler::SchedulerType::FIFO, + ) + .await; +} + +async fn get_time_from_server() -> Result<(SignedDuration, SignedDuration), ()> { + let config = UdpConfig { + addr: IpAddr::new_v4(INTERFACE_ADDR), + port: Some(20000), + ..Default::default() + }; + let mut socket = + awkernel_async_lib::net::udp::UdpSocket::bind_on_interface(INTERFACE_ID, &config).map_err( + |e| { + log::error!("Failed to bind UDP socket: {:?}", e); + () + }, + )?; + let server_addr = IpAddr::new_v4(NTP_SERVER_ADDR); + let mut buf = [0u8; 48]; + + let mut packet = NtpPacket::new(); + let originate_ts = SystemClock::now().into(); + + packet.transmit_timestamp = originate_ts; + + socket + .send(&packet.to_bytes(), &server_addr, NTP_SERVER_PORT) + .await + .map_err(|e| { + log::error!("Failed to send a NTP packet: {:?}", e); + })?; + + socket.recv(&mut buf).await.unwrap(); + + let destination_ts = SystemClock::now().into(); + let packet = NtpPacket::from_bytes(&buf); + let (delay, ofs) = packet.parse_response(originate_ts, destination_ts); + + Ok((delay, ofs)) +} + +async fn poll_time_from_server() { + for _ in 0..3 { + match get_time_from_server().await { + Ok((delay, ofs)) => { + log::info!("Delay: {:?}", delay); + log::info!("Offset: {:?}", ofs); + } + Err(_) => { + log::error!("Failed to get time from server"); + } + }; + + awkernel_async_lib::sleep(Duration::from_secs(2)).await; + } +} + +async fn set_time() { + SystemClock::set(1041379200_000_000_000); // 2003-11-01 + + let clock = SystemClock::now(); + log::info!("set_time_from_packet: Current time: {:?}", clock); + + let st = SystemTime::new(1041379200_000_000_000); + assert!( + clock.duration_since(st).unwrap().as_micros() < 100, + "Time set incorrectly", + ); +} + +async fn get_time_from_kernel() { + let clock = SystemClock::now(); + log::info!("get_time_from_packet: Current time: {:?}", clock); +} + +async fn synchronize_with_server() { + let (_delay, offset) = match get_time_from_server().await { + Ok((delay, offset)) => (delay, offset), + Err(_) => { + log::error!("Failed to get time from server"); + return; + } + }; + + log::info!("Time before adjustment: {:?}", SystemClock::now()); + SystemClock::adjust(offset); + log::info!("Time set to: {:?}", SystemClock::now()); +} diff --git a/awkernel_lib/src/arch.rs b/awkernel_lib/src/arch.rs index aae2a9ffe..501028f93 100644 --- a/awkernel_lib/src/arch.rs +++ b/awkernel_lib/src/arch.rs @@ -32,6 +32,7 @@ pub(crate) use self::std_common::StdCommon as ArchImpl; #[cfg(not(feature = "std"))] trait Arch: super::delay::Delay + + super::ntp::Ntp + super::interrupt::Interrupt + super::cpu::CPU + super::paging::Mapper @@ -42,6 +43,10 @@ trait Arch: #[allow(dead_code)] #[cfg(feature = "std")] trait Arch: - super::delay::Delay + super::interrupt::Interrupt + super::cpu::CPU + super::dvfs::Dvfs + super::delay::Delay + + super::ntp::Ntp + + super::interrupt::Interrupt + + super::cpu::CPU + + super::dvfs::Dvfs { } diff --git a/awkernel_lib/src/arch/std_common.rs b/awkernel_lib/src/arch/std_common.rs index f012ca042..73cd11543 100644 --- a/awkernel_lib/src/arch/std_common.rs +++ b/awkernel_lib/src/arch/std_common.rs @@ -2,6 +2,7 @@ pub(super) mod cpu; pub(super) mod delay; pub(super) mod dvfs; pub(super) mod interrupt; +pub(super) mod ntp; pub fn init() { delay::init(); diff --git a/awkernel_lib/src/arch/std_common/ntp.rs b/awkernel_lib/src/arch/std_common/ntp.rs new file mode 100644 index 000000000..e0600ec68 --- /dev/null +++ b/awkernel_lib/src/arch/std_common/ntp.rs @@ -0,0 +1,21 @@ +use awkernel_sync::{mcs::MCSNode, mutex::Mutex}; + +use crate::ntp::{Ntp, SystemTime}; + +impl Ntp for super::StdCommon { + /// Get time in microseconds. + fn get_time() -> SystemTime { + let mut tp = libc::timespec { + tv_sec: 0, + tv_nsec: 0, + }; + unsafe { libc::clock_gettime(libc::CLOCK_MONOTONIC, &mut tp) }; + let t = tp.tv_sec as u128 * 1_000_000_000 + tp.tv_nsec as u128; + + SystemTime::new(t) + } + + fn set_time(new: u64) {} + + fn adjust_time(_offset: SignedDuration) {} +} diff --git a/awkernel_lib/src/arch/x86_64.rs b/awkernel_lib/src/arch/x86_64.rs index 5b9ebf032..b9b25a862 100644 --- a/awkernel_lib/src/arch/x86_64.rs +++ b/awkernel_lib/src/arch/x86_64.rs @@ -10,6 +10,7 @@ pub(super) mod interrupt; pub mod interrupt_remap; pub mod kvm; pub mod msr; +pub mod ntp; pub mod page_allocator; pub mod page_table; pub(super) mod paging; diff --git a/awkernel_lib/src/arch/x86_64/ntp.rs b/awkernel_lib/src/arch/x86_64/ntp.rs new file mode 100644 index 000000000..8bec0a959 --- /dev/null +++ b/awkernel_lib/src/arch/x86_64/ntp.rs @@ -0,0 +1,40 @@ +use core::time::Duration; + +use awkernel_sync::{mcs::MCSNode, mutex::Mutex}; + +use crate::{ + delay, + ntp::{Ntp, SignedDuration, SystemTime}, +}; + +/// The time offset from the Unix epoch in nanoseconds. +static TIME_BASE: Mutex = Mutex::new(1004572800_000_000_000); // 2001-11-01 + +impl Ntp for super::X86 { + fn get_time() -> SystemTime { + let up = delay::uptime(); + let mut node = MCSNode::new(); + let guard = TIME_BASE.lock(&mut node); + let syst = SystemTime::new(*guard as u128); + syst + Duration::from_micros(up) + } + + fn set_time(new: u128) { + let mut node = MCSNode::new(); + let mut guard = TIME_BASE.lock(&mut node); + let up = delay::uptime() as u128 * 1000; + *guard = new - up; + } + + fn adjust_time(offset: SignedDuration) { + let mut node = MCSNode::new(); + let mut guard = TIME_BASE.lock(&mut node); + + let offset = offset.as_nanos(); + if offset > 0 { + *guard = guard.wrapping_add(offset as u128); + } else { + *guard = guard.wrapping_sub(-offset as u128); + } + } +} diff --git a/awkernel_lib/src/lib.rs b/awkernel_lib/src/lib.rs index 3c879cdee..614de2d5d 100644 --- a/awkernel_lib/src/lib.rs +++ b/awkernel_lib/src/lib.rs @@ -22,6 +22,7 @@ pub mod local_heap; pub mod logger; pub mod mmio; pub mod net; +pub mod ntp; pub mod priority_queue; pub mod sanity; pub mod sync; diff --git a/awkernel_lib/src/net/if_net.rs b/awkernel_lib/src/net/if_net.rs index 1221d8192..8ad65a4ca 100644 --- a/awkernel_lib/src/net/if_net.rs +++ b/awkernel_lib/src/net/if_net.rs @@ -210,6 +210,11 @@ impl IfNetInner { self.interface.routes_mut().remove_default_ipv4_route(); } + self.interface + .routes_mut() + .add_default_ipv4_route(gateway) + .unwrap(); + log::error!("adding {gateway:?} as a gateway"); self.default_gateway_ipv4 = Some(gateway); } } diff --git a/awkernel_lib/src/ntp.rs b/awkernel_lib/src/ntp.rs new file mode 100644 index 000000000..21f8ad326 --- /dev/null +++ b/awkernel_lib/src/ntp.rs @@ -0,0 +1,187 @@ +use alloc::string::String; +use alloc::{format, string::ToString}; + +use crate::arch::ArchImpl; +use core::{ + fmt::Debug, + ops::{Add, Sub}, + time::Duration, +}; + +pub mod packet; +pub mod timestamp; + +/// Represents the system time. +/// TODO: could be merged with time.rs? +#[derive(Copy, Clone)] +pub struct SystemTime { + /// Nanoseconds since UNIX epoch. + nsecs: u128, +} +impl SystemTime { + /// Create a new SystemTime instance with the given nanoseconds since UNIX epoch. + pub fn new(nsecs: u128) -> Self { + Self { nsecs } + } + + /// Represents the UNIX epoch (1970-01-01 00:00:00 UTC). + pub const fn epoch() -> Self { + Self { nsecs: 0 } + } + + /// Calculate the duration since other. + pub fn duration_since(&self, other: Self) -> Result { + Ok(Duration::from_nanos((self.nsecs - other.nsecs) as u64)) + } +} + +impl ToString for SystemTime { + fn to_string(&self) -> String { + let secs = self.nsecs / 1_000_000_000; + let nsecs = (self.nsecs % 1_000_000_000) as u32; + + let mut seconds_remaining = secs; + + // Calculate years + let mut year = 1970; + loop { + let days_in_year = if is_leap_year(year) { 366 } else { 365 }; + let seconds_in_year = days_in_year * 24 * 60 * 60; + + if seconds_remaining < seconds_in_year { + break; + } + + seconds_remaining -= seconds_in_year; + year += 1; + } + + // Calculate month and day + let days_in_month = [ + 31, // January + if is_leap_year(year) { 29 } else { 28 }, // February + 31, // March + 30, // April + 31, // May + 30, // June + 31, // July + 31, // August + 30, // September + 31, // October + 30, // November + 31, // December + ]; + + let mut month = 0; + let mut day_of_month = 0; + + let days_remaining = seconds_remaining / (24 * 60 * 60); + seconds_remaining %= 24 * 60 * 60; + + let mut days_counted = 0; + for (i, &days) in days_in_month.iter().enumerate() { + if days_counted + days > days_remaining { + month = i + 1; // 1-based month + day_of_month = (days_remaining - days_counted) + 1; // 1-based day + break; + } + days_counted += days; + } + + // Calculate time components + let hour = seconds_remaining / (60 * 60); + seconds_remaining %= 60 * 60; + let minute = seconds_remaining / 60; + let second = seconds_remaining % 60; + + format!( + "{:04}-{:02}-{:02} {:02}:{:02}:{:02}.{:09}", + year, month, day_of_month, hour, minute, second, nsecs + ) + } +} + +fn is_leap_year(year: u128) -> bool { + (year % 4 == 0 && year % 100 != 0) || (year % 400 == 0) +} + +impl Debug for SystemTime { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + write!( + f, + "SystemTime({} [ns] since epoch = {})", + self.nsecs, + self.to_string() + ) + } +} + +impl Add for SystemTime { + type Output = Self; + + fn add(self, dur: Duration) -> Self { + SystemTime { + nsecs: self.nsecs + dur.as_nanos() as u128, + } + } +} + +impl Sub for SystemTime { + type Output = Self; + + fn sub(self, dur: Duration) -> Self { + SystemTime { + nsecs: self.nsecs - dur.as_nanos() as u128, + } + } +} + +/// Represents a signed duration with a boolean indicating if it's positive or negative. +#[derive(Debug, Clone)] +pub struct SignedDuration(pub Duration, pub bool); + +impl SignedDuration { + /// Get the duration in nanoseconds. + pub fn as_nanos(&self) -> i128 { + if self.1 { + self.0.as_nanos() as i128 + } else { + -(self.0.as_nanos() as i128) + } + } +} + +/// Module for NTP and system clock. +/// +/// This module provides the interface for NTP daemons and managing the system clock. +#[derive(Copy, Clone)] +pub struct SystemClock {} + +impl SystemClock { + pub fn new() -> Self { + Self {} + } + + pub fn now() -> SystemTime { + ArchImpl::get_time() + } + + pub fn set(new: u128) { + ArchImpl::set_time(new); + } + + pub fn adjust(offset: SignedDuration) { + ArchImpl::adjust_time(offset); + } +} + +pub trait Ntp { + /// Get the current time in nanoseconds. + fn get_time() -> SystemTime; + + /// Set the current time in nanoseconds. + fn set_time(new: u128); + + /// Adjust the current time by the offset calculated from the NTP response. + fn adjust_time(offset: SignedDuration); +} diff --git a/awkernel_lib/src/ntp/packet.rs b/awkernel_lib/src/ntp/packet.rs new file mode 100644 index 000000000..a7dac656b --- /dev/null +++ b/awkernel_lib/src/ntp/packet.rs @@ -0,0 +1,130 @@ +use core::time::Duration; + +use crate::ntp::{ + timestamp::{NtpTimestamp, NTP_TIMESTAMP_DELTA}, + SignedDuration, SystemTime, +}; + +#[derive(Debug)] +pub struct NtpPacket { + pub li_vn_mode: u8, + pub stratum: u8, + pub poll: i8, + pub precision: i8, + pub root_delay: u32, + pub root_dispersion: u32, + pub ref_id: u32, + pub ref_timestamp: NtpTimestamp, + pub origin_timestamp: NtpTimestamp, + pub recv_timestamp: NtpTimestamp, + pub transmit_timestamp: NtpTimestamp, +} + +impl NtpPacket { + pub fn new() -> Self { + NtpPacket { + li_vn_mode: 0x1b, // LI = 0, VN = 3, Mode = 3 (client) + stratum: 0, + poll: 0, + precision: 0, + root_delay: 0, + root_dispersion: 0, + ref_id: 0, + ref_timestamp: NtpTimestamp(0), + origin_timestamp: NtpTimestamp(0), + recv_timestamp: NtpTimestamp(0), + transmit_timestamp: NtpTimestamp(0), + } + } + + pub fn to_bytes(self) -> [u8; 48] { + let mut buffer = [0u8; 48]; + buffer[0] = self.li_vn_mode; + buffer[1] = self.stratum; + buffer[2] = self.poll as u8; + buffer[3] = self.precision as u8; + + buffer[4..8].copy_from_slice(&self.root_delay.to_be_bytes()); + buffer[8..12].copy_from_slice(&self.root_dispersion.to_be_bytes()); + buffer[12..16].copy_from_slice(&self.ref_id.to_be_bytes()); + buffer[16..24].copy_from_slice(&self.ref_timestamp.0.to_be_bytes()); + buffer[24..32].copy_from_slice(&self.origin_timestamp.0.to_be_bytes()); + buffer[32..40].copy_from_slice(&self.recv_timestamp.0.to_be_bytes()); + buffer[40..48].copy_from_slice(&self.transmit_timestamp.0.to_be_bytes()); + + buffer + } + + pub fn from_bytes(bytes: &[u8]) -> Self { + let mut packet = NtpPacket::new(); + packet.li_vn_mode = bytes[0]; + packet.stratum = bytes[1]; + packet.poll = bytes[2] as i8; + packet.precision = bytes[3] as i8; + packet.root_delay = u32::from_be_bytes([bytes[4], bytes[5], bytes[6], bytes[7]]); + packet.root_dispersion = u32::from_be_bytes([bytes[8], bytes[9], bytes[10], bytes[11]]); + packet.ref_id = u32::from_be_bytes([bytes[12], bytes[13], bytes[14], bytes[15]]); + packet.ref_timestamp = NtpTimestamp(u64::from_be_bytes([ + bytes[16], bytes[17], bytes[18], bytes[19], bytes[20], bytes[21], bytes[22], bytes[23], + ])); + packet.origin_timestamp = NtpTimestamp(u64::from_be_bytes([ + bytes[24], bytes[25], bytes[26], bytes[27], bytes[28], bytes[29], bytes[30], bytes[31], + ])); + packet.recv_timestamp = NtpTimestamp(u64::from_be_bytes([ + bytes[32], bytes[33], bytes[34], bytes[35], bytes[36], bytes[37], bytes[38], bytes[39], + ])); + packet.transmit_timestamp = NtpTimestamp(u64::from_be_bytes([ + bytes[40], bytes[41], bytes[42], bytes[43], bytes[44], bytes[45], bytes[46], bytes[47], + ])); + packet + } + + /// Parse NTP response. Returns delay and offset. + pub fn parse_response( + &self, + originate_ts: NtpTimestamp, + destination_ts: NtpTimestamp, + ) -> (SignedDuration, SignedDuration) { + // Ideally originate_ts should be equal to self.origin_timestamp but in most cases we have the difference up to 10ns probably the errors in calculation. + assert!( + self.origin_timestamp.diff(&originate_ts.into()).0 < Duration::from_nanos(10), + "origin timestamp mismatch" + ); + + let ot = self.origin_timestamp.0 as i64; + let rt = self.recv_timestamp.0 as i64; + let tt = self.transmit_timestamp.0 as i64; + + let ntp_time = self.transmit_timestamp.0 >> 32; + log::debug!("Seconds since epoch: {}", ntp_time - NTP_TIMESTAMP_DELTA); + + let dts = NtpTimestamp::from(destination_ts); + let dt = dts.0 as i64; + let d = (dt - ot) - (tt - rt); + let t = (((rt as i128) - (ot as i128)) + ((tt as i128) - (dt as i128))) / 2; + + let delay = diff_to_signed_duration(d); + let offset = diff_to_signed_duration(t as i64); + + log::debug!("Delay: {:?}", delay); + log::debug!("Offset: {:?}", offset); + log::debug!("Origin time: {:?}", SystemTime::from(self.origin_timestamp)); + log::debug!("Receive time: {:?}", SystemTime::from(self.recv_timestamp)); + log::debug!( + "Transmit time: {:?}", + SystemTime::from(self.transmit_timestamp) + ); + log::debug!("Destination time: {:?}", SystemTime::from(dts)); + + (delay, offset) + } +} + +/// Convert the diff of NTP timestamp into SignedDuration. +fn diff_to_signed_duration(n: i64) -> SignedDuration { + let n_ = n.abs(); + let secs = n_ >> 32; + let nsecs = ((n_ & 0xffffffff) * 1_000_000_000) >> 32; + let dur = Duration::new(secs as u64, nsecs as u32); + SignedDuration(dur, n >= 0) +} diff --git a/awkernel_lib/src/ntp/timestamp.rs b/awkernel_lib/src/ntp/timestamp.rs new file mode 100644 index 000000000..46706aa7b --- /dev/null +++ b/awkernel_lib/src/ntp/timestamp.rs @@ -0,0 +1,151 @@ +use super::{SignedDuration, SystemTime}; +use core::{fmt::Display, ops::Sub, time::Duration}; + +pub const NTP_TIMESTAMP_DELTA: u64 = 2_208_988_800; // seconds between 1900 and 1970 + +static UNIX_EPOCH: SystemTime = SystemTime::epoch(); +/// NTP timestamp in 64-bit fixed-point format. The first 32 bits represent the number of seconds since 1900, and the last 32 bits represent the fraction of a second. +#[derive(Debug, Copy, Clone)] +pub struct NtpTimestamp(pub u64); + +impl NtpTimestamp { + /// Calculate the difference `self - other`. The first value is the difference in time, and the second value is true if `self` is greater than `other`. + pub fn diff(&self, other: &Self) -> SignedDuration { + if self.0 > other.0 { + let diff = self.0 - other.0; + let secs = diff >> 32; + let nsecs = ((diff & 0xffffffff) * 1_000_000_000) >> 32; + SignedDuration(Duration::new(secs as u64, nsecs as u32), true) + } else { + let diff = other.0 - self.0; + let secs = diff >> 32; + let nsecs = ((diff & 0xffffffff) * 1_000_000_000) >> 32; + SignedDuration(Duration::new(secs as u64, nsecs as u32), false) + } + } + + pub fn from_epoch_us(us: u64) -> Self { + let secs = us / 1_000_000; + let nsecs = ((us % 1_000_000) * 1_000) as u32; + NtpTimestamp((NTP_TIMESTAMP_DELTA + secs) << 32 | nsecs as u64) + } +} + +impl Display for NtpTimestamp { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> Result<(), core::fmt::Error> { + let secs = self.0 >> 32; + let nsecs = ((self.0 & 0xffffffff) * 1_000_000_000) >> 32; + write!(f, "{}.{:09}", secs, nsecs) + } +} + +impl From for SystemTime { + fn from(ntp: NtpTimestamp) -> SystemTime { + let secs = (ntp.0 >> 32).saturating_sub(NTP_TIMESTAMP_DELTA); + let nsecs = ((ntp.0 & 0xffffffff) * 1_000_000_000) >> 32; + UNIX_EPOCH + Duration::new(secs, nsecs as u32) + } +} + +impl From for NtpTimestamp { + fn from(system: SystemTime) -> NtpTimestamp { + let dur = system.duration_since(UNIX_EPOCH).unwrap(); + let int = dur.as_secs() + NTP_TIMESTAMP_DELTA; + let frac = ((dur.subsec_nanos() as u64) << 32) / 1_000_000_000; + NtpTimestamp(int << 32 | frac) + } +} + +impl Sub for NtpTimestamp { + type Output = SignedDuration; + + fn sub(self, rhs: Self) -> Self::Output { + self.diff(&rhs) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn convert_ntp_to_system_int() { + let ntp = (NTP_TIMESTAMP_DELTA + 3) << 32; + let system = SystemClock::from(NtpTimestamp(ntp)); + assert_eq!(system, UNIX_EPOCH + Duration::from_secs(3)); + } + + #[test] + fn convert_ntp_to_system_frac() { + let ntp = NTP_TIMESTAMP_DELTA << 32 | 1 << 31; + let system = SystemClock::from(NtpTimestamp(ntp)); + assert_eq!(system, UNIX_EPOCH + Duration::from_millis(500)); + } + + #[test] + fn convert_system_to_ntp_int() { + let system = UNIX_EPOCH + Duration::from_secs(3); + let ntp = NtpTimestamp::from(system); + assert_eq!(ntp.0, (NTP_TIMESTAMP_DELTA + 3) << 32); + } + + #[test] + fn convert_system_to_ntp_frac() { + let system = UNIX_EPOCH + Duration::from_millis(500); + log::debug!("{:?}", system); + let ntp = NtpTimestamp::from(system); + log::debug!("left: {:x}", ntp.0); + log::debug!("right: {:x}", NTP_TIMESTAMP_DELTA << 32 | 1 << 31); + assert_eq!(ntp.0, NTP_TIMESTAMP_DELTA << 32 | 1 << 31); + } + + #[test] + fn convert_epoch_system_to_ntp_frac() { + let system = UNIX_EPOCH; + let ntp = NtpTimestamp::from(system); + assert_eq!(ntp.0, NTP_TIMESTAMP_DELTA << 32); + } + + #[test] + fn test_diff() { + let t1 = UNIX_EPOCH + Duration::from_secs(1); + let t2 = UNIX_EPOCH + Duration::from_secs(2); + let ntp1 = NtpTimestamp::from(t1); + let ntp2 = NtpTimestamp::from(t2); + let duration = ntp2.diff(&ntp1); + assert_eq!(duration, Duration::from_secs(1)); + } + + #[test] + fn test_parse_response() { + let originate_ts = SystemClock::now(); + + let packet = NtpPacket { + li_vn_mode: 0x1b, + stratum: 1, + poll: 4, + precision: -6, + root_delay: 0, + root_dispersion: 0, + ref_id: 0, + ref_timestamp: originate_ts.into(), + origin_timestamp: originate_ts.into(), + recv_timestamp: (originate_ts + Duration::from_secs(1)).into(), + transmit_timestamp: (originate_ts + Duration::from_secs(2)).into(), + }; + + log::debug!("{:?}", packet.origin_timestamp.0); + log::debug!("{:?}", packet.recv_timestamp.0); + log::debug!("{:?}", packet.transmit_timestamp.0); + + let buf = packet.to_bytes(); + let destination_ts = originate_ts + Duration::from_secs(3); + let (delay, offset) = parse_response(buf, originate_ts, destination_ts); + + log::debug!("Delay: {:?}", delay); + log::debug!("Offset: {:?}", offset); + + assert_eq!(delay, Duration::from_secs(2)); + assert_eq!(offset, Duration::from_secs(0)); + } +} diff --git a/scripts/ntpserver.py b/scripts/ntpserver.py new file mode 100755 index 000000000..ae7fe2eae --- /dev/null +++ b/scripts/ntpserver.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +#20200301 +#Jan Mojzis +#Public domain. +# Downloaded from https://github.com/janmojzis/ntpserver + +import logging +import socket +import time +import datetime +import os +import random +import sys +import struct +import getopt + +NTPFORMAT = ">3B b 3I 4Q" +NTPDELTA = 2208988800.0 + +def s2n(t = 0.0): + """ + System to NTP + """ + + t += NTPDELTA + return (int(t) << 32) + int(abs(t - int(t)) * (1<<32)) + +def n2s(x = 0): + """ + NTP to System + """ + + t = float(x >> 32) + float(x & 0xffffffff) / (1<<32) + return t - NTPDELTA + +def tfmt(t = 0.0): + """ + Format System Timestamp + """ + + return datetime.datetime.fromtimestamp(t).strftime("%Y-%m-%d_%H:%M:%S.%f") + +def usage(): + """ + Print the usage + """ + + print("ntpserver.py [-vh] [ip] [port] [chroot directory]", file = sys.stderr) + sys.exit(100) + +loglevel = logging.INFO + +# parse program parameters +try: + options, arguments = getopt.getopt(sys.argv[1:], 'hv') +except Exception as e: + #bad option + usage() + +# process options +for opt, val in options: + if opt == "-h": + usage() + if opt == "-v": + loglevel = logging.DEBUG + +try: + localip = arguments[0] +except IndexError: + localip = "0.0.0.0" + +try: + localport = int(arguments[1]) +except IndexError: + localport = 123 + +try: + root = arguments[2] +except IndexError: + root = '/var/lib/ntpserver' + +logging.basicConfig( + format='%(asctime)s %(levelname)-8s %(message)s', + level = loglevel, + datefmt='%Y-%m-%d_%H:%M:%S') + +logging.info("starting ntpserver.py %s %d" % (localip, localport)) + +# chroot +if not os.path.exists(root): + logging.warning('%s not exist, making the directory' % (root)) + os.mkdir(root) +os.chdir(root) +root = os.getcwd() +os.chroot(".") +logging.debug('chrooted into directory %s' % (root)) + +# create socket +s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +s.bind((localip, localport)) +logging.debug('local socket: %s:%d' % s.getsockname()) + +# drop privileges +try: + uid = 100000000 + 100000 * random.randint(0, 999) + os.getpid() + os.setgid(uid) + os.setuid(uid) +except OSError: + uid = 10000 + random.randint(0, 9999) + os.setgid(uid) + os.setuid(uid) +logging.debug('UID/GID set to %d' % (uid)) + +# get the precision +try: + hz = int(1 / time.clock_getres(time.CLOCK_REALTIME)) +except AttributeError: + hz = 1000000000 +precision = 0 +while hz > 1: + precision -= 1; + hz >>= 1 + +while True: + try: + # receive the query + data, addr = s.recvfrom(struct.calcsize(NTPFORMAT)) + serverrecv = s2n(time.time()) + if len(data) != struct.calcsize(NTPFORMAT): + raise Exception("Invalid NTP packet: packet too short: %d bytes" % (len(data))) + try: + data = struct.unpack(NTPFORMAT, data) + except struct.error: + raise Exception("Invalid NTP packet: unable to parse packet") + data = list(data) + + # parse the NTP query (only Version, Mode, Transmit Timestamp) + version = data[0] >> 3 & 0x7 + if (version > 4): + raise Exception("Invalid NTP packet: bad version %d" % (version)) + mode = data[0] & 0x7 + if (mode != 3): + raise Exception("Invalid NTP packet: bad client mode %d" % (mode)) + clienttx = data[10] + + # create the NTP response + data[0] = version << 3 | 4 # Leap, Version, Mode + data[1] = 1 # Stratum + data[2] = 0 # Poll + data[3] = precision # Precision + data[4] = 0 # Synchronizing Distance + data[5] = 0 # Synchronizing Dispersion + data[6] = 0 # Reference Clock Identifier + data[7] = serverrecv # Reference Timestamp + data[8] = clienttx # Originate Timestamp + data[9] = serverrecv # Receive Timestamp + data[10] = s2n(time.time()) # Transmit Timestamp + + # send the response + data = struct.pack(NTPFORMAT, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8], data[9], data[10]) + s.sendto(data, addr) + + except Exception as e: + logging.warning("%s: failed: %s" % (addr[0], e)) + else: + logging.info('%s: ok: client="%s", server="%s"' % (addr[0], tfmt(n2s(clienttx)), tfmt(n2s(serverrecv)))) diff --git a/scripts/udp.py b/scripts/udp.py index 10482eab0..ba90f470e 100644 --- a/scripts/udp.py +++ b/scripts/udp.py @@ -3,7 +3,7 @@ sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) -addr = ('localhost', 26099) +addr = ('0.0.0.0', 26099) # addr = ('10.0.2.2', 26099) # addr = ('192.168.100.1', 26099) print('listening on %s port %s' % addr, file=sys.stderr) diff --git a/userland/Cargo.toml b/userland/Cargo.toml index 583de9ba2..eb0cdbc42 100644 --- a/userland/Cargo.toml +++ b/userland/Cargo.toml @@ -26,6 +26,10 @@ optional = true path = "../applications/tests/test_network" optional = true +[dependencies.test_ntp] +path = "../applications/tests/test_ntp" +optional = true + [dependencies.test_rpi_hal] path = "../applications/tests/test_rpi_hal" optional = true @@ -91,6 +95,7 @@ rd_gen_to_dags = ["dep:rd_gen_to_dags"] # Test applications test_network = ["dep:test_network"] +test_ntp = ["dep:test_ntp"] test_pubsub = ["dep:test_pubsub"] test_rpi_hal = ["dep:test_rpi_hal"] test_graphics = ["dep:test_graphics"] diff --git a/userland/src/lib.rs b/userland/src/lib.rs index 917600021..7ef9b7178 100644 --- a/userland/src/lib.rs +++ b/userland/src/lib.rs @@ -13,6 +13,9 @@ pub async fn main() -> Result<(), Cow<'static, str>> { #[cfg(feature = "test_network")] test_network::run().await; // test for network + #[cfg(feature = "test_ntp")] + test_ntp::run().await; // test for NTP + #[cfg(feature = "test_pubsub")] test_pubsub::run().await; // test for pubsub