preliminary v6 UDP server support

lib/MiLight/MiLightRadio.h

@@ -16,7 +16,7 @@
 #include "AbstractPL1167.h"
 #include <MiLightRadioConfig.h>
 
-#define DEBUG_PRINTF
+// #define DEBUG_PRINTF
 
 #ifndef MILIGHTRADIO_H_
 #define MILIGHTRADIO_H_

lib/MiLight/PL1167_nRF24.h

@@ -12,7 +12,7 @@
 #include "AbstractPL1167.h"
 #include "RF24.h"
 
-#define DEBUG_PRINTF
+// #define DEBUG_PRINTF
 
 #ifndef PL1167_NRF24_H_
 #define PL1167_NRF24_H_

lib/MiLight/RgbCctPacketFormatter.cpp

@@ -47,12 +47,12 @@ void RgbCctPacketFormatter::updateBrightness(uint8_t brightness) {
 }
   
 void RgbCctPacketFormatter::updateHue(uint16_t value) {
-  uint8_t remapped = rescale(value, 255, 360) + 0x5F;
+  uint8_t remapped = rescale(value, 255, 360);
   updateColorRaw(remapped);
 }
 
 void RgbCctPacketFormatter::updateColorRaw(uint8_t value) {
-  command(RGB_CCT_COLOR, value);
+  command(RGB_CCT_COLOR, 0x5F + value);
 }
   
 void RgbCctPacketFormatter::updateTemperature(uint8_t value) {

lib/Udp/MiLightUdpServer.cpp

@@ -1,5 +1,6 @@
 #include <MiLightUdpServer.h>
 #include <V5MiLightUdpServer.h>
+#include <V6MiLightUdpServer.h>
 
 MiLightUdpServer::MiLightUdpServer(MiLightClient*& client, uint16_t port, uint16_t deviceId)
   : client(client), 
@@ -27,24 +28,22 @@ void MiLightUdpServer::handleClient() {
     socket.read(packetBuffer, packetSize);
     
 #ifdef MILIGHT_UDP_DEBUG
-    Serial.print("Handling packet: ");
+    printf("Handling packet: ");
     for (size_t i = 0; i < packetSize; i++) {
-      Serial.printf("%02X ", packetBuffer[0])
+      printf("%02X ", packetBuffer[i]);
     }
-    Serial.println();
+    printf("\n");
 #endif
     
-    size_t responseSize = handlePacket(packetBuffer, packetSize, responseBuffer);
-    
-    if (responseSize > 0) {
-      socket.write(responseBuffer, responseSize);
-    }
+    handlePacket(packetBuffer, packetSize);
   }
 }
 
 MiLightUdpServer* MiLightUdpServer::fromVersion(uint8_t version, MiLightClient*& client, uint16_t port, uint16_t deviceId) {
   if (version == 0 || version == 5) {
     return new V5MiLightUdpServer(client, port, deviceId);
+  } else if (version == 6) {
+    return new V6MiLightUdpServer(client, port, deviceId);
   }
   
   return NULL;

lib/Udp/MiLightUdpServer.h

@@ -8,7 +8,7 @@
 #define MILIGHT_PACKET_BUFFER_SIZE 30 
 
 // Uncomment to enable Serial printing of packets
-//#define MILIGHT_UDP_DEBUG
+#define MILIGHT_UDP_DEBUG
 
 #ifndef _MILIGHT_UDP_SERVER
 #define _MILIGHT_UDP_SERVER 
@@ -34,7 +34,7 @@ protected:
   uint8_t responseBuffer[MILIGHT_PACKET_BUFFER_SIZE];
   
   // Should return size of the response packet
-  virtual size_t handlePacket(uint8_t* packet, size_t packetSize, uint8_t* responseBuffer) = 0;
+  virtual void handlePacket(uint8_t* packet, size_t packetSize) = 0;
 };
 
 #endif

lib/Udp/V5MiLightUdpServer.cpp

@@ -1,14 +1,12 @@
 #include <V5MiLightUdpServer.h>
 
-size_t V5MiLightUdpServer::handlePacket(uint8_t* packet, size_t packetSize, uint8_t* responseBuffer) {
+void V5MiLightUdpServer::handlePacket(uint8_t* packet, size_t packetSize) {
   if (packetSize == 2 || packetSize == 3) {
     handleCommand(packet[0], packet[1]);
   } else {
     Serial.print("V5MilightUdpServer: unexpected packet length. Should always be 2-3, was: ");
     Serial.println(packetSize);  
   }
-  
-  return 0;
 }
 
 void V5MiLightUdpServer::handleCommand(uint8_t command, uint8_t commandArg) {

lib/Udp/V5MiLightUdpServer.h

@@ -52,7 +52,7 @@ public:
   { }
   
   // Should return size of the response packet
-  virtual size_t handlePacket(uint8_t* packet, size_t packetSize, uint8_t* responseBuffer);
+  virtual void handlePacket(uint8_t* packet, size_t packetSize);
     
 protected:
   void handleCommand(uint8_t command, uint8_t commandArg);

lib/Udp/V6MiLightUdpServer.cpp

@@ -1,5 +1,207 @@
 #include <V6MiLightUdpServer.h>
+#include <ESP8266WiFi.h>
+  
+uint8_t V6MiLightUdpServer::START_SESSION_COMMAND[] = {
+  0x20, 0x00, 0x00, 0x00, 0x16, 0x02, 0x62, 0x3A, 0xD5, 0xED, 0xA3, 0x01, 0xAE, 
+  0x08, 0x2D, 0x46, 0x61, 0x41, 0xA7, 0xF6, 0xDC, 0xAF, 0xD3, 0xE6, 0x00, 0x00,
+  0x1E
+};
+
+uint8_t V6MiLightUdpServer::START_SESSION_RESPONSE[] = {
+   0x28, 0x00, 0x00, 0x00, 0x11, 0x00, 0x02, 
+   0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,  // should be replaced with hw addr
+   0x69, 0xF0, 0x3C, 0x23, 0x00, 0x01,
+   0xFF, 0xFF, // should be replaced with a session ID
+   0x00
+};
+
+uint8_t V6MiLightUdpServer::COMMAND_HEADER[] = {
+  0x80, 0x00, 0x00, 0x00
+};
+
+template<typename T, size_t sz>
+size_t size(T(&)[sz]) {
+    return sz;
+}
+
+template <typename T>
+T V6MiLightUdpServer::readInt(uint8_t* packet) {
+  size_t numBytes = sizeof(T);
+  T value = 0;
+  
+  for (size_t i = 0; i < numBytes; i++) {
+    value |= packet[i] << (8 * (numBytes - i - 1));
+  }
+  
+  return value;
+}
+
+template <typename T>
+uint8_t* V6MiLightUdpServer::writeInt(const T& value, uint8_t* packet) {
+  size_t numBytes = sizeof(T);
+  
+  for (size_t i = 0; i < numBytes; i++) {
+    packet[i] = (value >> (8 * (numBytes - i - 1))) & 0xFF;
+  }
+  
+  return packet + numBytes;
+}
+    
+uint16_t V6MiLightUdpServer::beginSession() {
+  const uint16_t id = sessionId++;
+  
+  V6Session session(socket.remoteIP(), socket.remotePort(), id);
+  sessions.push_back(session);
+  return id;
+}
+
+void V6MiLightUdpServer::handleStartSession() {
+  size_t len = size(START_SESSION_RESPONSE);
+  uint8_t response[len];
+  uint16_t sessionId = beginSession();
+  
+  memcpy(response, START_SESSION_RESPONSE, len);
+  WiFi.macAddress(response + 7);
+  response[19] = sessionId >> 8;
+  response[20] = sessionId & 0xFF;
+  
+  sendResponse(sessionId, response, len);
+}
+  
+void V6MiLightUdpServer::sendResponse(uint16_t sessionId, uint8_t* responseBuffer, size_t responseSize) {
+  V6Session* session = NULL;
+  
+  for (size_t i = 0; i < sessions.size(); i++) {
+    if (sessions[i].sessionId == sessionId) {
+      session = &sessions[i];
+    }
+  }
+  
+  if (session == NULL) {
+    Serial.print("Tried to send response to untracked session id: ");
+    Serial.println(sessionId);
+  }
+  
+  Serial.println("Sending a response!");
+  Serial.print("IP: ");
+  Serial.println(session->ipAddr.toString());
+  Serial.print("Port: ");
+  Serial.println(session->port);
+  
+  socket.beginPacket(session->ipAddr, session->port);
+  socket.write(responseBuffer, responseSize);
+  socket.endPacket();
+}
+
+bool V6MiLightUdpServer::handleV1BulbCommand(uint8_t group, uint32_t _cmd, uint32_t _arg) {
+}
+
+bool V6MiLightUdpServer::handleV2BulbCommand(uint8_t group, uint32_t _cmd, uint32_t _arg) {
+  const uint8_t cmd = _cmd & 0xFF;
+  const uint8_t arg = _arg >> 24;
+  
+  client->prepare(MilightRgbCctConfig, deviceId, group);
+  
+  switch (cmd) {
+    case V2_STATUS:
+      if (arg == 0x01) {
+        client->updateStatus(ON);
+      } else if (arg == 0x02) {
+        client->updateStatus(OFF);
+      } else if (arg == 0x05) {
+        client->updateBrightness(0);
+      }
+      break;
+      
+    case V2_COLOR:
+      client->updateColorRaw(arg);
+      break;
+      
+    case V2_KELVIN:
+      client->updateTemperature(arg);
+      break;
+      
+    case V2_BRIGHTNESS:
+      client->updateBrightness(arg);
+      break;
+      
+    case V2_SATURATION:
+      client->updateSaturation(arg);
+      break;
+      
+    default:
+      return false;
+  }
+  
+  return true;
+}
+  
+void V6MiLightUdpServer::handleCommand(
+  uint16_t sessionId,
+  uint8_t sequenceNum,
+  uint8_t* cmd,
+  uint8_t group,
+  uint8_t checksum
+) {
+  
+  uint8_t cmdType = readInt<uint8_t>(cmd);
+  uint32_t cmdHeader = readInt<uint32_t>(cmd+1);
+  uint32_t cmdArg = readInt<uint32_t>(cmd+5);
+  
+#ifdef MILIGHT_UDP_DEBUG
+  printf("Command type: %02X, command: %08X, arg: %08X\n", cmdType, cmdHeader, cmdArg);
+#endif
+  
+  bool handled = false;
+  
+  if ((cmdHeader & 0x0800) == 0x0800) {
+    printf("Yup.\n");
+    handled = handleV2BulbCommand(group, cmdHeader, cmdArg);
+  } else if ((cmdHeader & 0x0700) == 0x0700) {
+    handled = handleV1BulbCommand(group, cmdHeader, cmdArg);
+  }
+  
+  if (handled) {
+    uint8_t* responsePacket = this->responseBuffer;
+    uint8_t* packetStart = responseBuffer;
+    
+    responsePacket = writeInt<uint32_t>(0x88000000, responsePacket);
+    responsePacket = writeInt<uint16_t>(0x0300, responsePacket);
+    responsePacket = writeInt<uint8_t>(sequenceNum, responsePacket);
+    responsePacket = writeInt<uint8_t>(0, responsePacket);
+    
+    sendResponse(sessionId, packetStart, responsePacket - packetStart);
+    
+    return;
+  }
+  
+#ifdef MILIGHT_UDP_DEBUG
+  printf("V6MiLightUdpServer - Unhandled command: ");
+  for (size_t i = 0; i < V6_COMMAND_LEN; i++) {
+    printf("%02X ", cmd[i]);
+  }
+  printf("\n");
+#endif
+}
 
-size_t V6MiLightUdpServer::handlePacket(uint8_t* packet, size_t packetSize, uint8_t* responseBuffer) {
+void V6MiLightUdpServer::handlePacket(uint8_t* packet, size_t packetSize) {
+  printf("Packet size: %d\n", packetSize);
   
+  if (packetSize == size(START_SESSION_COMMAND) && memcmp(START_SESSION_COMMAND, packet, packetSize) == 0) {
+    handleStartSession();
+  } else if (packetSize == 22 && memcmp(COMMAND_HEADER, packet, size(COMMAND_HEADER)) == 0) {
+    uint16_t sessionId = (packet[5] << 8) | packet[6];
+    uint8_t sequenceNum = packet[8];
+    uint8_t* cmd = packet+10;
+    uint8_t group = packet[19];
+    uint8_t checksum = packet[21];
+    
+#ifdef MILIGHT_UDP_DEBUG
+    printf("session: %04X, sequence: %d, group: %d, checksum: %d\n", sessionId, sequenceNum, group, checksum);
+#endif
+    
+    handleCommand(sessionId, sequenceNum, cmd, group, checksum);
+  } else {
+    Serial.println("Unhandled V6 packet");
+  }
 }

lib/Udp/V6MiLightUdpServer.h

@@ -5,20 +5,82 @@
 #include <MiLightClient.h>
 #include <WiFiUdp.h>
 #include <MiLightUdpServer.h>
+#include <Vector.h>
+
+#define V6_COMMAND_LEN 8
 
 #ifndef _V6_MILIGHT_UDP_SERVER
 #define _V6_MILIGHT_UDP_SERVER 
 
+enum V2CommandIds {
+  V2_COLOR = 0x01,
+  V2_SATURATION = 0x02,
+  V2_BRIGHTNESS = 0x03,
+  V2_STATUS = 0x04,
+  V2_KELVIN = 0x05
+};
+
+struct V6Session {
+  V6Session(IPAddress ipAddr, uint16_t port, uint16_t sessionId)
+    : ipAddr(ipAddr),
+      port(port),
+      sessionId(sessionId),
+      next(NULL)
+  { }
+  
+  IPAddress ipAddr;
+  uint16_t port;
+  uint16_t sessionId;
+  V6Session* next;
+};
+
 class V6MiLightUdpServer : public MiLightUdpServer {
 public:
   V6MiLightUdpServer(MiLightClient*& client, uint16_t port, uint16_t deviceId)
-    : MiLightUdpServer(client, port, deviceId)
+    : MiLightUdpServer(client, port, deviceId),
+      sessionId(0)
   { }
   
   // Should return size of the response packet
-  virtual size_t handlePacket(uint8_t* packet, size_t packetSize, uint8_t* responseBuffer);
+  virtual void handlePacket(uint8_t* packet, size_t packetSize);
+  
+  template <typename T>
+  static T readInt(uint8_t* packet);
+  
+  template <typename T>
+  static uint8_t* writeInt(const T& value, uint8_t* packet);
     
 protected:
+  static uint8_t START_SESSION_COMMAND[];
+  static uint8_t START_SESSION_RESPONSE[];
+  static uint8_t COMMAND_HEADER[];
+  
+  Vector<V6Session> sessions;
+  uint16_t sessionId;
+  
+  uint16_t beginSession();
+  void sendResponse(uint16_t sessionId, uint8_t* responseBuffer, size_t responseSize);
+  
+  void handleStartSession();
+  void handleCommand(
+    uint16_t sessionId,
+    uint8_t sequenceNum,
+    uint8_t* cmd,
+    uint8_t group,
+    uint8_t checksum
+  );
+  
+  bool handleV1BulbCommand(
+    uint8_t group,
+    uint32_t cmd,
+    uint32_t cmdArg
+  );
+  
+  bool handleV2BulbCommand(
+    uint8_t group,
+    uint32_t cmd,
+    uint32_t cmdArg
+  );
 };
 
 #endif