Initial commit

SSDP.cpp

@@ -0,0 +1,856 @@
+#include <functional>
+#include <AsyncUDP.h>
+#include "Utils.h"
+#include "ConfigSettings.h"
+#include "SSDP.h"
+
+
+#define SSDP_PORT         1900
+#define SSDP_METHOD_SIZE  10
+#define SSDP_URI_SIZE     2
+#define SSDP_BUFFER_SIZE  64
+#define SSDP_MULTICAST_ADDR 239, 255, 255, 250
+extern ConfigSettings settings;
+
+static const char _ssdp_uuid_template[] PROGMEM = "C2496952-5610-47E6-A968-2FC1%02x%02x%02x%02x";
+static const char _ssdp_serial_number_template[] PROGMEM = "ESP32-%02x%02x%02x";
+static const char _ssdp_usn_root_template[] PROGMEM = "%s::upnp::rootdevice";
+static const char _ssdp_usn_uuid_template[] PROGMEM = "%s::%s";
+static const char _ssdp_response_template[] PROGMEM =
+  "HTTP/1.1 200 OK\r\n"
+  "EXT:\r\n";
+static const char _ssdp_notify_template[] PROGMEM =
+  "NOTIFY * HTTP/1.1\r\n"
+  "HOST: 239.255.255.250:1900\r\n"
+  "NTS: ssdp:alive\r\n";
+static const char _ssdp_bye_template[] PROGMEM =
+  "NOTIFY * HTTP/1.1\r\n"
+  "HOST: 239.255.255.250:1900\r\n"
+  "NTS: ssdp:byebye\r\n";
+static const char _ssdp_packet_template[] PROGMEM =
+  "%s" // _ssdp_response_template / _ssdp_notify_template
+  "CACHE-CONTROL: max-age=%u\r\n" // _interval
+  "SERVER: Arduino/1.0 UPNP/1.1 %s/%s\r\n" // _modelName, _modelNumber
+  "USN: %s\r\n" // _uuid
+  "%s: %s\r\n"  // "NT" or "ST", _deviceType
+  "LOCATION: http://%u.%u.%u.%u:%u/%s\r\n" // WiFi.localIP(), _port, _schemaURL
+  "\r\n";
+static const char _ssdp_device_schema_template[] PROGMEM =
+  "<device>"
+  "<deviceType>%s</deviceType>"
+  "<friendlyName>%s</friendlyName>"
+  "<presentationURL>%s</presentationURL>"
+  "<serialNumber>%s</serialNumber>"
+  "<modelName>%s</modelName>"
+  "<modelNumber>%s</modelNumber>"
+  "<modelURL>%s</modelURL>"
+  "<manufacturer>%s</manufacturer>"
+  "<manufacturerURL>%s</manufacturerURL>"
+  "<modelDescription>Somfy RTS Controller</modelDescription>"
+  "<firmwareVersion>%s</firmwareVersion>"
+  "<UDN>%s</UDN>"
+  "<serviceList></serviceList>"
+  "<deviceList></deviceList>"
+  "</device>";
+static const char _ssdp_schema_template[] PROGMEM =
+  "HTTP/1.1 200 OK\r\n"
+  "Content-Type: text/xml\r\n"
+  "Connection: close\r\n"
+  "Access-Control-Allow-Origin: *\r\n"
+  "\r\n"
+  "<?xml version=\"1.0\"?>"
+  "<root xmlns=\"urn:schemas-upnp-org:device-1-0\">"
+  "<specVersion>"
+  "<major>1</major>"
+  "<minor>0</minor>"
+  "</specVersion>"
+  "<URLBase>http://%s:%u/</URLBase>";    // WiFi.localIP(), _port
+
+
+UPNPDeviceType::UPNPDeviceType() {
+    lastNotified = 0;
+    schemaURL[0] = '\0';
+    uuid[0] = '\0';
+    deviceType[0] = '\0';
+    friendlyName[0] = '\0';
+    serialNumber[0] = '\0';
+    presentationURL[0] = '\0';
+    manufacturer[0] = '\0';
+    manufacturerURL[0] = '\0';
+    modelName[0] = '\0';
+    modelURL[0] = '\0';
+    modelNumber[0] = '\0';  
+    m_usn[0] = '\0';
+    isActive = true;
+}
+UPNPDeviceType::UPNPDeviceType(const char *deviceType) { this->setDeviceType(deviceType); }
+UPNPDeviceType::UPNPDeviceType(const char *deviceType, const char *uuid){ this->setDeviceType(deviceType); this->setUUID(uuid); }
+UPNPDeviceType::UPNPDeviceType(const char *deviceType, const char *uuid, const char*friendlyName) { this->setDeviceType(deviceType); this->setUUID(uuid); this->setName(friendlyName);}
+void UPNPDeviceType::setSchemaURL(const char *url) { strlcpy(schemaURL, url, sizeof(schemaURL)); }
+void UPNPDeviceType::setDeviceType(const char *dtype) { strlcpy(deviceType, dtype, sizeof(deviceType)); }
+void UPNPDeviceType::setUUID(const char *id) { snprintf_P(uuid, sizeof(uuid), PSTR("uuid:%s"), id); }
+void UPNPDeviceType::setName(const char *name) { strlcpy(friendlyName, name, sizeof(friendlyName)); }
+void UPNPDeviceType::setURL(const char *url) { strlcpy(presentationURL, url, sizeof(presentationURL)); }
+void UPNPDeviceType::setSerialNumber(const char *sn) { strlcpy(serialNumber, sn, sizeof(serialNumber)); }
+void UPNPDeviceType::setSerialNumber(const uint32_t sn) { snprintf(serialNumber, sizeof(uint32_t) * 2 + 1, "%08X", sn); }
+void UPNPDeviceType::setModelName(const char *name) { strlcpy(modelName, name, sizeof(modelName)); }
+void UPNPDeviceType::setModelNumber(const char *num) { strlcpy(modelNumber, num, sizeof(modelNumber));}
+void UPNPDeviceType::setModelURL(const char *url) { strlcpy(modelURL, url, sizeof(modelURL)); }
+void UPNPDeviceType::setManufacturer(const char *name) { strlcpy(manufacturer, name, sizeof(manufacturer)); }
+void UPNPDeviceType::setManufacturerURL(const char *url) { strlcpy(manufacturerURL, url, sizeof(manufacturerURL)); }
+//char * UPNPDeviceType::getUSN() { return this->getUSN(this->deviceType); }
+char * UPNPDeviceType::getUSN(const char *st) { 
+  #ifdef DEBUG_SSDP
+  DEBUG_SSDP.print("GETUSN ST: ");
+  DEBUG_SSDP.println(st);
+  DEBUG_SSDP.print("GETUSN UUID: ");
+  DEBUG_SSDP.println(this->uuid);
+  DEBUG_SSDP.print("sizeof(this->m_usn)");
+  DEBUG_SSDP.println(sizeof(this->m_usn));
+  memset(this->m_usn, 0x00, sizeof(this->m_usn));
+  #endif
+  if(strncmp("upnp:root", st, strlen(st)) == 0) {
+    snprintf_P(this->m_usn, sizeof(this->m_usn) - 1, _ssdp_usn_root_template, this->uuid); 
+  }
+  else {
+    snprintf_P(this->m_usn, sizeof(this->m_usn) - 1, _ssdp_usn_uuid_template, this->uuid, st); 
+  }
+  #ifdef DEBUG_SSDP
+  DEBUG_SSDP.print("RESUSN UUID: ");
+  DEBUG_SSDP.println(this->m_usn);
+  #endif
+  return this->m_usn; 
+}
+void UPNPDeviceType::setChipId(uint32_t chipId) {
+  snprintf_P(this->uuid, sizeof(this->uuid), _ssdp_uuid_template,
+    0x40,
+    (uint16_t)((chipId >> 16) & 0xff),
+    (uint16_t)((chipId >> 8) & 0xff),
+    (uint16_t)chipId & 0xff);
+  snprintf_P(this->serialNumber, sizeof(this->serialNumber),
+    _ssdp_serial_number_template,
+    (uint16_t)((chipId >> 16) & 0xff),
+    (uint16_t)((chipId >> 8) & 0xff),
+    (uint16_t)chipId & 0xff);    
+}
+SSDPClass::SSDPClass():sendQueue{NULL} {}
+SSDPClass::~SSDPClass() { end(); }
+bool SSDPClass::begin() { 
+  for(int i = 0; i < SSDP_QUEUE_SIZE; i++) {
+    this->sendQueue[i].waiting = false;
+  }
+  this->end();
+  //assert(NULL == _server);
+  if(_server.connected()) {
+    #ifdef DEBUG_SSDP
+    DEBUG_SSDP.println(PSTR("Already connected to SSDP."));
+    this->isStarted = true;
+    #endif
+    return false;
+  }
+  _server.onPacket([](void * arg, AsyncUDPPacket& packet) { ((SSDPClass*)(arg))->_processRequest(packet); }, this);
+  if(!_server.listenMulticast(IPAddress(SSDP_MULTICAST_ADDR), SSDP_PORT)) {
+    #ifdef DEBUG_SSDP
+    IPAddress mcast(SSDP_MULTICAST_ADDR);
+    DEBUG_SSDP.println(PSTR("Error starting SSDP listener on:"));
+    DEBUG_SSDP.print(mcast);
+    DEBUG_SSDP.print(":");
+    DEBUG_SSDP.println(SSDP_PORT);
+    #endif
+    return false;
+  }
+  this->isStarted = true;
+  this->_sendByeBye();
+  this->_sendNotify();
+  Serial.println("Connected to SSDP"); 
+  return true;
+}
+void SSDPClass::end() { 
+  if(!this->_server || !this->_server.connected()) return; // server isn't connected nothing to do
+  #ifdef DEBUG_SSDP
+  DEBUG_SSDP.printf(PSTR("SSDP end ... "));
+  #endif
+  this->_sendByeBye();
+  //this->_stopTimer();
+  this->_server.close();
+  this->isStarted = false;
+  Serial.println("Disconnected from SSDP");
+}
+UPNPDeviceType* SSDPClass::getDeviceType(uint8_t ndx) { if(ndx < this->m_cdeviceTypes) return &this->deviceTypes[ndx];  return nullptr; }
+UPNPDeviceType* SSDPClass::findDeviceByType(char *devType) { 
+  for(uint8_t i = 0; i < this->m_cdeviceTypes; i++) {
+    if (strcasecmp(devType, this->deviceTypes[i].deviceType) == 0) return &this->deviceTypes[i];
+  }
+  return nullptr;
+}
+UPNPDeviceType* SSDPClass::findDeviceByUUID(char *uuid) {
+  for(uint8_t i = 0; i < this->m_cdeviceTypes; i++) {
+    if (strcasecmp(uuid, this->deviceTypes[i].uuid) == 0) return &this->deviceTypes[i];
+  }
+  return nullptr;
+}
+
+bool SSDPClass::_startsWith(const char *sw, const char *str) { return strncmp(sw, str, strlen(sw)) == 0; }
+void SSDPClass::_parsePacket(ssdp_packet_t *pkt, AsyncUDPPacket &p) {
+  //MSEARCH = "M-SEARCH * HTTP/1.1\nHost: 239.255.255.250:1900\nMan: \"ssdp:discover\"\nST: roku:ecp\n";   
+  memset(pkt, 0x00, sizeof(ssdp_packet_t));
+  pkt->method = NONE; 
+  pkt->type = UNKNOWN;
+  pkt->recvd = millis();
+  pkt->valid = true;
+  char buffer[SSDP_BUFFER_SIZE] = {0};
+  uint8_t pos = 0;
+  while(p.available() > 0) {
+    char c = p.read();
+    if(pkt->method == NONE) {
+      if(c == ' ') {
+        _trim(buffer);
+        pos = 0;
+        if(strcmp(buffer, "M-SEARCH") == 0) pkt->method = SEARCH;
+        else if(strcmp(buffer, "NOTIFY") == 0) pkt->method = NOTIFY;
+      }
+      else {
+        if(pos < SSDP_BUFFER_SIZE - 1) {
+          buffer[pos++] = c;
+          buffer[pos] = '\0';
+        }
+      }
+    }
+    else {
+      if(c == '\r' || c == '\n') {
+        _trim(buffer);
+        if(strcasecmp(buffer, "* HTTP/1.1") != 0) pkt->valid = false;
+        break;
+      }
+      else {
+        if(pos < SSDP_BUFFER_SIZE) {
+          buffer[pos++] = c;
+          buffer[pos] = '\0';
+        }
+      }
+    }
+    if(c == ' ' && pkt->method == NONE) {
+      pkt->valid = false;
+      break; // We didn't get a method we want. 
+    }
+  }
+  enum {KEY, HOST, MAN, ST, MX, AGENT, HEAD, SERVER, NTS, LOCATION, USN, NT} keys;
+  keys = KEY;
+  if(pkt->method == SEARCH) {
+    // Get all the key:value pairs
+    pos = 0;
+    buffer[0] = '\0';
+    while(p.available() > 0) {
+      char c = p.read();
+      //#ifdef DEBUG_SSDP_PACKET
+      //DEBUG_SSDP.print(c);
+      //#endif
+      if(keys == KEY) {      
+        if(c == ':') {
+          _trim(buffer);
+          if(strcasecmp(buffer, "MAN") == 0) keys = MAN;
+          else if(strcasecmp(buffer, "ST") == 0) keys = ST; 
+          else if(strcasecmp(buffer, "MX") == 0) keys = MX;
+          else if(strcasecmp(buffer, "HOST") == 0) keys = HOST;
+          else if(strcasecmp(buffer, "USER-AGENT") == 0) keys = AGENT;
+          else if(strcasecmp(buffer, "SERVER") == 0) keys = SERVER;
+          else if(strcasecmp(buffer, "NTS") == 0) keys = NTS;
+          else if(strcasecmp(buffer, "NT") == 0) keys = NT;
+          else if(strcasecmp(buffer, "LOCATION") == 0) keys = LOCATION;
+          else if(strcasecmp(buffer, "USN") == 0) keys = USN;
+          else keys = HEAD;
+          //#ifdef DEBUG_SSDP_PACKET
+          //DEBUG_SSDP.printf("Found a key: %s ", buffer);
+          //#endif
+          pos = 0;
+          buffer[0] = '\0';
+          
+        }
+        else if(c == '\r' || c == '\n') {
+          // If we find a key that ends before a : then we need to bugger out.
+          keys = KEY;
+          pos = 0;
+          buffer[0] = '\0';
+          //break;
+        }
+        else {
+          if(pos < SSDP_BUFFER_SIZE - 1) {
+            buffer[pos++] = c;
+            buffer[pos] = '\0';
+          }
+        }
+      }
+      else {
+        // We are reading a value
+        if(c == '\r' || c == '\n') {
+          _trim(buffer);
+          #ifdef DEBUG_SSDP_PACKET
+            DEBUG_SSDP.println(buffer);
+          #endif            
+          switch(keys) {
+            case HOST:
+              if(strcasecmp(buffer, "239.255.255.250:1900") == 0) pkt->type = MULTICAST;
+              else pkt->type = UNICAST;
+              strcpy(pkt->host, buffer);
+              break;
+            case MAN:
+              strcpy(pkt->man, buffer);
+              break;
+            case ST:
+              strcpy(pkt->st, buffer);
+              break;
+            case MX:
+              pkt->mx = atoi(buffer);
+              break;
+            case AGENT:
+              strcpy(pkt->agent, buffer);
+              break;
+          }
+          keys = KEY;
+          pos = 0;
+          buffer[0] = '\0';
+        }
+        else {
+          if(pos < SSDP_BUFFER_SIZE - 1) {
+            buffer[pos++] = c;
+            buffer[pos] = '\0';
+          }
+        }
+      }
+    }
+  }
+  if(pkt->valid) {
+    if(pkt->method == NONE) pkt->valid = false;
+    if(pkt->method == SEARCH) {
+      #ifdef DEBUG_SSDP_PACKET
+      //this->_printPacket(pkt);
+      #endif
+      if(strcmp(pkt->man, "ssdp:discover") != 0) pkt->valid = false;
+      if(strlen(pkt->st) == 0) pkt->valid = false;
+      else if(strcmp(pkt->st, "ssdp:all") != 0 && 
+        strcmp(pkt->st, "upnp:rootdevice") != 0 &&
+        !this->_startsWith("uuid:", pkt->st) &&
+        !this->_startsWith("urn:", pkt->st)) 
+        pkt->valid = false;
+      if(pkt->type == UNICAST) {
+        if(strlen(pkt->host) == 0) pkt->valid = false;
+      }
+      else if(pkt->type == MULTICAST) {
+        if(pkt->mx == 0 || isnan(pkt->mx)) pkt->valid = false;
+      }
+    }
+    else if(pkt->method == NOTIFY) {
+      // This is somebody else's notify so we should blow it off.
+    }
+    else {
+      pkt->valid = false;
+    }
+  }
+}
+IPAddress SSDPClass::localIP()
+{
+    // Make sure we don't get a null IPAddress.
+    tcpip_adapter_ip_info_t ip;
+    if (WiFi.getMode() == WIFI_STA) {
+        if (tcpip_adapter_get_ip_info(TCPIP_ADAPTER_IF_STA, &ip)) {
+            return IPAddress();
+        }
+    } else if (WiFi.getMode() == WIFI_OFF) {
+        if (tcpip_adapter_get_ip_info(TCPIP_ADAPTER_IF_ETH, &ip)) {
+            return IPAddress();
+        }
+    }
+    return IPAddress(ip.ip.addr);
+}    
+void SSDPClass::_sendResponse(IPAddress addr, uint16_t port, UPNPDeviceType *d, char *st, bool sendUUID) {
+  char buffer[1460];
+  IPAddress ip = this->localIP();
+  //IPAddress ip = WiFi.localIP();
+  
+  char *pbuff = (char *)malloc(strlen_P(_ssdp_response_template)+1);
+  if(!pbuff) {
+    #ifdef DEBUG_SSDP
+    DEBUG_SSDP.println("Out of memory for SSDP response");
+    #endif
+    return;
+  }
+  strcpy_P(pbuff, _ssdp_response_template);
+
+  #ifdef DEBUG_SSDP
+  //3FFC40B8
+  DEBUG_SSDP.println("Before sprintf ");
+  DEBUG_SSDP.print("CACHE-CONTROL: max-age=");
+  DEBUG_SSDP.println(this->_interval);
+  DEBUG_SSDP.print("SERVER:  Arduino/1.0 UPNP/1.1 ");
+  DEBUG_SSDP.print(d->modelName);
+  DEBUG_SSDP.print("/");
+  DEBUG_SSDP.println(d->modelNumber);
+  DEBUG_SSDP.print("USN: ");
+  DEBUG_SSDP.println(d->getUSN(st));
+  DEBUG_SSDP.print("ST: ");
+  DEBUG_SSDP.println((sendUUID) ? d->uuid : st);
+  DEBUG_SSDP.print("LOCATION: http://");
+  DEBUG_SSDP.print(ip[0]);
+  DEBUG_SSDP.print(ip[1]);
+  DEBUG_SSDP.print(ip[2]);
+  DEBUG_SSDP.print(ip[3]);
+  DEBUG_SSDP.print(":");
+  DEBUG_SSDP.print(this->_port);
+  DEBUG_SSDP.println(d->schemaURL);
+  #endif
+  // Don't use ip.toString as this fragments the heap like no tomorrow.
+  int len = snprintf_P(buffer, sizeof(buffer)-1,
+                       _ssdp_packet_template,
+                       pbuff,
+                       this->_interval,
+                       d->modelName, d->modelNumber,
+                       d->getUSN(st),
+                       "ST", (sendUUID) ? d->uuid : st,
+                       ip[0], ip[1], ip[2], ip[3], this->_port, d->schemaURL);
+  #ifdef DEBUG_SSDP
+  DEBUG_SSDP.println(buffer);
+  #endif
+
+  free(pbuff);
+/*
+static const char _ssdp_packet_template[] PROGMEM =
+  "%s" // _ssdp_response_template / _ssdp_notify_template
+  "CACHE-CONTROL: max-age=%u\r\n" // _interval
+  "SERVER: Arduino/1.0 UPNP/1.1 %s/%s\r\n" // _modelName, _modelNumber
+  "USN: %s\r\n" // _uuid
+  "%s: %s\r\n"  // "NT" or "ST", _deviceType
+  "LOCATION: http://%u.%u.%u.%u:%u/%s\r\n" // WiFi.localIP(), _port, _schemaURL
+  "\r\n";
+*/
+
+                       
+  buffer[sizeof(buffer) - 1] = '\0';
+  #ifdef DEBUG_SSDP
+  DEBUG_SSDP.print("Sending Response to ");
+  DEBUG_SSDP.print(IPAddress(addr));
+  DEBUG_SSDP.print(":");
+  DEBUG_SSDP.println(port);
+  DEBUG_SSDP.println(buffer);
+  #endif
+  
+  _server.writeTo((const uint8_t *)buffer, len, addr, port);
+}
+void SSDPClass::_sendNotify() {
+  for(uint8_t i = 0; i < this->m_cdeviceTypes; i++) {
+    UPNPDeviceType *dev = &this->deviceTypes[i];
+    if(strlen(dev->deviceType) > 0 && dev->isActive) {
+      uint16_t elapsed = (millis() - dev->lastNotified);
+      if(!dev->lastNotified || (elapsed * 4) > (this->_interval * 1000)) {
+        #ifdef DEBUG_SSDP
+        DEBUG_SSDP.print(dev->deviceType);
+        DEBUG_SSDP.print(" Time since last notified: ");
+        DEBUG_SSDP.print(elapsed);
+        DEBUG_SSDP.print("msec ");
+        DEBUG_SSDP.print(this->_interval);
+        DEBUG_SSDP.println("msec");
+        #endif
+        this->_sendNotify(dev, i == 0);
+      }
+      else {
+        #ifdef DEBUG_SSDP
+        DEBUG_SSDP.print(dev->deviceType);
+        DEBUG_SSDP.print(" Time since last notified: ");
+        DEBUG_SSDP.print(elapsed/1000);
+        DEBUG_SSDP.print("msec ");
+        DEBUG_SSDP.print(this->_interval);
+        DEBUG_SSDP.println("msec -- SKIPPING");
+        #endif
+      }
+    }
+  }
+}
+void SSDPClass::_sendNotify(const char *msg) {
+    //_server->append(msg, strlen(msg));
+    //_server->send(IPAddress(SSDP_MULTICAST_ADDR), SSDP_PORT);
+    _server.writeTo((uint8_t *)msg, strlen(msg), IPAddress(SSDP_MULTICAST_ADDR), SSDP_PORT);
+}
+void SSDPClass::_sendNotify(UPNPDeviceType *d) { this->_sendByeBye(d, strcmp(d->deviceType, this->deviceTypes[0].deviceType) == 0); }
+void SSDPClass::_sendNotify(UPNPDeviceType *d, bool root) {
+  char buffer[1460];
+  IPAddress ip = this->localIP();
+  /*
+  "NOTIFY * HTTP/1.1\r\n"
+  "HOST: 239.255.255.250:1900\r\n"
+  "NTS: ssdp:alive\r\n";
+  "CACHE-CONTROL: max-age=%u\r\n" // _interval
+  "SERVER: Arduino/1.0 UPNP/1.1 %s/%s\r\n" // _modelName, _modelNumber
+  "USN: %s\r\n" // _uuid
+  "%s: %s\r\n"  // "NT" or "ST", _deviceType
+  "LOCATION: http://%s:%u/%s\r\n" // WiFi.localIP(), _port, _schemaURL
+  */
+  char *pbuff = (char *)malloc(strlen_P(_ssdp_notify_template)+1);
+  if(!pbuff) {
+    #ifdef DEBUG_SSDP
+    DEBUG_SSDP.println(PSTR("Out of memory for SSDP response"));
+    #endif
+    return;
+  }
+  strcpy_P(pbuff, _ssdp_response_template);
+
+  #ifdef DEBUG_SSDP
+  //3FFC40B8
+  DEBUG_SSDP.print("Before Notify sprintf ");
+  DEBUG_SSDP.print((uintptr_t)d, HEX);
+  DEBUG_SSDP.println(" ");
+  #endif
+  
+  if(root) {
+    // Send 1 for root
+    snprintf_P(buffer, sizeof(buffer),
+                         _ssdp_packet_template,
+                         pbuff,
+                         this->_interval,
+                         d->modelName, d->modelNumber,
+                         d->getUSN("upnp:rootdevice"), // USN
+                         "NT", "upnp:rootdevice",
+                         ip[0], ip[1], ip[2], ip[3], this->_port, d->schemaURL);
+     this->_sendNotify(buffer);
+  }
+  else {
+    // Send 1 for uuid
+    snprintf_P(buffer, sizeof(buffer),
+                         _ssdp_packet_template,
+                         pbuff,
+                         this->_interval,
+                         d->modelName,
+                         d->modelNumber,
+                         d->uuid,
+                         "NT", d->uuid,
+                         ip[0], ip[1], ip[2], ip[3], _port, d->schemaURL);
+     this->_sendNotify(buffer);
+    // Send 1 for deviceType
+    snprintf_P(buffer, sizeof(buffer),
+                         _ssdp_packet_template,
+                         pbuff,
+                         this->_interval,
+                         d->modelName,
+                         d->modelNumber,
+                         d->getUSN(d->deviceType),
+                         "NT", d->deviceType,
+                         ip[0], ip[1], ip[2], ip[3], _port, d->schemaURL);
+     this->_sendNotify(buffer);
+  }
+  d->lastNotified = millis();
+}
+void SSDPClass::setActive(uint8_t ndx, bool isActive) {
+  UPNPDeviceType *d = &this->deviceTypes[ndx];
+  if(!isActive) this->_sendByeBye(d, ndx == 0);
+  else {
+    d->isActive = true;
+    d->lastNotified = 0;
+    this->_sendNotify(d, ndx == 0);
+  }
+}
+void SSDPClass::_sendByeBye() {
+  for(uint8_t i = 0; i < this->m_cdeviceTypes; i++) {
+    UPNPDeviceType *dev = &this->deviceTypes[i];
+    if(strlen(dev->deviceType) > 0) {
+      #ifdef DEBUG_SSDP
+      DEBUG_SSDP.print(dev->deviceType);
+      //DEBUG_SSDP.print(" Time since last notified: ");
+      //DEBUG_SSDP.print(elapsed);
+      //DEBUG_SSDP.print("msec ");
+      DEBUG_SSDP.print(this->_interval);
+      DEBUG_SSDP.println("msec");
+      #endif
+      this->_sendByeBye(dev, i == 0);
+    }
+  }
+}
+void SSDPClass::_sendByeBye(const char *msg) {
+    //_server->append(msg, strlen(msg));
+    //_server->send(IPAddress(SSDP_MULTICAST_ADDR), SSDP_PORT);
+    _server.writeTo((uint8_t *)msg, strlen(msg), IPAddress(SSDP_MULTICAST_ADDR), SSDP_PORT);
+}
+void SSDPClass::_sendByeBye(UPNPDeviceType *d) { this->_sendByeBye(d, strcmp(d->deviceType, this->deviceTypes[0].deviceType) == 0); }
+void SSDPClass::_sendByeBye(UPNPDeviceType *d, bool root) {
+  char buffer[1460];
+  IPAddress ip = this->localIP();
+  /*
+  "NOTIFY * HTTP/1.1\r\n"
+  "HOST: 239.255.255.250:1900\r\n"
+  "NTS: ssdp:byebye\r\n";
+  "CACHE-CONTROL: max-age=%u\r\n" // _interval
+  "SERVER: Arduino/1.0 UPNP/1.1 %s/%s\r\n" // _modelName, _modelNumber
+  "USN: %s\r\n" // _uuid
+  "%s: %s\r\n"  // "NT" or "ST", _deviceType
+  "LOCATION: http://%s:%u/%s\r\n" // WiFi.localIP(), _port, _schemaURL
+  */
+  if(root) {
+    // Send 1 for root
+    snprintf_P(buffer, sizeof(buffer)-1,
+                         _ssdp_packet_template,
+                         _ssdp_notify_template,
+                         this->_interval,
+                         d->modelName, d->modelNumber,
+                         d->getUSN("upnp:rootdevice"), // USN
+                         "NT", "upnp:rootdevice",
+                         ip[0], ip[1], ip[2], ip[3], this->_port, d->schemaURL);
+     this->_sendNotify(buffer);
+  }
+  else {
+    // Send 1 for uuid
+    snprintf_P(buffer, sizeof(buffer)-1,
+                         _ssdp_packet_template,
+                         _ssdp_notify_template,
+                         this->_interval,
+                         d->modelName,
+                         d->modelNumber,
+                         d->uuid,
+                         "NT", d->uuid,
+                         ip[0], ip[1], ip[2], ip[3], this->_port, d->schemaURL);
+     this->_sendNotify(buffer);
+    // Send 1 for deviceType
+    snprintf_P(buffer, sizeof(buffer)-1,
+                         _ssdp_packet_template,
+                         _ssdp_notify_template,
+                         this->_interval,
+                         d->modelName,
+                         d->modelNumber,
+                         d->getUSN(d->deviceType),
+                         "NT", d->deviceType,
+                         ip[0], ip[1], ip[2], ip[3], this->_port, d->schemaURL);
+     this->_sendNotify(buffer);
+  }
+  d->isActive = false;
+}
+void SSDPClass::_addToSendQueue(IPAddress addr, uint16_t port, UPNPDeviceType *d, char *st, uint8_t sec, bool sendUUID) {
+  /*
+  typedef struct ssdp_response_t {
+    IPAddress address;
+    uint16_t port;
+    UPNPDeviceType *dev;
+    bool sendUUID;
+    unsigned long sendTime;
+    char st[SSDP_DEVICE_TYPE_SIZE];
+  };
+  */
+  for(uint8_t i = 0; i < SSDP_QUEUE_SIZE; i++) {
+    if(this->sendQueue[i].waiting) {
+      // Check to see if this is a reply to the same place.
+      ssdp_response_t *q = &this->sendQueue[i];
+      if(q->address == addr && q->port == port && q->sendUUID == sendUUID) {
+        #ifdef DEBUG_SSDP
+        DEBUG_SSDP.printf("There is already a response to this query in slot #u\n", i);
+        #endif
+        return;
+      }
+    }
+  }
+  // Find an empty queue slot and fill it.
+  for(uint8_t i = 0; i < SSDP_QUEUE_SIZE; i++) {
+    if(!this->sendQueue[i].waiting) {
+        #ifdef DEBUG_SSDP
+        DEBUG_SSDP.printf("Queueing SSDP response in slot %u\n", i);
+        DEBUG_SSDP.println("*********************************");
+        #endif
+        ssdp_response_t *q = &this->sendQueue[i];
+        q->dev = d;
+        q->sendTime = millis() + (random(0, sec - 1) * 1000L);
+        q->address = addr;
+        q->port = _port;
+        q->sendUUID = sendUUID;
+        strlcpy(q->st, st, sizeof(ssdp_response_t::st)-1);
+        q->waiting = true;
+        return;
+    }
+  }
+  // If we made it here then there were was not space available on the queue.
+  #ifdef DEBUG_SSDP
+  DEBUG_SSDP.println("The SSDP response queue was full.  Dropping request");
+  #endif
+}
+void SSDPClass::_sendQueuedResponses() {
+  for(uint8_t i = 0; i < SSDP_QUEUE_SIZE; i++) {
+    if(this->sendQueue[i].waiting) {
+      ssdp_response_t *q = &this->sendQueue[i];
+      if(q->sendTime < millis()) {
+          // Send the response and delete the pointer.
+          #ifdef DEBUG_SSDP
+            DEBUG_SSDP.print("Sending SSDP queued response ");
+            DEBUG_SSDP.println(i);
+          #endif
+          this->_sendResponse(q->address, q->port, q->dev, q->st, q->sendUUID);
+          q->waiting = false;
+          return;
+      }
+    }
+  }
+}
+void SSDPClass::_printPacket(ssdp_packet_t *pkt) {
+  Serial.printf("Rec: %d\n", pkt->recvd);
+  switch(pkt->method) {
+    case NONE:
+      Serial.println("Method: NONE");
+      break;
+    case SEARCH:
+      Serial.println("Method: SEARCH");
+      break;
+    case NOTIFY:
+      Serial.println("Method: NOTIFY");
+      break;
+    default:
+      Serial.println("Method: UNKOWN");
+      break;
+  }
+  Serial.printf("ST: %s\n", pkt->st);
+  Serial.printf("MAN: %s\n", pkt->man);
+  Serial.printf("AGENT: %s\n", pkt->agent);
+  Serial.printf("HOST: %s\n", pkt->host);
+  Serial.printf("MX: %d\n", pkt->mx);
+  Serial.printf("type: %d\n", pkt->type);
+  Serial.printf("valid: %d\n", pkt->valid);
+}
+void SSDPClass::_processRequest(AsyncUDPPacket &p) {
+  // This pending BS should probably be for unicast request only but we will play along for now.
+  if(p.length() == 0) {
+    //this->_sendQueuedResponses();
+    return;
+  }
+  ssdp_packet_t pkt;
+  this->_parsePacket(&pkt, p);
+  if(pkt.valid && pkt.method == SEARCH) {
+    // Check to see if we have anything to respond to from this packet.
+    if(strcmp("ssdp:all", pkt.st) == 0) {
+      for(uint8_t i = 0; i < this->m_cdeviceTypes; i++) {
+        if(strlen(this->deviceTypes[i].deviceType) > 0)
+          this->_addToSendQueue(p.remoteIP(), p.remotePort(), &this->deviceTypes[i], this->deviceTypes[i].deviceType, pkt.mx, false);
+      }
+    }
+    else if(strcmp("upnp:rootdevice", pkt.st) == 0) {
+      UPNPDeviceType *dev = &this->deviceTypes[0];
+      #ifdef DEBUG_SSDP
+      this->_printPacket(&pkt);
+      DEBUG_SSDP.println("---------------   ROOT   ---------------------");
+      #endif
+      if(pkt.method == MULTICAST) 
+        this->_addToSendQueue(IPAddress(SSDP_MULTICAST_ADDR), SSDP_PORT, dev, pkt.st, pkt.mx, false);
+      else 
+        this->_sendResponse(p.remoteIP(), p.remotePort(), dev, pkt.st, false);
+    }
+    else {
+      UPNPDeviceType *dev;
+      bool useUUID = false;
+      if(this->_startsWith("uuid:", pkt.st)) {
+        dev = this->findDeviceByUUID(pkt.st);
+        useUUID = true;
+      }
+      else if(this->_startsWith("urn:", pkt.st)) { dev = this->findDeviceByType(pkt.st); }
+      if(dev) {
+        #ifdef DEBUG_SSDP
+        this->_printPacket(&pkt);
+        DEBUG_SSDP.println("--------------   ACCEPT   --------------------");
+        #endif
+        if(pkt.method == MULTICAST)
+          this->_addToSendQueue(IPAddress(SSDP_MULTICAST_ADDR), SSDP_PORT, dev, pkt.st, pkt.mx, useUUID);
+        else {
+          this->_sendResponse(p.remoteIP(), p.remotePort(), dev, pkt.st, useUUID);
+        }
+      }
+    }
+  }
+  else if(pkt.method == SEARCH) {
+  }
+}
+void SSDPClass::loop() {
+  this->_sendQueuedResponses();
+}
+void SSDPClass::schema(Print &client) {
+  IPAddress ip = WiFi.localIP();
+  uint8_t devCount = 0;
+  for(uint8_t i = 0; i < this->m_cdeviceTypes; i++) {
+    if(this->deviceTypes[i].deviceType && strlen(this->deviceTypes[i].deviceType) > 0) devCount++;
+  }
+  char schema_template[strlen_P(_ssdp_schema_template)+1];
+  char device_template[strlen_P(_ssdp_device_schema_template)+1];
+  strcpy_P(schema_template, _ssdp_schema_template);
+  strcpy_P(device_template, _ssdp_device_schema_template);
+  char buff[sizeof(device_template) + sizeof(UPNPDeviceType)];
+  buff[0] = '\0';
+  client.printf(schema_template,
+    ip.toString().c_str(), _port);
+  UPNPDeviceType *r = &this->deviceTypes[0];
+  sprintf(buff, device_template,
+        r->deviceType,
+        r->friendlyName,
+        r->presentationURL,
+        r->serialNumber,
+        r->modelName,
+        r->modelNumber,
+        r->modelURL,
+        r->manufacturer,
+        r->manufacturerURL,
+        settings.fwVersion,
+        r->uuid );
+  char *devList = strstr(buff, "</device>") - strlen("</deviceList>");
+  devList[0] = '\0';
+  client.print(buff);
+  for(uint8_t i = 1; i < this->m_cdeviceTypes; i++) {
+    UPNPDeviceType *dev = &this->deviceTypes[i];
+    if(strlen(dev->deviceType) > 0) {
+        //Serial.print(devList);
+        client.printf(device_template,
+          dev->deviceType,
+          dev->friendlyName,
+          dev->presentationURL,
+          dev->serialNumber,
+          dev->modelName,
+          dev->modelNumber,
+          dev->modelURL,
+          dev->manufacturer,
+          dev->manufacturerURL,
+          settings.fwVersion,
+          dev->uuid);
+    }
+  }
+  client.print(F("</deviceList></device></root>\r\n"));
+  #ifdef DEBUG_SSDP
+  DEBUG_SSDP.println("Sending Device.xml");
+  #endif
+}
+/*
+void SSDPClass::setDeviceType(uint8_t ndx, UPNPDeviceType *dt) {
+  this->setDeviceType(ndx, dt->deviceType);
+  this->setName(ndx, dt->friendlyName);
+  this->setURL(ndx, dt->presentationURL);
+  this->setSerialNumber(ndx, dt->serialNumber);
+  this->setUUID(ndx, dt->uuid);
+  this->setModelName(ndx, dt->modelName);
+  this->setModelNumber(ndx, dt->modelNumber);
+  this->setManufacturer(ndx, dt->manufacturer);
+  this->setManufacturerURL(ndx, dt->manufacturerURL);
+}
+*/
+void SSDPClass::setTTL(const uint8_t ttl) { _ttl = ttl; }
+void SSDPClass::setHTTPPort(uint16_t port) { _port = port; }
+void SSDPClass::setInterval(uint32_t interval) { _interval = interval; }
+void SSDPClass::setDeviceType(uint8_t ndx, const char *type) { this->deviceTypes[ndx].setDeviceType(type); }
+void SSDPClass::setSchemaURL(uint8_t ndx, const char *url) {  this->deviceTypes[ndx].setSchemaURL(url); }
+void SSDPClass::setUUID(uint8_t ndx, const char *uuid) { this->deviceTypes[ndx].setUUID(uuid); }
+void SSDPClass::setUUID(uint8_t ndx, const char *prefix, uint32_t id) { 
+    char svcuuid[50];
+    sprintf(svcuuid, "%s%02x%02x%02x",
+      prefix,
+      (uint16_t)((id >> 16) & 0xff),
+      (uint16_t)((id >> 8) & 0xff),
+      (uint16_t)id & 0xff);
+    this->setUUID(ndx, svcuuid);
+}
+void SSDPClass::setName(uint8_t ndx, const char *name) { this->deviceTypes[ndx].setName(name); }
+void SSDPClass::setURL(uint8_t ndx, const char *url) { this->deviceTypes[ndx].setURL(url); }
+void SSDPClass::setSerialNumber(uint8_t ndx, const char *serialNumber) { this->deviceTypes[ndx].setSerialNumber(serialNumber); }
+//void SSDPClass::setSerialNumber(uint8_t ndx, const uint32_t serialNumber) { this->deviceTypes[ndx].setSerialNumber(serialNumber); }
+void SSDPClass::setModelName(uint8_t ndx, const char *name) { this->deviceTypes[ndx].setModelName(name); }
+void SSDPClass::setModelNumber(uint8_t ndx, const char *num) { this->deviceTypes[ndx].setModelNumber(num); }
+void SSDPClass::setModelURL(uint8_t ndx, const char *url) { this->deviceTypes[ndx].setModelURL(url); }
+void SSDPClass::setManufacturer(uint8_t ndx, const char *name) { this->deviceTypes[ndx].setManufacturer(name); }
+void SSDPClass::setManufacturerURL(uint8_t ndx, const char *url) { this->deviceTypes[ndx].setManufacturerURL(url); }
+void SSDPClass::setChipId(uint8_t ndx, uint32_t chipId) { this->deviceTypes[ndx].setChipId(chipId); }
+#if !defined(NO_GLOBAL_INSTANCES) && !defined(NO_GLOBAL_SSDP)
+SSDPClass SSDP;
+#endif