bsncubed/aes67-ESP32-P4
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
5d1627f8d0aea8239a8d053586b8955aeec7ff85
aes67-ESP32-P4/components/aes67_tx/aes67_tx.c
T
bsncubed 122e46e862 Pink noise as a source (mode "pink") 
Core: aes67_tx_pink_noise(), a ready-made TX source: xorshift32 white noise
through Paul Kellet's pink filter, -18 dBFS RMS (peaks about -6 dBFS), same
on L and R. The player uses it like the tone (straight to TX, no volume or
gain). Checked on the board: -18 dBFS RMS, octave bands 63 Hz-4 kHz flat
within +-0.4 dB, L = R.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-09-25 23:37:05 +10:00

366 lines
14 KiB
C
Raw Blame History

#include "aes67_tx.h"
#include <math.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_random.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "lwip/inet.h"
#include "lwip/sockets.h"
#define MAX_FRAMES 192 // 4 ms at 48 kHz
#define MAX_CH 2
#define RTP_HDR 12
#define MAX_LAG_NS 20000000 // more than 20 ms behind: resync instead of bursting
#define TONE_HZ 1000
#define TONE_DBFS -18.0
#define WAKE_MARGIN_US 20 // wake just after a packet's last sample is due
static const char *TAG = "aes67_tx";
static TaskHandle_t s_task;
static volatile aes67_tx_read_cb_t s_read;
static volatile bool s_reconfig = true;
static volatile uint32_t s_packets, s_underruns;
// Defaults follow the Riedel Director 4-wire AES67 output; channels 2 (core default).
static const char AES67_DEFAULTS[] =
"{\"name\":\"AES67\",\"enabled\":true,\"discovery\":\"sap\",\"mcast\":\"239.69.1.10\","
"\"port\":5004,\"ttl\":32,\"dscp\":34,\"channels\":2,\"mono_sum\":false,\"encoding\":\"L24\","
"\"rate\":48000,\"ptime\":1,\"pt\":96,\"ssrc\":0,\"clk_offset\":0,"
"\"session_id\":1,\"session_ver\":1}";
// RTP multicast range 224.0.2.0 - 239.255.255.255 (keeps clear of 224.0.0.x/224.0.1.x, where PTP lives).
static bool check_mcast(const cJSON *g, char *err, size_t n)
{
const cJSON *v = cJSON_GetObjectItemCaseSensitive(g, "mcast");
struct in_addr a;
if (cJSON_IsString(v) && inet_aton(v->valuestring, &a)) {
uint32_t ip = ntohl(a.s_addr);
if (ip >= 0xE0000200 && ip <= 0xEFFFFFFF) {
return true;
}
}
snprintf(err, n, "mcast: must be 224.0.2.0 - 239.255.255.255");
return false;
}
static bool aes67_validate(const cJSON *g, char *err, size_t n)
{
static const char *const discovery[] = { "manual", "sap", NULL };
static const char *const encodings[] = { "L16", "L24", NULL };
static const double rates[] = { 48000, 96000 };
static const double ptimes[] = { 0.125, 0.25, 0.333, 1, 4 };
bool ok = cfg_check_str(g, "name", 1, 63, err, n) &&
cfg_check_enum(g, "discovery", discovery, err, n) &&
check_mcast(g, err, n) &&
cfg_check_int(g, "port", 1024, 65535, err, n) &&
cfg_check_int(g, "ttl", 1, 255, err, n) &&
cfg_check_int(g, "dscp", 0, 63, err, n) &&
cfg_check_int(g, "channels", 1, 2, err, n) &&
cfg_check_enum(g, "encoding", encodings, err, n) &&
cfg_check_num_in(g, "rate", rates, 2, err, n) &&
cfg_check_num_in(g, "ptime", ptimes, 5, err, n) &&
cfg_check_int(g, "pt", 96, 127, err, n) &&
cfg_check_int(g, "ssrc", 0, 4294967295.0, err, n) &&
cfg_check_int(g, "clk_offset", 0, 4294967295.0, err, n) &&
cfg_check_int(g, "session_id", 0, 4294967295.0, err, n) &&
cfg_check_int(g, "session_ver", 0, 4294967295.0, err, n);
if (ok && cJSON_IsTrue(cJSON_GetObjectItemCaseSensitive(g, "mono_sum")) &&
cJSON_GetObjectItemCaseSensitive(g, "channels")->valuedouble != 1) {
snprintf(err, n, "channels: must be 1 with mono_sum");
ok = false;
}
return ok;
}
static void tx_status(cJSON *st)
{
cJSON_AddNumberToObject(st, "tx_packets", s_packets);
cJSON_AddNumberToObject(st, "underruns", s_underruns);
}
static void aes67_apply(const cJSON *g)
{
s_reconfig = true; // the TX task picks up the new settings
}
void aes67_tx_set_source(aes67_tx_read_cb_t read)
{
s_read = read;
}
/* ----- sender ----- */
typedef struct {
bool enabled;
struct sockaddr_in dst;
uint8_t ttl, dscp, pt;
uint32_t ssrc, clk_offset;
int channels, rate, bytes; // bytes per sample: 3 (L24) or 2 (L16)
bool mono_sum; // 1-channel stream carries (L + R) / 2, else L
int frames; // samples per channel per packet
} tx_cfg_t;
static bool load(tx_cfg_t *c)
{
cJSON *a = cfg_get("aes67");
if (!a) {
return false;
}
#define NUM(k) cJSON_GetObjectItemCaseSensitive(a, k)->valuedouble
c->enabled = cJSON_IsTrue(cJSON_GetObjectItemCaseSensitive(a, "enabled"));
memset(&c->dst, 0, sizeof(c->dst));
c->dst.sin_family = AF_INET;
c->dst.sin_port = htons((uint16_t)NUM("port"));
inet_aton(cJSON_GetObjectItemCaseSensitive(a, "mcast")->valuestring, &c->dst.sin_addr);
c->ttl = (uint8_t)NUM("ttl");
c->dscp = (uint8_t)NUM("dscp");
c->pt = (uint8_t)NUM("pt");
c->ssrc = (uint32_t)NUM("ssrc");
c->clk_offset = (uint32_t)NUM("clk_offset");
c->channels = (int)NUM("channels");
c->mono_sum = cJSON_IsTrue(cJSON_GetObjectItemCaseSensitive(a, "mono_sum"));
c->rate = (int)NUM("rate");
c->bytes = strcmp(cJSON_GetObjectItemCaseSensitive(a, "encoding")->valuestring, "L16") == 0 ? 2 : 3;
c->frames = (int)lround(NUM("ptime") * c->rate / 1000.0); // 0.333 ms -> 16 at 48 kHz
#undef NUM
cJSON_Delete(a);
return c->frames > 0 && c->frames <= MAX_FRAMES && c->channels <= MAX_CH;
}
static int open_socket(const tx_cfg_t *c)
{
int fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (fd < 0) {
return -1;
}
int tos = c->dscp << 2;
uint8_t ttl = c->ttl, loop = 0;
setsockopt(fd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos));
setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl));
setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &loop, sizeof(loop));
esp_netif_ip_info_t ip;
if (esp_netif_get_ip_info(aes67_net_netif(), &ip) == ESP_OK) {
struct in_addr ifa = { .s_addr = ip.ip.addr };
setsockopt(fd, IPPROTO_IP, IP_MULTICAST_IF, &ifa, sizeof(ifa));
}
return fd;
}
// Sample index since the PTP epoch (no overflow: seconds * rate first).
static int64_t ns_to_samples(int64_t ns, int rate)
{
return (ns / 1000000000LL) * rate + (ns % 1000000000LL) * rate / 1000000000LL;
}
// PTP time at which sample index s starts (rounded up).
static int64_t samples_to_ns(int64_t s, int rate)
{
return (s / rate) * 1000000000LL + ((s % rate) * 1000000000LL + rate - 1) / rate;
}
// 1 kHz tone, phase from the PTP sample index, so every sender's tone lines up.
// One period is rate / 1000 samples (48 at 48 kHz, 96 at 96 kHz): precomputed table.
// Pink noise: xorshift32 white noise through Paul Kellet's refined pink filter (within 0.05 dB
// above 9 Hz). The scale gives -18 dBFS RMS (measured over 10 s; peaks about -6 dBFS).
#define PINK_SCALE 0.0714f
size_t aes67_tx_pink_noise(int32_t *buf, size_t frames)
{
static uint32_t rnd = 0x12345678;
static float b0, b1, b2, b3, b4, b5, b6;
for (size_t i = 0; i < frames; i++) {
rnd ^= rnd << 13;
rnd ^= rnd >> 17;
rnd ^= rnd << 5;
float w = (int32_t)rnd * (1.0f / 2147483648.0f); // -1..1
b0 = 0.99886f * b0 + w * 0.0555179f;
b1 = 0.99332f * b1 + w * 0.0750759f;
b2 = 0.96900f * b2 + w * 0.1538520f;
b3 = 0.86650f * b3 + w * 0.3104856f;
b4 = 0.55000f * b4 + w * 0.5329522f;
b5 = -0.7616f * b5 - w * 0.0168980f;
float pink = b0 + b1 + b2 + b3 + b4 + b5 + b6 + w * 0.5362f;
b6 = w * 0.115926f;
float x = pink * PINK_SCALE;
x = x > 0.999f ? 0.999f : x < -0.999f ? -0.999f : x; // never reached in practice
int32_t v = (int32_t)(x * 2147483647.0f);
buf[i * AES67_TX_SRC_CHANNELS] = v;
buf[i * AES67_TX_SRC_CHANNELS + 1] = v;
}
return frames;
}
// Stereo source -> 1-channel stream, in place. The sum is in 64 bit, so it can't clip; identical
// L/R comes out at the same level.
static void to_mono(int32_t *buf, int frames, bool sum)
{
for (int i = 0; i < frames; i++) {
buf[i] = sum ? (int32_t)(((int64_t)buf[2 * i] + buf[2 * i + 1]) / 2) : buf[2 * i];
}
}
static void tone(int32_t *buf, int frames, int channels, int rate, int64_t s0)
{
static int32_t table[96000 / TONE_HZ];
static int table_rate;
int period = rate / TONE_HZ;
if (table_rate != rate) {
double amp = pow(10.0, TONE_DBFS / 20.0) * 2147483647.0;
for (int i = 0; i < period; i++) {
table[i] = (int32_t)(amp * sin(2.0 * M_PI * i / period));
}
table_rate = rate;
}
int ph = (int)(s0 % period);
for (int i = 0; i < frames; i++) {
int32_t v = table[ph];
ph = ph + 1 == period ? 0 : ph + 1;
for (int ch = 0; ch < channels; ch++) {
buf[i * channels + ch] = v;
}
}
}
static void timer_cb(void *arg)
{
xTaskNotifyGive(s_task);
}
static void tx_task(void *arg)
{
tx_cfg_t c = { 0 };
int fd = -1;
esp_timer_handle_t timer = NULL;
const esp_timer_create_args_t targs = { .callback = timer_cb, .name = "aes67_tx" };
esp_timer_create(&targs, &timer);
static int32_t pcm[MAX_FRAMES * (MAX_CH > AES67_TX_SRC_CHANNELS ? MAX_CH : AES67_TX_SRC_CHANNELS)];
static uint8_t pkt[RTP_HDR + MAX_FRAMES * MAX_CH * 3];
uint16_t seq = (uint16_t)esp_random();
int64_t next = 0; // sample index of the next packet; 0 = resync needed
const char *state = NULL, *new_state;
while (1) {
ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(100));
if (s_reconfig) {
s_reconfig = false;
if (fd >= 0) {
close(fd);
fd = -1;
}
esp_timer_stop(timer);
if (!load(&c)) {
ESP_LOGE(TAG, "unsupported config (ptime/rate/channels)");
c.enabled = false;
}
next = 0;
if (c.enabled) {
fd = open_socket(&c);
}
}
int64_t now;
if (!c.enabled || fd < 0) {
new_state = "disabled";
} else if (!aes67_ptp_locked() || aes67_ptp_now_ns(&now) != ESP_OK) {
new_state = "waiting for PTP lock";
next = 0;
} else {
new_state = "sending";
int64_t due = ns_to_samples(now, c.rate);
// (Re)start aligned to a packet boundary, or after falling behind / a clock step back.
if (!next || due - next > (int64_t)c.rate * MAX_LAG_NS / 1000000000LL || next - due > 2 * c.frames) {
if (next) {
ESP_LOGW(TAG, "resync (%lld samples off)", due - next);
}
next = (due / c.frames + 1) * c.frames;
}
// Send every packet whose last sample is in the past.
while (samples_to_ns(next + c.frames, c.rate) <= now) {
aes67_tx_read_cb_t read = s_read;
if (read) {
size_t got = read(pcm, c.frames);
if (got < (size_t)c.frames) {
memset(pcm + got * AES67_TX_SRC_CHANNELS, 0,
(c.frames - got) * AES67_TX_SRC_CHANNELS * sizeof(int32_t));
s_underruns++;
}
if (c.channels == 1) {
to_mono(pcm, c.frames, c.mono_sum);
}
} else {
tone(pcm, c.frames, c.channels, c.rate, next);
}
uint32_t ts = (uint32_t)next + c.clk_offset;
pkt[0] = 0x80; // V=2
pkt[1] = c.pt & 0x7f;
pkt[2] = seq >> 8;
pkt[3] = seq & 0xff;
pkt[4] = ts >> 24;
pkt[5] = ts >> 16;
pkt[6] = ts >> 8;
pkt[7] = ts;
pkt[8] = c.ssrc >> 24;
pkt[9] = c.ssrc >> 16;
pkt[10] = c.ssrc >> 8;
pkt[11] = c.ssrc;
uint8_t *p = pkt + RTP_HDR;
for (int i = 0; i < c.frames * c.channels; i++) {
uint32_t v = (uint32_t)pcm[i]; // big-endian, top bytes of the 32-bit sample
*p++ = v >> 24;
*p++ = v >> 16;
if (c.bytes == 3) {
*p++ = v >> 8;
}
}
if (sendto(fd, pkt, p - pkt, 0, (struct sockaddr *)&c.dst, sizeof(c.dst)) > 0) {
s_packets++;
}
seq++;
next += c.frames;
}
// Wake when the next packet's last sample is due (PTP time), not on a free-running
// period, so each packet leaves right after it is complete. Re-read the clock:
// building and sending took time.
aes67_ptp_now_ns(&now);
int64_t wait_us = (samples_to_ns(next + c.frames, c.rate) - now) / 1000 + WAKE_MARGIN_US;
esp_timer_stop(timer);
esp_timer_start_once(timer, wait_us < 50 ? 50 : wait_us > 10000 ? 10000 : wait_us);
}
if (new_state != state) {
state = new_state;
if (c.enabled && state[0] == 's') {
ESP_LOGI(TAG, "%s: %s:%u, %s/%d/%d, %d samples per packet, %s", state,
inet_ntoa(c.dst.sin_addr), ntohs(c.dst.sin_port), c.bytes == 3 ? "L24" : "L16",
c.rate, c.channels, c.frames, s_read ? "source" : "1 kHz test tone");
} else {
ESP_LOGI(TAG, "%s", state);
}
}
}
}
esp_err_t aes67_tx_start(void)
{
return xTaskCreate(tx_task, "aes67_tx", 4096, NULL, 16, &s_task) == pdPASS ? ESP_OK : ESP_ERR_NO_MEM;
}
esp_err_t aes67_tx_init(void)
{
esp_err_t err = cfg_register("aes67", AES67_DEFAULTS, aes67_validate, aes67_apply);
if (err != ESP_OK) {
return err;
}
return status_register(tx_status);
}
Reference in New Issue View Git Blame Copy Permalink