86 lines
2.3 KiB
C
86 lines
2.3 KiB
C
|
// -----------------------------------------------------------------------------
|
||
|
|
// Stream Injector
|
||
|
|
// -----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include <Stream.h>
|
||
|
|
|
||
|
|
class StreamInjector : public Stream {
|
||
|
|
|
||
|
|
public:
|
||
|
|
|
||
|
|
typedef std::function<void(uint8_t ch)> writeCallback;
|
||
|
|
|
||
|
|
StreamInjector(Stream& serial, size_t buflen = 128) : _stream(serial), _buffer_size(buflen) {
|
||
|
|
_buffer = new char[buflen];
|
||
|
|
}
|
||
|
|
|
||
|
|
~StreamInjector() {
|
||
|
|
delete[] _buffer;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual void callback(writeCallback c) {
|
||
|
|
_callback = c;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual size_t write(uint8_t ch) {
|
||
|
|
if (_callback) _callback(ch);
|
||
|
|
return _stream.write(ch);
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual int read() {
|
||
|
|
int ch = _stream.read();
|
||
|
|
if (ch == -1) {
|
||
|
|
if (_buffer_read != _buffer_write) {
|
||
|
|
ch = _buffer[_buffer_read];
|
||
|
|
_buffer_read = (_buffer_read + 1) % _buffer_size;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return ch;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual int available() {
|
||
|
|
unsigned int bytes = _stream.available();
|
||
|
|
if (_buffer_read > _buffer_write) {
|
||
|
|
bytes += (_buffer_write - _buffer_read + _buffer_size);
|
||
|
|
} else if (_buffer_read < _buffer_write) {
|
||
|
|
bytes += (_buffer_write - _buffer_read);
|
||
|
|
}
|
||
|
|
return bytes;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual int peek() {
|
||
|
|
int ch = _stream.peek();
|
||
|
|
if (ch == -1) {
|
||
|
|
if (_buffer_read != _buffer_write) {
|
||
|
|
ch = _buffer[_buffer_read];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return ch;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual void flush() {
|
||
|
|
_stream.flush();
|
||
|
|
_buffer_read = _buffer_write;
|
||
|
|
}
|
||
|
|
|
||
|
|
virtual void inject(char *data, size_t len) {
|
||
|
|
for (int i=0; i<len; i++) {
|
||
|
|
_buffer[_buffer_write] = data[i];
|
||
|
|
_buffer_write = (_buffer_write + 1) % _buffer_size;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
|
||
|
|
Stream& _stream;
|
||
|
|
char * _buffer;
|
||
|
|
unsigned char _buffer_size;
|
||
|
|
unsigned char _buffer_write = 0;
|
||
|
|
unsigned char _buffer_read = 0;
|
||
|
|
writeCallback _callback = NULL;
|
||
|
|
|
||
|
|
|
||
|
|
};
|