rfbridge: properly check hex<->byte functions results

code/espurna/rfbridge.cpp

@@ -123,28 +123,27 @@ void _rfbWebSocketOnData(JsonObject& root) {
 #endif // WEB_SUPPORT
 
 // From a byte array to an hexa char array ("A220EE...", double the size)
-static bool _rfbHexFromBytearray(uint8_t * in, size_t in_size, char * out, size_t out_size) {
- if ((2 * in_size + 1) > (out_size)) return false;
+static size_t _rfbHexFromBytearray(uint8_t * in, size_t in_size, char * out, size_t out_size) {
+ if ((2 * in_size + 1) > (out_size)) return 0;
 
  static const char base16[] = "0123456789ABCDEF";
  unsigned char index = 0;
 
- for (;;) {
- if (index >= in_size) break;
+ while (index < in_size) {
  out[(index*2)] = base16[(in[index] & 0xf0) >> 4];
  out[(index*2)+1] = base16[(in[index] & 0xf)];
  ++index;
  }
 
  out[2*index] = '\0';
 
- return index <= out_size;
+ return index ? (1 + (2 * index)) : 0;
 }
 
 
 // From an hexa char array ("A220EE...") to a byte array (half the size)
-static bool _rfbBytearrayFromHex(const char* in, size_t in_size, uint8_t* out, uint8_t out_size) {
- if (out_size < (in_size / 2)) return false;
+static size_t _rfbBytearrayFromHex(const char* in, size_t in_size, uint8_t* out, uint8_t out_size) {
+ if (out_size < (in_size / 2)) return 0;
 
  unsigned char index = 0;
  unsigned char out_index = 0;
@@ -161,17 +160,15 @@ static bool _rfbBytearrayFromHex(const char* in, size_t in_size, uint8_t* out, u
  }
  };
 
- for (;;) {
- if (index >= in_size) break;
-
+ while (index < in_size) {
  out[out_index] = char2byte(in[index]) << 4;
  out[out_index] += char2byte(in[index + 1]);
 
  index += 2;
  out_index += 1;
  }
 
- return true;
+ return out_index ? (1 + out_index) : 0;
 }
 
 void _rfbAckImpl();
@@ -361,8 +358,8 @@ void _rfbParseRaw(char * raw) {
  DEBUG_MSG_P(PSTR("[RF] Sending RAW MESSAGE '%s'\n"), raw);
 
  uint8_t message[RF_MAX_MESSAGE_SIZE];
- _rfbBytearrayFromHex(raw, (size_t)rawlen, message, sizeof(message));
- _rfbSendRaw(message, (rawlen / 2));
+ size_t bytes = _rfbBytearrayFromHex(raw, (size_t)rawlen, message, sizeof(message));
+ _rfbSendRaw(message, bytes);
 }
 
 #else // RFB_DIRECT
@@ -683,6 +680,18 @@ void _rfbInitCommands() {
 
  });
 
+ #if !RFB_DIRECT
+ terminalRegisterCommand(F("RFB.WRITE"), [](Embedis* e) {
+ if (e->argc != 2) return;
+ String arg(e->argv[1]);
+ uint8_t data[RF_MAX_MESSAGE_SIZE];
+ size_t bytes = _rfbBytearrayFromHex(arg.c_str(), arg.length(), data, sizeof(data));
+ if (bytes) {
+ _rfbSendRaw(data, bytes);
+ }
+ });
+ #endif
+
 }
 
 #endif // TERMINAL_SUPPORT
@@ -711,25 +720,18 @@ String rfbRetrieve(unsigned char id, bool status) {
 void rfbStatus(unsigned char id, bool status) {
 
  String value = rfbRetrieve(id, status);
- if (value.length() > 0) {
+ if (value.length() && !(value.length() & 1)) {
 
  uint8_t message[RF_MAX_MESSAGE_SIZE];
- int len = _rfbBytearrayFromHex(value.c_str(), value.length(), message, sizeof(message));
-
- if (len == RF_MESSAGE_SIZE && // probably a standard msg
- (message[0] != RF_CODE_START || // raw would start with 0xAA
- message[1] != RF_CODE_RFOUT_BUCKET || // followed by 0xB0,
- message[2] + 4 != len || // needs a valid length,
- message[len-1] != RF_CODE_STOP)) { // and finish with 0x55
-
- if (!_rfbin) {
+ size_t bytes = _rfbBytearrayFromHex(value.c_str(), value.length(), message, sizeof(message));
+ if (bytes && !_rfbin) {
+ if (value.length() == (RF_MESSAGE_SIZE * 2)) {
  _rfbEnqueue(message, _rfbSameOnOff(id) ? 1 : _rfb_repeat);
+ } else {
+ #if !RFB_DIRECT
+ _rfbSendRaw(message, bytes);
+ #endif
  }
-
- } else {
- #if !RFB_DIRECT
- _rfbSendRaw(message, len); // send a raw message
- #endif
  }
 
  }