Modify the uart_buffer code to support reading

For reads, we push one character at a time and then read multiple ones.
We want to do this for the camera board.

Change-Id: I8a32dcaf0aa822a5642f388c58e02e17564203f3
diff --git a/motors/peripheral/uart.cc b/motors/peripheral/uart.cc
index 3f7ddaf..9cd4014 100644
--- a/motors/peripheral/uart.cc
+++ b/motors/peripheral/uart.cc
@@ -39,18 +39,24 @@
   // M_UART_RIE /* Enable RX interrupt or DMA */
   // Also set in C5: M_UART_TDMAS /* Do DMA for TX */ |
   // M_UART_RDMAS /* Do DMA for RX */
-  c2_value_ = M_UART_TE;
+  c2_value_ = 0;
   module_->C2 = c2_value_;
   module_->PFIFO =
       M_UART_TXFE /* Enable TX FIFO */ | M_UART_RXFE /* Enable RX FIFO */;
   module_->CFIFO =
       M_UART_TXFLUSH /* Flush TX FIFO */ | M_UART_RXFLUSH /* Flush RX FIFO */;
+  c2_value_ = M_UART_TE | M_UART_RE;
+  module_->C2 = c2_value_;
   // TODO(Brian): Adjust for DMA?
   module_->TWFIFO = tx_fifo_size_ - 1;
-  module_->RWFIFO = rx_fifo_size_ - 1;
+  module_->RWFIFO = 1;
 }
 
 void Uart::DoWrite(gsl::span<const char> data) {
+  // In theory, we could be more efficient about this by writing the number of
+  // bytes we know there's space for and only calling SpaceAvailable() (or
+  // otherwise reading S1) before the final one. In practice, the FIFOs are so
+  // short on this part it probably won't help anything.
   for (int i = 0; i < data.size(); ++i) {
     while (!SpaceAvailable()) {
     }
@@ -58,38 +64,91 @@
   }
 }
 
-Uart::~Uart() { DisableTransmitInterrupt(); }
+aos::SizedArray<char, 4> Uart::DoRead() {
+  // In theory, we could be more efficient about this by reading the number of
+  // bytes we know to be accessible and only calling DataAvailable() (or
+  // otherwise reading S1) before the final one. In practice, the FIFOs are so
+  // short on this part it probably won't help anything.
+  aos::SizedArray<char, 4> result;
+  while (DataAvailable() && !result.full()) {
+    result.push_back(ReadCharacter());
+  }
+  return result;
+}
+
+Uart::~Uart() {
+  DoDisableTransmitInterrupt();
+  DoDisableReceiveInterrupt();
+}
+
+InterruptBufferedUart::~InterruptBufferedUart() {
+  uart_.DisableReceiveInterrupt(DisableInterrupts());
+}
 
 void InterruptBufferedUart::Initialize(int baud_rate) {
   uart_.Initialize(baud_rate);
+  {
+    DisableInterrupts disable_interrupts;
+    uart_.EnableReceiveInterrupt(disable_interrupts);
+  }
 }
 
 void InterruptBufferedUart::Write(gsl::span<const char> data) {
   DisableInterrupts disable_interrupts;
-  uart_.EnableTransmitInterrupt();
-  static_assert(buffer_.size() >= 8,
-                "Need enough space to not turn the interrupt off each time");
+  uart_.EnableTransmitInterrupt(disable_interrupts);
   while (!data.empty()) {
-    const int bytes_written = buffer_.PushSpan(data);
+    const int bytes_written = transmit_buffer_.PushSpan(data);
     data = data.subspan(bytes_written);
     WriteCharacters(data.empty(), disable_interrupts);
     ReenableInterrupts{&disable_interrupts};
   }
 }
 
-void InterruptBufferedUart::WriteCharacters(bool disable_empty,
-                                            const DisableInterrupts &) {
+gsl::span<char> InterruptBufferedUart::Read(gsl::span<char> buffer) {
+  size_t bytes_read = 0;
+  {
+    DisableInterrupts disable_interrupts;
+    const gsl::span<const char> read_data =
+        receive_buffer_.PopSpan(buffer.size());
+    std::copy(read_data.begin(), read_data.end(), buffer.begin());
+    bytes_read += read_data.size();
+  }
+  {
+    DisableInterrupts disable_interrupts;
+    const gsl::span<const char> read_data =
+        receive_buffer_.PopSpan(buffer.size() - bytes_read);
+    std::copy(read_data.begin(), read_data.end(),
+              buffer.subspan(bytes_read).begin());
+    bytes_read += read_data.size();
+  }
+  return buffer.subspan(0, bytes_read);
+}
+
+void InterruptBufferedUart::WriteCharacters(
+    bool disable_empty, const DisableInterrupts &disable_interrupts) {
   while (true) {
-    if (buffer_.empty()) {
+    if (transmit_buffer_.empty()) {
       if (disable_empty) {
-        uart_.DisableTransmitInterrupt();
+        uart_.DisableTransmitInterrupt(disable_interrupts);
       }
       return;
     }
     if (!uart_.SpaceAvailable()) {
       return;
     }
-    uart_.WriteCharacter(buffer_.PopSingle());
+    uart_.WriteCharacter(transmit_buffer_.PopSingle());
+  }
+}
+
+void InterruptBufferedUart::ReadCharacters(const DisableInterrupts &) {
+  while (true) {
+    if (receive_buffer_.full()) {
+      return;
+    }
+    if (!uart_.DataAvailable()) {
+      return;
+    }
+    receive_buffer_.PushSingle(uart_.ReadCharacter());
   }
 }