1use core::cell::UnsafeCell;
47use core::ffi::c_long;
48use core::fmt::{Debug, Display};
49use core::marker::PhantomData;
50use core::ops::Deref;
51use core::time::Duration;
52
53use crate::os::types::ClockMonotonicHandle;
54use crate::posix::config::TICK_PERIOD_MS;
55use crate::posix::ffi::{
56 CLOCK_MONOTONIC, ETIMEDOUT, PTHREAD_PRIO_INHERIT, clock_gettime, pthread_cond_broadcast, pthread_cond_destroy, pthread_cond_init, pthread_cond_t, pthread_cond_timedwait, pthread_cond_wait,
57 pthread_condattr_init, pthread_condattr_setclock, pthread_condattr_t, pthread_mutex_destroy, pthread_mutex_init, pthread_mutex_lock, pthread_mutex_t, pthread_mutex_trylock, pthread_mutex_unlock,
58 pthread_mutexattr_init, pthread_mutexattr_setprotocol, pthread_mutexattr_t, timespec,
59};
60#[cfg(not(feature = "serde"))]
61use crate::traits::{Deserialize, Serialize};
62use crate::traits::{BytesHasLen, QueueFn, QueueStreamedFn, ToTick};
63use crate::utils::{Error, Result};
64use crate::posix::types::{QueueHandle, TickType, UBaseType};
65
66#[cfg(feature = "serde")]
67use osal_rs_serde::{Deserialize, Serialize, from_bytes, to_bytes};
68
69#[cfg_attr(not(feature = "serde"), doc = "```")]
80#[cfg_attr(feature = "serde", doc = "```ignore")]
81pub trait StructSerde: Serialize + BytesHasLen + Deserialize {}
111
112impl<T> StructSerde for T where T: Serialize + BytesHasLen + Deserialize {}
113
114fn monotonic_deadline(timeout: Duration) -> timespec {
119 let mut now = timespec::default();
120 unsafe {
121 clock_gettime(CLOCK_MONOTONIC, &mut now);
122 }
123
124 let mut tv_sec = now.tv_sec + timeout.as_secs() as c_long;
125 let mut tv_nsec = now.tv_nsec + timeout.subsec_nanos() as c_long;
126
127 if tv_nsec >= 1_000_000_000 {
128 tv_sec += 1;
129 tv_nsec -= 1_000_000_000;
130 }
131
132 timespec { tv_sec, tv_nsec }
133}
134
135pub struct Queue{
139 handle: UnsafeCell<QueueHandle>,
140 r: UnsafeCell<usize>,
141 w: UnsafeCell<usize>,
142 count: UnsafeCell<usize>,
143 size: usize,
144 message_size: usize,
145 msg: UnsafeCell<Vec<u8>>
146
147}
148
149unsafe impl Send for Queue {}
150unsafe impl Sync for Queue {}
151
152impl Queue {
153 pub fn new(size: UBaseType, message_size: UBaseType) -> Result<Self> {
165 if size == 0 || message_size == 0 {
166 return Err(Error::InvalidQueueSize)
167 }
168
169 let size = size as usize;
170 let message_size = message_size as usize;
171
172 let mut mutex: pthread_mutex_t = Default::default();
173 let mut mutex_attr: pthread_mutexattr_t = Default::default();
174 let mut cond: pthread_cond_t = Default::default();
175 let mut cond_attr: pthread_condattr_t = Default::default();
176
177
178 unsafe {
179 pthread_condattr_init(&mut cond_attr);
182 pthread_condattr_setclock (&mut cond_attr, CLOCK_MONOTONIC);
183 pthread_cond_init (&mut cond, &cond_attr);
184 pthread_mutexattr_init (&mut mutex_attr);
188 pthread_mutexattr_setprotocol (&mut mutex_attr, PTHREAD_PRIO_INHERIT);
189 pthread_mutex_init (&mut mutex, &mutex_attr);
190 }
191
192 Ok(Self {
193 handle: UnsafeCell::new(ClockMonotonicHandle(mutex, cond)),
194 r: UnsafeCell::new(0),
195 w: UnsafeCell::new(0),
196 count: UnsafeCell::new(0),
197 size,
198 message_size,
199 msg: UnsafeCell::new(vec![0u8; size * message_size])
200 })
201 }
202
203 #[inline]
220 pub fn fetch_with_to_tick(&self, buffer: &mut [u8], time: impl ToTick) -> Result<()> {
221 self.fetch(buffer, time.to_ticks())
222 }
223
224 #[inline]
241 pub fn post_with_to_tick(&self, item: &[u8], time: impl ToTick) -> Result<()> {
242 self.post(item, time.to_ticks())
243 }
244
245 fn mutex_ptr(&self) -> *mut pthread_mutex_t {
249 unsafe { &raw mut (*self.handle.get()).0 }
250 }
251
252 fn cond_ptr(&self) -> *mut pthread_cond_t {
253 unsafe { &raw mut (*self.handle.get()).1 }
254 }
255}
256
257impl QueueFn for Queue {
258 fn is_null(&self) -> bool {
272 unsafe { (*self.handle.get()).is_empty() }
273 }
274
275 fn fetch(&self, buffer: &mut [u8], time: TickType) -> Result<()> {
297 if self.is_null() {
298 return Err(Error::NullPtr);
299 }
300
301 if buffer.len() < self.message_size {
302 return Err(Error::InvalidQueueSize);
303 }
304
305 unsafe {
306 pthread_mutex_lock(self.mutex_ptr());
307 }
308
309 let received = if time == TickType::MAX {
312 loop {
313 if unsafe { *self.count.get() } > 0 {
314 break true;
315 }
316 unsafe {
317 pthread_cond_wait(self.cond_ptr(), self.mutex_ptr());
318 }
319 }
320 } else {
321 let deadline = monotonic_deadline(Duration::from_millis((time as u64).saturating_mul(TICK_PERIOD_MS)));
322
323 loop {
324 if unsafe { *self.count.get() } > 0 {
325 break true;
326 }
327 if unsafe { pthread_cond_timedwait(self.cond_ptr(), self.mutex_ptr(), &deadline) } == ETIMEDOUT {
328 break false;
329 }
330 }
331 };
332
333 if received {
334 unsafe {
335 let r = *self.r.get();
336 let offset = r * self.message_size;
337 let msg = &*self.msg.get();
338 buffer[..self.message_size].copy_from_slice(&msg[offset..offset + self.message_size]);
339
340 *self.r.get() = (r + 1) % self.size;
341 *self.count.get() -= 1;
342
343 pthread_cond_broadcast(self.cond_ptr());
346 }
347 }
348
349 unsafe {
350 pthread_mutex_unlock(self.mutex_ptr());
351 }
352
353 if received { Ok(()) } else { Err(Error::Timeout) }
354 }
355
356 fn fetch_from_isr(&self, buffer: &mut [u8]) -> Result<()> {
374 if self.is_null() {
375 return Err(Error::NullPtr);
376 }
377
378 if buffer.len() < self.message_size {
379 return Err(Error::InvalidQueueSize);
380 }
381
382 if unsafe { pthread_mutex_trylock(self.mutex_ptr()) } != 0 {
386 return Err(Error::QueueFull);
387 }
388
389 let received = unsafe { *self.count.get() } > 0;
390
391 if received {
392 unsafe {
393 let r = *self.r.get();
394 let offset = r * self.message_size;
395 let msg = &*self.msg.get();
396 buffer[..self.message_size].copy_from_slice(&msg[offset..offset + self.message_size]);
397
398 *self.r.get() = (r + 1) % self.size;
399 *self.count.get() -= 1;
400
401 pthread_cond_broadcast(self.cond_ptr());
402 }
403 }
404
405 unsafe {
406 pthread_mutex_unlock(self.mutex_ptr());
407 }
408
409 if received { Ok(()) } else { Err(Error::Timeout) }
410 }
411
412 fn post(&self, item: &[u8], time: TickType) -> Result<()> {
430 if self.is_null() {
431 return Err(Error::NullPtr);
432 }
433
434 if item.len() < self.message_size {
435 return Err(Error::InvalidQueueSize);
436 }
437
438 unsafe {
439 pthread_mutex_lock(self.mutex_ptr());
440 }
441
442 let sent = if time == TickType::MAX {
443 loop {
444 if unsafe { *self.count.get() } < self.size {
445 break true;
446 }
447 unsafe {
448 pthread_cond_wait(self.cond_ptr(), self.mutex_ptr());
449 }
450 }
451 } else {
452 let deadline = monotonic_deadline(Duration::from_millis((time as u64).saturating_mul(TICK_PERIOD_MS)));
453
454 loop {
455 if unsafe { *self.count.get() } < self.size {
456 break true;
457 }
458 if unsafe { pthread_cond_timedwait(self.cond_ptr(), self.mutex_ptr(), &deadline) } == ETIMEDOUT {
459 break false;
460 }
461 }
462 };
463
464 if sent {
465 unsafe {
466 let w = *self.w.get();
467 let offset = w * self.message_size;
468 let msg = &mut *self.msg.get();
469 msg[offset..offset + self.message_size].copy_from_slice(&item[..self.message_size]);
470
471 *self.w.get() = (w + 1) % self.size;
472 *self.count.get() += 1;
473
474 pthread_cond_broadcast(self.cond_ptr());
477 }
478 }
479
480 unsafe {
481 pthread_mutex_unlock(self.mutex_ptr());
482 }
483
484 if sent { Ok(()) } else { Err(Error::Timeout) }
485 }
486
487 fn post_from_isr(&self, item: &[u8]) -> Result<()> {
505 if self.is_null() {
506 return Err(Error::NullPtr);
507 }
508
509 if item.len() < self.message_size {
510 return Err(Error::InvalidQueueSize);
511 }
512
513 if unsafe { pthread_mutex_trylock(self.mutex_ptr()) } != 0 {
514 return Err(Error::QueueFull);
515 }
516
517 let sent = unsafe { *self.count.get() } < self.size;
518
519 if sent {
520 unsafe {
521 let w = *self.w.get();
522 let offset = w * self.message_size;
523 let msg = &mut *self.msg.get();
524 msg[offset..offset + self.message_size].copy_from_slice(&item[..self.message_size]);
525
526 *self.w.get() = (w + 1) % self.size;
527 *self.count.get() += 1;
528
529 pthread_cond_broadcast(self.cond_ptr());
530 }
531 }
532
533 unsafe {
534 pthread_mutex_unlock(self.mutex_ptr());
535 }
536
537 if sent { Ok(()) } else { Err(Error::QueueFull) }
538 }
539
540 fn delete(&mut self) {
554 if self.is_null() {
555 return;
556 }
557
558 unsafe {
559 pthread_mutex_destroy(self.mutex_ptr());
560 pthread_cond_destroy(self.cond_ptr());
561 }
562
563 *self.handle.get_mut() = QueueHandle::default();
567 *self.r.get_mut() = 0;
568 *self.w.get_mut() = 0;
569 *self.count.get_mut() = 0;
570 self.msg.get_mut().clear();
571 }
572}
573
574impl Drop for Queue {
575 fn drop(&mut self) {
576 if self.is_null() {
577 return;
578 }
579 self.delete();
581 }
582}
583
584impl Deref for Queue {
585 type Target = QueueHandle;
586
587 fn deref(&self) -> &Self::Target {
588 unsafe { &*self.handle.get() }
590 }
591}
592
593impl Debug for Queue {
594 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
595 f.debug_struct("Queue")
596 .field("handle", unsafe { &*self.handle.get() })
597 .field("count", unsafe { &*self.count.get() })
598 .field("size", &self.size)
599 .field("message_size", &self.message_size)
600 .finish()
601 }
602}
603
604impl Display for Queue {
605 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
606 write!(
607 f,
608 "Queue {{ handle: {:?}, count: {}, size: {}, message_size: {} }}",
609 unsafe { &*self.handle.get() },
610 unsafe { *self.count.get() },
611 self.size,
612 self.message_size
613 )
614 }
615}
616
617#[cfg_attr(not(feature = "serde"), doc = "```")]
627#[cfg_attr(feature = "serde", doc = "```ignore")]
628pub struct QueueStreamed<T: StructSerde>(Queue, PhantomData<T>);
661
662unsafe impl<T: StructSerde> Send for QueueStreamed<T> {}
663unsafe impl<T: StructSerde> Sync for QueueStreamed<T> {}
664
665impl<T> QueueStreamed<T>
666where
667 T: StructSerde,
668{
669 #[inline]
675 pub fn new(size: UBaseType, message_size: UBaseType) -> Result<Self> {
676 Ok(Self(Queue::new(size, message_size)?, PhantomData))
677 }
678
679 #[allow(dead_code)]
680 #[inline]
681 fn fetch_with_to_tick(&self, buffer: &mut T, time: impl ToTick) -> Result<()> {
682 self.fetch(buffer, time.to_ticks())
683 }
684
685 #[allow(dead_code)]
686 #[inline]
687 fn post_with_to_tick(&self, item: &T, time: impl ToTick) -> Result<()> {
688 self.post(item, time.to_ticks())
689 }
690}
691
692#[cfg(not(feature = "serde"))]
693impl<T> QueueStreamedFn<T> for QueueStreamed<T>
694where
695 T: StructSerde,
696{
697 fn fetch(&self, buffer: &mut T, time: TickType) -> Result<()> {
701 let mut buf_bytes = vec![0u8; buffer.len()];
702
703 self.0.fetch(&mut buf_bytes, time)?;
704 *buffer = T::from_bytes(&buf_bytes)?;
705
706 Ok(())
707 }
708
709 fn fetch_from_isr(&self, buffer: &mut T) -> Result<()> {
711 let mut buf_bytes = vec![0u8; buffer.len()];
712
713 self.0.fetch_from_isr(&mut buf_bytes)?;
714 *buffer = T::from_bytes(&buf_bytes)?;
715
716 Ok(())
717 }
718
719 #[inline]
723 fn post(&self, item: &T, time: TickType) -> Result<()> {
724 self.0.post(&item.to_bytes(), time)
725 }
726
727 #[inline]
729 fn post_from_isr(&self, item: &T) -> Result<()> {
730 self.0.post_from_isr(&item.to_bytes())
731 }
732
733 #[inline]
735 fn delete(&mut self) {
736 self.0.delete()
737 }
738}
739
740#[cfg(feature = "serde")]
741impl<T> QueueStreamedFn<T> for QueueStreamed<T>
742where
743 T: StructSerde,
744{
745 fn fetch(&self, buffer: &mut T, time: TickType) -> Result<()> {
748 let mut buf_bytes = vec![0u8; buffer.len()];
749
750 self.0.fetch(&mut buf_bytes, time)?;
751 *buffer = from_bytes(&buf_bytes).map_err(|_| Error::Unhandled("Deserializiation error"))?;
752
753 Ok(())
754 }
755
756 fn fetch_from_isr(&self, buffer: &mut T) -> Result<()> {
758 let mut buf_bytes = vec![0u8; buffer.len()];
759
760 self.0.fetch_from_isr(&mut buf_bytes)?;
761 *buffer = from_bytes(&buf_bytes).map_err(|_| Error::Unhandled("Deserializiation error"))?;
762
763 Ok(())
764 }
765
766 fn post(&self, item: &T, time: TickType) -> Result<()> {
768 let mut buf_bytes = vec![0u8; item.len()];
769
770 to_bytes(item, &mut buf_bytes).map_err(|_| Error::Unhandled("Serialization error"))?;
771
772 self.0.post(&buf_bytes, time)
773 }
774
775 fn post_from_isr(&self, item: &T) -> Result<()> {
777 let mut buf_bytes = vec![0u8; item.len()];
778
779 to_bytes(item, &mut buf_bytes).map_err(|_| Error::Unhandled("Serialization error"))?;
780
781 self.0.post_from_isr(&buf_bytes)
782 }
783
784 #[inline]
786 fn delete(&mut self) {
787 self.0.delete()
788 }
789}
790
791impl<T> Deref for QueueStreamed<T>
792where
793 T: StructSerde,
794{
795 type Target = QueueHandle;
796
797 fn deref(&self) -> &Self::Target {
798 unsafe { &*self.0.handle.get() }
799 }
800}
801
802impl<T> Debug for QueueStreamed<T>
803where
804 T: StructSerde,
805{
806 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
807 f.debug_struct("QueueStreamed")
808 .field("handle", unsafe { &*self.0.handle.get() })
809 .finish()
810 }
811}
812
813impl<T> Display for QueueStreamed<T>
814where
815 T: StructSerde,
816{
817 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
818 write!(f, "QueueStreamed {{ handle: {:?} }}", unsafe { &*self.0.handle.get() })
819 }
820}