From 52adb462a985cada78ff1859aceced1a205d165e Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@galois.com>
Date: Fri, 14 Sep 2012 17:30:39 -0700
Subject: [PATCH] AP_HAL_AVR: Finished implementation of AVRConsoleDriver

---
 libraries/AP_HAL_AVR/Console.cpp | 25 +++++++++++++++++++------
 libraries/AP_HAL_AVR/Console.h   |  6 +++++-
 2 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_HAL_AVR/Console.cpp b/libraries/AP_HAL_AVR/Console.cpp
index 4416ef8502..b5825f0e58 100644
--- a/libraries/AP_HAL_AVR/Console.cpp
+++ b/libraries/AP_HAL_AVR/Console.cpp
@@ -80,8 +80,7 @@ void AVRConsoleDriver::_printf_P(const prog_char *fmt, ...) {
 // Stream method implementations /////////////////////////////////////////
 int AVRConsoleDriver::available(void) {
     if (_user_backend) {
-        // XXX TODO
-        return INT_MAX;
+        return _rxbuf.bytes_used();
     } else {
         return _base_uart->available();
     }
@@ -89,8 +88,7 @@ int AVRConsoleDriver::available(void) {
 
 int AVRConsoleDriver::txspace(void) {
     if (_user_backend) {
-        // XXX TODO
-        return INT_MAX;
+        return _rxbuf.bytes_free();
     } else {
         return _base_uart->txspace();
     }
@@ -106,8 +104,7 @@ int AVRConsoleDriver::read() {
 
 int AVRConsoleDriver::peek() {
     if (_user_backend) {
-        // XXX TODO
-        return -1;
+        return _rxbuf.peek();
     } else {
         return _base_uart->peek();
     }
@@ -169,3 +166,19 @@ int AVRConsoleDriver::Buffer::pop() {
     return (int) b;
 }
 
+int AVRConsoleDriver::Buffer::peek() {
+    if ( _tail == _head ) {
+        return -1;
+    }
+    uint8_t b = _bytes[_tail];
+    return (int) b;
+}
+
+uint16_t AVRConsoleDriver::Buffer::bytes_free() {
+    return ((_head - _tail) & _mask);
+}
+
+uint16_t AVRConsoleDriver::Buffer::bytes_used() {
+    return ((_mask+1) - ((_head - _tail) & _mask));
+}
+
diff --git a/libraries/AP_HAL_AVR/Console.h b/libraries/AP_HAL_AVR/Console.h
index 8dbf34170f..41b924403e 100644
--- a/libraries/AP_HAL_AVR/Console.h
+++ b/libraries/AP_HAL_AVR/Console.h
@@ -38,7 +38,11 @@ private:
         /* public methods:*/
         bool allocate(int size);
         bool push(uint8_t b);
-        int pop();
+        int  pop();
+        int  peek();
+
+        uint16_t bytes_free();
+        uint16_t bytes_used();
     private:
         uint16_t _head, _tail; /* Head and tail indicies */
         uint16_t _mask;       /* Buffer size mask for index wrap */