PageRenderTime 47ms CodeModel.GetById 22ms RepoModel.GetById 1ms app.codeStats 0ms

/firmware/lib/mqtt/proto.c

https://gitlab.com/cpana1/espweather
C | 130 lines | 112 code | 18 blank | 0 comment | 27 complexity | 5940360b8e709fc06f691b91e080fc28 MD5 | raw file
  1. #include <esp8266.h>
  2. #include "proto.h"
  3. #include "ringbuf.h"
  4. I8 ICACHE_FLASH_ATTR PROTO_Init(PROTO_PARSER *parser, PROTO_PARSE_CALLBACK *completeCallback, U8 *buf, U16 bufSize)
  5. {
  6. parser->buf = buf;
  7. parser->bufSize = bufSize;
  8. parser->dataLen = 0;
  9. parser->callback = completeCallback;
  10. parser->isEsc = 0;
  11. return 0;
  12. }
  13. I8 ICACHE_FLASH_ATTR PROTO_ParseByte(PROTO_PARSER *parser, U8 value)
  14. {
  15. switch(value){
  16. case 0x7D:
  17. parser->isEsc = 1;
  18. break;
  19. case 0x7E:
  20. parser->dataLen = 0;
  21. parser->isEsc = 0;
  22. parser->isBegin = 1;
  23. break;
  24. case 0x7F:
  25. if (parser->callback != NULL)
  26. parser->callback();
  27. parser->isBegin = 0;
  28. return 0;
  29. break;
  30. default:
  31. if(parser->isBegin == 0) break;
  32. if(parser->isEsc){
  33. value ^= 0x20;
  34. parser->isEsc = 0;
  35. }
  36. if(parser->dataLen < parser->bufSize)
  37. parser->buf[parser->dataLen++] = value;
  38. break;
  39. }
  40. return -1;
  41. }
  42. I8 ICACHE_FLASH_ATTR PROTO_Parse(PROTO_PARSER *parser, U8 *buf, U16 len)
  43. {
  44. while(len--)
  45. PROTO_ParseByte(parser, *buf++);
  46. return 0;
  47. }
  48. I16 ICACHE_FLASH_ATTR PROTO_ParseRb(RINGBUF* rb, U8 *bufOut, U16* len, U16 maxBufLen)
  49. {
  50. U8 c;
  51. PROTO_PARSER proto;
  52. PROTO_Init(&proto, NULL, bufOut, maxBufLen);
  53. while(RINGBUF_Get(rb, &c) == 0){
  54. if(PROTO_ParseByte(&proto, c) == 0){
  55. *len = proto.dataLen;
  56. return 0;
  57. }
  58. }
  59. return -1;
  60. }
  61. I16 ICACHE_FLASH_ATTR PROTO_Add(U8 *buf, const U8 *packet, I16 bufSize)
  62. {
  63. U16 i = 2;
  64. U16 len = *(U16*) packet;
  65. if (bufSize < 1) return -1;
  66. *buf++ = 0x7E;
  67. bufSize--;
  68. while (len--) {
  69. switch (*packet) {
  70. case 0x7D:
  71. case 0x7E:
  72. case 0x7F:
  73. if (bufSize < 2) return -1;
  74. *buf++ = 0x7D;
  75. *buf++ = *packet++ ^ 0x20;
  76. i += 2;
  77. bufSize -= 2;
  78. break;
  79. default:
  80. if (bufSize < 1) return -1;
  81. *buf++ = *packet++;
  82. i++;
  83. bufSize--;
  84. break;
  85. }
  86. }
  87. if (bufSize < 1) return -1;
  88. *buf++ = 0x7F;
  89. return i;
  90. }
  91. I16 ICACHE_FLASH_ATTR PROTO_AddRb(RINGBUF *rb, const U8 *packet, I16 len)
  92. {
  93. U16 i = 2;
  94. if(RINGBUF_Put(rb, 0x7E) == -1) return -1;
  95. while (len--) {
  96. switch (*packet) {
  97. case 0x7D:
  98. case 0x7E:
  99. case 0x7F:
  100. if(RINGBUF_Put(rb, 0x7D) == -1) return -1;
  101. if(RINGBUF_Put(rb, *packet++ ^ 0x20) == -1) return -1;
  102. i += 2;
  103. break;
  104. default:
  105. if(RINGBUF_Put(rb, *packet++) == -1) return -1;
  106. i++;
  107. break;
  108. }
  109. }
  110. if(RINGBUF_Put(rb, 0x7F) == -1) return -1;
  111. return i;
  112. }