1
#include <math.h>
2
#include <stdatomic.h>
3
#include <stdio.h>
4
#include <stdlib.h>
5
#include <string.h>
6
#include "esp_heap_caps.h"
7
#include "esp_partition.h"
8
#include "esp_peer.h"
9
#include "esp_peer_default.h"
10
#include "esp_random.h"
11
#include "esp_rom_crc.h"
12
#include "esp_system.h"
13
#include "esp_timer.h"
14
#include "esp_wifi.h"
15
#include "freertos/FreeRTOS.h"
16
#include "freertos/queue.h"
17
#include "freertos/task.h"
18
#include "radio.h"
19
 
20
static esp_peer_handle_t peer;
21
static QueueHandle_t commands;
22
static atomic_bool connected;
23
static bool playing, paused;
24
static uint16_t robot_id, spectrum_id;
25
static uint8_t *music;
26
static uint32_t *offsets, frame_count, frame_index, elapsed_frames;
27
static int64_t next_frame, next_telemetry;
28
static uint32_t audio_errors, data_errors, skipped_frames, telemetry_seq;
29
static unsigned led_rgb[3] = {0, 12, 4};
30
static char channel_labels[2][16];
31
static unsigned channel_count;
32
 
33
static cJSON *event(const char *name)
34
{
35
    cJSON *msg = cJSON_CreateObject();
36
    cJSON_AddStringToObject(msg, "event", name);
37
    return msg;
38
}
39
 
40
static uint32_t read32(const uint8_t *data)
41
{
42
    return (uint32_t)data[0] | (uint32_t)data[1] << 8 | (uint32_t)data[2] << 16 | (uint32_t)data[3] << 24;
43
}
44
 
45
static int load_music(void)
46
{
47
    if (music) return 0;
48
    const esp_partition_t *part = esp_partition_find_first(ESP_PARTITION_TYPE_DATA, 0x40, "music");
49
    uint8_t header[64];
50
    if (!part || esp_partition_read(part, 0, header, sizeof(header))) return -1;
51
    frame_count = read32(header + 12);
52
    if (memcmp(header, "S3MUSIC\0", 8) || read32(header + 8) != 1 || !frame_count || frame_count > 30000 ||
53
        read32(header + 16) != 48000 || read32(header + 20) != 2 || read32(header + 24) != 20) return -1;
54
    uint8_t *buffer = heap_caps_malloc(part->size, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
55
    uint32_t *index = heap_caps_malloc(frame_count * sizeof(uint32_t), MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT);
56
    if (!buffer || !index || esp_partition_read(part, 0, buffer, part->size)) {
57
        free(buffer); free(index); return -1;
58
    }
59
    uint32_t cursor = 64;
60
    for (uint32_t i = 0; i < frame_count; i++) {
61
        if (cursor + 34 > part->size) goto invalid;
62
        uint16_t size = buffer[cursor] | buffer[cursor + 1] << 8;
63
        if (!size || size > 1275 || cursor + 34 + size > part->size) goto invalid;
64
        index[i] = cursor;
65
        cursor += 34 + size;
66
    }
67
    if (esp_rom_crc32_le(0, buffer + 64, cursor - 64) != read32(header + 28)) goto invalid;
68
    music = buffer;
69
    offsets = index;
70
    return 0;
71
invalid:
72
    free(buffer); free(index); return -1;
73
}
74
 
75
static int send_json(cJSON *message)
76
{
77
    char *text = cJSON_PrintUnformatted(message);
78
    int result = -1;
79
    if (text) {
80
        esp_peer_data_frame_t frame = {.type = ESP_PEER_DATA_CHANNEL_STRING, .stream_id = robot_id,
81
            .data = (uint8_t *)text, .size = strlen(text)};
82
        result = esp_peer_send_data(peer, &frame);
83
        free(text);
84
    }
85
    cJSON_Delete(message);
86
    if (result) data_errors++;
87
    return result;
88
}
89
 
90
static int on_state(esp_peer_state_t state, void *ctx)
91
{
92
    if (state == ESP_PEER_STATE_DATA_CHANNEL_CONNECTED) connected = true;
93
    if (state == ESP_PEER_STATE_DISCONNECTED || state == ESP_PEER_STATE_CLOSED || state == ESP_PEER_STATE_CONNECT_FAILED)
94
        connected = false;
95
    cJSON *msg = event("peer_state");
96
    cJSON_AddNumberToObject(msg, "state", state);
97
    radio_emit(msg);
98
    return 0;
99
}
100
 
101
static int on_signal(esp_peer_msg_t *signal, void *ctx)
102
{
103
    char *text = calloc(1, signal->size + 1);
104
    if (!text) return -1;
105
    memcpy(text, signal->data, signal->size);
106
    cJSON *msg = event(signal->type == ESP_PEER_MSG_TYPE_SDP ? "sdp" : "candidate");
107
    cJSON_AddStringToObject(msg, "text", text);
108
    radio_emit(msg);
109
    free(text);
110
    return 0;
111
}
112
 
113
static int on_data(esp_peer_data_frame_t *frame, void *ctx)
114
{
115
    if (frame->stream_id != robot_id || frame->size <= 0 || frame->size > 512) return 0;
116
    cJSON *command = cJSON_ParseWithLength((const char *)frame->data, frame->size);
117
    if (!cJSON_IsObject(command)) { cJSON_Delete(command); return 0; }
118
    // Network commands have only the documented application fields. The USB
119
    // maintenance protocol is not exposed through a DataChannel.
120
    cJSON *safe = cJSON_CreateObject();
121
    const char *keys[] = {"led", "action", "command_id"};
122
    for (unsigned i = 0; i < 3; i++) {
123
        const cJSON *value = cJSON_GetObjectItemCaseSensitive(command, keys[i]);
124
        if (value) cJSON_AddItemToObject(safe, keys[i], cJSON_Duplicate(value, true));
125
    }
126
    cJSON_Delete(command);
127
    command = safe;
128
    // Callbacks can arrive on the transport task. Execute commands on the peer
129
    // task so playback and LED state have one owner and never race.
130
    if (!xQueueSend(commands, &command, 0)) cJSON_Delete(command);
131
    return 0;
132
}
133
 
134
static void control(cJSON *command)
135
{
136
    const cJSON *color = cJSON_GetObjectItemCaseSensitive(command, "led");
137
    int result = -1;
138
    if (cJSON_IsArray(color) && cJSON_GetArraySize(color) == 3) {
139
        bool valid = true;
140
        unsigned rgb[3];
141
        for (unsigned i = 0; i < 3; i++) {
142
            const cJSON *v = cJSON_GetArrayItem(color, i);
143
            if (!cJSON_IsNumber(v) || v->valuedouble < 0 || v->valuedouble > 255 || floor(v->valuedouble) != v->valuedouble)
144
                valid = false;
145
            rgb[i] = cJSON_IsNumber(v) ? v->valueint : 0;
146
        }
147
        if (valid) { result = radio_set_led(rgb[0], rgb[1], rgb[2]); if (!result) memcpy(led_rgb, rgb, sizeof(rgb)); }
148
    }
149
    const cJSON *action = cJSON_GetObjectItemCaseSensitive(command, "action");
150
    if (cJSON_IsString(action)) {
151
        if (!strcmp(action->valuestring, "pause")) { paused = true; result = 0; }
152
        else if (!strcmp(action->valuestring, "play")) { paused = false; result = 0; }
153
        else if (!strcmp(action->valuestring, "restart")) { frame_index = 0; paused = false; result = 0; }
154
    }
155
    cJSON *ack = event("ack");
156
    const cJSON *id = cJSON_GetObjectItemCaseSensitive(command, "command_id");
157
    if (cJSON_IsString(id) && strlen(id->valuestring) <= 64) cJSON_AddStringToObject(ack, "command_id", id->valuestring);
158
    cJSON_AddNumberToObject(ack, "result", result);
159
    cJSON_AddItemToObject(ack, "led", cJSON_CreateIntArray((int *)led_rgb, 3));
160
    cJSON_AddBoolToObject(ack, "paused", paused);
161
    send_json(ack);
162
}
163
 
164
static void execute(cJSON *command)
165
{
166
    const cJSON *name = cJSON_GetObjectItemCaseSensitive(command, "cmd");
167
    if (!cJSON_IsString(name)) { if (playing) control(command); return; }
168
    int result = ESP_PEER_ERR_INVALID_ARG;
169
    if (!strcmp(name->valuestring, "peer_init") && !peer) {
170
        result = load_music();
171
        if (!result) {
172
            esp_peer_default_cfg_t extra = {.agent_recv_timeout = 10,
173
                .data_ch_cfg = {.send_cache_size = 16384, .recv_cache_size = 16384},
174
                .rtp_cfg = {.send_pool_size = 32768, .send_queue_num = 32}};
175
            esp_peer_cfg_t config = {.role = ESP_PEER_ROLE_CONTROLLING,
176
                .audio_info = {.codec = ESP_PEER_AUDIO_CODEC_OPUS, .sample_rate = 48000, .channel = 2},
177
                .audio_dir = ESP_PEER_MEDIA_DIR_SEND_ONLY, .video_dir = ESP_PEER_MEDIA_DIR_NONE,
178
                .enable_data_channel = true, .manual_ch_create = true, .no_auto_reconnect = true,
179
                .on_state = on_state, .on_msg = on_signal, .on_data = on_data,
180
                .extra_cfg = &extra, .extra_size = sizeof(extra)};
181
            result = esp_peer_open(&config, esp_peer_get_default_impl(), &peer);
182
            if (!result) result = esp_peer_new_connection(peer);
183
        }
184
    } else if (!strcmp(name->valuestring, "sdp") && peer) {
185
        const cJSON *text = cJSON_GetObjectItemCaseSensitive(command, "text");
186
        if (cJSON_IsString(text)) {
187
            esp_peer_msg_t signal = {.type = ESP_PEER_MSG_TYPE_SDP,
188
                .data = (uint8_t *)text->valuestring, .size = strlen(text->valuestring)};
189
            result = esp_peer_send_msg(peer, &signal);
190
        }
191
    } else if (!strcmp(name->valuestring, "create_channel") && peer && connected && channel_count < 2) {
192
        const char *label = channel_count == 0 ? "robot" : "spectrum";
193
        strcpy(channel_labels[channel_count], label);
194
        esp_peer_data_channel_cfg_t channel = {.label = channel_labels[channel_count],
195
            .type = channel_count == 0 ? ESP_PEER_DATA_CHANNEL_RELIABLE : ESP_PEER_DATA_CHANNEL_PARTIAL_RELIABLE_RETX,
196
            .ordered = channel_count == 0, .max_retransmit_count = 0};
197
        result = esp_peer_create_data_channel(peer, &channel);
198
        if (!result) channel_count++;
199
    } else if (!strcmp(name->valuestring, "start") && peer && connected && channel_count == 2 && !playing) {
200
        const cJSON *r = cJSON_GetObjectItemCaseSensitive(command, "robot_id");
201
        const cJSON *s = cJSON_GetObjectItemCaseSensitive(command, "spectrum_id");
202
        // The tested fixed SDK allocation is 2, then 4. Fail clearly if the
203
        // SFU ever returns another allocation instead of silently misrouting.
204
        if (cJSON_IsNumber(r) && cJSON_IsNumber(s) && r->valueint == 2 && s->valueint == 4) {
205
            robot_id = r->valueint; spectrum_id = s->valueint;
206
            playing = true; next_frame = esp_timer_get_time(); next_telemetry = next_frame;
207
            result = 0;
208
        }
209
    } else if (!strcmp(name->valuestring, "ping")) result = 0;
210
    else if (!strcmp(name->valuestring, "restart_device")) { vTaskDelay(pdMS_TO_TICKS(100)); esp_restart(); }
211
    cJSON *reply = event("command_result");
212
    cJSON_AddStringToObject(reply, "cmd", name->valuestring);
213
    cJSON_AddNumberToObject(reply, "result", result);
214
    radio_emit(reply);
215
}
216
 
217
static void tick(void)
218
{
219
    if (!playing || !connected) return;
220
    int64_t now = esp_timer_get_time();
221
    if (now >= next_frame) {
222
        // Keep real time through stalls: skip old frames, never burst a backlog.
223
        uint32_t late = (now - next_frame) / 20000;
224
        if (late) { elapsed_frames += late; if (!paused) frame_index = (frame_index + late) % frame_count; skipped_frames += late; }
225
        next_frame += (late + 1) * 20000;
226
        uint8_t *record = music + offsets[frame_index];
227
        uint16_t size = record[0] | record[1] << 8;
228
        // RFC 6716 Opus silence (20 ms), preserving the RTP clock while paused.
229
        static uint8_t silence[] = {0xf8, 0xff, 0xfe};
230
        esp_peer_audio_frame_t audio = {.pts = elapsed_frames * 20,
231
            .data = paused ? silence : record + 34, .size = paused ? sizeof(silence) : size};
232
        if (esp_peer_send_audio(peer, &audio)) audio_errors++;
233
        if (elapsed_frames % 2 == 0) {
234
            // v1: version, paused, reserved[2], RTP-relative milliseconds,
235
            // song milliseconds, then 32 unsigned normalized spectrum bands.
236
            uint8_t packet[44] = {1, paused ? 1 : 0};
237
            uint32_t pts = elapsed_frames * 20, position = frame_index * 20;
238
            memcpy(packet + 4, &pts, 4); memcpy(packet + 8, &position, 4);
239
            if (!paused) memcpy(packet + 12, record + 2, 32);
240
            esp_peer_data_frame_t spectrum = {.type = ESP_PEER_DATA_CHANNEL_DATA, .stream_id = spectrum_id,
241
                .data = packet, .size = sizeof(packet)};
242
            if (esp_peer_send_data(peer, &spectrum)) data_errors++;
243
        }
244
        elapsed_frames++;
245
        if (!paused) frame_index = (frame_index + 1) % frame_count;
246
    }
247
    if (now >= next_telemetry) {
248
        next_telemetry = now + 500000;
249
        wifi_ap_record_t ap = {0};
250
        esp_wifi_sta_get_ap_info(&ap);
251
        cJSON *msg = event("telemetry");
252
        cJSON_AddNumberToObject(msg, "sequence", ++telemetry_seq);
253
        cJSON_AddNumberToObject(msg, "random", esp_random() % 1000);
254
        cJSON_AddNumberToObject(msg, "uptimeMs", now / 1000);
255
        cJSON_AddNumberToObject(msg, "positionMs", frame_index * 20);
256
        cJSON_AddNumberToObject(msg, "durationMs", frame_count * 20);
257
        cJSON_AddBoolToObject(msg, "paused", paused);
258
        cJSON_AddNumberToObject(msg, "rssi", ap.rssi);
259
        cJSON_AddNumberToObject(msg, "heap", heap_caps_get_free_size(MALLOC_CAP_INTERNAL));
260
        cJSON_AddNumberToObject(msg, "audioErrors", audio_errors);
261
        cJSON_AddNumberToObject(msg, "dataErrors", data_errors);
262
        cJSON_AddNumberToObject(msg, "skippedFrames", skipped_frames);
263
        cJSON_AddItemToObject(msg, "led", cJSON_CreateIntArray((int *)led_rgb, 3));
264
        send_json(msg);
265
    }
266
}
267
 
268
static void run(void *arg)
269
{
270
    for (;;) {
271
        cJSON *command = NULL;
272
        if (xQueueReceive(commands, &command, 0)) { execute(command); cJSON_Delete(command); }
273
        if (peer) esp_peer_main_loop(peer);
274
        tick();
275
        vTaskDelay(1);
276
    }
277
}
278
 
279
void radio_init(void)
280
{
281
    if (!commands) {
282
        commands = xQueueCreate(16, sizeof(cJSON *));
283
        if (!commands || xTaskCreate(run, "radio", 16384, NULL, 5, NULL) != pdPASS) abort();
284
    }
285
}
286
 
287
void radio_command(const cJSON *command)
288
{
289
    cJSON *copy = cJSON_Duplicate(command, true);
290
    if (copy && !xQueueSend(commands, &copy, pdMS_TO_TICKS(100))) cJSON_Delete(copy);
291
}