Skip to content
rachel/esp32-radio
File

Blob: archive/fft-benchmark/firmware/platform/spectrum_benchmark.c

c272 lines
1/* A bounded shadow workload for measuring Opus decode + FFT during WebRTC.
2 * Built only by build.py into an isolated copy of the normal firmware.
3 * All codec/DSP state belongs to one worker pinned to CPU1. The radio only
4 * copies compressed packets into a nonblocking queue; no Rust pointers survive.
5 */
6#include <math.h>
7#include <stdatomic.h>
8#include <stdio.h>
9#include <stdlib.h>
10#include <string.h>
11#include "spectrum_benchmark.h"
12#include "dsps_fft2r.h"
13#include "esp_audio_dec.h"
14#include "esp_opus_dec.h"
15#include "esp_heap_caps.h"
16#include "esp_timer.h"
17#include "freertos/FreeRTOS.h"
18#include "freertos/queue.h"
19#include "freertos/task.h"
20 
21#define MAX_FFT 2048
22#define PHASE_FRAMES 2500
23#define QUEUE_DEPTH 8
24#define SAMPLE_RATE 48000
25#define PI_F 3.14159265358979323846f
26 
27typedef struct {
28    uint32_t pts, size;
29    uint64_t queued_us;
30    uint8_t bytes[1275];
31} packet;
32typedef struct {
33    uint64_t sum;
34    uint32_t count, max;
35    uint32_t histogram[401]; /* 50 us bins; final bin includes >=20 ms. */
36} timing;
37typedef struct {
38    void *decoder;
39    float ring[MAX_FFT], window[MAX_FFT];
40    int16_t pcm[960 * 2];
41    packet incoming;
42    timing decode, float_fft, fixed_fft, analysis, latency;
43    uint32_t frames, transforms, errors, gaps, over_budget, position, filled, previous_pts;
44    uint32_t heap_min, nonzero;
45    float window_sum;
46} state;
47 
48static QueueHandle_t packets;
49static atomic_bool accepting;
50static atomic_uint dropped, radio_cores;
51static atomic_int radio_affinity;
52static float *fft;
53static int16_t *fixed_fft;
54 
55static uint64_t now_us(void) { return esp_timer_get_time(); }
56static uint32_t heap_free(void) { return heap_caps_get_free_size(MALLOC_CAP_INTERNAL); }
57 
58static void record(timing *t, uint64_t elapsed)
59{
60    uint32_t us = elapsed > UINT32_MAX ? UINT32_MAX : elapsed;
61    t->sum += us;
62    t->count++;
63    if (us > t->max) t->max = us;
64    t->histogram[us / 50 < 400 ? us / 50 : 400]++;
65}
66 
67static unsigned percentile95(const timing *t)
68{
69    unsigned remaining = (t->count * 95 + 99) / 100;
70    for (unsigned i = 0; i < 401; i++) {
71        if (t->histogram[i] >= remaining) return i == 400 ? t->max : (i + 1) * 50;
72        remaining -= t->histogram[i];
73    }
74    return 20050;
75}
76 
77static void print_timing(const char *name, const timing *t)
78{
79    printf("\"%s\":{\"mean_us\":%lu,\"p95_bin_upper_us\":%u,\"max_us\":%lu}", name,
80           (unsigned long)(t->count ? t->sum / t->count : 0), percentile95(t), (unsigned long)t->max);
81}
82 
83static void set_window(state *s, unsigned n)
84{
85    s->window_sum = 0;
86    for (unsigned i = 0; i < n; i++) {
87        s->window[i] = 0.5f - 0.5f * cosf(2 * PI_F * i / n);
88        s->window_sum += s->window[i];
89    }
90}
91 
92static void self_test(state *s)
93{
94    for (unsigned n = 1024; n <= MAX_FFT; n *= 2) {
95        set_window(s, n);
96        unsigned expected = n / 32;
97        for (unsigned i = 0; i < n; i++) {
98            float sample = 0.5f * sinf(2 * PI_F * expected * i / n) * s->window[i];
99            fft[2 * i] = sample; fft[2 * i + 1] = 0;
100            fixed_fft[2 * i] = lrintf(sample * 32767); fixed_fft[2 * i + 1] = 0;
101        }
102        ESP_ERROR_CHECK(dsps_fft2r_fc32(fft, n));
103        ESP_ERROR_CHECK(dsps_bit_rev_fc32(fft, n));
104        ESP_ERROR_CHECK(dsps_fft2r_sc16(fixed_fft, n));
105        ESP_ERROR_CHECK(dsps_bit_rev_sc16_ansi(fixed_fft, n));
106        float peak = 0; int64_t fixed_peak = 0;
107        unsigned float_bin = 0, fixed_bin = 0;
108        for (unsigned k = 1; k < n / 2; k++) {
109            float power = fft[2 * k] * fft[2 * k] + fft[2 * k + 1] * fft[2 * k + 1];
110            int32_t real = fixed_fft[2 * k], imag = fixed_fft[2 * k + 1];
111            int64_t fixed_power = (int64_t)real * real + (int64_t)imag * imag;
112            if (power > peak) { peak = power; float_bin = k; }
113            if (fixed_power > fixed_peak) { fixed_peak = fixed_power; fixed_bin = k; }
114        }
115        float amplitude = sqrtf(peak) * 2 / s->window_sum;
116        bool passed = float_bin == expected && fixed_bin == expected && fabsf(amplitude - 0.5f) < 0.002f;
117        printf("FFT_SELF_TEST {\"n\":%u,\"float_bin\":%u,\"fixed_bin\":%u,\"amplitude\":%.5f,\"passed\":%s}\n",
118               n, float_bin, fixed_bin, amplitude, passed ? "true" : "false");
119        if (!passed) abort();
120    }
121}
122 
123static void bands(state *s, unsigned n, uint8_t out[32])
124{
125    const float norm = 4 / (s->window_sum * s->window_sum);
126    for (unsigned band = 0; band < 32; band++) {
127        float low = 30 * powf(20000.0f / 30, band / 32.0f);
128        float high = 30 * powf(20000.0f / 30, (band + 1) / 32.0f);
129        unsigned first = (unsigned)ceilf(low * n / SAMPLE_RATE);
130        unsigned end = (unsigned)ceilf(high * n / SAMPLE_RATE);
131        if (first == end) { first = lroundf((low + high) * n / (2 * SAMPLE_RATE)); end = first + 1; }
132        if (first < 1) first = 1;
133        float power = 1e-12f;
134        for (unsigned k = first; k < end && k < n / 2; k++) {
135            float value = (fft[2 * k] * fft[2 * k] + fft[2 * k + 1] * fft[2 * k + 1]) * norm;
136            if (value > power) power = value;
137        }
138        float level = (10 * log10f(power) + 72) * (255.0f / 66);
139        out[band] = (uint8_t)fminf(255, fmaxf(0, level));
140    }
141}
142 
143static void summarize(state *s, unsigned n)
144{
145    printf("FFT_RESULT {\"n\":%u,\"sample_rate\":48000,\"channels\":2,\"frames\":%lu,\"transforms\":%lu,",
146           n, (unsigned long)s->frames, (unsigned long)s->transforms);
147    print_timing("decode", &s->decode); printf(",");
148    print_timing("float_fft_and_reorder", &s->float_fft); printf(",");
149    print_timing("fixed_fft_and_reorder", &s->fixed_fft); printf(",");
150    print_timing("window_both_ffts_and_bands", &s->analysis); printf(",");
151    print_timing("queue_to_done", &s->latency);
152    printf(",\"decode_errors\":%lu,\"pts_gaps\":%lu,\"dropped\":%u,\"over_20ms\":%lu,\"heap_min\":%lu,"
153           "\"stack_free\":%u,\"worker_core\":%d,\"radio_cores_mask\":%u,\"radio_affinity\":%d,\"nonzero_frames\":%lu}\n",
154           (unsigned long)s->errors, (unsigned long)s->gaps, atomic_load(&dropped), (unsigned long)s->over_budget,
155           (unsigned long)s->heap_min, (unsigned)uxTaskGetStackHighWaterMark(NULL), xPortGetCoreID(),
156           atomic_load(&radio_cores), atomic_load(&radio_affinity), (unsigned long)s->nonzero);
157}
158 
159static void run(void *unused)
160{
161    state *s = heap_caps_calloc(1, sizeof(*s), MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
162    fft = heap_caps_aligned_alloc(16, 2 * MAX_FFT * sizeof(float), MALLOC_CAP_INTERNAL | MALLOC_CAP_8BIT);
163    fixed_fft = heap_caps_aligned_alloc(16, 2 * MAX_FFT * sizeof(int16_t), MALLOC_CAP_INTERNAL | MALLOC_CAP_8BIT);
164    if (!s || !fft || !fixed_fft) abort();
165    uint32_t before = heap_free();
166    esp_opus_dec_cfg_t config = {.sample_rate = SAMPLE_RATE, .channel = 2,
167        .frame_duration = ESP_OPUS_DEC_FRAME_DURATION_20_MS, .self_delimited = false};
168    ESP_ERROR_CHECK(esp_opus_dec_open(&config, sizeof(config), &s->decoder));
169    uint32_t codec_internal = before - heap_free();
170    ESP_ERROR_CHECK(dsps_fft2r_init_fc32(NULL, MAX_FFT));
171    ESP_ERROR_CHECK(dsps_fft2r_init_sc16(NULL, MAX_FFT));
172    self_test(s);
173    printf("FFT_READY {\"codec_version\":\"2.6.2\",\"codec_internal_bytes\":%lu,\"heap_free\":%lu,\"core\":%d}\n",
174           (unsigned long)codec_internal, (unsigned long)heap_free(), xPortGetCoreID());
175    atomic_store(&accepting, true);
176    for (unsigned n = 1024; n <= MAX_FFT; n *= 2) {
177        set_window(s, n);
178        memset(&s->decode, 0, sizeof(timing) * 5);
179        s->frames = s->transforms = s->errors = s->gaps = s->over_budget = s->nonzero = 0;
180        s->heap_min = heap_free();
181        while (s->frames < PHASE_FRAMES) {
182            if (!xQueueReceive(packets, &s->incoming, portMAX_DELAY)) continue;
183            packet *p = &s->incoming;
184            if (s->frames && p->pts != s->previous_pts + 20) {
185                s->gaps++;
186                ESP_ERROR_CHECK(esp_opus_dec_reset(s->decoder));
187                s->filled = s->position = 0;
188                memset(s->ring, 0, sizeof(s->ring));
189            }
190            s->previous_pts = p->pts;
191            esp_audio_dec_in_raw_t input = {.buffer = p->bytes, .len = p->size};
192            esp_audio_dec_out_frame_t output = {.buffer = (uint8_t *)s->pcm, .len = sizeof(s->pcm)};
193            esp_audio_dec_info_t info = {0};
194            uint64_t start = now_us();
195            int err = esp_opus_dec_decode(s->decoder, &input, &output, &info);
196            record(&s->decode, now_us() - start);
197            s->frames++;
198            if (err || input.consumed != p->size || output.decoded_size != sizeof(s->pcm) ||
199                info.sample_rate != SAMPLE_RATE || info.channel != 2 || info.bits_per_sample != 16) {
200                s->errors++;
201                if (s->errors < 3) printf("FFT_ERROR {\"code\":%d,\"consumed\":%lu,\"decoded\":%lu}\n",
202                                        err, (unsigned long)input.consumed, (unsigned long)output.decoded_size);
203                continue;
204            }
205            bool nonzero = false;
206            for (unsigned i = 0; i < 960; i++) {
207                int32_t value = (int32_t)s->pcm[2 * i] + s->pcm[2 * i + 1];
208                s->ring[s->position] = value / 65536.0f;
209                s->position = (s->position + 1) % MAX_FFT;
210                if (s->filled < MAX_FFT) s->filled++;
211                if (value) nonzero = true;
212            }
213            s->nonzero += nonzero;
214            if (s->frames % 2 == 0 && s->filled >= n) {
215                start = now_us();
216                for (unsigned i = 0; i < n; i++) {
217                    float sample = s->ring[(s->position + MAX_FFT - n + i) % MAX_FFT] * s->window[i];
218                    fft[2 * i] = sample; fft[2 * i + 1] = 0;
219                    fixed_fft[2 * i] = lrintf(sample * 32767); fixed_fft[2 * i + 1] = 0;
220                }
221                uint64_t kernel = now_us();
222                ESP_ERROR_CHECK(dsps_fft2r_fc32(fft, n));
223                ESP_ERROR_CHECK(dsps_bit_rev_fc32(fft, n));
224                record(&s->float_fft, now_us() - kernel);
225                kernel = now_us();
226                ESP_ERROR_CHECK(dsps_fft2r_sc16(fixed_fft, n));
227                ESP_ERROR_CHECK(dsps_bit_rev_sc16_ansi(fixed_fft, n));
228                record(&s->fixed_fft, now_us() - kernel);
229                uint8_t levels[32];
230                bands(s, n, levels);
231                record(&s->analysis, now_us() - start);
232                s->transforms++;
233                if (s->frames % 500 == 0) {
234                    printf("FFT_SAMPLE {\"pts\":%lu,\"n\":%u,\"bands\":[", (unsigned long)p->pts, n);
235                    for (unsigned i = 0; i < 32; i++) printf("%s%u", i ? "," : "", levels[i]);
236                    printf("]}\n");
237                }
238            }
239            uint64_t latency = now_us() - p->queued_us;
240            record(&s->latency, latency);
241            if (latency > 20000) s->over_budget++;
242            uint32_t available = heap_free();
243            if (available < s->heap_min) s->heap_min = available;
244        }
245        summarize(s, n);
246    }
247    atomic_store(&accepting, false);
248    printf("FFT_COMPLETE\n");
249    /* The isolated experiment retains its allocations until normal firmware is
250     * restored. No task is deleted while it might own decoder/DSP state. */
251    for (;;) vTaskDelay(pdMS_TO_TICKS(1000));
252}
253 
254void spectrum_benchmark_start(void)
255{
256    static StaticQueue_t queue_control;
257    uint8_t *storage = heap_caps_malloc(QUEUE_DEPTH * sizeof(packet), MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
258    if (!storage) abort();
259    packets = xQueueCreateStatic(QUEUE_DEPTH, sizeof(packet), storage, &queue_control);
260    if (!packets || xTaskCreatePinnedToCore(run, "spectrum-bench", 16384, NULL, 4, NULL, 1) != pdPASS) abort();
261}
262 
263void spectrum_benchmark_push(uint32_t pts, const uint8_t *opus, size_t size)
264{
265    if (!atomic_load(&accepting) || !size || size > 1275) return;
266    packet p = {.pts = pts, .size = size, .queued_us = now_us()};
267    memcpy(p.bytes, opus, size);
268    atomic_fetch_or(&radio_cores, 1u << xPortGetCoreID());
269    atomic_store(&radio_affinity, xTaskGetCoreID(NULL));
270    if (!xQueueSend(packets, &p, 0)) atomic_fetch_add(&dropped, 1);
271}