1 files changed, 44 insertions, 23 deletions
diff --git a/os/various/memstreams.c b/os/various/memstreams.c
index 5eddeda9c..df17a4046 100644
--- a/os/various/memstreams.c
+++ b/os/various/memstreams.c
@@ -31,17 +31,22 @@
 #include "ch.h"
 #include "memstreams.h"
 
-/*
- * @brief   Write virtual method implementation.
- *
- * @param[in] ip        pointer to a @p MemoryStream object
- * @param[in] bp        pointer to the data buffer
- * @param[in] n         the maximum amount of data to be transferred
- * @return              The number of bytes transferred. The return value can
- *                      be less than the specified number of bytes if the
- *                      stream reaches a physical end of file and cannot be
- *                      extended.
- */
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
 static size_t writes(void *ip, const uint8_t *bp, size_t n) {
   MemoryStream *msp = ip;
 
@@ -52,16 +57,6 @@ static size_t writes(void *ip, const uint8_t *bp, size_t n) {
   return n;
 }
 
-/*
- * @brief   Read virtual method implementation.
- *
- * @param[in] ip        pointer to a @p MemoryStream object
- * @param[out] bp       pointer to the data buffer
- * @param[in] n         the maximum amount of data to be transferred
- * @return              The number of bytes transferred. The return value can
- *                      be less than the specified number of bytes if the
- *                      stream reaches the end of the available data.
- */
 static size_t reads(void *ip, uint8_t *bp, size_t n) {
   MemoryStream *msp = ip;
 
@@ -72,7 +67,32 @@ static size_t reads(void *ip, uint8_t *bp, size_t n) {
   return n;
 }
 
-static const struct MemStreamVMT vmt = {writes, reads};
+static msg_t put(void *ip, uint8_t b) {
+  MemoryStream *msp = ip;
+
+  if (msp->size - msp->eos <= 0)
+    return RDY_RESET;
+  *(msp->buffer + msp->eos) = b;
+  msp->eos += 1;
+  return RDY_OK;
+}
+
+static msg_t get(void *ip) {
+  uint8_t b;
+  MemoryStream *msp = ip;
+
+  if (msp->eos - msp->offset <= 0)
+    return RDY_RESET;
+  b = *(msp->buffer + msp->offset);
+  msp->offset += 1;
+  return b;
+}
+
+static const struct MemStreamVMT vmt = {writes, reads, put, get};
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
 
 /**
  * @brief   Memory stream object initialization.
@@ -84,7 +104,8 @@ static const struct MemStreamVMT vmt = {writes, reads};
  *                      put this to zero for RAM buffers or equal to @p size
  *                      for ROM streams.
  */
-void msObjectInit(MemoryStream *msp, uint8_t *buffer, size_t size, size_t eos) {
+void msObjectInit(MemoryStream *msp, uint8_t *buffer,
+                  size_t size, size_t eos) {
 
   msp->vmt    = &vmt;
   msp->buffer = buffer;