add decoding, better formatting for v2 packets

lib/MiLight/CctPacketFormatter.cpp

@@ -73,4 +73,8 @@ uint8_t CctPacketFormatter::getCctStatusButton(uint8_t groupId, MiLightStatus st
   }
   
   return button;
+}
+
+void CctPacketFormatter::format(uint8_t const* packet, char* buffer) {
+  PacketFormatter::formatV1Packet(packet, buffer);
 }

lib/MiLight/CctPacketFormatter.h

@@ -17,6 +17,7 @@ public:
   virtual void decreaseTemperature();
   virtual void increaseBrightness();
   virtual void decreaseBrightness();
+  virtual void format(uint8_t const* packet, char* buffer);
   
   virtual void reset();
   

lib/MiLight/MiLightClient.cpp

@@ -200,32 +200,8 @@ void MiLightClient::command(uint8_t command, uint8_t arg) {
   flushPacket();
 }
 
-void MiLightClient::formatPacket(MiLightRadioConfig& config, uint8_t* packet, char* buffer) {
-  if (config.type == RGBW || config.type == CCT) {
-    String format = String("Request type  : %02X\n") 
-      + "Device ID     : %02X%02X\n"
-      + "b1            : %02X\n"
-      + "b2            : %02X\n"
-      + "b3            : %02X\n"
-      + "Sequence Num. : %02X";
-      
-    sprintf(
-      buffer,
-      format.c_str(),
-      packet[0],
-      packet[1], packet[2],
-      packet[3],
-      packet[4],
-      packet[5],
-      packet[6]
-    );
-  } else {
-    for (int i = 0; i < config.packetLength; i++) {
-      sprintf(buffer, "%02X ", packet[i]);
-      buffer += 3;
-    }
-    sprintf(buffer, "\n\n");
-  }
+void MiLightClient::formatPacket(uint8_t* packet, char* buffer) {
+  formatter->format(packet, buffer);
 }
     
 void MiLightClient::flushPacket() {

lib/MiLight/MiLightClient.h

@@ -67,7 +67,7 @@ class MiLightClient {
     
     void updateSaturation(const uint8_t saturation);
     
-    void formatPacket(MiLightRadioConfig& config, uint8_t* packet, char* buffer);
+    void formatPacket(uint8_t* packet, char* buffer);
     
     
   protected:

lib/MiLight/PacketFormatter.cpp

@@ -21,4 +21,42 @@ void PacketFormatter::increaseBrightness() { }
 void PacketFormatter::decreaseBrightness() { }
 
 void PacketFormatter::updateTemperature(uint8_t value) { }
-void PacketFormatter::updateSaturation(uint8_t value) { }
+void PacketFormatter::updateSaturation(uint8_t value) { }
+  
+uint8_t* PacketFormatter::buildPacket() {
+  return this->packet;
+}
+
+void PacketFormatter::prepare(uint16_t deviceId, uint8_t groupId) {
+  this->deviceId = deviceId;
+  this->groupId = groupId;
+  reset();
+}
+
+void PacketFormatter::format(uint8_t const* packet, char* buffer) {
+  for (int i = 0; i < packetLength; i++) {
+    sprintf(buffer, "%02X ", packet[i]);
+    buffer += 3;
+  }
+  sprintf(buffer, "\n\n");
+}
+
+void PacketFormatter::formatV1Packet(uint8_t const* packet, char* buffer) {
+  String format = String("Request type  : %02X\n") 
+    + "Device ID     : %02X%02X\n"
+    + "b1            : %02X\n"
+    + "b2            : %02X\n"
+    + "b3            : %02X\n"
+    + "Sequence Num. : %02X";
+    
+  sprintf(
+    buffer,
+    format.c_str(),
+    packet[0],
+    packet[1], packet[2],
+    packet[3],
+    packet[4],
+    packet[5],
+    packet[6]
+  );
+}

lib/MiLight/PacketFormatter.h

@@ -7,9 +7,10 @@
 
 class PacketFormatter {
 public:
-  PacketFormatter(const size_t packetLength) {
-    this->packet = new uint8_t[packetLength];
-  }
+  PacketFormatter(const size_t packetLength) 
+    : packetLength(packetLength),
+      packet(new uint8_t[packetLength])
+  { }
   
   ~PacketFormatter() {
     delete this->packet;
@@ -41,20 +42,11 @@ public:
   
   virtual void reset() = 0;
   
-  virtual uint8_t* buildPacket() {
-    printf("Packet: ");
-    for(size_t i = 0; i < 7; i++) {
-      printf("%02X ", this->packet[i]);
-    }
-    printf("\n");
-    return this->packet;
-  }
+  virtual uint8_t* buildPacket();
+  virtual void prepare(uint16_t deviceId, uint8_t groupId);
+  virtual void format(uint8_t const* packet, char* buffer);
   
-  void prepare(uint16_t deviceId, uint8_t groupId) {
-    this->deviceId = deviceId;
-    this->groupId = groupId;
-    reset();
-  }
+  static void formatV1Packet(uint8_t const* packet, char* buffer);
   
   template <typename T>
   static T rescale(T value, uint8_t newMax, float oldMax = 255.0) {
@@ -63,6 +55,7 @@ public:
   
 protected:
   uint8_t* packet;
+  size_t packetLength;
   uint16_t deviceId;
   uint8_t groupId;
   uint8_t sequenceNum;

lib/MiLight/RgbCctPacketFormatter.cpp

@@ -1,6 +1,10 @@
 #include <RgbCctPacketFormatter.h>
 
-#define V2_OFFSET(byte, key) ( V2_OFFSETS[byte-1][key%4] )
+#define V2_OFFSET(byte, key, jumpStart) ( \
+  V2_OFFSETS[byte-1][key%4] \
+    + \
+  ((jumpStart > 0 && key >= jumpStart && key <= jumpStart+0x80) ? 0x80 : 0) \
+)
 
 uint8_t const RgbCctPacketFormatter::V2_OFFSETS[][4] = {
   { 0x45, 0x1F, 0x14, 0x5C },
@@ -12,6 +16,23 @@ uint8_t const RgbCctPacketFormatter::V2_OFFSETS[][4] = {
   { 0xAF, 0x04, 0xDD, 0x07 },
   { 0xE1, 0x93, 0xB8, 0xE4 }
 };
+  
+uint8_t const RgbCctPacketFormatter::BYTE_JUMP_STARTS[] = {
+  0, // key byte doesn't get shifted
+  0x54, 0x54, 0x14, 0x54, 
+  0, // argument byte gets different shifts for different commands
+  0x54, 0x54,
+  0  // checksum isn't shifted
+};
+
+uint8_t const RgbCctPacketFormatter::ARG_JUMP_STARTS[] = {
+  0,    // no command with id = 0
+  0x14, // on
+  0x14, // color
+  0x14, // kelvin
+  0x54, // brightness, saturation
+  0x14  // mode
+};
 
 void RgbCctPacketFormatter::reset() {
   size_t packetPtr = 0;
@@ -78,6 +99,14 @@ uint8_t RgbCctPacketFormatter::xorKey(uint8_t key) {
   return ( msn | lsn );
 }
 
+uint8_t RgbCctPacketFormatter::decodeByte(uint8_t byte, uint8_t s1, uint8_t xorKey, uint8_t s2) {
+  uint8_t value = byte - s2;
+  value = value ^ xorKey;
+  value = value - s1;
+  
+  return value;
+}
+
 uint8_t RgbCctPacketFormatter::encodeByte(uint8_t byte, uint8_t s1, uint8_t xorKey, uint8_t s2) {
   uint8_t value = (byte + s1) % 0x100;
   value = value ^ xorKey;
@@ -86,20 +115,55 @@ uint8_t RgbCctPacketFormatter::encodeByte(uint8_t byte, uint8_t s1, uint8_t xorK
   return value;
 }
 
-void RgbCctPacketFormatter::encodeV2Packet(uint8_t *packet) {
+void RgbCctPacketFormatter::decodeV2Packet(uint8_t *packet) {
   uint8_t key = xorKey(packet[0]);
-  uint8_t sum = key;
   
-  printf("Packet: ");
-  for (size_t i = 0; i < 9; i++) {
-    printf("%02X ", packet[i]);
+  for (size_t i = 1; i <= 8; i++) {
+    if (i != 5) {
+      packet[i] = decodeByte(packet[i], 0, key, V2_OFFSET(i, packet[0], BYTE_JUMP_STARTS[i]));
+    }
   }
-  printf("\n");
+      
+  packet[5] = decodeByte(packet[5], 0, key, V2_OFFSET(5, packet[0], ARG_JUMP_STARTS[packet[4]]));
+}
+
+void RgbCctPacketFormatter::encodeV2Packet(uint8_t *packet) {
+  uint8_t key = xorKey(packet[0]);
+  uint8_t sum = key;
+  uint8_t command = packet[4];
+  size_t ptr = 0;
   
   for (size_t i = 1; i <= 7; i++) {
     sum += packet[i];
-    packet[i] = encodeByte(packet[i], 0, key, V2_OFFSET(i, packet[0]));
+    
+    if (i != 5) {
+      printf("%d s2 = %02X\n", i, V2_OFFSET(i, packet[0], BYTE_JUMP_STARTS[i]));
+      packet[i] = encodeByte(packet[i], 0, key, V2_OFFSET(i, packet[0], BYTE_JUMP_STARTS[i]));
+    }
   }
   
-  packet[8] = encodeByte(sum, 2, key, V2_OFFSET(8, packet[0]));
+  packet[5] = encodeByte(packet[5], 0, key, V2_OFFSET(5, packet[0], ARG_JUMP_STARTS[command]));
+  packet[8] = encodeByte(sum, 2, key, V2_OFFSET(8, packet[0], 0));
+}
+
+void RgbCctPacketFormatter::format(uint8_t const* packet, char* buffer) {
+  buffer += sprintf(buffer, "Raw packet: ");
+  for (int i = 0; i < packetLength; i++) {
+    buffer += sprintf(buffer, "%02X ", packet[i]);
+  }
+  
+  uint8_t decodedPacket[packetLength];
+  memcpy(decodedPacket, packet, packetLength);
+  
+  decodeV2Packet(decodedPacket);
+  
+  buffer += sprintf(buffer, "\n\nDecoded:\n");
+  buffer += sprintf(buffer, "Key      : %02X\n", decodedPacket[0]);
+  buffer += sprintf(buffer, "b1       : %02X\n", decodedPacket[1]);
+  buffer += sprintf(buffer, "ID       : %02X%02X\n", decodedPacket[2], decodedPacket[3]);
+  buffer += sprintf(buffer, "Command  : %02X\n", decodedPacket[4]);
+  buffer += sprintf(buffer, "Argument : %02X\n", decodedPacket[5]);
+  buffer += sprintf(buffer, "Sequence : %02X\n", decodedPacket[6]);
+  buffer += sprintf(buffer, "Group    : %02X\n", decodedPacket[7]);
+  buffer += sprintf(buffer, "Checksum : %02X", decodedPacket[8]);
 }

lib/MiLight/RgbCctPacketFormatter.h

@@ -9,6 +9,8 @@
 class RgbCctPacketFormatter : public PacketFormatter {
 public:
   static uint8_t const V2_OFFSETS[][4];
+  static uint8_t const BYTE_JUMP_STARTS[];
+  static uint8_t const ARG_JUMP_STARTS[];
     
   RgbCctPacketFormatter(size_t packetLength)
     : PacketFormatter(packetLength)
@@ -24,12 +26,15 @@ public:
   virtual void updateColorWhite();
   virtual void updateTemperature(uint8_t value);
   virtual void updateSaturation(uint8_t value);
+  virtual void format(uint8_t const* packet, char* buffer);
   
   virtual uint8_t* buildPacket();
     
   static void encodeV2Packet(uint8_t* packet);
+  static void decodeV2Packet(uint8_t* packet);
   static uint8_t xorKey(uint8_t key);
   static uint8_t encodeByte(uint8_t byte, uint8_t s1, uint8_t xorKey, uint8_t s2);
+  static uint8_t decodeByte(uint8_t byte, uint8_t s1, uint8_t xorKey, uint8_t s2);
 };
 
 #endif

lib/MiLight/RgbwPacketFormatter.cpp

@@ -49,4 +49,8 @@ void RgbwPacketFormatter::updateColorRaw(uint8_t value) {
 void RgbwPacketFormatter::updateColorWhite() {
   uint8_t button = RGBW_GROUP_1_MAX_LEVEL + ((groupId - 1)*2);
   command(button, 0);
+}
+
+void RgbwPacketFormatter::format(uint8_t const* packet, char* buffer) {
+  PacketFormatter::formatV1Packet(packet, buffer);
 }

lib/MiLight/RgbwPacketFormatter.h

@@ -19,6 +19,7 @@ public:
   virtual void updateHue(uint16_t value);
   virtual void updateColorRaw(uint8_t value);
   virtual void updateColorWhite();
+  virtual void format(uint8_t const* packet, char* buffer);
   
   virtual void reset();
 };

lib/WebServer/MiLightHttpServer.cpp

@@ -162,7 +162,7 @@ void MiLightHttpServer::handleListenGateway(const UrlTokenBindings* bindings) {
   response += ":\n";
   
   char ppBuffer[200];
-  milightClient->formatPacket(*config, packet, ppBuffer);
+  milightClient->formatPacket(packet, ppBuffer);
   response += String(ppBuffer);
   
   response += "\n\n";