Add a working CDC TTY USB device
I'm pretty sure it reliably moves data in both directions, but it has a
lot of TODOs left wrt error handling.
Change-Id: I1d96a63eea7bdc08f6ec78c868c4a1ccbfbda600
diff --git a/motors/usb/queue.cc b/motors/usb/queue.cc
new file mode 100644
index 0000000..82fa32f
--- /dev/null
+++ b/motors/usb/queue.cc
@@ -0,0 +1,32 @@
+#include "motors/usb/queue.h"
+
+#include <string.h>
+
+namespace frc971 {
+namespace teensy {
+
+size_t Queue::Read(char *out_data, size_t out_size) {
+ const size_t read_cursor = read_cursor_.load(::std::memory_order_relaxed);
+ const size_t write_cursor = write_cursor_.load(::std::memory_order_acquire);
+ const size_t r = ::std::min(out_size, space_used(read_cursor, write_cursor));
+ const size_t first_chunk = ::std::min(r, size_ - read_cursor);
+ memcpy(out_data, &data_[read_cursor], first_chunk);
+ memcpy(out_data + first_chunk, &data_[0], r - first_chunk);
+ read_cursor_.store(wrap(read_cursor + r), ::std::memory_order_release);
+ return r;
+}
+
+size_t Queue::Write(const char *in_data, size_t in_size) {
+ const size_t write_cursor = write_cursor_.load(::std::memory_order_relaxed);
+ const size_t read_cursor = read_cursor_.load(::std::memory_order_acquire);
+ const size_t r =
+ ::std::min(in_size, size_ - space_used(read_cursor, write_cursor) - 1);
+ const size_t first_chunk = ::std::min(r, size_ - write_cursor);
+ memcpy(&data_[write_cursor], in_data, first_chunk);
+ memcpy(&data_[0], in_data + first_chunk, r - first_chunk);
+ write_cursor_.store(wrap(write_cursor + r), ::std::memory_order_release);
+ return r;
+}
+
+} // namespace teensy
+} // namespace frc971