coppino/src/slip.cpp

#include "slip.h"

namespace slip {

  void send(const char* buffer, int len) {
    // Serial.write(END);   // frame-out any possible noise on the line

    for (int i = 0; i < len; ++i) {
      switch (buffer[i]) {
        case END:
          Serial.write(ESC);
          Serial.write(ESC_END);
          break;
        case ESC:
          Serial.write(ESC);
          Serial.write(ESC_ESC);
          break;
        default:
          Serial.write(buffer[i]);
          break;
      }
    }

    Serial.write(END);
  }

  int recv(char* buffer, int len) {
    char r;
    int i = 0;

    while (i < len) {
      if (Serial.available()) {
        r = Serial.read();

        switch (r) {
          case ESC:
            while (Serial.available() <= 0);
            r = Serial.read();
            if (r == ESC_END) {
                buffer[i++] = END;
            } else if (r == ESC_ESC) {
                buffer[i++] = ESC;
            }
            break;
          case END:
            return i;
            break;
          default:
            buffer[i++] = r;
            break;
        }
      }
    }

    return i;
  }

} /* namespace slip */