From 237b6abf94526d0a194017d70de4f126e8d0b231 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 | 16 +++ src/lib/rust_px4_nuttx/Cargo.toml | 12 ++ .../px4_nuttx_macros/Cargo.toml | 14 +++ .../px4_nuttx_macros/src/lib.rs | 19 +++ .../px4_nuttx_macros/src/module_main.rs | 24 ++++ src/lib/rust_px4_nuttx/src/lib.rs | 111 ++++++++++++++++++ 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 | 85 ++++++++++++++ src/lib/rust_px4_nuttx/src/nuttx/time.rs | 53 +++++++++ src/lib/rust_px4_nuttx/src/px4.rs | 1 + src/lib/rust_px4_nuttx/src/px4/logger.rs | 93 +++++++++++++++ 13 files changed, 458 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..c52b0eeea59a --- /dev/null +++ b/src/lib/rust_px4_nuttx/Cargo.lock @@ -0,0 +1,16 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "log" +version = "0.4.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" + +[[package]] +name = "px4_nuttx" +version = "0.1.0" +dependencies = [ + "log", +] diff --git a/src/lib/rust_px4_nuttx/Cargo.toml b/src/lib/rust_px4_nuttx/Cargo.toml new file mode 100644 index 000000000000..078dcfe10ce6 --- /dev/null +++ b/src/lib/rust_px4_nuttx/Cargo.toml @@ -0,0 +1,12 @@ +[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 + +[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..e4e26e64a5fc --- /dev/null +++ b/src/lib/rust_px4_nuttx/px4_nuttx_macros/src/module_main.rs @@ -0,0 +1,24 @@ +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! { + #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..0554595d2c77 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/lib.rs @@ -0,0 +1,111 @@ +#![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 nuttx::alloc::string::String; +use core::fmt::Write; +use core::ffi::CStr; + +#[cfg(not(target_arch="x86_64"))] +use core::panic::PanicInfo; + + +#[cfg(not(target_arch="x86_64"))] +#[panic_handler] +fn panic(info: &PanicInfo<'_>) -> ! { + let payload: &str = if let Some(s) = info.payload().downcast_ref::<&'static str>() { + s + } else if let Some(s) = info.payload().downcast_ref::() { + &s + } else { + "[unknown]" + }; + let mut message = String::new(); + write!(message, "panicked at '{}'", payload).unwrap(); + if let Some(loc) = info.location() { + write!(message, ", {}", loc).unwrap(); + } + message.push('\0'); + logger::log_raw( + logger::LogLevel::Panic, + &message, + ); + loop {} +} + +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..a6d19420fc24 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/nuttx/net/udp.rs @@ -0,0 +1,85 @@ +use core::fmt; +use embedded_io::ErrorKind; +use crate::nuttx::galloc::alloc::vec::Vec; + +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) -> i32; +} + +/// A UDP socket. + +pub struct UdpSocket { + fd: i32, +} + +impl UdpSocket { + const AF_INET: u16 = 2; + const SOCK_DGRAM: i32 = 2; + + 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{fd: -1}; + socket.do_bind(&addr)?; + return Ok(socket); + } + +/* + pub fn recv_from(&self, buf: &mut [u8]) -> io::Result<(usize, ErrorKind)> { + // TODO: impl + } +*/ + + 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(), 16) }; + 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..4f0578c0a239 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/nuttx/time.rs @@ -0,0 +1,53 @@ +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_000u64 + nanos as u64; + Duration(sec) + } + + pub fn from_secs(secs: u64) -> Duration { + let sec = secs * 1000_000_000u64; + Duration(sec) + } + + pub fn from_millis(millis: u64) -> Duration { + let sec = millis * 1000_000u64; + Duration(sec) + } + + pub fn as_secs(&self) -> u64 { + self.0 / 1000_000_000u64 + } + + pub fn subsec_nanos(&self) -> u64 { + self.0 % 1000_000_000u64 + } +} + +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..af6f878b9064 --- /dev/null +++ b/src/lib/rust_px4_nuttx/src/px4/logger.rs @@ -0,0 +1,93 @@ +use log::{Metadata, Record}; +use crate::nuttx::alloc::format; + +#[allow(dead_code)] +extern "C" { + fn px4_log_modulename(level: i32, module: *const u8, fmt: *const u8, ...); + fn px4_log_raw(level: i32, fmt: *const u8, ...); +} + +pub enum LogLevel { + Debug = 0, + Info = 1, + Warn = 2, + Error = 3, + Panic = 4, +} +#[allow(dead_code)] +pub fn log_raw(level: LogLevel, message: &str) { + unsafe { + px4_log_raw( + level as i32, + "%.*s\0".as_ptr(), + message.len() as i32, + message.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 => LogLevel::Error, + log::Level::Warn => LogLevel::Warn, + log::Level::Info => LogLevel::Info, + log::Level::Debug => LogLevel::Debug, + log::Level::Trace => LogLevel::Debug, + }; + + // We allocate both nul-terminated strings as one single String, to + // save on heap allocations. + let target = record.target(); + let s = format!("{}\0{}\0", target, record.args()); + let (module, message) = s.as_bytes().split_at(target.len() + 1); + + unsafe { + px4_log_modulename( + level as i32, + module.as_ptr(), + "%s\0".as_ptr(), + message.as_ptr(), + ); + } + } + + fn flush(&self) {} +} + +static LOGGER: Px4Logger = Px4Logger; + +pub fn init() { + /* + unsafe { + px4_log_modulename( + LogLevel::Info as i32, + b"rust_px4_nuttx\0".as_ptr(), + b"initializing\0".as_ptr(), + ); + } + */ + if log::set_logger(&LOGGER).is_ok() { + log::set_max_level(log::LevelFilter::Info); + } +}