File content as of revision 7:1e8c677e3d28:

//
// SSDP Server 
//
// This is an SSDP server. It relies on a web server running on this same node.
//
// 
#include "SSDP.h"
#include "EthernetInterface.h"
#include "SW_String.h"

#define DEBUG "SSDP"      //Debug is disabled by default

#include <cstdio>
#if (defined(DEBUG) && !defined(TARGET_LPC11U24))
#define DBG(x, ...)  std::printf("[DBG %s %4d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
#define WARN(x, ...) std::printf("[WRN %s %4d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
#define ERR(x, ...)  std::printf("[ERR %s %4d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
#define INFO(x, ...) std::printf("[INF %s %4d] "x"\r\n", DEBUG, __LINE__, ##__VA_ARGS__);
#else
#define DBG(x, ...)
#define WARN(x, ...)
#define ERR(x, ...)
#define INFO(x, ...)
#endif
extern void ShowSignOfLife(int which);
static const char* MCAST_GRP = "239.255.255.250";
static const int MCAST_PORT = 1900;
static Thread * pThr;

// sprintf(buffer, SSDP_HTTP, "myIPString", myPort, "myIdentity", "myIdentity");
// Requires IP address as a string
static const char * SSDP_HTTP = 
    "HTTP/1.1 200 OK\r\n"
    "CACHE-CONTROL: max-age=1800\r\n"
    "DATE: Mon, 22 Jun 2015 17:24:01 GMT\r\n"
    "EXT:\r\n"
    "LOCATION: http://%s:%d/setup.xml\r\n"                      // "my.ip.string", portNum
    "OPT: \"http://schemas.upnp.org/upnp/1/0/\"; ns=01\r\n"
    "01-NLS: %s\r\n"                                            // "Unique Identity"
    "SERVER: Smartware, UPnP/1.0, Smartware\r\n"
    "ST: upnp:rootdevice\r\n"
    "USN: uuid:Node-1_0-%s::upnp:rootdevice\r\n"                // "Unique Identity"
    "X-User-Agent: Smartware\r\n"
    "\r\n";

// Addr:    "###.###.###.###"                   [15]
// Port:    12345                               [5]
// Ident:   "#########0#########0#########0"    [30] x 2
//
#define SSDP_HTTP_OVERHEAD 50   // Number of bytes to fill in the information


// sprintf(buffer, SSDP_NOTIFY, myIPasString, myPort);
// Requires IP address as a string
static const char * SSDP_NOTIFY = 
    "NOTIFY * HTTP/1.1\r\n"
    "HOST: 239.255.255.250:1900\r\n"
    "CACHE-CONTROL: max-age=1800\r\n"
    "LOCATION: http://%s:%u/setup.xml\r\n"
    "NTS: ssdp:alive\r\n\r\n"
    "";

// Addr:    "###.###.###.###"                   [15]
// Port:    12345                               [5]
//
#define SSDP_NOTIFY_OVERHEAD 25   // Number of bytes to fill in the information (+5)


// The SSDP listener thread
static void SSDPListener(void const * args) {
    UDPSocket server;
    SSDP_Config_T * cfg = (SSDP_Config_T *)args;
    
    server.bind(MCAST_PORT);
    if (server.join_multicast_group(MCAST_GRP) != 0) {
        ERR("Error joining the multicast group");
        while (true) {}
    }
    server.set_blocking(false, 50); // non-blocking with 50ms timeout
    Endpoint client;
    char buffer[256];
    while (true) {
        //INFO("Wait for packet...");
        ShowSignOfLife(2);
        int n = server.receiveFrom(client, buffer, sizeof(buffer)-1);
        if (n > 0) {
            char * p = buffer;
            volatile int delay = 0;
            uint8_t mask = 0x00;    // fragments we found in the received packet
            
            buffer[n] = '\0';
            INFO("SSDP receiveFrom %d bytes from %s:%d", n, 
                client.get_address(), client.get_port());
            INFO("SSDP\n%s", buffer);
            while (*p) {
                char * e = strstr(p, "\r\n");
                if (e && (e - buffer) < n) {
                    *e = '\0';
                    if (sw_stristr(p, "M-SEARCH * HTTP/1.1")) {
                        mask |= 0x01;                               // M-SEARCH * HTTP/1.1
                    } else if (sw_stristr(p, "MAN:") && sw_stristr(p,"\"ssdp:discover\"")) {
                        mask |= 0x02;                               // MAN: "ssdp:discover"
                    } else if (sw_stristr(p, "MX:")) {
                        mask |= 0x04;                               // MX: \d
                        delay = atoi(p + 3);
                    } else if (sw_stristr(p, "ST:") && sw_stristr(p, "upnp:rootdevice")) {
                        mask |= 0x08;
                    } else if (sw_stristr(p, "ST:") && sw_stristr(p, "ssdp:all")) {
                        mask |= 0x08;
                    } else if (sw_stristr(p, "HOST: ")) {
                        mask |= 0x10;                               // HOST: 239.255.255.250:49152
                        char * pColon = strchr(p+6, ':');
                        if (pColon) {
                            pColon = '\0';
                        }
                    }
                    p = e + 1;
                }
                p++;
            }
            INFO("     ***** %02X", mask);
            if ((mask & 0x1F) == 0x1F) {
                char * out_buffer = (char *)malloc(strlen(SSDP_HTTP) + SSDP_HTTP_OVERHEAD);
                
                if (out_buffer) {
                    sprintf(out_buffer, SSDP_HTTP, cfg->ipAddr, cfg->port, cfg->ident, cfg->ident);
                    // It would be polite to delay, but the recommendation is from 1 to 2 seconds.
                    // Send the response twice, to improve reliability of node discovery
                    for (int i=0; i<1; i++) {
                        INFO("SSDPListener: reply >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
                        int i = server.sendTo(client, out_buffer, strlen(out_buffer));
                        INFO("  sendTo %3d: reply <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<", i);
                    }
                    free(out_buffer);
                    INFO("SSDPListener: stack-used: %d, total: %d", pThr->max_stack(), pThr->stack_size());
                } else {
                    ERR("Can't get memory for response");
                }
            }
        }
    }
}

SSDP::SSDP(const char * name, const char * ident, const char * ipAddr, int port) {
    pThr = NULL;
    _config.name = NULL;
    SetFriendlyName(name);
    _config.ident = NULL;
    SetFriendlyName(ident);
    _config.ipAddr = NULL;
    SetIPAddress(ipAddr);
    _config.port = port;
    StartListener();
    INFO("SSDP(......) constructor done. Listener Started.");
    SendNotify();
}

SSDP::SSDP(const SSDP_Config_T * config) {
    pThr = NULL;
    memcpy(&_config, config, sizeof(SSDP_Config_T));
    StartListener();
    INFO("SSDP(.) constructor done. Listener Started.");
    SendNotify();
}

SSDP::~SSDP() {
    if (pThr)
        pThr->terminate();
    pThr = NULL;
    DelFriendlyName();
    DelIdentity();
    DelIPAddress();
}

void SSDP::SendNotify() {
    char * out_buffer = (char *)malloc(strlen(SSDP_NOTIFY) + SSDP_NOTIFY_OVERHEAD);
    if (out_buffer) {
        UDPSocket sock;
        Endpoint broadcast;
        int i;
        
        i = sock.init();
        printf(" %d = sock.init()\n", i);
        i = sock.set_broadcasting();
        printf(" %d = sock.set_broadcasting()\n", i);
        i = broadcast.set_address(MCAST_GRP, MCAST_PORT);
        printf(" %d = sock.set_address(%s,%d)\n", i, MCAST_GRP, MCAST_PORT);
        sprintf(out_buffer, SSDP_NOTIFY, _config.ipAddr, _config.port);
        printf("SendNotify:\n%s\n", out_buffer);
        i = sock.sendTo(broadcast, out_buffer, strlen(out_buffer));
        printf(" %d = sendTo(%s, ..., %d)\n", i, broadcast.get_address(), strlen(out_buffer));
        free(out_buffer);
    }
}

bool SSDP::SetFriendlyName(const char * name) {
    DelFriendlyName();
    _config.name = (char *)malloc(strlen(name) + 1);
    if (_config.name) {
        strcpy(_config.name, name);
        return true;
    } else {
        return false;
    }
}

void SSDP::DelFriendlyName() {
    if (_config.name)
        free(_config.name);
    _config.name = NULL;
}

bool SSDP::SetIdentity(const char * ident) {
    DelIdentity();
    _config.ident = (char *)malloc(strlen(ident) + 1);
    if (_config.ident) {
        strcpy(_config.ident, ident);
        return true;
    } else {
        return false;
    }
}

void SSDP::DelIdentity() {
    if (_config.ident)
        free(_config.ident);
    _config.ident = NULL;
}

bool SSDP::SetIPAddress(const char * ipAddr) {
    DelIPAddress();
    _config.ipAddr = (char *)malloc(strlen(ipAddr) + 1);
    if (_config.ipAddr) {
        strcpy(_config.ipAddr, ipAddr);
        return true;
    } else {
        return false;
    }
}

void SSDP::DelIPAddress() {
    if (_config.ipAddr)
        free(_config.ipAddr);
    _config.ipAddr = NULL;
}

bool SSDP::SetPort(int port) {
    _config.port = port;
    return true;
}

void SSDP::StartListener() {
    pThr = new Thread(SSDPListener, (void *)&_config, osPriorityLow, 768);
}