aes67-ESP32-P4/components/aes67_sdp_sap/aes67_sdp_sap.c

#include "aes67_sdp_sap.h"

#include <stdio.h>
#include <string.h>

#include "aes67_cfg.h"
#include "aes67_net.h"
#include "aes67_ptp.h"
#include "aes67_web.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "esp_system.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "lwip/sockets.h"

#define SDP_MAX         1024
#define SAP_ADDR        "239.255.255.255"
#define SAP_PORT        9875
#define SAP_INTERVAL_US (30 * 1000000LL)
#define SAP_MIME        "application/sdp"

static const char *TAG = "sap";

// Same lines, in the same order, as the SDP preview in web/index.html.
size_t aes67_sdp_build(char *buf, size_t size)
{
    cJSON *a = cfg_get("aes67");
    cJSON *p = cfg_get("ptp");
    if (!a || !p) {
        cJSON_Delete(a);
        cJSON_Delete(p);
        return 0;
    }
#define NUM(o, k) cJSON_GetObjectItemCaseSensitive(o, k)->valuedouble
#define STR(o, k) cJSON_GetObjectItemCaseSensitive(o, k)->valuestring
    char ip[16] = "0.0.0.0";
    esp_netif_ip_info_t info;
    esp_netif_t *netif = aes67_net_netif();
    if (netif && esp_netif_get_ip_info(netif, &info) == ESP_OK && info.ip.addr) {
        snprintf(ip, sizeof(ip), IPSTR, IP2STR(&info.ip));
    }
    char gm[24];
    if (!aes67_ptp_gm_id(gm)) {
        strcpy(gm, "00-00-00-00-00-00-00-00");
    }
    int ch = (int)NUM(a, "channels");
    char info_ch[16];
    snprintf(info_ch, sizeof(info_ch), ch == 1 ? "mono" : ch == 2 ? "stereo" : "%d ch", ch);
    int dom = (int)NUM(p, "domain");
    int pt = (int)NUM(a, "pt");

    int n = snprintf(buf, size,
                     "v=0\r\n"
                     "o=- %.0f %.0f IN IP4 %s\r\n"
                     "s=%s\r\n"
                     "c=IN IP4 %s/%d\r\n"
                     "t=0 0\r\n"
                     "a=clock-domain:PTPv2 %d\r\n"
                     "m=audio %d RTP/AVP %d\r\n"
                     "i=%s\r\n"
                     "a=rtpmap:%d %s/%d/%d\r\n"
                     "a=recvonly\r\n"
                     "a=ptime:%g\r\n"
                     "a=ts-refclk:ptp=IEEE1588-2008:%s:%d\r\n"
                     "a=mediaclk:direct=%.0f\r\n",
                     NUM(a, "session_id"), NUM(a, "session_ver"), ip,
                     STR(a, "name"),
                     STR(a, "mcast"), (int)NUM(a, "ttl"),
                     dom,
                     (int)NUM(a, "port"), pt,
                     info_ch,
                     pt, STR(a, "encoding"), (int)NUM(a, "rate"), ch,
                     NUM(a, "ptime"),
                     gm, dom,
                     NUM(a, "clk_offset"));
#undef NUM
#undef STR
    cJSON_Delete(a);
    cJSON_Delete(p);
    return n > 0 && (size_t)n < size ? (size_t)n : 0;
}

static esp_err_t sdp_get(httpd_req_t *req)
{
    char buf[SDP_MAX];
    size_t len = aes67_sdp_build(buf, sizeof(buf));
    if (!len) {
        return httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "SDP build failed");
    }
    httpd_resp_set_type(req, "application/sdp");
    httpd_resp_set_hdr(req, "Cache-Control", "no-store");
    return httpd_resp_send(req, buf, len);
}

/* ----- SAP (RFC 2974) ----- */

static struct {
    int fd;
    uint8_t ttl;
    uint32_t origin;                // IPv4 of the announcement (network order)
    char sdp[SDP_MAX];              // what is currently announced ("" = nothing)
    size_t len;
    uint16_t hash;
} s_sap = { .fd = -1 };

static uint16_t sdp_hash(const char *sdp, size_t len)
{
    uint32_t h = 2166136261u;       // FNV-1a folded to 16 bits; 0 is not allowed
    for (size_t i = 0; i < len; i++) {
        h = (h ^ (uint8_t)sdp[i]) * 16777619u;
    }
    uint16_t v = (uint16_t)(h ^ (h >> 16));
    return v ? v : 1;
}

static void sap_send(bool deletion)
{
    static uint8_t pkt[8 + sizeof(SAP_MIME) + SDP_MAX];
    pkt[0] = deletion ? 0x24 : 0x20;    // V=1, IPv4, announce / delete (T bit)
    pkt[1] = 0;                         // no authentication
    pkt[2] = s_sap.hash >> 8;
    pkt[3] = s_sap.hash & 0xff;
    memcpy(pkt + 4, &s_sap.origin, 4);
    memcpy(pkt + 8, SAP_MIME, sizeof(SAP_MIME));   // includes the terminating NUL
    size_t n = 8 + sizeof(SAP_MIME);
    memcpy(pkt + n, s_sap.sdp, s_sap.len);
    n += s_sap.len;

    struct sockaddr_in dst = { .sin_family = AF_INET, .sin_port = htons(SAP_PORT) };
    inet_aton(SAP_ADDR, &dst.sin_addr);
    if (sendto(s_sap.fd, pkt, n, 0, (struct sockaddr *)&dst, sizeof(dst)) < 0) {
        ESP_LOGW(TAG, "send failed (errno %d)", errno);
    }
}

static void sap_delete(const char *why)
{
    if (s_sap.len && s_sap.fd >= 0) {
        sap_send(true);
        ESP_LOGI(TAG, "deleted announcement %04x (%s)", s_sap.hash, why);
    }
    s_sap.len = 0;
    s_sap.sdp[0] = 0;
}

// Runs from esp_restart() (reboot, OTA): withdraw the session so receivers drop it now.
static void sap_shutdown(void)
{
    if (s_sap.len) {
        sap_delete("shutdown");
        vTaskDelay(pdMS_TO_TICKS(20));  // let lwIP put it on the wire
    }
}

static bool sap_wanted(uint8_t *ttl)
{
    cJSON *a = cfg_get("aes67");
    bool on = cJSON_IsTrue(cJSON_GetObjectItemCaseSensitive(a, "enabled")) &&
              strcmp(cJSON_GetObjectItemCaseSensitive(a, "discovery")->valuestring, "sap") == 0;
    *ttl = (uint8_t)cJSON_GetObjectItemCaseSensitive(a, "ttl")->valuedouble;
    cJSON_Delete(a);
    return on;
}

static void sap_task(void *arg)
{
    static char sdp[SDP_MAX];
    int64_t last_us = 0;
    uint8_t ttl_set = 0;

    while (1) {
        vTaskDelay(pdMS_TO_TICKS(1000));
        esp_netif_ip_info_t ip;
        esp_netif_t *netif = aes67_net_netif();
        bool have_ip = netif && esp_netif_get_ip_info(netif, &ip) == ESP_OK && ip.ip.addr;
        uint8_t ttl;
        char gm[24];
        if (!sap_wanted(&ttl) || !have_ip) {
            sap_delete(have_ip ? "discovery off or stream disabled" : "no IP");
            continue;
        }
        if (!aes67_ptp_gm_id(gm)) {
            // Without a GM the SDP's ts-refclk would be all zeros: don't advertise that.
            sap_delete("no PTP TimeTransmitter");
            continue;
        }
        if (s_sap.fd < 0) {
            s_sap.fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
            uint8_t loop = 0;
            setsockopt(s_sap.fd, IPPROTO_IP, IP_MULTICAST_LOOP, &loop, sizeof(loop));
        }
        if (ttl != ttl_set) {   // same scope as the session
            setsockopt(s_sap.fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl));
            ttl_set = ttl;
        }
        struct in_addr ifa = { .s_addr = ip.ip.addr };
        setsockopt(s_sap.fd, IPPROTO_IP, IP_MULTICAST_IF, &ifa, sizeof(ifa));

        size_t len = aes67_sdp_build(sdp, sizeof(sdp));
        if (!len) {
            continue;
        }
        int64_t now = esp_timer_get_time();
        if (len != s_sap.len || memcmp(sdp, s_sap.sdp, len) != 0 || ip.ip.addr != s_sap.origin) {
            sap_delete("SDP changed");
            memcpy(s_sap.sdp, sdp, len);
            s_sap.len = len;
            s_sap.hash = sdp_hash(sdp, len);
            s_sap.origin = ip.ip.addr;
            sap_send(false);
            last_us = now;
            ESP_LOGI(TAG, "announcing %04x to " SAP_ADDR ":%d every 30 s", s_sap.hash, SAP_PORT);
        } else if (now - last_us >= SAP_INTERVAL_US) {
            sap_send(false);
            last_us = now;
        }
    }
}

esp_err_t aes67_sdp_sap_init(void)
{
    static const httpd_uri_t uri = { .uri = "/stream.sdp", .method = HTTP_GET, .handler = sdp_get };
    esp_err_t err = web_register_uri(&uri);
    if (err == ESP_OK) {
        err = esp_register_shutdown_handler(sap_shutdown);
    }
    if (err == ESP_OK && xTaskCreate(sap_task, "sap", 4096, NULL, 3, NULL) != pdPASS) {
        err = ESP_ERR_NO_MEM;
    }
    return err;
}