| Brian Silverman | eda63f3 | 2017-10-08 18:57:33 -0400 | [diff] [blame] | 1 | #include "motors/usb/queue.h" |
| 2 | |
| 3 | #include <string.h> |
| 4 | |
| Austin Schuh | d9af8f5 | 2021-12-31 21:50:35 -0800 | [diff] [blame] | 5 | #include <algorithm> |
| 6 | |
| Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame] | 7 | namespace frc971::teensy { |
| Brian Silverman | eda63f3 | 2017-10-08 18:57:33 -0400 | [diff] [blame] | 8 | |
| 9 | size_t Queue::Read(char *out_data, size_t out_size) { |
| 10 | const size_t read_cursor = read_cursor_.load(::std::memory_order_relaxed); |
| 11 | const size_t write_cursor = write_cursor_.load(::std::memory_order_acquire); |
| 12 | const size_t r = ::std::min(out_size, space_used(read_cursor, write_cursor)); |
| 13 | const size_t first_chunk = ::std::min(r, size_ - read_cursor); |
| 14 | memcpy(out_data, &data_[read_cursor], first_chunk); |
| 15 | memcpy(out_data + first_chunk, &data_[0], r - first_chunk); |
| 16 | read_cursor_.store(wrap(read_cursor + r), ::std::memory_order_release); |
| 17 | return r; |
| 18 | } |
| 19 | |
| 20 | size_t Queue::Write(const char *in_data, size_t in_size) { |
| 21 | const size_t write_cursor = write_cursor_.load(::std::memory_order_relaxed); |
| 22 | const size_t read_cursor = read_cursor_.load(::std::memory_order_acquire); |
| 23 | const size_t r = |
| 24 | ::std::min(in_size, size_ - space_used(read_cursor, write_cursor) - 1); |
| 25 | const size_t first_chunk = ::std::min(r, size_ - write_cursor); |
| 26 | memcpy(&data_[write_cursor], in_data, first_chunk); |
| 27 | memcpy(&data_[0], in_data + first_chunk, r - first_chunk); |
| 28 | write_cursor_.store(wrap(write_cursor + r), ::std::memory_order_release); |
| 29 | return r; |
| 30 | } |
| 31 | |
| Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame] | 32 | } // namespace frc971::teensy |