implemented queues, broke threads on the way :/

mission/freeRTOS_rust_helper.c

@@ -2,6 +2,8 @@
 #include "semphr.h"
 #include "task.h"
 
+//TODO namespace the names
+
 /**
  * Task descriptors
  */
@@ -26,13 +28,9 @@ void initFreeRTOSHelper() {
   queueMutex = xSemaphoreCreateRecursiveMutexStatic(&queueMutexDescriptor);
 }
 
-const char * get_task_name() {
+const char *get_task_name() { return pcTaskGetName(NULL); }
-    return pcTaskGetName( NULL );
-}
 
-void stop_it() {
+void stop_it() { taskENTER_CRITICAL(); }
-    taskENTER_CRITICAL();
-}
 
 // TODO return some error code?
 void *create_task(TaskFunction_t taskFunction, void *parameter, void *buffer,
@@ -69,8 +67,25 @@ void * create_queue(size_t length, size_t element_size, uint8_t * buffer) {
       sizeof(queueDescriptors) / sizeof(*queueDescriptors)) {
     return NULL;
   }
-QueueHandle_t newQueue = xQueueCreateStatic(length, element_size, buffer, queueDescriptors + nextFreeQueueDescriptor);
+  QueueHandle_t newQueue = xQueueCreateStatic(
+      length, element_size, buffer, queueDescriptors + nextFreeQueueDescriptor);
   nextFreeQueueDescriptor++;
   xSemaphoreGiveRecursive(queueMutex);
   return newQueue;
 }
+
+uint8_t queue_receive(void *queue, void *message) {
+  if (xQueueReceive(queue, message, portMAX_DELAY) == pdPASS) {
+    return 1;
+  } else {
+    return 0;
+  }
+}
+
+uint8_t queue_send(void *queue, void *message) {
+  if (xQueueSend(queue, message, 0) != pdPASS) {
+    return 1;
+  } else {
+    return 0;
+  }
+}

mission/main.c

@@ -175,7 +175,7 @@ void mission(void *) {
 
   // printf("Starting Mission\n");
 
-  testIp();
+  //testIp();
 
   rust_main();
 

mission_rust/src/lib.rs

@@ -1,6 +1,7 @@
 #![no_std]
 
 //TODO look into using core::ffi (some types do not seem to work)
+//TODO os errors in API calls
 
 #[macro_export]
 macro_rules! sifln {
@@ -71,6 +72,9 @@ extern "C" {
         element_size: cty::size_t,
         buffer: *mut cty::c_void,
     ) -> *const cty::c_void;
+
+    fn queue_receive(queue: *const cty::c_void, message: *const cty::c_void) -> cty::uint8_t;
+    fn queue_send(queue: *const cty::c_void, message: *const cty::c_void) -> cty::uint8_t;
 }
 
 #[no_mangle]
@@ -83,6 +87,7 @@ extern "C" fn rust_main() {
 
 #[no_mangle]
 extern "C" fn task_entry(task_object: *mut cty::c_void) {
+    sifln!("Task Entry");
     let task: &mut PeriodicTaskRunner;
     unsafe {
         let pointer = task_object as *mut PeriodicTaskRunner;
@@ -107,6 +112,7 @@ struct PeriodicTask<'a, const STACKSIZE: usize> {
 
 impl<'a> PeriodicTaskRunner<'a> {
     fn execute(&mut self) {
+        sifln!("Task Runner {}", self.period);
         loop {
             self.task_object.perform();
             unsafe {
@@ -131,7 +137,6 @@ impl<'a, const STACKSIZE: usize> PeriodicTask<'a, STACKSIZE> {
         unsafe {
             instance.task = create_task(task_entry, runner_pointer, stack_pointer, STACKSIZE);
         }
-        let a: core::ffi::c_char;
         if instance.task == 0 as *mut cty::c_void {
             panic!("could not create Task");
         }
@@ -149,53 +154,139 @@ impl<'a, const STACKSIZE: usize> Drop for PeriodicTask<'a, STACKSIZE> {
 
 struct Handler {
     id: u32,
+    //command_queue: MessageQueue<Message, 10>,
+}
+
+struct HandlerSender {
+    id: u32,
+    cycle: u8,
+    other_handler: MessageQueueSender<Message>,
+}
+
+impl Handler {
+    fn handle_message(&self, message: Message) {
+        match message {
+            Message::OK=> {sifln!("OK");},
+            Message::FAILED => {sifln!("FAILED");},
+            Message::DATA(data) => {sifln!("p1: {}, p2 {}", data.p1, data.p2);}
+        }
+
+    }
 }
 
 impl ExecutableObjectIF for Handler {
     fn perform(&mut self) {
         sifln!("Handler {} performs", self.id);
+        // let result = self.command_queue.receive();
+        // match result {
+        //     Ok(message) => self.handle_message(message),
+        //     Err(_) => {sifln!("Handler {} got nothing", self.id);}
+        // }
+    }
+}
+
+impl ExecutableObjectIF for HandlerSender {
+    fn perform(&mut self) {
+        sifln!("HandlerSender {} performs step {}", self.id, self.cycle);
+        // match self.cycle {
+        //     0 => {let _ = self.other_handler.send(Message::OK);},
+        //     1 => {let _ = self.other_handler.send(Message::FAILED);},
+        //     2 => {let _ = self.other_handler.send(Message::DATA(GenericMessageData { p1: 1, p2: 2 }));},
+        //     _ => (),
+        // }
+        // self.cycle += 1;
     }
 }
 
 struct MessageQueue<T, const LENGTH: usize> {
-    queueId: *const cty::c_void,
+    queue_id: *const cty::c_void,
     buffer: [T; LENGTH],
 }
 
 struct MessageQueueSender<T> {
-    queueId: *const cty::c_void,
+    queue_id: Option<*const cty::c_void>,
+    _unused: Option<T> //need to constrain the sender to one message type for safety, but compiler needs that to be used
 }
 
 impl<T: Copy, const LENGTH: usize> MessageQueue<T, LENGTH> {
     fn new() -> Self {
+        let mut instance: Self;
         unsafe {
-        let mut instance = Self {
+            instance = Self {
-            queueId: 0 as *const cty::c_void,
+                queue_id: 0 as *const cty::c_void,
-            buffer: [core::mem::MaybeUninit::zeroed().assume_init();LENGTH]
+                buffer: [core::mem::MaybeUninit::zeroed().assume_init(); LENGTH], //Gets passed to C/FreeRTOS, so we never ever touch it ever again
             };
             let buffer_pointer: *mut cty::c_void =
                 &mut instance.buffer as *mut _ as *mut cty::c_void;
-            instance.queueId = create_queue(LENGTH, core::mem::size_of::<T>(), buffer_pointer);
+            instance.queue_id = create_queue(LENGTH, core::mem::size_of::<T>(), buffer_pointer);
 
-        if instance.queueId == 0 as *mut cty::c_void {
+            if instance.queue_id == 0 as *mut cty::c_void {
                 panic!("could not create Queue");
             }
             instance
         }
     }
 
-    fn getSender(self) -> MessageQueueSender<T> {
+    fn get_sender(&self) -> MessageQueueSender<T> {
-        let  instance:MessageQueueSender<T>  = MessageQueueSender::<T>{queueId: self.queueId};
+        let instance: MessageQueueSender<T> = MessageQueueSender::<T> {
+            queue_id: Some(self.queue_id),
+            _unused: None
+        };
         instance
     }
+
+    fn receive(&self) -> Result<T, ()> {
+        let mut message: T;
+        let res: cty::uint8_t;
+        unsafe {
+            message = core::mem::MaybeUninit::zeroed().assume_init(); // We only return it if the queue received something
+            let message_pointer: *mut cty::c_void = &mut message as *mut _ as *mut cty::c_void;
+            res = queue_receive(self.queue_id, message_pointer);
+        }
+        if res == 1 {
+            Ok(message)
+        } else {
+            Err(())
+        }
+    }
 }
 
 impl<T> MessageQueueSender<T> {
-    fn send(message: T) {
+    fn new() -> Self {
-
+        Self {
+            queue_id: None,
+            _unused: None
         }
     }
 
+    fn send(&self, message: T) -> Result<(), ()> {
+        let queue_id = self.queue_id.expect("unitialized Message Queue");
+        let res: cty::uint8_t;
+        unsafe {
+            let message_pointer: *const cty::c_void = &message as *const _ as *const cty::c_void;
+            res = queue_send(queue_id, message_pointer);
+        }
+        if res == 1 {
+            Ok(())
+        } else {
+            Err(())
+        }
+    }
+}
+
+#[derive(Clone, Copy)]
+struct GenericMessageData {
+    p1: u32,
+    p2: u32,
+}
+
+#[derive(Copy, Clone)]
+enum Message {
+    OK,
+    FAILED,
+    DATA(GenericMessageData),
+}
+
 struct Outbytes {}
 
 use core::fmt::{Error, Write};
@@ -211,22 +302,29 @@ impl Write for Outbytes {
     }
 }
 
-fn panics(a: &mut [i32]) {
-    a[4] = 3;
-}
 
 fn mission() {
     sifln!("Mission enter");
 
-    let mut h1 = Handler { id: 1 };
+    let mut h1 = Handler {
-    let mut h2 = Handler { id: 2 };
+        id: 1,
+        //command_queue: MessageQueue::new(),
+    };
+    // let mut h2 = HandlerSender {
+    //     id: 2,
+    //     cycle: 0,
+    //     other_handler: MessageQueueSender::<Message>::new(),
+    // };
+
+    // h2.other_handler = h1.command_queue.get_sender();
 
     let _t1: PeriodicTask<'_, 512> = PeriodicTask::new(&mut h1);
 
-    let _t2: PeriodicTask<'_, 512> = PeriodicTask::new(&mut h2);
+    //let _t2: PeriodicTask<'_, 512> = PeriodicTask::new(&mut h2);
 
-    sifln!("Mission done delay");
+    sifln!("Mission delay");
     unsafe {
         task_delay(3000);
     }
+    sifln!("Mission delay done");
 }