1use rusty_rtos_core::config::Config;
22use rusty_rtos_core::error::{Error, Result};
23use rusty_rtos_core::handle::{StreamBufferHandle, TaskHandle};
24use rusty_rtos_core::hooks::TickHook;
25use rusty_rtos_core::isr::Woken;
26use rusty_rtos_core::port::Port;
27use rusty_rtos_core::trace::{Event, Trace};
28
29use crate::kernel::{Kernel, NotifyAction};
30use crate::queue::{Blocked, Ready, Wait};
31
32#[derive(Debug, Clone, Copy, Default)]
34pub(crate) struct StreamBuffer {
35 pub(crate) base: usize,
37 pub(crate) length: usize,
41 pub(crate) trigger: usize,
43 pub(crate) head: usize,
45 pub(crate) tail: usize,
47 pub(crate) waiting_to_receive: TaskHandle,
49 pub(crate) waiting_to_send: TaskHandle,
51 pub(crate) is_message: bool,
53 pub(crate) notify_index: usize,
55}
56
57impl StreamBuffer {
58 pub(crate) const fn bytes_in_buffer(&self) -> usize {
65 let mut count = self
66 .length
67 .saturating_add(self.head)
68 .saturating_sub(self.tail);
69 if count >= self.length {
70 count = count.saturating_sub(self.length);
71 }
72 count
73 }
74
75 pub(crate) const fn spaces_available(&self) -> usize {
78 let mut space = self
79 .length
80 .saturating_add(self.tail)
81 .saturating_sub(self.head)
82 .saturating_sub(1);
83 if space >= self.length {
84 space = space.saturating_sub(self.length);
85 }
86 space
87 }
88
89 pub(crate) const fn meets_trigger(&self, bytes: usize) -> bool {
92 bytes >= self.trigger
93 }
94}
95
96impl<
97 C: Config,
98 P: Port,
99 T: Trace,
100 H,
101 const TASKS: usize,
102 const ITEMS: usize,
103 const LISTS: usize,
104 const QUEUES: usize,
105 const SLOTS: usize,
106 const BUFFERS: usize,
107 const BYTES: usize,
108 const TIMERS: usize,
109 const GROUPS: usize,
110> Kernel<C, P, T, H, TASKS, ITEMS, LISTS, QUEUES, SLOTS, BUFFERS, BYTES, TIMERS, GROUPS>
111where
112 H: TickHook<Self>,
113{
114 pub const MESSAGE_LENGTH_BYTES: usize = C::MESSAGE_LENGTH_BYTES;
116
117 pub fn stream_buffer_create(
126 &mut self,
127 size: usize,
128 trigger: usize,
129 ) -> Result<StreamBufferHandle> {
130 self.new_stream_buffer(size, trigger, false)
131 }
132
133 pub fn message_buffer_create(&mut self, size: usize) -> Result<StreamBufferHandle> {
139 if size <= Self::MESSAGE_LENGTH_BYTES {
140 return Err(Error::InvalidArgument);
141 }
142 self.new_stream_buffer(size, 1, true)
143 }
144
145 fn new_stream_buffer(
146 &mut self,
147 size: usize,
148 trigger: usize,
149 is_message: bool,
150 ) -> Result<StreamBufferHandle> {
151 if size == 0 || trigger > size {
152 return Err(Error::InvalidArgument);
153 }
154 let trigger = if trigger == 0 { 1 } else { trigger };
155 let length = size.saturating_add(1);
157 let base = self.take_bytes(length).ok_or(Error::Full)?;
158 let handle = self
159 .buffers
160 .try_insert(StreamBuffer {
161 base,
162 length,
163 trigger,
164 head: 0,
165 tail: 0,
166 waiting_to_receive: TaskHandle::NULL,
167 waiting_to_send: TaskHandle::NULL,
168 is_message,
169 notify_index: 0,
170 })
171 .map_err(|_| Error::Full)?;
172 self.account_for_allocation();
173 let tick = self.tick;
174 self.trace.note_exits(self.port.exits());
175 self.trace.event(
176 tick,
177 Event::StreamBufferCreate {
178 buffer: handle,
179 is_message_buffer: is_message,
180 },
181 );
182 Ok(handle)
183 }
184
185 fn take_bytes(&mut self, length: usize) -> Option<usize> {
192 for i in 0..self.free_count {
193 let (base, free) = *self.free_blocks.get(i)?;
194 if free < length {
195 continue;
196 }
197 if free == length {
198 self.drop_free_block(i);
199 } else if let Some(slot) = self.free_blocks.get_mut(i) {
200 *slot = (base.saturating_add(length), free.saturating_sub(length));
201 }
202 return Some(base);
203 }
204 let end = self.bytes_used.checked_add(length)?;
205 if end > BYTES {
206 return None;
207 }
208 let base = self.bytes_used;
209 self.bytes_used = end;
210 Some(base)
211 }
212
213 fn give_bytes(&mut self, base: usize, length: usize) {
216 let mut base = base;
217 let mut length = length;
218 let mut i = 0;
219 while i < self.free_count {
220 let Some(&(other_base, other_len)) = self.free_blocks.get(i) else {
221 break;
222 };
223 if other_base.saturating_add(other_len) == base {
224 base = other_base;
225 length = length.saturating_add(other_len);
226 self.drop_free_block(i);
227 continue;
228 }
229 if base.saturating_add(length) == other_base {
230 length = length.saturating_add(other_len);
231 self.drop_free_block(i);
232 continue;
233 }
234 i = i.saturating_add(1);
235 }
236 if base.saturating_add(length) == self.bytes_used {
239 self.bytes_used = base;
240 return;
241 }
242 if let Some(slot) = self.free_blocks.get_mut(self.free_count) {
243 *slot = (base, length);
244 self.free_count = self.free_count.saturating_add(1);
245 }
246 }
247
248 fn drop_free_block(&mut self, index: usize) {
249 let last = self.free_count.saturating_sub(1);
250 if index < last {
251 if let Some(&moved) = self.free_blocks.get(last) {
252 if let Some(slot) = self.free_blocks.get_mut(index) {
253 *slot = moved;
254 }
255 }
256 }
257 self.free_count = last;
258 }
259
260 pub fn stream_buffer_delete(&mut self, buffer: StreamBufferHandle) -> Result<()> {
266 let b = *self.buffers.resolve(buffer)?;
267 let _ = self.buffers.remove(buffer);
268 self.give_bytes(b.base, b.length);
269 Ok(())
270 }
271
272 pub fn stream_buffer_bytes_available(&self, buffer: StreamBufferHandle) -> Result<usize> {
277 Ok(self.buffers.resolve(buffer)?.bytes_in_buffer())
278 }
279
280 pub fn stream_buffer_spaces_available(&self, buffer: StreamBufferHandle) -> Result<usize> {
285 Ok(self.buffers.resolve(buffer)?.spaces_available())
286 }
287
288 pub fn stream_buffer_is_empty(&self, buffer: StreamBufferHandle) -> Result<bool> {
293 let b = self.buffers.resolve(buffer)?;
294 Ok(b.head == b.tail)
295 }
296
297 pub fn stream_buffer_is_full(&self, buffer: StreamBufferHandle) -> Result<bool> {
306 let b = self.buffers.resolve(buffer)?;
307 let prefix = if b.is_message {
308 Self::MESSAGE_LENGTH_BYTES
309 } else {
310 0
311 };
312 Ok(b.spaces_available() <= prefix)
313 }
314
315 pub fn stream_buffer_set_trigger_level(
320 &mut self,
321 buffer: StreamBufferHandle,
322 trigger: usize,
323 ) -> Result<bool> {
324 let length = self.buffers.resolve(buffer)?.length;
325 if trigger >= length {
326 return Ok(false);
327 }
328 let trigger = if trigger == 0 { 1 } else { trigger };
329 self.buffers.resolve_mut(buffer)?.trigger = trigger;
330 Ok(true)
331 }
332
333 pub fn stream_buffer_reset(&mut self, buffer: StreamBufferHandle) -> Result<bool> {
338 self.enter_critical();
339 let done = match self.buffers.resolve_mut(buffer) {
340 Ok(b)
341 if b.waiting_to_receive == TaskHandle::NULL
342 && b.waiting_to_send == TaskHandle::NULL =>
343 {
344 b.head = 0;
345 b.tail = 0;
346 true
347 }
348 _ => false,
349 };
350 self.exit_critical();
351 Ok(done)
352 }
353
354 pub fn stream_buffer_next_message_length(&self, buffer: StreamBufferHandle) -> Result<usize> {
359 let b = *self.buffers.resolve(buffer)?;
360 if !b.is_message {
361 return Ok(0);
362 }
363 let available = b.bytes_in_buffer();
364 if available <= Self::MESSAGE_LENGTH_BYTES {
365 return Ok(0);
366 }
367 Ok(self.read_length_prefix(&b, b.tail).0)
368 }
369
370 pub fn stream_buffer_send(
381 &mut self,
382 buffer: StreamBufferHandle,
383 data: &[u8],
384 ticks: u64,
385 ) -> Result<Wait<usize>> {
386 let caller = self.current;
387 let snapshot = *self.buffers.resolve(buffer)?;
388 let max_reported = snapshot.length.saturating_sub(1);
389 let mut required = data.len();
390 let mut ticks = ticks;
391 if snapshot.is_message {
392 required = required.saturating_add(Self::MESSAGE_LENGTH_BYTES);
393 if required > max_reported {
394 ticks = 0;
396 }
397 } else if required > max_reported {
398 required = max_reported;
399 }
400
401 let mut sampled = self.take_stream_resume(caller);
402 if sampled.is_some() {
403 } else if self.notify_wait_pending(caller) {
406 match self.notify_wait(snapshot.notify_index, 0, 0, ticks)? {
407 Blocked => return Ok(Blocked),
408 Ready(_) => {}
409 }
410 self.buffers.resolve_mut(buffer)?.waiting_to_send = TaskHandle::NULL;
411 } else if ticks > 0 {
412 self.enter_critical();
413 let space = self.buffers.resolve(buffer)?.spaces_available();
414 let must_block = space < required;
415 if must_block {
416 let _ = self.notify_state_clear(None, snapshot.notify_index);
417 self.buffers.resolve_mut(buffer)?.waiting_to_send = caller;
418 }
419 self.exit_critical();
420 if self.current != caller {
425 self.set_stream_resume(caller, space);
426 return Ok(Blocked);
427 }
428 if must_block {
429 match self.notify_wait(snapshot.notify_index, 0, 0, ticks)? {
432 Blocked => return Ok(Blocked),
433 Ready(_) => {
434 self.buffers.resolve_mut(buffer)?.waiting_to_send = TaskHandle::NULL;
435 }
436 }
437 }
438 }
439
440 let space = match sampled.take() {
441 Some(space) => space,
442 None => self.buffers.resolve(buffer)?.spaces_available(),
443 };
444 let written = self.write_message(buffer, data, space, required)?;
445 if written > 0 {
446 let tick = self.tick;
447 self.trace.note_exits(self.port.exits());
448 self.trace.event(
449 tick,
450 Event::StreamBufferSend {
451 buffer,
452 bytes: written,
453 },
454 );
455 let bytes = self.buffers.resolve(buffer)?.bytes_in_buffer();
456 if self.buffers.resolve(buffer)?.meets_trigger(bytes) {
457 self.send_completed(buffer)?;
458 }
459 }
460 Ok(Ready(written))
461 }
462
463 pub fn stream_buffer_send_from_isr(
468 &mut self,
469 buffer: StreamBufferHandle,
470 data: &[u8],
471 ) -> Result<(usize, Woken)> {
472 let snapshot = *self.buffers.resolve(buffer)?;
473 let max_reported = snapshot.length.saturating_sub(1);
474 let mut required = data.len();
475 if snapshot.is_message {
476 required = required.saturating_add(Self::MESSAGE_LENGTH_BYTES);
477 } else if required > max_reported {
478 required = max_reported;
479 }
480 let space = snapshot.spaces_available();
481 let written = self.write_message(buffer, data, space, required)?;
482 let mut woken = Woken::NO;
483 if written > 0 {
484 let bytes = self.buffers.resolve(buffer)?.bytes_in_buffer();
488 if self.buffers.resolve(buffer)?.meets_trigger(bytes) {
489 woken = self.send_completed_from_isr(buffer)?;
490 }
491 }
492 Ok((written, woken))
493 }
494
495 pub fn stream_buffer_receive(
502 &mut self,
503 buffer: StreamBufferHandle,
504 out: &mut [u8],
505 ticks: u64,
506 ) -> Result<Wait<usize>> {
507 let caller = self.current;
508 let snapshot = *self.buffers.resolve(buffer)?;
509 let prefix = if snapshot.is_message {
510 Self::MESSAGE_LENGTH_BYTES
511 } else {
512 0
513 };
514 let mut available;
515 if let Some(local) = self.take_stream_resume(caller) {
516 available = local;
518 } else if self.notify_wait_pending(caller) {
519 match self.notify_wait(snapshot.notify_index, 0, 0, ticks)? {
522 Blocked => return Ok(Blocked),
523 Ready(_) => {}
524 }
525 self.buffers.resolve_mut(buffer)?.waiting_to_receive = TaskHandle::NULL;
526 available = self.buffers.resolve(buffer)?.bytes_in_buffer();
527 } else if ticks > 0 {
528 self.enter_critical();
529 available = self.buffers.resolve(buffer)?.bytes_in_buffer();
530 let must_block = available <= prefix;
531 if must_block {
532 let _ = self.notify_state_clear(None, snapshot.notify_index);
533 self.buffers.resolve_mut(buffer)?.waiting_to_receive = caller;
534 }
535 self.exit_critical();
536 if self.current != caller {
541 self.set_stream_resume(caller, available);
542 return Ok(Blocked);
543 }
544 if must_block {
545 match self.notify_wait(snapshot.notify_index, 0, 0, ticks)? {
546 Blocked => return Ok(Blocked),
547 Ready(_) => {
548 self.buffers.resolve_mut(buffer)?.waiting_to_receive = TaskHandle::NULL;
549 available = self.buffers.resolve(buffer)?.bytes_in_buffer();
550 }
551 }
552 }
553 } else {
554 available = self.buffers.resolve(buffer)?.bytes_in_buffer();
555 }
556
557 let mut received = 0;
558 if available > prefix {
559 received = self.read_message(buffer, out, available)?;
560 if received != 0 {
561 let tick = self.tick;
562 self.trace.note_exits(self.port.exits());
563 self.trace.event(
564 tick,
565 Event::StreamBufferReceive {
566 buffer,
567 bytes: received,
568 },
569 );
570 self.receive_completed(buffer)?;
571 }
572 }
573 Ok(Ready(received))
574 }
575
576 pub fn stream_buffer_receive_from_isr(
581 &mut self,
582 buffer: StreamBufferHandle,
583 out: &mut [u8],
584 ) -> Result<(usize, Woken)> {
585 let snapshot = *self.buffers.resolve(buffer)?;
586 let prefix = if snapshot.is_message {
587 Self::MESSAGE_LENGTH_BYTES
588 } else {
589 0
590 };
591 let available = snapshot.bytes_in_buffer();
592 let mut received = 0;
593 let mut woken = Woken::NO;
594 if available > prefix {
595 received = self.read_message(buffer, out, available)?;
596 if received != 0 {
597 woken = self.receive_completed_from_isr(buffer)?;
599 }
600 }
601 Ok((received, woken))
602 }
603
604 fn send_completed(&mut self, buffer: StreamBufferHandle) -> Result<()> {
613 if H::send_completed(self, buffer) {
614 return Ok(());
615 }
616 self.suspend_all();
617 let waiting = self.buffers.resolve(buffer)?.waiting_to_receive;
618 if waiting != TaskHandle::NULL {
619 let index = self.buffers.resolve(buffer)?.notify_index;
620 let _ = self.notify(waiting, index, 0, NotifyAction::None)?;
621 self.buffers.resolve_mut(buffer)?.waiting_to_receive = TaskHandle::NULL;
622 }
623 let _ = self.resume_all();
624 Ok(())
625 }
626
627 pub fn send_completed_from_isr(&mut self, buffer: StreamBufferHandle) -> Result<Woken> {
637 let waiting = self.buffers.resolve(buffer)?.waiting_to_receive;
638 if waiting == TaskHandle::NULL {
639 return Ok(Woken::NO);
640 }
641 let index = self.buffers.resolve(buffer)?.notify_index;
642 let (_, woken) = self.notify_from_isr(waiting, index, 0, NotifyAction::None)?;
643 self.buffers.resolve_mut(buffer)?.waiting_to_receive = TaskHandle::NULL;
644 Ok(woken)
645 }
646
647 fn receive_completed(&mut self, buffer: StreamBufferHandle) -> Result<()> {
649 self.suspend_all();
650 let waiting = self.buffers.resolve(buffer)?.waiting_to_send;
651 if waiting != TaskHandle::NULL {
652 let index = self.buffers.resolve(buffer)?.notify_index;
653 let _ = self.notify(waiting, index, 0, NotifyAction::None)?;
654 self.buffers.resolve_mut(buffer)?.waiting_to_send = TaskHandle::NULL;
655 }
656 let _ = self.resume_all();
657 Ok(())
658 }
659
660 fn receive_completed_from_isr(&mut self, buffer: StreamBufferHandle) -> Result<Woken> {
662 let waiting = self.buffers.resolve(buffer)?.waiting_to_send;
663 if waiting == TaskHandle::NULL {
664 return Ok(Woken::NO);
665 }
666 let index = self.buffers.resolve(buffer)?.notify_index;
667 let (_, woken) = self.notify_from_isr(waiting, index, 0, NotifyAction::None)?;
668 self.buffers.resolve_mut(buffer)?.waiting_to_send = TaskHandle::NULL;
669 Ok(woken)
670 }
671
672 fn write_message(
676 &mut self,
677 buffer: StreamBufferHandle,
678 data: &[u8],
679 space: usize,
680 required: usize,
681 ) -> Result<usize> {
682 let b = *self.buffers.resolve(buffer)?;
683 let mut next_head = b.head;
684 let mut length = data.len();
685 let mut space = space;
686 if b.is_message {
687 if space >= required {
688 next_head = self.write_length_prefix(&b, length, next_head);
689 } else {
690 length = 0;
691 }
692 space = space.saturating_sub(Self::MESSAGE_LENGTH_BYTES);
693 }
694 let length = length.min(space);
695 if length != 0 {
696 let chunk = data.get(..length).unwrap_or(data);
697 let head = self.write_bytes(&b, chunk, next_head);
698 self.buffers.resolve_mut(buffer)?.head = head;
699 }
700 Ok(length)
701 }
702
703 fn read_message(
705 &mut self,
706 buffer: StreamBufferHandle,
707 out: &mut [u8],
708 available: usize,
709 ) -> Result<usize> {
710 let b = *self.buffers.resolve(buffer)?;
711 let mut next_tail = b.tail;
712 let mut available = available;
713 let next_length;
714 if b.is_message {
715 let (message_length, tail) = self.read_length_prefix(&b, next_tail);
716 next_tail = tail;
717 available = available.saturating_sub(Self::MESSAGE_LENGTH_BYTES);
718 next_length = if message_length > out.len() {
719 0
721 } else {
722 message_length
723 };
724 } else {
725 next_length = out.len();
726 }
727 let count = next_length.min(available);
728 if count != 0 {
729 let slot = out.get_mut(..count);
730 let tail = match slot {
731 Some(slot) => self.read_bytes(&b, slot, next_tail),
732 None => next_tail,
733 };
734 self.buffers.resolve_mut(buffer)?.tail = tail;
735 }
736 Ok(count)
737 }
738
739 fn write_bytes(&mut self, b: &StreamBuffer, data: &[u8], head: usize) -> usize {
741 let mut head = head;
742 for byte in data {
743 if let Some(slot) = self.bytes.get_mut(b.base.saturating_add(head)) {
744 *slot = *byte;
745 }
746 head = head.saturating_add(1);
747 if head >= b.length {
748 head = 0;
749 }
750 }
751 head
752 }
753
754 fn read_bytes(&mut self, b: &StreamBuffer, out: &mut [u8], tail: usize) -> usize {
756 let mut tail = tail;
757 for slot in out.iter_mut() {
758 *slot = self
759 .bytes
760 .get(b.base.saturating_add(tail))
761 .copied()
762 .unwrap_or(0);
763 tail = tail.saturating_add(1);
764 if tail >= b.length {
765 tail = 0;
766 }
767 }
768 tail
769 }
770
771 fn write_length_prefix(&mut self, b: &StreamBuffer, length: usize, head: usize) -> usize {
774 let bytes = (length as u64).to_le_bytes();
775 let mut head = head;
776 for i in 0..Self::MESSAGE_LENGTH_BYTES {
777 let byte = bytes.get(i).copied().unwrap_or(0);
778 if let Some(slot) = self.bytes.get_mut(b.base.saturating_add(head)) {
779 *slot = byte;
780 }
781 head = head.saturating_add(1);
782 if head >= b.length {
783 head = 0;
784 }
785 }
786 head
787 }
788
789 fn read_length_prefix(&self, b: &StreamBuffer, tail: usize) -> (usize, usize) {
791 let mut raw = [0_u8; 8];
792 let mut tail = tail;
793 for i in 0..Self::MESSAGE_LENGTH_BYTES {
794 let byte = self
795 .bytes
796 .get(b.base.saturating_add(tail))
797 .copied()
798 .unwrap_or(0);
799 if let Some(slot) = raw.get_mut(i) {
800 *slot = byte;
801 }
802 tail = tail.saturating_add(1);
803 if tail >= b.length {
804 tail = 0;
805 }
806 }
807 (u64::from_le_bytes(raw) as usize, tail)
808 }
809}