commit 925ed68df79937669042c2e5809cf2cbdc5a3258 Author: Simon Maddocks Date: Thu Mar 12 21:32:15 2020 +0000 Add existing code diff --git a/Client/SSDP_Discover.cpp b/Client/SSDP_Discover.cpp new file mode 100644 index 0000000..dce6563 --- /dev/null +++ b/Client/SSDP_Discover.cpp @@ -0,0 +1,144 @@ +#include "SSDP_discover.h" +#include + +#ifdef _DEBUG +#include +#define _DEBUG_PRINT(x) utils::debug_print(x) +#else +#define _DEBUG_PRINT(x) +#endif + +namespace Control_System +{ + +SSDP_discover::SSDP_discover() : discoveredServices(new std::vector()), server(new WiFiUDP()) {} + +SSDP_discover::~SSDP_discover() +{ + server->flush(); + yield(); + server->stopAll(); +} + +bool SSDP_discover::discover() +{ + auto success = false; + + for (size_t i = 0; i < SEARCH_RETRY && success == false; i++) + { + _DEBUG_PRINT("Sending M-SEARCH"); + _DEBUG_PRINT(IPAddress(MULTICAST_ADDRESS).toString()); + auto mcast1 = server->beginPacketMulticast(IPAddress(MULTICAST_ADDRESS), SSDP_PORT, WiFi.localIP()); + if (mcast1 != 1) + { + _DEBUG_PRINT("begin packet failed"); + continue; + } + + auto write = server->write(SSDP_MSEARCH, strnlen(SSDP_MSEARCH, 500)); + if (write <= 0) + { + _DEBUG_PRINT("write failed"); + continue; + } + + auto end = server->endPacket(); + if (end != 1) + { + _DEBUG_PRINT("end failed"); + continue; + } + + await_response(); + + _DEBUG_PRINT("done awaiting"); + + if (discoveredServices->empty()) + { + _DEBUG_PRINT("No services found"); + continue; + } + + for (auto &c : *discoveredServices) + { + _DEBUG_PRINT(c.toString()); + } + + success = true; + } + + return success; +} + +void SSDP_discover::await_response() +{ + server->begin(SSDP_PORT); + byte buffer[INPUT_BUFFER_SIZE + 1] = {0}; + size_t bufferContentLength = 0; + _DEBUG_PRINT("UDP Await"); + + //Count down from delay time to 0, assume this takes ~1ms + for (auto delaytime = SEARCH_TIMEOUT * 1000; delaytime > 0; delaytime--) + { + auto correctService = false; + auto location = IPAddress(); + + // Process a packet + while (server->available() > 0) + { + bufferContentLength = server->readBytesUntil('\n', buffer, INPUT_BUFFER_SIZE); + buffer[bufferContentLength] = '\0'; + _DEBUG_PRINT((char *)buffer); + + // strlen, but they're contexpr, so its ok?... + if (bufferContentLength >= strlen(SERVICE) && + memcmp(SERVICE, buffer, strlen(SERVICE)) == 0) + { + _DEBUG_PRINT(F("Service line")); + + if (strstr(reinterpret_cast(buffer), SERVICE_NAME) != nullptr) + { + _DEBUG_PRINT("Service valid"); + correctService = true; + } + } + // + 2 for ": " + else if (bufferContentLength >= strlen(LOCATION) + 2 && + memcmp(LOCATION, buffer, strlen(LOCATION)) == 0) + { + _DEBUG_PRINT(F("Location Line")); + + auto offset = buffer + (strlen(LOCATION) + 2); + char ipBuffer[IP4ADDR_STRLEN_MAX + 1] = {0}; + + strncpy(ipBuffer, reinterpret_cast(offset), IP4ADDR_STRLEN_MAX); + + _DEBUG_PRINT((char *)offset); + if (IPAddress::isValid(ipBuffer)) + { + location.fromString(ipBuffer); + } + else + { + _DEBUG_PRINT("Resolve hostname"); + // If the ip was invalid, maybe its a hostname? + if (WiFi.hostByName(reinterpret_cast(offset), location) != 1) + { + // Nope it was just invalid + _DEBUG_PRINT("Invalid Service"); + } + } + } + } + + if (location.isSet() && correctService) + { + _DEBUG_PRINT(F("Add as valid response")); + discoveredServices->push_back(location); + } + + delay(1); + } + server->stop(); +} +} // namespace Control_System \ No newline at end of file diff --git a/Client/SSDP_discover.h b/Client/SSDP_discover.h new file mode 100644 index 0000000..8845ee6 --- /dev/null +++ b/Client/SSDP_discover.h @@ -0,0 +1,45 @@ +#pragma once +#include +#include +#include + +#include + +namespace Control_System +{ +class SSDP_discover +{ +public: + std::unique_ptr> discoveredServices; + + SSDP_discover(); + ~SSDP_discover(); + bool discover(); + +private: + static constexpr const char *SSDP_MSEARCH = + "M-SEARCH * HTTP/1.1\r\n" + "HOST: 239.255.255.250:1900\r\n" + "MAN: \"ssdp:discover\"\r\n" + "MX: 3\r\n" + "ST: urn:control-system-custom_sm:service:control-server:" __CONTROL_VERSION__ "\r\n" + "USER-AGENT: ESP_ARDUINO/" __VERSION__ " UPnP/1.1 CONTROL_SERVER/" __CONTROL_VERSION__ "\r\n\r\n"; + + static constexpr const uint32_t MULTICAST_ADDRESS = 0xFAFFFFEF; + static constexpr const uint16_t SSDP_PORT = 1900; + static constexpr const uint8_t SSDP_TTL = 3; + + static constexpr const uint8_t SEARCH_TIMEOUT = 4; + static constexpr const uint8_t SEARCH_RETRY = 1; + + static constexpr const uint8_t INPUT_BUFFER_SIZE = 150; + + static constexpr const char* LOCATION = "LOCATION"; + static constexpr const char* SERVICE = "ST"; + static constexpr const char* SERVICE_NAME = "urn:control-system-custom_sm:service:control-server"; + + const std::unique_ptr server; + + void await_response(); +}; +} // namespace Control_System diff --git a/Server/SSDP_Response.py b/Server/SSDP_Response.py new file mode 100644 index 0000000..60c63ce --- /dev/null +++ b/Server/SSDP_Response.py @@ -0,0 +1,147 @@ +import socket +import struct +import threading +import queue +import time +import datetime +import uuid +import random + +class ssdp_response(): + + SSDP_GROUP = '239.255.255.250' + SSDP_PORT = 1900 + + + TEMPLATE ="""HTTP/1.1 200 OK +CACHE-CONTROL: max-age={} +DATE: {} +EXT: +LOCATION: {} +SERVER: {} +ST: {} +USN: {} +BOOTID.UPNP.ORG: {} + +""" + MAX_AGE = 500 + LOCATION = "" + SERVER = "PYTHON/3 UPnP/1.1 CONTROL_SERVER/1.0.0" + USN = uuid.uuid4() + BOOTID = 0 + + + queues = {} + queues_lock = threading.Lock() + + requests = queue.Queue() + + filters = {"HEAD":"M-SEARCH"} + + def __init__(self, filter, location=None): + self.filters.update(filter) + if not location: + self.LOCATION = socket.gethostbyname_ex(socket.getfqdn())[2] + else: + self.LOCATION = location + + def start_listener(self): + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind(("", self.SSDP_PORT)) + + multicast_request = struct.pack("4sl", socket.inet_aton(self.SSDP_GROUP), socket.INADDR_ANY) + sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, multicast_request) + #start listen and respond thread + + while True: + try: + data,addr = sock.recvfrom(1024) + if addr in self.queues: + self.queues[addr].put(data) + else: + q = queue.Queue() + q.put(data) + self.queues_lock.acquire() + self.queues[addr] = q + self.queues_lock.release() + except Exception as e: + print(e) + print("recv error") + + def filter(self, request): + ret = True + for k in self.filters.keys(): + val = request.get(k) + if not val: + ret = False + break + else: + if self.filters[k] not in val: + ret = False + break + return ret + + def parse_request(self): + requests_buffer = {} + while True: + time.sleep(1) + self.queues_lock.acquire() + timeouts = [] + for k, v in self.queues.items(): + try: + data = v.get(timeout=5) + string = data.decode() + lines = string.splitlines() + for line in lines: + val = line.split(':', 1) + if len(val) == 1: + if "M-SEARCH" in val[0]: + requests_buffer[k] = {} + requests_buffer[k]["HEAD"] = val[0] + else: + requests_buffer[k]["CLIENT"] = k[0] + if self.filter(requests_buffer[k]): + print(requests_buffer[k]) + self.requests.put(requests_buffer.pop(k)) + else: + requests_buffer[k][val[0]] = val[1] + except Exception as e: + print(e) + print("no data") + timeouts.append(k) + for k in timeouts: + del self.queues[k] + self.queues_lock.release() + + def construct_response(self, request): + response = self.TEMPLATE.format( + self.MAX_AGE, + datetime.datetime.now().replace(microsecond=0).isoformat(), + self.LOCATION, + self.SERVER, + request.get("ST"), + self.USN, + self.BOOTID) + self.BOOTID = self.BOOTID + 1 + return response + + def send_response(self): + while True: + request = self.requests.get() + time.sleep(random.randrange(int(request.get("MX")))) + response = self.construct_response(request) + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + address = (request.get("CLIENT"), self.SSDP_PORT) + print(response) + sock.connect(address) + sock.sendall(response.encode()) + sock.close(); + + +s = ssdp_response({"ST":"service:control-server"}, "192.168.0.184") +t = threading.Thread(target=s.start_listener) +t.start() +t2 = threading.Thread(target=s.parse_request) +t2.start() +s.send_response() diff --git a/Server/ssdp_responder.rc b/Server/ssdp_responder.rc new file mode 100644 index 0000000..fa5519a --- /dev/null +++ b/Server/ssdp_responder.rc @@ -0,0 +1,18 @@ +#!/sbin/openrc-run +name="ssdp-responder" +command="python3" +command_args="/bin/control_system/Home_Control_Flask/SSDP_Response.py" +command_background=true +pidfile="/run/${RC_SVCNAME}.pid" +extra_started_commands="reload" +error_log="/var/log/ssdp-responder.err" + +depend(){ + need net +} + +reload() { + ebegin "Reloading ${RC_SVCNAME}" + start-stop-daemon --signal HUP --pidfile "${pidfile}" + eend $? +} \ No newline at end of file