Fork of the espurna firmware for `mhsw` switches
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

78 lines
2.2 KiB

7 years ago
  1. // -----------------------------------------------------------------------------
  2. // Stream Injector
  3. // -----------------------------------------------------------------------------
  4. #pragma once
  5. #define STREAM_INJECTOR_BUFFER_SIZE 32
  6. class StreamInjector : public Stream {
  7. public:
  8. typedef std::function<void(uint8_t ch)> writeCallback;
  9. StreamInjector(Stream& serial) : _stream(serial) {}
  10. virtual void callback(writeCallback c) {
  11. _callback = c;
  12. }
  13. virtual size_t write(uint8_t ch) {
  14. if (_callback) _callback(ch);
  15. return _stream.write(ch);
  16. }
  17. virtual int read() {
  18. int ch = _stream.read();
  19. if (ch == -1) {
  20. if (_buffer_read != _buffer_write) {
  21. ch = _buffer[_buffer_read];
  22. _buffer_read = (_buffer_read + 1) % STREAM_INJECTOR_BUFFER_SIZE;
  23. }
  24. }
  25. return ch;
  26. }
  27. virtual int available() {
  28. unsigned int bytes = _stream.available();
  29. if (_buffer_read > _buffer_write) {
  30. bytes += (_buffer_write - _buffer_read + STREAM_INJECTOR_BUFFER_SIZE);
  31. } else if (_buffer_read < _buffer_write) {
  32. bytes += (_buffer_write - _buffer_read);
  33. }
  34. return bytes;
  35. }
  36. virtual int peek() {
  37. int ch = _stream.peek();
  38. if (ch == -1) {
  39. if (_buffer_read != _buffer_write) {
  40. ch = _buffer[_buffer_read];
  41. }
  42. }
  43. return ch;
  44. }
  45. virtual void flush() {
  46. _stream.flush();
  47. _buffer_read = _buffer_write;
  48. }
  49. virtual void inject(char *data, size_t len) {
  50. for (int i=0; i<len; i++) {
  51. _buffer[_buffer_write] = data[i];
  52. _buffer_write = (_buffer_write + 1) % STREAM_INJECTOR_BUFFER_SIZE;
  53. }
  54. }
  55. private:
  56. Stream& _stream;
  57. unsigned char _buffer[STREAM_INJECTOR_BUFFER_SIZE];
  58. unsigned char _buffer_write = 0;
  59. unsigned char _buffer_read = 0;
  60. writeCallback _callback = NULL;
  61. };