|
|
@@ -0,0 +1,187 @@
|
|
|
+/*
|
|
|
+ * SPDX-FileCopyrightText: 2026 Benjamin Harris
|
|
|
+ * SPDX-License-Identifier: Apache-2.0
|
|
|
+ *
|
|
|
+ * Dante CMC (device-announce) responder — clean-room, from
|
|
|
+ * docs/dante-protocol-notes.md.
|
|
|
+ */
|
|
|
+
|
|
|
+#include "dante_cmc.h"
|
|
|
+
|
|
|
+#include <string.h>
|
|
|
+#include <errno.h>
|
|
|
+#include <sys/time.h>
|
|
|
+#include "freertos/FreeRTOS.h"
|
|
|
+#include "freertos/task.h"
|
|
|
+#include "esp_log.h"
|
|
|
+#include "lwip/sockets.h"
|
|
|
+
|
|
|
+static const char *TAG = "dante_cmc";
|
|
|
+
|
|
|
+/* req/resp framing (shared with ARC). */
|
|
|
+#define CMC_HEADER_LEN 10
|
|
|
+#define CMC_CODE_OK 1
|
|
|
+#define CMC_OP_REQUEST_DEVICE_ADVERTISEMENT 0x1001
|
|
|
+
|
|
|
+/* DeviceAdvertisement content is 22 bytes; full reply = header + content. */
|
|
|
+#define CMC_ADV_CONTENT_LEN 22
|
|
|
+#define CMC_REPLY_LEN (CMC_HEADER_LEN + CMC_ADV_CONTENT_LEN)
|
|
|
+
|
|
|
+#define CMC_RECV_BUF 512
|
|
|
+
|
|
|
+static dante_identity_t s_id;
|
|
|
+static volatile bool s_running;
|
|
|
+static int s_sock = -1;
|
|
|
+static TaskHandle_t s_task;
|
|
|
+
|
|
|
+/* Big-endian field helpers (wire format is big-endian; the P4 is little). */
|
|
|
+static inline uint16_t rd_u16(const uint8_t *p)
|
|
|
+{
|
|
|
+ return (uint16_t)((p[0] << 8) | p[1]);
|
|
|
+}
|
|
|
+static inline void wr_u16(uint8_t *p, uint16_t v)
|
|
|
+{
|
|
|
+ p[0] = (uint8_t)(v >> 8);
|
|
|
+ p[1] = (uint8_t)(v & 0xFF);
|
|
|
+}
|
|
|
+
|
|
|
+/* Build the DeviceAdvertisement content (22 bytes, big-endian) into `out`. */
|
|
|
+static void build_advertisement(uint8_t *out)
|
|
|
+{
|
|
|
+ wr_u16(&out[0], s_id.process_id);
|
|
|
+ memcpy(&out[2], s_id.device_id, DANTE_DEVICE_ID_LEN); /* [2..9] */
|
|
|
+ wr_u16(&out[10], 1); /* unknown, observed = 1 */
|
|
|
+ wr_u16(&out[12], 0); /* unknown, observed = 0 */
|
|
|
+ out[14] = (uint8_t)((s_id.ipv4 >> 24) & 0xFF); /* IP a.b.c.d */
|
|
|
+ out[15] = (uint8_t)((s_id.ipv4 >> 16) & 0xFF);
|
|
|
+ out[16] = (uint8_t)((s_id.ipv4 >> 8) & 0xFF);
|
|
|
+ out[17] = (uint8_t)(s_id.ipv4 & 0xFF);
|
|
|
+ wr_u16(&out[18], s_id.info_port);
|
|
|
+ wr_u16(&out[20], 0); /* unknown, observed = 0 */
|
|
|
+}
|
|
|
+
|
|
|
+static void handle_request(int sock, const uint8_t *req, int len,
|
|
|
+ const struct sockaddr_in *src, socklen_t src_len)
|
|
|
+{
|
|
|
+ if (len < CMC_HEADER_LEN) {
|
|
|
+ return; /* too short to be a valid message */
|
|
|
+ }
|
|
|
+
|
|
|
+ uint16_t start_code = rd_u16(&req[0]);
|
|
|
+ uint16_t seqnum = rd_u16(&req[4]);
|
|
|
+ uint16_t opcode1 = rd_u16(&req[6]);
|
|
|
+ uint16_t opcode2 = rd_u16(&req[8]);
|
|
|
+
|
|
|
+ /* opcode2 != 0 marks a response, not a request — ignore. */
|
|
|
+ if (opcode2 != 0) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if (opcode1 != CMC_OP_REQUEST_DEVICE_ADVERTISEMENT) {
|
|
|
+ ESP_LOGD(TAG, "unhandled CMC opcode1 0x%04x", opcode1);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ uint8_t reply[CMC_REPLY_LEN];
|
|
|
+ /* Echo start_code / seqnum / opcode1; opcode2 = OK. */
|
|
|
+ wr_u16(&reply[0], start_code);
|
|
|
+ wr_u16(&reply[2], CMC_REPLY_LEN);
|
|
|
+ wr_u16(&reply[4], seqnum);
|
|
|
+ wr_u16(&reply[6], opcode1);
|
|
|
+ wr_u16(&reply[8], CMC_CODE_OK);
|
|
|
+ build_advertisement(&reply[CMC_HEADER_LEN]);
|
|
|
+
|
|
|
+ int sent = sendto(sock, reply, sizeof(reply), 0,
|
|
|
+ (const struct sockaddr *)src, src_len);
|
|
|
+ if (sent < 0) {
|
|
|
+ ESP_LOGW(TAG, "sendto failed: errno %d", errno);
|
|
|
+ } else {
|
|
|
+ ESP_LOGD(TAG, "answered device-advertisement request");
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+static void cmc_task(void *arg)
|
|
|
+{
|
|
|
+ (void)arg;
|
|
|
+
|
|
|
+ int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
|
|
+ if (sock < 0) {
|
|
|
+ ESP_LOGE(TAG, "socket() failed: errno %d", errno);
|
|
|
+ s_task = NULL;
|
|
|
+ vTaskDelete(NULL);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ /* 500ms recv timeout so the loop can observe s_running for a clean stop. */
|
|
|
+ struct timeval tv = { .tv_sec = 0, .tv_usec = 500 * 1000 };
|
|
|
+ setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
|
|
|
+
|
|
|
+ struct sockaddr_in bind_addr = {
|
|
|
+ .sin_family = AF_INET,
|
|
|
+ .sin_port = htons(s_id.cmc_port),
|
|
|
+ .sin_addr = { .s_addr = htonl(INADDR_ANY) },
|
|
|
+ };
|
|
|
+ if (bind(sock, (struct sockaddr *)&bind_addr, sizeof(bind_addr)) < 0) {
|
|
|
+ ESP_LOGE(TAG, "bind(:%u) failed: errno %d", s_id.cmc_port, errno);
|
|
|
+ close(sock);
|
|
|
+ s_task = NULL;
|
|
|
+ vTaskDelete(NULL);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ s_sock = sock;
|
|
|
+ ESP_LOGI(TAG, "CMC responder listening on udp/%u", s_id.cmc_port);
|
|
|
+
|
|
|
+ uint8_t buf[CMC_RECV_BUF];
|
|
|
+ while (s_running) {
|
|
|
+ struct sockaddr_in src;
|
|
|
+ socklen_t src_len = sizeof(src);
|
|
|
+ int n = recvfrom(sock, buf, sizeof(buf), 0,
|
|
|
+ (struct sockaddr *)&src, &src_len);
|
|
|
+ if (n < 0) {
|
|
|
+ if (errno == EAGAIN || errno == EWOULDBLOCK) {
|
|
|
+ continue; /* timeout — re-check s_running */
|
|
|
+ }
|
|
|
+ if (s_running) {
|
|
|
+ ESP_LOGW(TAG, "recvfrom failed: errno %d", errno);
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ handle_request(sock, buf, n, &src, src_len);
|
|
|
+ }
|
|
|
+
|
|
|
+ s_sock = -1;
|
|
|
+ close(sock);
|
|
|
+ ESP_LOGI(TAG, "CMC responder stopped");
|
|
|
+ s_task = NULL;
|
|
|
+ vTaskDelete(NULL);
|
|
|
+}
|
|
|
+
|
|
|
+esp_err_t dante_cmc_start(const dante_identity_t *id)
|
|
|
+{
|
|
|
+ if (id == NULL) {
|
|
|
+ return ESP_ERR_INVALID_ARG;
|
|
|
+ }
|
|
|
+ if (s_task != NULL) {
|
|
|
+ ESP_LOGW(TAG, "CMC responder already running");
|
|
|
+ return ESP_ERR_INVALID_STATE;
|
|
|
+ }
|
|
|
+ s_id = *id;
|
|
|
+ s_running = true;
|
|
|
+
|
|
|
+ /* Core 0 for coherency with the net/audio tasks; modest priority — CMC is
|
|
|
+ * control-plane and low-rate, must never contend with the audio path. */
|
|
|
+ BaseType_t ok = xTaskCreatePinnedToCore(cmc_task, "dante_cmc",
|
|
|
+ 4096, NULL, 5, &s_task, 0);
|
|
|
+ if (ok != pdPASS) {
|
|
|
+ s_running = false;
|
|
|
+ s_task = NULL;
|
|
|
+ ESP_LOGE(TAG, "failed to create CMC task");
|
|
|
+ return ESP_ERR_NO_MEM;
|
|
|
+ }
|
|
|
+ return ESP_OK;
|
|
|
+}
|
|
|
+
|
|
|
+void dante_cmc_stop(void)
|
|
|
+{
|
|
|
+ s_running = false; /* task wakes on its recv timeout and self-deletes */
|
|
|
+}
|
Benjamin Harris