snapclient/components/rtprx/rtprx.c

/**
 * \file    rtprx.c
 * \author  Jorgen Kragh Jakobsen
 * \date    19.10.2019
 * \version   0.1
 *
 * \brief RTP audio stream receiver
 *
 * \warning This software is a PROTOTYPE version and is not designed or intended
 * for use in production, especially not for safety-critical applications! The
 * user represents and warrants that it will NOT use or redistribute the
 * Software for such purposes. This prototype is for research purposes only.
 * This software is provided "AS IS," without a warranty of any kind.
 */
#include "rtprx.h"

#include <lwip/netdb.h>

#include "i2s.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "esp_wifi.h"
#include "freertos/FreeRTOS.h"
#include "lwip/api.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include "opus.h"

// extern uint32_t sample_rate = 48000;   xxx fixme

static bool rtpRxState = 0;

void rtp_rx_start() {
  if (rtpRxState == 0) {
    setup_rtp_i2s();
    xTaskCreate(rtp_rx_task, "RTPRx", 12 * 1024, NULL, 0, NULL);
    rtpRxState = 1;
  }
}

void rtp_rx_stop() {
  if (rtpRxState == 1) {
    i2s_custom_driver_uninstall(0);
    // vTaskDelete(xTaskSignal);    xxx Fix me
    rtpRxState = 0;
  }
}

void rtp_rx_task(void *pvParameters) {
  OpusDecoder *decoder;

  // int size = opus_decoder_get_size(2);
  int oe = 0;
  decoder = opus_decoder_create(48000, 2, &oe);
  //  int error = opus_decoder_init(decoder, 48000, 2);
  printf("Initialized Decoder: %d", oe);

  // int32_t *audio32 = (int32_t*)malloc(960*sizeof(int32_t));
  int16_t *audio = (int16_t *)malloc(960 * 2 * sizeof(int16_t));
  i2s_custom_zero_dma_buffer(0);
  static struct netconn *conn;
  static struct netbuf *buf;
  static uint32_t pkg = 0;
  static uint32_t pkgerror = 0;
  err_t err;
  uint16_t oldseq = 1;
  uint16_t first = 1;
  conn = netconn_new(NETCONN_UDP);
  if (conn != NULL) {
    printf("Net RTP RX\n");
    netconn_bind(conn, IP_ADDR_ANY, 1350);
    netconn_listen(conn);
    printf("Net RTP will enter loopn\n");
    while (1) {
      netconn_recv(conn, &buf);
      if (buf == NULL) {
        printf("NETCONN RX error \n");
      }
      pkg++;

      uint8_t *p = (buf->p->payload);
      uint16_t seq = (p[2] << 8) + p[3];
      if ((seq != oldseq + 1) & (first != 1)) {
        printf("seq : %d, oldseq : %d \n", seq, oldseq);
        uint16_t errors = seq - oldseq - 1;
        pkgerror = pkgerror + errors;
        printf("ERROR --- Package drop : %d  %d \n", errors, pkgerror);
        size_t bWritten;
        // for (int i = 0; i;i++ )
        int ret = i2s_custom_write_expand(0, (char *)audio, 960 * 2 * sizeof(int16_t),
                                   16, 32, &bWritten, 100);
        printf("bWritten : %d  ret : %d \n ", bWritten, ret);

        // opus_pkg = NULL;
      }

      if (seq < oldseq) {
        printf("ERROR --- Package order:");
      }
      oldseq = seq;
      first = 0;
      // printf("UDP package len : %d ->  \n", buf->p->len);
      // printf("UDP package     : %02x %02x %02x %02x\n",p[0],p[1],p[2],p[3]);
      // printf("Timestamp       : %02x %02x %02x %02x\n",p[4],p[5],p[6],p[7]);
      // printf("Sync source     : %02x %02x %02x
      // %02x\n",p[8],p[9],p[10],p[11]); printf("R1              : %d
      // \n",(p[12]&0xf8)>>3) ; int size = 240;
      unsigned char *opus_pkg = buf->p->payload + 12;
      int size = opus_decode(decoder, (unsigned char *)opus_pkg,
                             buf->p->len - 12, (opus_int16 *)audio, 960, 0);
      if (size < 0) {
        printf("Decode error : %d \n", size);
      }

      // for (int i = 0; i < size*2; i++) {
      //  audio[i*2]   = 0x0000;
      //  audio[i*2+1] = 0x0000;
      //}

      size_t bWritten;
      int ret = i2s_custom_write_expand(0, (char *)audio, size * 2 * sizeof(int16_t),
                                 16, 32, &bWritten, 100);
      if (ret != 0)
        printf("Error I2S written: %d %d %d \n", ret,
               size * 2 * sizeof(int16_t), bWritten);

      if ((pkg % 1000) == 1) {
        // printf("I2S written: %d %d \n", size*sizeof(int32_t) ,bWritten);
        printf("%d > %d %d %d\n", pkg, size, buf->p->len, buf->p->tot_len);
        printf("UDP package len : %d ->  \n", buf->p->len);
        printf("UDP package     : %02x %02x %02x %02x\n", p[0], p[1], p[2],
               p[3]);
        printf("Timestamp       : %02x %02x %02x %02x\n", p[4], p[5], p[6],
               p[7]);
        printf("Sync source     : %02x %02x %02x %02x\n", p[8], p[9], p[10],
               p[11]);
        printf("R1              : %d \n", (p[12] & 0xf8) >> 3);

        for (int i = 0; i < 8; i++)
          printf("%02d %04x %04x\n", i, audio[2 * i], audio[2 * i + 1]);
      }

      // netbuf_free(buf);
      netbuf_delete(buf);
    }
  }
  netconn_close(conn);
  netconn_delete(conn);
}

void setup_rtp_i2s() {
  i2s_config_t i2s_config = {
      .mode = I2S_MODE_MASTER | I2S_MODE_TX,  // Only TX
      .sample_rate = 48000,
      .bits_per_sample = 32,
      .channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,  // 2-channels
      .communication_format = I2S_COMM_FORMAT_I2S,
      .dma_buf_count = 8,
      .dma_buf_len = 480,
      //.intr_alloc_flags = 1, //Default interrupt priority
      .use_apll = true,
      .fixed_mclk = 0,
      .tx_desc_auto_clear = true  // Auto clear tx descriptor on underflow
  };

  i2s_custom_driver_install(0, &i2s_config, 0, NULL);
  i2s_custom_start(0);
  i2s_custom_zero_dma_buffer(0);

  i2s_pin_config_t pin_config = {
      .bck_io_num = 12,    // CONFIG_EXAMPLE_I2S_BCK_PIN,
      .ws_io_num = 13,     // CONFIG_EXAMPLE_I2S_LRCK_PIN,
      .data_out_num = 14,  // CONFIG_EXAMPLE_I2S_DATA_PIN,
      .data_in_num = -1    // Not used
  };

  i2s_custom_set_pin(0, &pin_config);
  printf("Here... set pin\n");
}