path: root/toxav/codec.c

/**  codec.c
 *
 *   Audio and video codec intitialization, encoding/decoding and playback
 *
 *   Copyright (C) 2013 Tox project All Rights Reserved.
 *
 *   This file is part of Tox.
 *
 *   Tox is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   Tox is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with Tox. If not, see <http://www.gnu.org/licenses/>.
 *
 */


#ifdef HAVE_CONFIG_H
#include "config.h"
#endif /* HAVE_CONFIG_H */

#include "../toxcore/logger.h"

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include <time.h>

#include "msi.h"
#include "rtp.h"
#include "codec.h"

/* Assume 24 fps*/
#define MAX_ENCODE_TIME_US ((1000 / 24) * 1000)
#define MAX_DECODE_TIME_US 0

// TODO this has to be exchanged in msi
#define MAX_VIDEOFRAME_SIZE 0x40000 /* 256KiB */
#define VIDEOFRAME_PIECE_SIZE 0x500 /* 1.25 KiB*/
#define VIDEOFRAME_HEADER_SIZE 0x2

/* FIXME: Might not be enough */
#define VIDEO_DECODE_BUFFER_SIZE 20

#define ARRAY(TYPE__) struct { uint16_t size; TYPE__  data[]; }
#define PAIR(TYPE1__, TYPE2__) struct { TYPE1__ first; TYPE2__ second; }

typedef ARRAY(uint8_t) Payload;

static PAIR(CSVideoCallback, void *) vpcallback;
static PAIR(CSAudioCallback, void *) apcallback;

typedef struct {
    uint16_t size; /* Max size */
    uint16_t start;
    uint16_t end;
    Payload **packets;
} PayloadBuffer;

static _Bool buffer_full(const PayloadBuffer *b)
{
    return (b->end + 1) % b->size == b->start;
}

static _Bool buffer_empty(const PayloadBuffer *b)
{
    return b->end == b->start;
}

static void buffer_write(PayloadBuffer *b, Payload *p)
{
    b->packets[b->end] = p;
    b->end = (b->end + 1) % b->size;

    if (b->end == b->start) b->start = (b->start + 1) % b->size; /* full, overwrite */
}

static void buffer_read(PayloadBuffer *b, Payload **p)
{
    *p = b->packets[b->start];
    b->start = (b->start + 1) % b->size;
}

static void buffer_clear(PayloadBuffer *b)
{
    while (!buffer_empty(b)) {
        Payload *p;
        buffer_read(b, &p);
        free(p);
    }
}

static PayloadBuffer *buffer_new(int size)
{
    PayloadBuffer *buf = calloc(sizeof(PayloadBuffer), 1);

    if (!buf) return NULL;

    buf->size = size + 1; /* include empty elem */

    if (!(buf->packets = calloc(buf->size, sizeof(Payload *)))) {
        free(buf);
        return NULL;
    }

    return buf;
}

static void buffer_free(PayloadBuffer *b)
{
    if (b) {
        buffer_clear(b);
        free(b->packets);
        free(b);
    }
}

/* JITTER BUFFER WORK */
typedef struct _JitterBuffer {
    RTPMessage **queue;
    uint32_t     size;
    uint32_t     capacity;
    uint16_t     bottom;
    uint16_t     top;
} JitterBuffer;

static JitterBuffer *jbuf_new(uint32_t capacity)
{
    unsigned int size = 1;

    while (size <= capacity) {
        size *= 2;
    }

    JitterBuffer *q;

    if ( !(q = calloc(sizeof(JitterBuffer), 1)) ) return NULL;

    if (!(q->queue = calloc(sizeof(RTPMessage *), size))) {
        free(q);
        return NULL;
    }

    q->size = size;
    q->capacity = capacity;
    return q;
}

static void jbuf_clear(JitterBuffer *q)
{
    for (; q->bottom != q->top; ++q->bottom) {
        if (q->queue[q->bottom % q->size]) {
            rtp_free_msg(NULL, q->queue[q->bottom % q->size]);
            q->queue[q->bottom % q->size] = NULL;
        }
    }
}

static void jbuf_free(JitterBuffer *q)
{
    if (!q) return;

    jbuf_clear(q);
    free(q->queue);
    free(q);
}

static void jbuf_write(JitterBuffer *q, RTPMessage *m)
{
    uint16_t sequnum = m->header->sequnum;

    unsigned int num = sequnum % q->size;

    if ((uint32_t)(sequnum - q->bottom) > q->size) {
        jbuf_clear(q);
        q->bottom = sequnum;
        q->queue[num] = m;
        q->top = sequnum + 1;
        return;
    }

    if (q->queue[num])
        return;

    q->queue[num] = m;

    if ((sequnum - q->bottom) >= (q->top - q->bottom))
        q->top = sequnum + 1;
}

/* Success is 0 when there is nothing to dequeue,
 * 1 when there's a good packet,
 * 2 when there's a lost packet */
static RTPMessage *jbuf_read(JitterBuffer *q, int32_t *success)
{
    if (q->top == q->bottom) {
        *success = 0;
        return NULL;
    }

    unsigned int num = q->bottom % q->size;

    if (q->queue[num]) {
        RTPMessage *ret = q->queue[num];
        q->queue[num] = NULL;
        ++q->bottom;
        *success = 1;
        return ret;
    }

    if ((uint32_t)(q->top - q->bottom) > q->capacity) {
        ++q->bottom;
        *success = 2;
        return NULL;
    }

    *success = 0;
    return NULL;
}

static int init_video_decoder(CSSession *cs)
{
    int rc = vpx_codec_dec_init_ver(&cs->v_decoder, VIDEO_CODEC_DECODER_INTERFACE, NULL, 0, VPX_DECODER_ABI_VERSION);

    if ( rc != VPX_CODEC_OK) {
        LOGGER_ERROR("Init video_decoder failed: %s", vpx_codec_err_to_string(rc));
        return -1;
    }

    return 0;
}

static int init_audio_decoder(CSSession *cs)
{
    int rc;
    cs->audio_decoder = opus_decoder_create(cs->audio_decoder_sample_rate, cs->audio_decoder_channels, &rc );

    if ( rc != OPUS_OK ) {
        LOGGER_ERROR("Error while starting audio decoder: %s", opus_strerror(rc));
        return -1;
    }

    return 0;
}

static int init_video_encoder(CSSession *cs, uint16_t max_width, uint16_t max_height, uint32_t video_bitrate)
{
    vpx_codec_enc_cfg_t  cfg;
    int rc = vpx_codec_enc_config_default(VIDEO_CODEC_ENCODER_INTERFACE, &cfg, 0);

    if (rc != VPX_CODEC_OK) {
        LOGGER_ERROR("Failed to get config: %s", vpx_codec_err_to_string(rc));
        return -1;
    }

    cfg.rc_target_bitrate = video_bitrate;
    cfg.g_w = max_width;
    cfg.g_h = max_height;
    cfg.g_pass = VPX_RC_ONE_PASS;
    cfg.g_error_resilient = VPX_ERROR_RESILIENT_DEFAULT | VPX_ERROR_RESILIENT_PARTITIONS;
    cfg.g_lag_in_frames = 0;
    cfg.kf_min_dist = 0;
    cfg.kf_max_dist = 5;
    cfg.kf_mode = VPX_KF_AUTO;

    cs->max_width = max_width;
    cs->max_height = max_height;

    rc = vpx_codec_enc_init_ver(&cs->v_encoder, VIDEO_CODEC_ENCODER_INTERFACE, &cfg, 0, VPX_ENCODER_ABI_VERSION);

    if ( rc != VPX_CODEC_OK) {
        LOGGER_ERROR("Failed to initialize encoder: %s", vpx_codec_err_to_string(rc));
        return -1;
    }

    rc = vpx_codec_control(&cs->v_encoder, VP8E_SET_CPUUSED, 8);

    if ( rc != VPX_CODEC_OK) {
        LOGGER_ERROR("Failed to set encoder control setting: %s", vpx_codec_err_to_string(rc));
        return -1;
    }

    return 0;
}

static int init_audio_encoder(CSSession *cs)
{
    int rc = OPUS_OK;
    cs->audio_encoder = opus_encoder_create(cs->audio_encoder_sample_rate,
                                            cs->audio_encoder_channels, OPUS_APPLICATION_AUDIO, &rc);

    if ( rc != OPUS_OK ) {
        LOGGER_ERROR("Error while starting audio encoder: %s", opus_strerror(rc));
        return -1;
    }

    rc = opus_encoder_ctl(cs->audio_encoder, OPUS_SET_BITRATE(cs->audio_encoder_bitrate));

    if ( rc != OPUS_OK ) {
        LOGGER_ERROR("Error while setting encoder ctl: %s", opus_strerror(rc));
        return -1;
    }

    rc = opus_encoder_ctl(cs->audio_encoder, OPUS_SET_COMPLEXITY(10));

    if ( rc != OPUS_OK ) {
        LOGGER_ERROR("Error while setting encoder ctl: %s", opus_strerror(rc));
        return -1;
    }

    return 0;
}

static float calculate_sum_sq (int16_t *n, uint16_t k)
{
    float result = 0;
    uint16_t i = 0;

    for ( ; i < k; i ++) result += (float) (n[i] * n[i]);

    return result;
}



/* PUBLIC */
int cs_split_video_payload(CSSession *cs, const uint8_t *payload, uint16_t length)
{
    if (!cs || !length || length > cs->max_video_frame_size) {
        LOGGER_ERROR("Invalid  CodecState or video frame size: %u", length);
        return -1;
    }

    cs->split_video_frame[0] = cs->frameid_out++;
    cs->split_video_frame[1] = 0;
    cs->processing_video_frame = payload;
    cs->processing_video_frame_size = length;

    return ((length - 1) / cs->video_frame_piece_size) + 1;
}

const uint8_t *cs_get_split_video_frame(CSSession *cs, uint16_t *size)
{
    if (!cs || !size) return NULL;

    if (cs->processing_video_frame_size > cs->video_frame_piece_size) {
        memcpy(cs->split_video_frame + VIDEOFRAME_HEADER_SIZE,
               cs->processing_video_frame,
               cs->video_frame_piece_size);

        cs->processing_video_frame += cs->video_frame_piece_size;
        cs->processing_video_frame_size -= cs->video_frame_piece_size;

        *size = cs->video_frame_piece_size + VIDEOFRAME_HEADER_SIZE;
    } else {
        memcpy(cs->split_video_frame + VIDEOFRAME_HEADER_SIZE,
               cs->processing_video_frame,
               cs->processing_video_frame_size);

        *size = cs->processing_video_frame_size + VIDEOFRAME_HEADER_SIZE;
    }

    cs->split_video_frame[1]++;

    return cs->split_video_frame;
}

void cs_do(CSSession *cs)
{
    if (!cs) return;

    pthread_mutex_lock(cs->queue_mutex);

    if (!cs->active) {
        pthread_mutex_unlock(cs->queue_mutex);
        return;
    }

    /*
    /* Iterate over whole buffers and call playback callback * /
    if (cs->abuf_ready) while (!DecodedAudioBuffer_empty(cs->abuf_ready)) {
        DecodedAudio* p;
        DecodedAudioBuffer_read(cs->abuf_ready, &p);
        if (apcallback.first)
            apcallback.first(cs->agent, cs->call_idx, p->data, p->size, apcallback.second);

        free(p);
    }

    if (cs->vbuf_ready) while (!DecodedVideoBuffer_empty(cs->vbuf_ready)) {
        vpx_image_t* p;
        DecodedVideoBuffer_read(cs->vbuf_ready, &p);
        if (vpcallback.first)
            vpcallback.first(cs->agent, cs->call_idx, p, vpcallback.second);

        vpx_img_free(p);
    }
    */

    Payload *p;
    int rc;

    if (cs->abuf_raw && !buffer_empty(cs->abuf_raw)) {
        /* Decode audio */
        buffer_read(cs->abuf_raw, &p);

        uint16_t fsize = (cs->audio_decoder_channels *
                          (cs->audio_decoder_sample_rate * cs->audio_decoder_frame_duration) / 1000);
        int16_t tmp[fsize];

        rc = opus_decode(cs->audio_decoder, p->data, p->size, tmp, fsize, (p->size == 0));
        free(p);

        if (rc < 0)
            LOGGER_WARNING("Decoding error: %s", opus_strerror(rc));
        else
            /* Play */
            apcallback.first(cs->agent, cs->call_idx, tmp, rc, apcallback.second);
    }

    if (cs->vbuf_raw && !buffer_empty(cs->vbuf_raw)) {
        /* Decode video */
        buffer_read(cs->vbuf_raw, &p);

        rc = vpx_codec_decode(&cs->v_decoder, p->data, p->size, NULL, MAX_DECODE_TIME_US);
        free(p);

        if (rc != VPX_CODEC_OK) {
            LOGGER_ERROR("Error decoding video: %s", vpx_codec_err_to_string(rc));
        } else {
            vpx_codec_iter_t iter = NULL;
            vpx_image_t *dest = vpx_codec_get_frame(&cs->v_decoder, &iter);

            /* Play decoded images */
            for (; dest; dest = vpx_codec_get_frame(&cs->v_decoder, &iter)) {
                vpcallback.first(cs->agent, cs->call_idx, dest, vpcallback.second);
                vpx_img_free(dest);
            }
        }
    }

    pthread_mutex_unlock(cs->queue_mutex);
}

void cs_register_audio_callback(CSAudioCallback cb, void *data)
{
    apcallback.first = cb;
    apcallback.second = data;
}

void cs_register_video_callback(CSVideoCallback cb, void *data)
{
    vpcallback.first = cb;
    vpcallback.second = data;
}

int cs_set_video_encoder_resolution(CSSession *cs, uint16_t width, uint16_t height)
{
    vpx_codec_enc_cfg_t cfg = *cs->v_encoder.config.enc;

    if (cfg.g_w == width && cfg.g_h == height)
        return 0;

    if (width * height > cs->max_width * cs->max_height)
        return -1;

    LOGGER_DEBUG("New video resolution: %u %u", width, height);
    cfg.g_w = width;
    cfg.g_h = height;
    int rc = vpx_codec_enc_config_set(&cs->v_encoder, &cfg);

    if ( rc != VPX_CODEC_OK) {
        LOGGER_ERROR("Failed to set encoder control setting: %s", vpx_codec_err_to_string(rc));
        return -1;
    }

    return 0;
}

int cs_set_video_encoder_bitrate(CSSession *cs, uint32_t video_bitrate)
{
    vpx_codec_enc_cfg_t cfg = *cs->v_encoder.config.enc;

    if (cfg.rc_target_bitrate == video_bitrate)
        return 0;

    LOGGER_DEBUG("New video bitrate: %u", video_bitrate);
    cfg.rc_target_bitrate = video_bitrate;
    int rc = vpx_codec_enc_config_set(&cs->v_encoder, &cfg);

    if ( rc != VPX_CODEC_OK) {
        LOGGER_ERROR("Failed to set encoder control setting: %s", vpx_codec_err_to_string(rc));
        return -1;
    }

    return 0;
}

CSSession *cs_new(const ToxAvCSettings *cs_self, const ToxAvCSettings *cs_peer, uint32_t jbuf_size, int has_video)
{
    CSSession *cs = calloc(sizeof(CSSession), 1);

    if (!cs) {
        LOGGER_WARNING("Allocation failed! Application might misbehave!");
        return NULL;
    }

    if ( !(cs->j_buf = jbuf_new(jbuf_size)) ) {
        LOGGER_WARNING("Jitter buffer creaton failed!");
        goto error;
    }

    cs->audio_encoder_bitrate        = cs_self->audio_bitrate;
    cs->audio_encoder_sample_rate    = cs_self->audio_sample_rate;
    cs->audio_encoder_channels       = cs_self->audio_channels;
    cs->audio_encoder_frame_duration = cs_self->audio_frame_duration;

    cs->audio_decoder_bitrate        = cs_peer->audio_bitrate;
    cs->audio_decoder_sample_rate    = cs_peer->audio_sample_rate;
    cs->audio_decoder_channels       = cs_peer->audio_channels;
    cs->audio_decoder_frame_duration = cs_peer->audio_frame_duration;


    cs->capabilities |= ( 0 == init_audio_encoder(cs) ) ? a_encoding : 0;
    cs->capabilities |= ( 0 == init_audio_decoder(cs) ) ? a_decoding : 0;

    if ( !(cs->capabilities & a_encoding) || !(cs->capabilities & a_decoding) ) goto error;

    if ( !(cs->abuf_raw = buffer_new(jbuf_size)) ) goto error;

    pthread_mutexattr_t attr;

    if (pthread_mutexattr_init(&attr) != 0) goto error;

    if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE) != 0) {
        if (pthread_mutexattr_destroy(&attr) != 0)
            LOGGER_WARNING("Failed to destroy mutex attribute!");

        goto error;
    }

    if (pthread_mutex_init(cs->queue_mutex, &attr) != 0) {
        pthread_mutexattr_destroy(&attr);
        goto error;
    }

    if ((cs->support_video = has_video)) {
        cs->max_video_frame_size = MAX_VIDEOFRAME_SIZE;
        cs->video_frame_piece_size = VIDEOFRAME_PIECE_SIZE;

        cs->capabilities |= ( 0 == init_video_encoder(cs, cs_self->max_video_width,
                              cs_self->max_video_height, cs_self->video_bitrate) ) ? v_encoding : 0;
        cs->capabilities |= ( 0 == init_video_decoder(cs) ) ? v_decoding : 0;

        if ( !(cs->capabilities & v_encoding) || !(cs->capabilities & v_decoding) ) goto error;

        if ( !(cs->frame_buf = calloc(cs->max_video_frame_size, 1)) ) goto error;

        if ( !(cs->split_video_frame = calloc(cs->video_frame_piece_size + VIDEOFRAME_HEADER_SIZE, 1)) )
            goto error;

        if ( !(cs->vbuf_raw = buffer_new(VIDEO_DECODE_BUFFER_SIZE)) ) goto error;
    }

    if (pthread_mutexattr_destroy(&attr) != 0)
        LOGGER_WARNING("Failed to destroy mutex attribute!");


    cs->active = 1;
    return cs;

error:
    LOGGER_WARNING("Error initializing codec session! Application might misbehave!");

    buffer_free(cs->abuf_raw);

    if ( cs->audio_encoder ) opus_encoder_destroy(cs->audio_encoder);

    if ( cs->audio_decoder ) opus_decoder_destroy(cs->audio_decoder);


    if (has_video) {
        if ( cs->capabilities & v_decoding ) vpx_codec_destroy(&cs->v_decoder);

        if ( cs->capabilities & v_encoding ) vpx_codec_destroy(&cs->v_encoder);

        buffer_free(cs->vbuf_raw);

        free(cs->frame_buf);
        free(cs->split_video_frame);
    }

    jbuf_free(cs->j_buf);
    free(cs);

    return NULL;
}

void cs_kill(CSSession *cs)
{
    if (!cs) return;

    /* Lock running mutex and signal that cs is no longer active */
    pthread_mutex_lock(cs->queue_mutex);
    cs->active = 0;

    /* Wait threads to close */
    pthread_mutex_unlock(cs->queue_mutex);

    pthread_mutex_destroy(cs->queue_mutex);


    if ( cs->audio_encoder )
        opus_encoder_destroy(cs->audio_encoder);

    if ( cs->audio_decoder )
        opus_decoder_destroy(cs->audio_decoder);

    if ( cs->capabilities & v_decoding )
        vpx_codec_destroy(&cs->v_decoder);

    if ( cs->capabilities & v_encoding )
        vpx_codec_destroy(&cs->v_encoder);

    jbuf_free(cs->j_buf);
    buffer_free(cs->abuf_raw);
    buffer_free(cs->vbuf_raw);
    free(cs->frame_buf);

    LOGGER_DEBUG("Terminated codec state: %p", cs);
    free(cs);
}

void cs_set_vad_treshold(CSSession *cs, uint32_t treshold, uint16_t frame_duration)
{
    cs->EVAD_tolerance = treshold > frame_duration ? treshold / frame_duration : frame_duration;
}

int cs_calculate_vad(CSSession *cs, int16_t *PCM, uint16_t frame_size, float energy)
{
    float frame_energy = sqrt(calculate_sum_sq(PCM, frame_size)) / frame_size;

    if ( frame_energy > energy) {
        cs->EVAD_tolerance_cr = cs->EVAD_tolerance; /* Reset counter */
        return 1;
    }

    if ( cs->EVAD_tolerance_cr ) {
        cs->EVAD_tolerance_cr --;
        return 1;
    }

    return 0;
}




/* Called from RTP */
void queue_message(RTPSession *session, RTPMessage *msg)
{
    CSSession *cs = session->cs;

    if (!cs || !cs->active) return;

    /* Audio */
    if (session->payload_type == type_audio % 128) {
        jbuf_write(cs->j_buf, msg);

        pthread_mutex_lock(cs->queue_mutex);
        int success = 0;

        while ((msg = jbuf_read(cs->j_buf, &success)) || success == 2) {
            Payload *p;

            if (success == 2) {
                p = malloc(sizeof(Payload));

                if (p) p->size = 0;

            } else {
                p = malloc(sizeof(Payload) + msg->length);

                if (p) {
                    p->size = msg->length;
                    memcpy(p->data, msg->data, msg->length);
                }

                rtp_free_msg(NULL, msg);
            }

            if (p) {
                buffer_write(cs->abuf_raw, p);
            } else {
                LOGGER_WARNING("Allocation failed! Program might misbehave!");
            }
        }

        pthread_mutex_unlock(cs->queue_mutex);
    }
    /* Video */
    else {
        uint8_t *packet = msg->data;
        uint32_t packet_size = msg->length;

        if (packet_size < VIDEOFRAME_HEADER_SIZE)
            goto end;

        if (packet[0] > cs->frameid_in || (msg->header->timestamp > cs->last_timestamp)) { /* New frame */
            /* Flush last frames' data and get ready for this frame */
            Payload *p = malloc(sizeof(Payload) + cs->frame_size);

            if (p) {
                pthread_mutex_lock(cs->queue_mutex);

                if (buffer_full(cs->vbuf_raw)) {
                    LOGGER_DEBUG("Dropped video frame");
                    Payload *tp;
                    buffer_read(cs->vbuf_raw, &tp);
                    free(tp);
                } else {
                    p->size = cs->frame_size;
                    memcpy(p->data, cs->frame_buf, cs->frame_size);
                }

                buffer_write(cs->vbuf_raw, p);
                pthread_mutex_unlock(cs->queue_mutex);
            } else {
                LOGGER_WARNING("Allocation failed! Program might misbehave!");
                goto end;
            }

            cs->last_timestamp = msg->header->timestamp;
            cs->frameid_in = packet[0];
            memset(cs->frame_buf, 0, cs->frame_size);
            cs->frame_size = 0;

        } else if (packet[0] < cs->frameid_in) { /* Old frame; drop */
            LOGGER_DEBUG("Old packet: %u", packet[0]);
            goto end;
        }

        /* Otherwise it's part of the frame so just process */
        /* LOGGER_DEBUG("Video Packet: %u %u", packet[0], packet[1]); */
        memcpy(cs->frame_buf + cs->frame_size,
               packet + VIDEOFRAME_HEADER_SIZE,
               packet_size - VIDEOFRAME_HEADER_SIZE);

        cs->frame_size += packet_size - VIDEOFRAME_HEADER_SIZE;

end:
        rtp_free_msg(NULL, msg);
    }
}