use espMqttClient, qos2 fixed

lib/espMqttClient/src/Packets/RemainingLength.cpp

@@ -0,0 +1,57 @@
+/*
+Copyright (c) 2022 Bert Melis. All rights reserved.
+
+This work is licensed under the terms of the MIT license.  
+For a copy, see <https://opensource.org/licenses/MIT> or
+the LICENSE file.
+*/
+
+#include "RemainingLength.h"
+
+namespace espMqttClientInternals {
+
+int32_t decodeRemainingLength(const uint8_t* stream) {
+  uint32_t multiplier = 1;
+  int32_t remainingLength = 0;
+  uint8_t currentByte = 0;
+  uint8_t encodedByte;
+
+  do {
+    encodedByte = stream[currentByte++];
+    remainingLength += (encodedByte & 127) * multiplier;
+    if (multiplier > 128 * 128 * 128) {
+      emc_log_e("Malformed Remaining Length");
+      return -1;
+    }
+    multiplier *= 128;
+  } while ((encodedByte & 128) != 0);
+
+  return remainingLength;
+}
+
+uint8_t remainingLengthLength(uint32_t remainingLength) {
+  if (remainingLength < 128) return 1;
+  if (remainingLength < 16384) return 2;
+  if (remainingLength < 2097152) return 3;
+  if (remainingLength > 268435455) return 0;
+  return 4;
+}
+
+uint8_t encodeRemainingLength(uint32_t remainingLength, uint8_t* destination) {
+  uint8_t currentByte = 0;
+  uint8_t bytesNeeded = 0;
+
+  do {
+    uint8_t encodedByte = remainingLength % 128;
+    remainingLength /= 128;
+    if (remainingLength > 0) {
+      encodedByte = encodedByte | 128;
+    }
+    destination[currentByte++] = encodedByte;
+    bytesNeeded++;
+  } while (remainingLength > 0);
+
+  return bytesNeeded;
+}
+
+}  // namespace espMqttClientInternals