rtp.c 24 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807
  1. /*
  2. * HairTunes - RAOP packet handler and slave-clocked replay engine
  3. * Copyright (c) James Laird 2011
  4. * All rights reserved.
  5. *
  6. * Modularisation: philippe_44@outlook.com, 2019
  7. *
  8. * Permission is hereby granted, free of charge, to any person
  9. * obtaining a copy of this software and associated documentation
  10. * files (the "Software"), to deal in the Software without
  11. * restriction, including without limitation the rights to use,
  12. * copy, modify, merge, publish, distribute, sublicense, and/or
  13. * sell copies of the Software, and to permit persons to whom the
  14. * Software is furnished to do so, subject to the following conditions:
  15. *
  16. * The above copyright notice and this permission notice shall be
  17. * included in all copies or substantial portions of the Software.
  18. *
  19. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  20. * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
  21. * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  22. * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
  23. * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
  24. * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  25. * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
  26. * OTHER DEALINGS IN THE SOFTWARE.
  27. */
  28. #include <stdio.h>
  29. #include <stdlib.h>
  30. #include <string.h>
  31. #include <stdarg.h>
  32. #include <sys/types.h>
  33. #include <pthread.h>
  34. #include <math.h>
  35. #include <errno.h>
  36. #include <sys/stat.h>
  37. #include <stdint.h>
  38. #include <fcntl.h>
  39. #include <assert.h>
  40. #include "platform.h"
  41. #include "rtp.h"
  42. #include "raop_sink.h"
  43. #include "log_util.h"
  44. #include "util.h"
  45. #ifdef WIN32
  46. #include <openssl/aes.h>
  47. #include "alac_wrapper.h"
  48. #define MSG_DONTWAIT 0
  49. #else
  50. #include "esp_pthread.h"
  51. #include "esp_system.h"
  52. #include <mbedtls/version.h>
  53. #include <mbedtls/aes.h>
  54. #include "alac_wrapper.h"
  55. #endif
  56. #define NTP2MS(ntp) ((((ntp) >> 10) * 1000L) >> 22)
  57. #define MS2NTP(ms) (((((u64_t) (ms)) << 22) / 1000) << 10)
  58. #define NTP2TS(ntp, rate) ((((ntp) >> 16) * (rate)) >> 16)
  59. #define TS2NTP(ts, rate) (((((u64_t) (ts)) << 16) / (rate)) << 16)
  60. #define MS2TS(ms, rate) ((((u64_t) (ms)) * (rate)) / 1000)
  61. #define TS2MS(ts, rate) NTP2MS(TS2NTP(ts,rate))
  62. extern log_level raop_loglevel;
  63. static log_level *loglevel = &raop_loglevel;
  64. //#define __RTP_STORE
  65. // default buffer size
  66. #define BUFFER_FRAMES ( (150 * RAOP_SAMPLE_RATE * 2) / (352 * 100) )
  67. #define MAX_PACKET 1408
  68. #define MIN_LATENCY 11025
  69. #define MAX_LATENCY ( (120 * RAOP_SAMPLE_RATE * 2) / 100 )
  70. #define RTP_STACK_SIZE (4*1024)
  71. #define RTP_SYNC (0x01)
  72. #define NTP_SYNC (0x02)
  73. #define RESEND_TO 200
  74. enum { DATA = 0, CONTROL, TIMING };
  75. static const u8_t silence_frame[MAX_PACKET] = { 0 };
  76. typedef u16_t seq_t;
  77. typedef struct audio_buffer_entry { // decoded audio packets
  78. int ready;
  79. u32_t rtptime, last_resend;
  80. s16_t *data;
  81. int len;
  82. } abuf_t;
  83. typedef struct rtp_s {
  84. #ifdef __RTP_STORE
  85. FILE *rtpIN, *rtpOUT;
  86. #endif
  87. bool running;
  88. unsigned char aesiv[16];
  89. #ifdef WIN32
  90. AES_KEY aes;
  91. #else
  92. mbedtls_aes_context aes;
  93. #endif
  94. bool decrypt;
  95. u8_t *decrypt_buf;
  96. u32_t frame_size, frame_duration;
  97. u32_t in_frames, out_frames;
  98. struct in_addr host;
  99. struct sockaddr_in rtp_host;
  100. struct {
  101. unsigned short rport, lport;
  102. int sock;
  103. } rtp_sockets[3]; // data, control, timing
  104. struct timing_s {
  105. u64_t local, remote;
  106. } timing;
  107. struct {
  108. u32_t rtp, time;
  109. u8_t status;
  110. } synchro;
  111. struct {
  112. u32_t time;
  113. seq_t seqno;
  114. u32_t rtptime;
  115. } record;
  116. int latency; // rtp hold depth in samples
  117. u32_t resent_req, resent_rec; // total resent + recovered frames
  118. u32_t silent_frames; // total silence frames
  119. u32_t discarded;
  120. abuf_t audio_buffer[BUFFER_FRAMES];
  121. seq_t ab_read, ab_write;
  122. pthread_mutex_t ab_mutex;
  123. #ifdef WIN32
  124. pthread_t thread;
  125. #else
  126. TaskHandle_t thread, joiner;
  127. StaticTask_t *xTaskBuffer;
  128. StackType_t xStack[RTP_STACK_SIZE] __attribute__ ((aligned (4)));
  129. #endif
  130. struct alac_codec_s *alac_codec;
  131. int flush_seqno;
  132. bool playing;
  133. raop_data_cb_t data_cb;
  134. raop_cmd_cb_t cmd_cb;
  135. } rtp_t;
  136. #define BUFIDX(seqno) ((seq_t)(seqno) % BUFFER_FRAMES)
  137. static void buffer_alloc(abuf_t *audio_buffer, int size);
  138. static void buffer_release(abuf_t *audio_buffer);
  139. static void buffer_reset(abuf_t *audio_buffer);
  140. static void buffer_push_packet(rtp_t *ctx);
  141. static bool rtp_request_resend(rtp_t *ctx, seq_t first, seq_t last);
  142. static bool rtp_request_timing(rtp_t *ctx);
  143. static void* rtp_thread_func(void *arg);
  144. static int seq_order(seq_t a, seq_t b);
  145. /*---------------------------------------------------------------------------*/
  146. static struct alac_codec_s* alac_init(int fmtp[32]) {
  147. struct alac_codec_s *alac;
  148. unsigned sample_rate;
  149. unsigned char sample_size, channels;
  150. struct {
  151. uint32_t frameLength;
  152. uint8_t compatibleVersion;
  153. uint8_t bitDepth;
  154. uint8_t pb;
  155. uint8_t mb;
  156. uint8_t kb;
  157. uint8_t numChannels;
  158. uint16_t maxRun;
  159. uint32_t maxFrameBytes;
  160. uint32_t avgBitRate;
  161. uint32_t sampleRate;
  162. } config;
  163. config.frameLength = htonl(fmtp[1]);
  164. config.compatibleVersion = fmtp[2];
  165. config.bitDepth = fmtp[3];
  166. config.pb = fmtp[4];
  167. config.mb = fmtp[5];
  168. config.kb = fmtp[6];
  169. config.numChannels = fmtp[7];
  170. config.maxRun = htons(fmtp[8]);
  171. config.maxFrameBytes = htonl(fmtp[9]);
  172. config.avgBitRate = htonl(fmtp[10]);
  173. config.sampleRate = htonl(fmtp[11]);
  174. alac = alac_create_decoder(sizeof(config), (unsigned char*) &config, &sample_size, &sample_rate, &channels);
  175. if (!alac) {
  176. LOG_ERROR("cannot create alac codec", NULL);
  177. return NULL;
  178. }
  179. return alac;
  180. }
  181. /*---------------------------------------------------------------------------*/
  182. rtp_resp_t rtp_init(struct in_addr host, int latency, char *aeskey, char *aesiv, char *fmtpstr,
  183. short unsigned pCtrlPort, short unsigned pTimingPort,
  184. raop_cmd_cb_t cmd_cb, raop_data_cb_t data_cb)
  185. {
  186. int i = 0;
  187. char *arg;
  188. int fmtp[12];
  189. bool rc = true;
  190. rtp_t *ctx = calloc(1, sizeof(rtp_t));
  191. rtp_resp_t resp = { 0, 0, 0, NULL };
  192. if (!ctx) return resp;
  193. ctx->host = host;
  194. ctx->decrypt = false;
  195. ctx->cmd_cb = cmd_cb;
  196. ctx->data_cb = data_cb;
  197. ctx->rtp_host.sin_family = AF_INET;
  198. ctx->rtp_host.sin_addr.s_addr = INADDR_ANY;
  199. pthread_mutex_init(&ctx->ab_mutex, 0);
  200. ctx->flush_seqno = -1;
  201. ctx->latency = latency;
  202. ctx->ab_read = ctx->ab_write;
  203. #ifdef __RTP_STORE
  204. ctx->rtpIN = fopen("airplay.rtpin", "wb");
  205. ctx->rtpOUT = fopen("airplay.rtpout", "wb");
  206. #endif
  207. ctx->rtp_sockets[CONTROL].rport = pCtrlPort;
  208. ctx->rtp_sockets[TIMING].rport = pTimingPort;
  209. if (aesiv && aeskey) {
  210. memcpy(ctx->aesiv, aesiv, 16);
  211. #ifdef WIN32
  212. AES_set_decrypt_key((unsigned char*) aeskey, 128, &ctx->aes);
  213. #else
  214. memset(&ctx->aes, 0, sizeof(mbedtls_aes_context));
  215. mbedtls_aes_setkey_dec(&ctx->aes, (unsigned char*) aeskey, 128);
  216. #endif
  217. ctx->decrypt = true;
  218. ctx->decrypt_buf = malloc(MAX_PACKET);
  219. }
  220. memset(fmtp, 0, sizeof(fmtp));
  221. while ((arg = strsep(&fmtpstr, " \t")) != NULL) fmtp[i++] = atoi(arg);
  222. ctx->frame_size = fmtp[1];
  223. ctx->frame_duration = (ctx->frame_size * 1000) / RAOP_SAMPLE_RATE;
  224. // alac decoder
  225. ctx->alac_codec = alac_init(fmtp);
  226. rc &= ctx->alac_codec != NULL;
  227. buffer_alloc(ctx->audio_buffer, ctx->frame_size*4);
  228. // create rtp ports
  229. for (i = 0; i < 3; i++) {
  230. ctx->rtp_sockets[i].sock = bind_socket(&ctx->rtp_sockets[i].lport, SOCK_DGRAM);
  231. rc &= ctx->rtp_sockets[i].sock > 0;
  232. }
  233. // create http port and start listening
  234. resp.cport = ctx->rtp_sockets[CONTROL].lport;
  235. resp.tport = ctx->rtp_sockets[TIMING].lport;
  236. resp.aport = ctx->rtp_sockets[DATA].lport;
  237. ctx->running = true;
  238. #ifdef WIN32
  239. pthread_create(&ctx->thread, NULL, rtp_thread_func, (void *) ctx);
  240. #else
  241. ctx->xTaskBuffer = (StaticTask_t*) heap_caps_malloc(sizeof(StaticTask_t), MALLOC_CAP_INTERNAL | MALLOC_CAP_8BIT);
  242. ctx->thread = xTaskCreateStatic( (TaskFunction_t) rtp_thread_func, "RTP_thread", RTP_STACK_SIZE, ctx,
  243. CONFIG_ESP32_PTHREAD_TASK_PRIO_DEFAULT + 1, ctx->xStack, ctx->xTaskBuffer );
  244. #endif
  245. // cleanup everything if we failed
  246. if (!rc) {
  247. LOG_ERROR("[%p]: cannot start RTP", ctx);
  248. rtp_end(ctx);
  249. ctx = NULL;
  250. }
  251. resp.ctx = ctx;
  252. return resp;
  253. }
  254. /*---------------------------------------------------------------------------*/
  255. void rtp_end(rtp_t *ctx)
  256. {
  257. int i;
  258. if (!ctx) return;
  259. if (ctx->running) {
  260. #if !defined WIN32
  261. ctx->joiner = xTaskGetCurrentTaskHandle();
  262. #endif
  263. ctx->running = false;
  264. #ifdef WIN32
  265. pthread_join(ctx->thread, NULL);
  266. #else
  267. ulTaskNotifyTake(pdFALSE, portMAX_DELAY);
  268. vTaskDelete(ctx->thread);
  269. heap_caps_free(ctx->xTaskBuffer);
  270. #endif
  271. }
  272. for (i = 0; i < 3; i++) closesocket(ctx->rtp_sockets[i].sock);
  273. if (ctx->alac_codec) alac_delete_decoder(ctx->alac_codec);
  274. if (ctx->decrypt_buf) free(ctx->decrypt_buf);
  275. pthread_mutex_destroy(&ctx->ab_mutex);
  276. buffer_release(ctx->audio_buffer);
  277. free(ctx);
  278. #ifdef __RTP_STORE
  279. fclose(ctx->rtpIN);
  280. fclose(ctx->rtpOUT);
  281. #endif
  282. }
  283. /*---------------------------------------------------------------------------*/
  284. bool rtp_flush(rtp_t *ctx, unsigned short seqno, unsigned int rtptime, bool exit_locked)
  285. {
  286. bool rc = true;
  287. u32_t now = gettime_ms();
  288. if (now < ctx->record.time + 250 || (ctx->record.seqno == seqno && ctx->record.rtptime == rtptime)) {
  289. rc = false;
  290. LOG_ERROR("[%p]: FLUSH ignored as same as RECORD (%hu - %u)", ctx, seqno, rtptime);
  291. } else {
  292. pthread_mutex_lock(&ctx->ab_mutex);
  293. buffer_reset(ctx->audio_buffer);
  294. ctx->playing = false;
  295. ctx->flush_seqno = seqno;
  296. if (!exit_locked) pthread_mutex_unlock(&ctx->ab_mutex);
  297. }
  298. LOG_INFO("[%p]: flush %hu %u", ctx, seqno, rtptime);
  299. return rc;
  300. }
  301. /*---------------------------------------------------------------------------*/
  302. void rtp_flush_release(rtp_t *ctx) {
  303. pthread_mutex_unlock(&ctx->ab_mutex);
  304. }
  305. /*---------------------------------------------------------------------------*/
  306. void rtp_record(rtp_t *ctx, unsigned short seqno, unsigned rtptime) {
  307. ctx->record.seqno = seqno;
  308. ctx->record.rtptime = rtptime;
  309. ctx->record.time = gettime_ms();
  310. LOG_INFO("[%p]: record %hu %u", ctx, seqno, rtptime);
  311. }
  312. /*---------------------------------------------------------------------------*/
  313. static void buffer_alloc(abuf_t *audio_buffer, int size) {
  314. int i;
  315. for (i = 0; i < BUFFER_FRAMES; i++) {
  316. audio_buffer[i].data = malloc(size);
  317. audio_buffer[i].ready = 0;
  318. }
  319. }
  320. /*---------------------------------------------------------------------------*/
  321. static void buffer_release(abuf_t *audio_buffer) {
  322. int i;
  323. for (i = 0; i < BUFFER_FRAMES; i++) {
  324. free(audio_buffer[i].data);
  325. }
  326. }
  327. /*---------------------------------------------------------------------------*/
  328. static void buffer_reset(abuf_t *audio_buffer) {
  329. int i;
  330. for (i = 0; i < BUFFER_FRAMES; i++) audio_buffer[i].ready = 0;
  331. }
  332. /*---------------------------------------------------------------------------*/
  333. // the sequence numbers will wrap pretty often.
  334. // this returns true if the second arg is after the first
  335. static int seq_order(seq_t a, seq_t b) {
  336. s16_t d = b - a;
  337. return d > 0;
  338. }
  339. /*---------------------------------------------------------------------------*/
  340. static void alac_decode(rtp_t *ctx, s16_t *dest, char *buf, int len, int *outsize) {
  341. unsigned char iv[16];
  342. int aeslen;
  343. assert(len<=MAX_PACKET);
  344. if (ctx->decrypt) {
  345. aeslen = len & ~0xf;
  346. memcpy(iv, ctx->aesiv, sizeof(iv));
  347. #ifdef WIN32
  348. AES_cbc_encrypt((unsigned char*)buf, ctx->decrypt_buf, aeslen, &ctx->aes, iv, AES_DECRYPT);
  349. #else
  350. mbedtls_aes_crypt_cbc(&ctx->aes, MBEDTLS_AES_DECRYPT, aeslen, iv, (unsigned char*) buf, ctx->decrypt_buf);
  351. #endif
  352. memcpy(ctx->decrypt_buf+aeslen, buf+aeslen, len-aeslen);
  353. alac_to_pcm(ctx->alac_codec, (unsigned char*) ctx->decrypt_buf, (unsigned char*) dest, 2, (unsigned int*) outsize);
  354. } else {
  355. alac_to_pcm(ctx->alac_codec, (unsigned char*) buf, (unsigned char*) dest, 2, (unsigned int*) outsize);
  356. }
  357. *outsize *= 4;
  358. }
  359. /*---------------------------------------------------------------------------*/
  360. static void buffer_put_packet(rtp_t *ctx, seq_t seqno, unsigned rtptime, bool first, char *data, int len) {
  361. abuf_t *abuf = NULL;
  362. u32_t playtime;
  363. pthread_mutex_lock(&ctx->ab_mutex);
  364. if (!ctx->playing) {
  365. if ((ctx->flush_seqno == -1 || seq_order(ctx->flush_seqno, seqno)) &&
  366. (ctx->synchro.status & RTP_SYNC) && (ctx->synchro.status & NTP_SYNC)) {
  367. ctx->ab_write = seqno-1;
  368. ctx->ab_read = seqno;
  369. ctx->flush_seqno = -1;
  370. ctx->playing = true;
  371. ctx->resent_req = ctx->resent_rec = ctx->silent_frames = ctx->discarded = 0;
  372. playtime = ctx->synchro.time + ((rtptime - ctx->synchro.rtp) * 10) / (RAOP_SAMPLE_RATE / 100);
  373. ctx->cmd_cb(RAOP_PLAY, playtime);
  374. } else {
  375. pthread_mutex_unlock(&ctx->ab_mutex);
  376. return;
  377. }
  378. }
  379. if (seqno == (u16_t) (ctx->ab_write+1)) {
  380. // expected packet
  381. abuf = ctx->audio_buffer + BUFIDX(seqno);
  382. ctx->ab_write = seqno;
  383. LOG_SDEBUG("packet expected seqno:%hu rtptime:%u (W:%hu R:%hu)", seqno, rtptime, ctx->ab_write, ctx->ab_read);
  384. } else if (seq_order(ctx->ab_write, seqno)) {
  385. seq_t i;
  386. u32_t now;
  387. // newer than expected
  388. if (ctx->latency && seq_order(ctx->latency / ctx->frame_size, seqno - ctx->ab_write - 1)) {
  389. // only get rtp latency-1 frames back (last one is seqno)
  390. LOG_WARN("[%p] too many missing frames %hu seq: %hu, (W:%hu R:%hu)", ctx, seqno - ctx->ab_write - 1, seqno, ctx->ab_write, ctx->ab_read);
  391. ctx->ab_write = seqno - ctx->latency / ctx->frame_size;
  392. }
  393. // need to request re-send and adjust timing of gaps
  394. rtp_request_resend(ctx, ctx->ab_write + 1, seqno-1);
  395. for (now = gettime_ms(), i = ctx->ab_write + 1; seq_order(i, seqno); i++) {
  396. ctx->audio_buffer[BUFIDX(i)].rtptime = rtptime - (seqno-i)*ctx->frame_size;
  397. ctx->audio_buffer[BUFIDX(i)].last_resend = now;
  398. }
  399. LOG_DEBUG("[%p]: packet newer seqno:%hu rtptime:%u (W:%hu R:%hu)", ctx, seqno, rtptime, ctx->ab_write, ctx->ab_read);
  400. abuf = ctx->audio_buffer + BUFIDX(seqno);
  401. ctx->ab_write = seqno;
  402. } else if (seq_order(ctx->ab_read, seqno + 1)) {
  403. // recovered packet, not yet sent
  404. abuf = ctx->audio_buffer + BUFIDX(seqno);
  405. ctx->resent_rec++;
  406. LOG_DEBUG("[%p]: packet recovered seqno:%hu rtptime:%u (W:%hu R:%hu)", ctx, seqno, rtptime, ctx->ab_write, ctx->ab_read);
  407. } else {
  408. // too late
  409. LOG_DEBUG("[%p]: packet too late seqno:%hu rtptime:%u (W:%hu R:%hu)", ctx, seqno, rtptime, ctx->ab_write, ctx->ab_read);
  410. }
  411. if (ctx->in_frames++ > 1000) {
  412. LOG_INFO("[%p]: fill [level:%hu rec:%u] [W:%hu R:%hu]", ctx, ctx->ab_write - ctx->ab_read, ctx->resent_rec, ctx->ab_write, ctx->ab_read);
  413. ctx->in_frames = 0;
  414. }
  415. if (abuf) {
  416. alac_decode(ctx, abuf->data, data, len, &abuf->len);
  417. abuf->ready = 1;
  418. // this is the local rtptime when this frame is expected to play
  419. abuf->rtptime = rtptime;
  420. buffer_push_packet(ctx);
  421. #ifdef __RTP_STORE
  422. fwrite(data, len, 1, ctx->rtpIN);
  423. fwrite(abuf->data, abuf->len, 1, ctx->rtpOUT);
  424. #endif
  425. }
  426. pthread_mutex_unlock(&ctx->ab_mutex);
  427. }
  428. /*---------------------------------------------------------------------------*/
  429. // push as many frames as possible through callback
  430. static void buffer_push_packet(rtp_t *ctx) {
  431. abuf_t *curframe = NULL;
  432. u32_t now, playtime, hold = max((ctx->latency * 1000) / (8 * RAOP_SAMPLE_RATE), 100);
  433. int i;
  434. // not ready to play yet
  435. if (!ctx->playing || ctx->synchro.status != (RTP_SYNC | NTP_SYNC)) return;
  436. // there is always at least one frame in the buffer
  437. do {
  438. // re-evaluate time in loop in case data callback blocks ...
  439. now = gettime_ms();
  440. // try to manage playtime so that we overflow as late as possible if we miss NTP (2^31 / 10 / 44100)
  441. curframe = ctx->audio_buffer + BUFIDX(ctx->ab_read);
  442. playtime = ctx->synchro.time + ((curframe->rtptime - ctx->synchro.rtp) * 10) / (RAOP_SAMPLE_RATE / 100);
  443. if (now > playtime) {
  444. LOG_DEBUG("[%p]: discarded frame now:%u missed by:%d (W:%hu R:%hu)", ctx, now, now - playtime, ctx->ab_write, ctx->ab_read);
  445. ctx->discarded++;
  446. curframe->ready = 0;
  447. } else if (playtime - now <= hold) {
  448. if (curframe->ready) {
  449. ctx->data_cb((const u8_t*) curframe->data, curframe->len, playtime);
  450. curframe->ready = 0;
  451. } else {
  452. LOG_DEBUG("[%p]: created zero frame (W:%hu R:%hu)", ctx, ctx->ab_write, ctx->ab_read);
  453. ctx->data_cb(silence_frame, ctx->frame_size * 4, playtime);
  454. ctx->silent_frames++;
  455. }
  456. } else if (curframe->ready) {
  457. ctx->data_cb((const u8_t*) curframe->data, curframe->len, playtime);
  458. curframe->ready = 0;
  459. } else {
  460. break;
  461. }
  462. ctx->ab_read++;
  463. ctx->out_frames++;
  464. } while (seq_order(ctx->ab_read, ctx->ab_write));
  465. if (ctx->out_frames > 1000) {
  466. LOG_INFO("[%p]: drain [level:%hd head:%d ms] [W:%hu R:%hu] [req:%u sil:%u dis:%u]",
  467. ctx, ctx->ab_write - ctx->ab_read, playtime - now, ctx->ab_write, ctx->ab_read,
  468. ctx->resent_req, ctx->silent_frames, ctx->discarded);
  469. ctx->out_frames = 0;
  470. }
  471. LOG_SDEBUG("playtime %u %d [W:%hu R:%hu] %d", playtime, playtime - now, ctx->ab_write, ctx->ab_read, curframe->ready);
  472. // each missing packet will be requested up to (latency_frames / 16) times
  473. for (i = 0; seq_order(ctx->ab_read + i, ctx->ab_write); i += 16) {
  474. abuf_t *frame = ctx->audio_buffer + BUFIDX(ctx->ab_read + i);
  475. if (!frame->ready && now - frame->last_resend > RESEND_TO) {
  476. rtp_request_resend(ctx, ctx->ab_read + i, ctx->ab_read + i);
  477. frame->last_resend = now;
  478. }
  479. }
  480. }
  481. /*---------------------------------------------------------------------------*/
  482. static void *rtp_thread_func(void *arg) {
  483. fd_set fds;
  484. int i, sock = -1;
  485. int count = 0;
  486. bool ntp_sent;
  487. char *packet = malloc(MAX_PACKET);
  488. rtp_t *ctx = (rtp_t*) arg;
  489. for (i = 0; i < 3; i++) {
  490. if (ctx->rtp_sockets[i].sock > sock) sock = ctx->rtp_sockets[i].sock;
  491. // send synchro request 3 times
  492. ntp_sent = rtp_request_timing(ctx);
  493. }
  494. while (ctx->running) {
  495. ssize_t plen;
  496. char type;
  497. socklen_t rtp_client_len = sizeof(struct sockaddr_in);
  498. int idx = 0;
  499. char *pktp = packet;
  500. struct timeval timeout = {0, 100*1000};
  501. FD_ZERO(&fds);
  502. for (i = 0; i < 3; i++) { FD_SET(ctx->rtp_sockets[i].sock, &fds); }
  503. if (select(sock + 1, &fds, NULL, NULL, &timeout) <= 0) continue;
  504. for (i = 0; i < 3; i++)
  505. if (FD_ISSET(ctx->rtp_sockets[i].sock, &fds)) idx = i;
  506. plen = recvfrom(ctx->rtp_sockets[idx].sock, packet, MAX_PACKET, MSG_DONTWAIT, (struct sockaddr*) &ctx->rtp_host, &rtp_client_len);
  507. if (!ntp_sent) {
  508. LOG_WARN("[%p]: NTP request not send yet", ctx);
  509. ntp_sent = rtp_request_timing(ctx);
  510. }
  511. if (plen <= 0) {
  512. LOG_WARN("Nothing received on a readable socket %d", plen);
  513. continue;
  514. }
  515. assert(plen <= MAX_PACKET);
  516. type = packet[1] & ~0x80;
  517. pktp = packet;
  518. switch (type) {
  519. seq_t seqno;
  520. unsigned rtptime;
  521. // re-sent packet
  522. case 0x56: {
  523. pktp += 4;
  524. plen -= 4;
  525. }
  526. // data packet
  527. case 0x60: {
  528. seqno = ntohs(*(u16_t*)(pktp+2));
  529. rtptime = ntohl(*(u32_t*)(pktp+4));
  530. // adjust pointer and length
  531. pktp += 12;
  532. plen -= 12;
  533. LOG_SDEBUG("[%p]: seqno:%hu rtp:%u (type: %x, first: %u)", ctx, seqno, rtptime, type, packet[1] & 0x80);
  534. // check if packet contains enough content to be reasonable
  535. if (plen < 16) break;
  536. if ((packet[1] & 0x80) && (type != 0x56)) {
  537. LOG_INFO("[%p]: 1st audio packet received", ctx);
  538. }
  539. buffer_put_packet(ctx, seqno, rtptime, packet[1] & 0x80, pktp, plen);
  540. break;
  541. }
  542. // sync packet
  543. case 0x54: {
  544. u32_t rtp_now_latency = ntohl(*(u32_t*)(pktp+4));
  545. u64_t remote = (((u64_t) ntohl(*(u32_t*)(pktp+8))) << 32) + ntohl(*(u32_t*)(pktp+12));
  546. u32_t rtp_now = ntohl(*(u32_t*)(pktp+16));
  547. u16_t flags = ntohs(*(u16_t*)(pktp+2));
  548. u32_t remote_gap = NTP2MS(remote - ctx->timing.remote);
  549. // try to get NTP every 3 sec or every time if we are not synced
  550. if (!count-- || !(ctx->synchro.status & NTP_SYNC)) {
  551. rtp_request_timing(ctx);
  552. count = 3;
  553. }
  554. // something is wrong, we should not have such gap
  555. if (remote_gap > 10000) {
  556. LOG_WARN("discarding remote timing information %u", remote_gap);
  557. break;
  558. }
  559. pthread_mutex_lock(&ctx->ab_mutex);
  560. // re-align timestamp and expected local playback time (and magic 11025 latency)
  561. ctx->latency = rtp_now - rtp_now_latency;
  562. if (flags == 7 || flags == 4) ctx->latency += 11025;
  563. if (ctx->latency < MIN_LATENCY) ctx->latency = MIN_LATENCY;
  564. else if (ctx->latency > MAX_LATENCY) ctx->latency = MAX_LATENCY;
  565. ctx->synchro.rtp = rtp_now - ctx->latency;
  566. ctx->synchro.time = ctx->timing.local + remote_gap;
  567. // now we are synced on RTP frames
  568. ctx->synchro.status |= RTP_SYNC;
  569. // 1st sync packet received (signals a restart of playback)
  570. if (packet[0] & 0x10) {
  571. LOG_INFO("[%p]: 1st sync packet received", ctx);
  572. }
  573. pthread_mutex_unlock(&ctx->ab_mutex);
  574. LOG_DEBUG("[%p]: sync packet latency:%d rtp_latency:%u rtp:%u remote ntp:%llx, local time:%u local rtp:%u (now:%u)",
  575. ctx, ctx->latency, rtp_now_latency, rtp_now, remote, ctx->synchro.time, ctx->synchro.rtp, gettime_ms());
  576. if ((ctx->synchro.status & RTP_SYNC) && (ctx->synchro.status & NTP_SYNC)) ctx->cmd_cb(RAOP_TIMING);
  577. break;
  578. }
  579. // NTP timing packet
  580. case 0x53: {
  581. u64_t expected;
  582. u32_t reference = ntohl(*(u32_t*)(pktp+12)); // only low 32 bits in our case
  583. u64_t remote =(((u64_t) ntohl(*(u32_t*)(pktp+16))) << 32) + ntohl(*(u32_t*)(pktp+20));
  584. u32_t roundtrip = gettime_ms() - reference;
  585. // better discard sync packets when roundtrip is suspicious
  586. if (roundtrip > 100) {
  587. // ask for another one only if we are not synced already
  588. if (!(ctx->synchro.status & NTP_SYNC)) rtp_request_timing(ctx);
  589. LOG_WARN("[%p]: discarding NTP roundtrip of %u ms", ctx, roundtrip);
  590. break;
  591. }
  592. /*
  593. The expected elapsed remote time should be exactly the same as
  594. elapsed local time between the two request, corrected by the
  595. drifting
  596. */
  597. expected = ctx->timing.remote + MS2NTP(reference - ctx->timing.local);
  598. ctx->timing.remote = remote;
  599. ctx->timing.local = reference;
  600. // now we are synced on NTP (mutex not needed)
  601. ctx->synchro.status |= NTP_SYNC;
  602. LOG_DEBUG("[%p]: Timing references local:%llu, remote:%llx (delta:%lld, sum:%lld, adjust:%lld, gaps:%d)",
  603. ctx, ctx->timing.local, ctx->timing.remote);
  604. break;
  605. }
  606. default: {
  607. LOG_WARN("Unknown packet received %x", (int) type);
  608. break;
  609. }
  610. }
  611. }
  612. free(packet);
  613. LOG_INFO("[%p]: terminating", ctx);
  614. #ifndef WIN32
  615. xTaskNotifyGive(ctx->joiner);
  616. vTaskSuspend(NULL);
  617. #endif
  618. return NULL;
  619. }
  620. /*---------------------------------------------------------------------------*/
  621. static bool rtp_request_timing(rtp_t *ctx) {
  622. unsigned char req[32];
  623. u32_t now = gettime_ms();
  624. int i;
  625. struct sockaddr_in host;
  626. LOG_DEBUG("[%p]: timing request now:%u (port: %hu)", ctx, now, ctx->rtp_sockets[TIMING].rport);
  627. req[0] = 0x80;
  628. req[1] = 0x52|0x80;
  629. *(u16_t*)(req+2) = htons(7);
  630. *(u32_t*)(req+4) = htonl(0); // dummy
  631. for (i = 0; i < 16; i++) req[i+8] = 0;
  632. *(u32_t*)(req+24) = 0;
  633. *(u32_t*)(req+28) = htonl(now); // this is not a real NTP, but a 32 ms counter in the low part of the NTP
  634. if (ctx->host.s_addr != INADDR_ANY) {
  635. host.sin_family = AF_INET;
  636. host.sin_addr = ctx->host;
  637. } else host = ctx->rtp_host;
  638. // no address from sender, need to wait for 1st packet to be received
  639. if (host.sin_addr.s_addr == INADDR_ANY) return false;
  640. host.sin_port = htons(ctx->rtp_sockets[TIMING].rport);
  641. if (sizeof(req) != sendto(ctx->rtp_sockets[TIMING].sock, req, sizeof(req), MSG_DONTWAIT, (struct sockaddr*) &host, sizeof(host))) {
  642. LOG_WARN("[%p]: SENDTO failed (%s)", ctx, strerror(errno));
  643. }
  644. return true;
  645. }
  646. /*---------------------------------------------------------------------------*/
  647. static bool rtp_request_resend(rtp_t *ctx, seq_t first, seq_t last) {
  648. unsigned char req[8]; // *not* a standard RTCP NACK
  649. // do not request silly ranges (happens in case of network large blackouts)
  650. if (seq_order(last, first) || last - first > BUFFER_FRAMES / 2) return false;
  651. ctx->resent_req += (seq_t) (last - first) + 1;
  652. LOG_DEBUG("resend request [W:%hu R:%hu first=%hu last=%hu]", ctx->ab_write, ctx->ab_read, first, last);
  653. req[0] = 0x80;
  654. req[1] = 0x55|0x80; // Apple 'resend'
  655. *(u16_t*)(req+2) = htons(1); // our seqnum
  656. *(u16_t*)(req+4) = htons(first); // missed seqnum
  657. *(u16_t*)(req+6) = htons(last-first+1); // count
  658. ctx->rtp_host.sin_port = htons(ctx->rtp_sockets[CONTROL].rport);
  659. if (sizeof(req) != sendto(ctx->rtp_sockets[CONTROL].sock, req, sizeof(req), MSG_DONTWAIT, (struct sockaddr*) &ctx->rtp_host, sizeof(ctx->rtp_host))) {
  660. LOG_WARN("[%p]: SENDTO failed (%s)", ctx, strerror(errno));
  661. }
  662. return true;
  663. }