Add lib/rust_px4_nuttx library

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" }

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"

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)
+}

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()
+}

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::<String>() {
+        &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<F, R>(_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,
+		}
+	}
+}

src/lib/rust_px4_nuttx/src/nuttx.rs

@@ -0,0 +1,5 @@
+mod galloc;
+pub mod net;
+pub mod time;
+
+pub use galloc::alloc;

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);
+	}
+}
+

src/lib/rust_px4_nuttx/src/nuttx/net.rs

@@ -0,0 +1,2 @@
+pub mod udp;
+pub use udp::UdpSocket;

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::<u16>().unwrap().to_be_bytes();        // BE for network
+        let addr: Vec<u8> = octets.iter().map(|x| x.parse::<u8>().unwrap()).collect::<Vec<u8>>().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<UdpSocket, ErrorKind> {
+        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<usize, ErrorKind> {
+        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()
+    }
+}

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;
+    }
+}

src/lib/rust_px4_nuttx/src/px4.rs

@@ -0,0 +1 @@
+pub mod logger;