From 169644b59f327724bc2b03212ea9e5691bbcec01 Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Thu, 25 Jul 2024 16:16:27 +0300 Subject: [PATCH] Add lib/rust_px4_nuttx library --- src/lib/rust_px4_nuttx/Cargo.lock | 75 +++++++++++ src/lib/rust_px4_nuttx/Cargo.toml | 18 +++ .../px4_nuttx_macros/Cargo.toml | 14 ++ .../px4_nuttx_macros/src/lib.rs | 19 +++ .../px4_nuttx_macros/src/module_main.rs | 41 ++++++ src/lib/rust_px4_nuttx/src/lib.rs | 81 ++++++++++++ src/lib/rust_px4_nuttx/src/nuttx.rs | 5 + src/lib/rust_px4_nuttx/src/nuttx/galloc.rs | 23 ++++ src/lib/rust_px4_nuttx/src/nuttx/net.rs | 2 + src/lib/rust_px4_nuttx/src/nuttx/net/udp.rs | 122 ++++++++++++++++++ src/lib/rust_px4_nuttx/src/nuttx/time.rs | 57 ++++++++ src/lib/rust_px4_nuttx/src/px4.rs | 1 + src/lib/rust_px4_nuttx/src/px4/logger.rs | 83 ++++++++++++ 13 files changed, 541 insertions(+) create mode 100644 src/lib/rust_px4_nuttx/Cargo.lock create mode 100644 src/lib/rust_px4_nuttx/Cargo.toml create mode 100644 src/lib/rust_px4_nuttx/px4_nuttx_macros/Cargo.toml create mode 100644 src/lib/rust_px4_nuttx/px4_nuttx_macros/src/lib.rs create mode 100644 src/lib/rust_px4_nuttx/px4_nuttx_macros/src/module_main.rs create mode 100644 src/lib/rust_px4_nuttx/src/lib.rs create mode 100644 src/lib/rust_px4_nuttx/src/nuttx.rs create mode 100644 src/lib/rust_px4_nuttx/src/nuttx/galloc.rs create mode 100644 src/lib/rust_px4_nuttx/src/nuttx/net.rs create mode 100644 src/lib/rust_px4_nuttx/src/nuttx/net/udp.rs create mode 100644 src/lib/rust_px4_nuttx/src/nuttx/time.rs create mode 100644 src/lib/rust_px4_nuttx/src/px4.rs create mode 100644 src/lib/rust_px4_nuttx/src/px4/logger.rs diff --git a/src/lib/rust_px4_nuttx/Cargo.lock b/src/lib/rust_px4_nuttx/Cargo.lock new file mode 100644 index 000000000000..c4ae407e359f --- /dev/null +++ b/src/lib/rust_px4_nuttx/Cargo.lock @@ -0,0 +1,75 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" + +[[package]] +name = "log" +version = "0.4.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" + +[[package]] +name = "no-std-net" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43794a0ace135be66a25d3ae77d41b91615fb68ae937f904090203e81f755b65" + +[[package]] +name = "proc-macro2" +version = "1.0.86" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5e719e8df665df0d1c8fbfd238015744736151d4445ec0836b8e628aae103b77" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "px4_nuttx" +version = "0.1.0" +dependencies = [ + "embedded-io", + "log", + "no-std-net", + "px4_nuttx_macros", +] + +[[package]] +name = "px4_nuttx_macros" +version = "0.1.0" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "quote" +version = "1.0.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fa76aaf39101c457836aec0ce2316dbdc3ab723cdda1c6bd4e6ad4208acaca7" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "syn" +version = "2.0.72" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc4b9b9bf2add8093d3f2c0204471e951b2285580335de42f9d2534f3ae7a8af" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "unicode-ident" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" diff --git a/src/lib/rust_px4_nuttx/Cargo.toml b/src/lib/rust_px4_nuttx/Cargo.toml new file mode 100644 index 000000000000..e02799a545c1 --- /dev/null +++ b/src/lib/rust_px4_nuttx/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "px4_nuttx" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[profile.dev] +panic = "abort" + +[profile.release] +panic = "abort" + +[dependencies] +log = "0.4" +embedded-io = "0.6.1" +no-std-net = "0.6.0" +px4_nuttx_macros = { path = "./px4_nuttx_macros" } diff --git a/src/lib/rust_px4_nuttx/px4_nuttx_macros/Cargo.toml b/src/lib/rust_px4_nuttx/px4_nuttx_macros/Cargo.toml new file mode 100644 index 000000000000..ad15bc92813d --- /dev/null +++ b/src/lib/rust_px4_nuttx/px4_nuttx_macros/Cargo.toml @@ -0,0 +1,14 @@ +[package] +name = "px4_nuttx_macros" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[lib] +proc-macro = true + +[dependencies] +proc-macro2 = "1.0" +syn = { version = "2.0", features = ["full"] } +quote = "1.0" diff --git a/src/lib/rust_px4_nuttx/px4_nuttx_macros/src/lib.rs b/src/lib/rust_px4_nuttx/px4_nuttx_macros/src/lib.rs new file mode 100644 index 000000000000..c536513fd2b8 --- /dev/null +++ b/src/lib/rust_px4_nuttx/px4_nuttx_macros/src/lib.rs @@ -0,0 +1,19 @@ +#![recursion_limit = "128"] + +extern crate proc_macro; + +use proc_macro::TokenStream; + +mod module_main; + +/* +#[proc_macro_attribute] +pub fn px4_message(args: TokenStream, input: TokenStream) -> TokenStream { + message::px4_message(args, input) +} +*/ + +#[proc_macro_attribute] +pub fn px4_module_main(attr: TokenStream, input: TokenStream) -> TokenStream { + module_main::px4_module_main(attr, input) +} diff --git a/src/lib/rust_px4_nuttx/px4_nuttx_macros/src/module_main.rs b/src/lib/rust_px4_nuttx/px4_nuttx_macros/src/module_main.rs new file mode 100644 index 000000000000..7321fb9bc79a --- /dev/null +++ b/src/lib/rust_px4_nuttx/px4_nuttx_macros/src/module_main.rs @@ -0,0 +1,41 @@ +use proc_macro::TokenStream; +use proc_macro2::{Ident, Span}; +use quote::quote; +use syn::parse_macro_input; + +pub fn px4_module_main(attr: TokenStream, input: TokenStream) -> TokenStream { + if !attr.is_empty() { + panic!("px4_module_main does not take any arguments"); + } + let crate_name = std::env::var("CARGO_PKG_NAME").unwrap(); + let main_fndef = Ident::new(&format!("rust_{}_main", &crate_name), Span::call_site()); + + let fndef = parse_macro_input!(input as syn::ItemFn); + let name = &fndef.sig.ident; + let expanded = quote! { + use core::panic::PanicInfo; + use px4_nuttx::alloc::format; + + extern "C" { + pub fn printf(format: *const u8, ...) -> i32; + } + + #[panic_handler] + fn rust_panic(info: &PanicInfo<'_>) -> ! { + let mut message = format!("PANIC: [{}] {:}", #crate_name, info); + message.push('\0'); + unsafe { + printf(b"%s\n\0" as *const u8, message.as_bytes().as_ptr()); + } + loop {} + } + + #fndef + + #[no_mangle] + pub extern "C" fn #main_fndef(argc: u32, argv: *mut *mut u8) -> i32 { + unsafe { px4_nuttx::_run(concat!(module_path!(), "\0").as_bytes(), argc, argv, #name) } + } + }; + expanded.into() +} diff --git a/src/lib/rust_px4_nuttx/src/lib.rs b/src/lib/rust_px4_nuttx/src/lib.rs new file mode 100644 index 000000000000..dc09ce7ff362 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/lib.rs @@ -0,0 +1,81 @@ +#![no_std] +#![no_main] + +pub mod nuttx; +pub mod px4; + +pub use nuttx::alloc; +pub use px4::logger; +pub use px4_nuttx_macros::px4_module_main; + +use nuttx::alloc::vec::Vec; +use core::ffi::CStr; + +pub fn init() { + px4::logger::init(); +} + +#[doc(hidden)] +pub unsafe fn _run(_modulename: &'static [u8], argc: u32, argv: *mut *mut u8, f: F) -> i32 +where + F: Fn(&[&str]) -> R + core::panic::UnwindSafe, + R: MainStatusCode, +{ + logger::init(); + let mut args = Vec::with_capacity(argc as usize); + for i in 0..argc { + args.push( + CStr::from_ptr(*argv.offset(i as isize) as *const i8) + .to_str() + .unwrap_or_else(|_| panic!("Invalid UTF-8 in arguments.")), + ); + } + f(&args).to_status_code() +} + +/// The return type of your `#[px4_module_main]` function. +pub trait MainStatusCode { + /// The status code to return. + fn to_status_code(self) -> i32; + + /// The status code to return in case of a panic. + /// + /// −1 by default. + fn panic_status_code() -> i32 { + -1 + } +} + +/// Returns 0. +impl MainStatusCode for () { + fn to_status_code(self) -> i32 { + 0 + } +} + +/// Returns the `i32` itself. +impl MainStatusCode for i32 { + fn to_status_code(self) -> i32 { + self + } +} + +/// Returns 0 for `Ok`, and 1 for `Err`. +impl MainStatusCode for Result<(), ()> { + fn to_status_code(self) -> i32 { + match self { + Ok(()) => 0, + Err(()) => 1, + } + } +} + +/// Returns 0 for `Ok`, and the `i32` itself for `Err`. +impl MainStatusCode for Result<(), i32> { + fn to_status_code(self) -> i32 { + match self { + Ok(()) => 0, + Err(s) => s, + } + } +} diff --git a/src/lib/rust_px4_nuttx/src/nuttx.rs b/src/lib/rust_px4_nuttx/src/nuttx.rs new file mode 100644 index 000000000000..55e0c81c4b6a --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/nuttx.rs @@ -0,0 +1,5 @@ +mod galloc; +pub mod net; +pub mod time; + +pub use galloc::alloc; diff --git a/src/lib/rust_px4_nuttx/src/nuttx/galloc.rs b/src/lib/rust_px4_nuttx/src/nuttx/galloc.rs new file mode 100644 index 000000000000..5a4bd05f98a2 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/nuttx/galloc.rs @@ -0,0 +1,23 @@ +pub extern crate alloc; +use alloc::alloc::{GlobalAlloc, Layout}; + +#[global_allocator] +static ALLOCATOR: Gallocator = Gallocator; + +extern "C" { // Import C Function + fn aligned_alloc(align: usize, size: usize) -> *mut u8; + fn free(p: *const u8); +} + +struct Gallocator; +unsafe impl GlobalAlloc for Gallocator { + unsafe fn alloc(&self, l: Layout) -> *mut u8 { + unsafe { + aligned_alloc(l.align(), l.size()) + } + } + unsafe fn dealloc(&self, p: *mut u8, _: Layout) { + free(p); + } +} + diff --git a/src/lib/rust_px4_nuttx/src/nuttx/net.rs b/src/lib/rust_px4_nuttx/src/nuttx/net.rs new file mode 100644 index 000000000000..7ea182483567 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/nuttx/net.rs @@ -0,0 +1,2 @@ +pub mod udp; +pub use udp::UdpSocket; diff --git a/src/lib/rust_px4_nuttx/src/nuttx/net/udp.rs b/src/lib/rust_px4_nuttx/src/nuttx/net/udp.rs new file mode 100644 index 000000000000..6503c7335f57 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/nuttx/net/udp.rs @@ -0,0 +1,122 @@ +use core::fmt; +use embedded_io::ErrorKind; +use crate::nuttx::time::Duration; +use crate::nuttx::galloc::alloc::vec::Vec; + +const ADDRLEN: u32 = 16; + +extern "C" { + fn socket(domain: i32, ty: i32, protocol: i32) -> i32; + fn bind(sockfd: i32, addr: *const u8, addrlen: u32) -> i32; + fn sendto(sockfd: i32, buf: *const u8, len: u64, flags: i32, dest_addr: *const u8, addrlen: u32) -> i64; + fn recvfrom(sockfd: i32, buf: *mut u8, len: u64, flags: i32, from: *mut u8, fromlen: *mut u8) -> i64; + fn close(sockfd: i32); +} + +/// A UDP socket. + +pub struct UdpSocket { + fd: i32, + timeout: i32, +} + +impl Drop for UdpSocket { + fn drop(&mut self) { + unsafe { + if self.fd >= 0 { + close(self.fd); + self.fd = -1; + } + } + } +} + +impl UdpSocket { + const AF_INET: u16 = 2; + const SOCK_DGRAM: i32 = 2; + + fn default() -> UdpSocket { + UdpSocket{fd: -1, timeout: -1} + } + + fn do_bind(&mut self, addr: &str) -> Result<(), ErrorKind> { + let fd = unsafe { socket(UdpSocket::AF_INET as i32, UdpSocket::SOCK_DGRAM, 0) }; + if fd < 0 { + Err(ErrorKind::Other) + } else { + self.fd = fd; + let socketaddr = UdpSocket::get_socketaddr(addr); + let ret = unsafe { bind(fd, socketaddr.as_ptr(), 16) }; + if ret < 0 { + return Err(ErrorKind::Other); + } + Ok(()) + } + } + + // a example: '192.168.200.100:12345'" + fn get_socketaddr(a: &str) -> [u8; 16] { + let addr_port: Vec<&str> = a.split(":").collect(); + let octets: Vec<&str> = addr_port[0].split(".").collect(); + let mut socketaddr = [0; 16]; + + let dom = UdpSocket::AF_INET.to_le_bytes(); // LE for host + let port = addr_port[1].parse::().unwrap().to_be_bytes(); // BE for network + let addr: Vec = octets.iter().map(|x| x.parse::().unwrap()).collect::>().as_slice().try_into().unwrap(); + + socketaddr[0] = dom[0]; + socketaddr[1] = dom[1]; + socketaddr[2] = port[0]; + socketaddr[3] = port[1]; + socketaddr[4] = addr[0]; + socketaddr[5] = addr[1]; + socketaddr[6] = addr[2]; + socketaddr[7] = addr[3]; + socketaddr + } + + pub fn bind(addr: &str) -> Result { + let mut socket = UdpSocket::default(); + socket.do_bind(&addr)?; + return Ok(socket); + } + + pub fn set_read_timeout(&mut self, dur: Option) -> Result<(), ErrorKind> { + if let Some(d) = dur { + self.timeout = (d.as_msecs() & 0x7fffffff) as i32; + } else { + self.timeout = -1; + } + Ok(()) + } + + pub fn recv_from(&self, buf: &mut [u8]) -> Result { + let mut from: [u8; 16] = [0; 16]; + let fromlen: u32 = ADDRLEN; + + let ret = unsafe { recvfrom(self.fd, buf.as_mut_ptr(), buf.len() as u64, 0, from.as_mut_ptr(), fromlen.to_le_bytes().as_mut_ptr()) }; + if ret < 0 { + return Err(ErrorKind::BrokenPipe); + } else if ret == 0 { + return Err(ErrorKind::NotFound) + } + Ok(ret as usize) + } + + pub fn send_to(&self, buf: &[u8], addr: &str) -> Result { + let socketaddr = UdpSocket::get_socketaddr(addr); + let ret = unsafe { sendto(self.fd, buf.as_ptr(), buf.len() as u64, 0, socketaddr.as_ptr(), ADDRLEN) }; + if ret < 0 { + return Err(ErrorKind::Other); + } + Ok(ret as usize) + } +} + + +impl fmt::Debug for UdpSocket { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + // TODO: print socket info + f.debug_struct("UdpSocket").finish() + } +} diff --git a/src/lib/rust_px4_nuttx/src/nuttx/time.rs b/src/lib/rust_px4_nuttx/src/nuttx/time.rs new file mode 100644 index 000000000000..060873832a07 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/nuttx/time.rs @@ -0,0 +1,57 @@ +use core::ops::Add; +use core::ops::AddAssign; + +extern "C" { + fn hrt_absolute_time() -> u64; +} + +pub fn hrt_time() -> u64 { + unsafe { + hrt_absolute_time() + } +} + +pub struct Duration(u64); + +impl Duration { + pub fn new(secs: u64, nanos: u32) -> Duration { + let sec = secs * 1000_000_000_u64 + nanos as u64; + Duration(sec) + } + + pub fn from_secs(secs: u64) -> Duration { + let sec = secs * 1000_000_000_u64; + Duration(sec) + } + + pub fn from_millis(millis: u64) -> Duration { + let sec = millis * 1000_000_u64; + Duration(sec) + } + + pub fn as_secs(&self) -> u64 { + self.0 / 1000_000_000_u64 + } + + pub fn subsec_nanos(&self) -> u64 { + self.0 % 1000_000_000_u64 + } + + pub fn as_msecs(&self) -> u64 { + self.0 / 1000_000_u64 + } +} + +impl Add for Duration { + type Output = Duration; + + fn add(self, other: Duration) -> Duration { + Duration(self.0 + other.0) + } +} + +impl AddAssign for Duration { + fn add_assign(&mut self, other: Duration) { + self.0 = self.0 + other.0; + } +} diff --git a/src/lib/rust_px4_nuttx/src/px4.rs b/src/lib/rust_px4_nuttx/src/px4.rs new file mode 100644 index 000000000000..d99172848776 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/px4.rs @@ -0,0 +1 @@ +pub mod logger; diff --git a/src/lib/rust_px4_nuttx/src/px4/logger.rs b/src/lib/rust_px4_nuttx/src/px4/logger.rs new file mode 100644 index 000000000000..a1d8e5498224 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/px4/logger.rs @@ -0,0 +1,83 @@ +use log::{Metadata, Record}; +use crate::nuttx::alloc::format; + +#[allow(dead_code)] +extern "C" { + fn printf(fmt: *const u8, ...) -> i32; +} + +pub enum LogLevel { + Debug = 0, + Info = 1, + Warn = 2, + Error = 3, + Panic = 4, +} + +#[allow(dead_code)] +pub fn log_raw(level: LogLevel, message: &str) { + let lvlstr = match level { + LogLevel::Panic => "PANIC", + LogLevel::Error => "ERROR", + LogLevel::Warn => "WARN ", + LogLevel::Info => "INFO ", + LogLevel::Debug => "DEBUG", + }; + let msg = format!("{} {}\n\0", lvlstr, message); + unsafe { + printf(msg.as_bytes().as_ptr()); + } +} + +#[macro_export] +macro_rules! info_raw { + ($arg:expr) => ( + $crate::log_raw($crate::LogLevel::Info, $arg) + ); + ($($arg:tt)+) => ( + $crate::log_raw($crate::LogLevel::Info, &format!($($arg)+)) + ); +} + +struct Px4Logger; + +impl log::Log for Px4Logger { + fn enabled(&self, metadata: &Metadata) -> bool { + metadata.level() <= log::Level::Info + } + + fn log(&self, record: &Record) { + if !self.enabled(record.metadata()) { + return; + } + + let level = match record.level() { + log::Level::Error => "ERROR", + log::Level::Warn => "WARN ", + log::Level::Info => "INFO ", + log::Level::Debug | log::Level::Trace => "DEBUG", + }; + + let target = record.target(); + let message = format!("{} [{}] {}\0", level, target, record.args()); + + unsafe { + let _ = printf("%s\n\0".as_ptr(), message.as_ptr()); + } + } + + fn flush(&self) {} +} + +static LOGGER: Px4Logger = Px4Logger; + +pub fn init() { + /* + unsafe { + printf(b"INFO [rust_px4_nuttx] initializing\n\0".as_ptr()); + } + */ + if log::set_logger(&LOGGER).is_ok() { + log::set_max_level(log::LevelFilter::Info); + } +}