HDLCStream.h

Go to the documentation of this file.

#pragma once
 
#include <stdbool.h>
#include <stdint.h>
 
#include "AudioTools/CoreAudio/Buffers.h"
#include "AudioToolsConfig.h"
 
namespace audio_tools {
 
class HDLCStream : public Stream {
 public:
  HDLCStream() = default;
 
  HDLCStream(int maxFrameSize) {
    resize(maxFrameSize);
  }
  HDLCStream(Stream& stream, size_t maxFrameSize) {
    setStream(stream);
    resize(maxFrameSize);
  }
 
  HDLCStream(Print& stream, size_t maxFrameSize)  {
    setPrint(stream);
    resize(maxFrameSize);
  }
 
  void setStream(Stream& stream) {
    p_stream = &stream;
    p_print = &stream;
  }
 
  void setPrint(Print& stream) { p_print = &stream; }
 
  int available() override {
    if (rx_frame_buffer.available() == 0) _processInput();
    return rx_frame_buffer.available();
  }
 
  int read() override { return -1; }
 
  size_t readBytes(uint8_t* buffer, size_t length) {
    assert(_maxFrameSize > 0);
    size_t available_bytes = rx_frame_buffer.available();
    // get more data
    if (available_bytes == 0) {
      _processInput();
      available_bytes = rx_frame_buffer.available();
    }
 
    // check that we consume the full frame
    if (length < available_bytes) {
      LOGE("readBytes len too small %u instead of %u", (unsigned)length,
           (unsigned)available_bytes);
      return 0;
    }
 
    // provide the data
    memcpy(buffer, rx_frame_buffer.data(), available_bytes);
    rx_frame_buffer.clear();
    _frameReady = false;
    return available_bytes;
  }
 
  int peek() override { return -1; }
 
  void flush() override { p_stream->flush(); }
 
  size_t write(uint8_t b) override { return 0; }
 
  size_t write(const uint8_t* data, size_t len) override {
    assert(_maxFrameSize > 0);
    return writeFrame(data, len);
  }
 
  bool resize(size_t size) {
    if (size > _maxFrameSize) return false;
    tx_frame_buffer.resize(size);
    rx_frame_buffer.resize(size);
    _rxBuffer.resize(size);
    _maxFrameSize = size;
    return true;
  }
 
 protected:
  Stream* p_stream = nullptr;
  Print* p_print = nullptr;
  size_t _maxFrameSize = 0;
  SingleBuffer<uint8_t> tx_frame_buffer;
  SingleBuffer<uint8_t> rx_frame_buffer;
  Vector<uint8_t> _rxBuffer;
  size_t _frameLen = 0;
  size_t _rxLen = 0;
  size_t _rxPos = 0;
  bool _frameReady = false;
 
  enum RxState { IDLE, RECEIVING, ESCAPED } _rxState = IDLE;
 
  static constexpr uint8_t HDLC_FLAG = 0x7E;
  static constexpr uint8_t HDLC_ESC = 0x7D;
  static constexpr uint8_t HDLC_ESC_XOR = 0x20;
 
  uint16_t _crc16(uint8_t data, uint16_t crc) {
    crc ^= (uint16_t)data << 8;
    for (int i = 0; i < 8; i++)
      crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1);
    return crc;
  }
 
  void _writeEscaped(uint8_t b) {
    if (b == HDLC_FLAG || b == HDLC_ESC) {
      tx_frame_buffer.write(HDLC_ESC);
      tx_frame_buffer.write(b ^ HDLC_ESC_XOR);
    } else {
      tx_frame_buffer.write(b);
    }
  }
 
  size_t writeFrame(const uint8_t* data, size_t len) {
    if (!data || len == 0) return 0;
 
    uint16_t crc = 0xFFFF;
    tx_frame_buffer.write(HDLC_FLAG);
 
    for (size_t i = 0; i < len; ++i) {
      crc = _crc16(data[i], crc);
      _writeEscaped(data[i]);
    }
 
    _writeEscaped(crc >> 8);
    _writeEscaped(crc & 0xFF);
    tx_frame_buffer.write(HDLC_FLAG);
    p_print->write(tx_frame_buffer.data(), tx_frame_buffer.available());
    p_print->flush();
    tx_frame_buffer.clear();
    return len;
  }
 
  void _processInput() {
    while (!_frameReady && p_stream->available()) {
      uint8_t b = p_stream->read();
 
      if (b == HDLC_FLAG) {
        if (_rxLen >= 3) {
          uint16_t recvCrc =
              (_rxBuffer[_rxLen - 2] << 8) | _rxBuffer[_rxLen - 1];
          uint16_t calcCrc = 0xFFFF;
          for (size_t i = 0; i < _rxLen - 2; ++i)
            calcCrc = _crc16(_rxBuffer[i], calcCrc);
 
          if (calcCrc == recvCrc) {
            for (int j = 0; j < _rxLen - 2; j++) {
              rx_frame_buffer.write(_rxBuffer[j]);
            }
 
            _frameLen = _rxLen - 2;
            _rxPos = 0;
            _frameReady = true;
          }
        }
        _rxState = IDLE;
        _rxLen = 0;
        continue;
      }
 
      switch (_rxState) {
        case IDLE:
          _rxLen = 0;
          if (b == HDLC_ESC) {
            _rxState = ESCAPED;
          } else {
            _rxState = RECEIVING;
            if (_rxLen < _maxFrameSize) _rxBuffer[_rxLen++] = b;
          }
          break;
 
        case RECEIVING:
          if (b == HDLC_ESC) {
            _rxState = ESCAPED;
          } else if (_rxLen < _maxFrameSize) {
            _rxBuffer[_rxLen++] = b;
          }
          break;
 
        case ESCAPED:
          if (_rxLen < _maxFrameSize) {
            _rxBuffer[_rxLen++] = b ^ HDLC_ESC_XOR;
          }
          _rxState = RECEIVING;
          break;
      }
 
      if (_rxLen >= _maxFrameSize) {
        _rxState = IDLE;  // overflow
        _rxLen = 0;
      }
    }
  }
};
 
}  // namespace audio_tools