1 files changed, 524 insertions, 136 deletions

diff --git a/toxav/codec.c b/toxav/codec.c
index 2f00319f..46fd6ba7 100644
--- a/toxav/codec.c
+++ b/toxav/codec.c
@@ -32,33 +32,124 @@
 #include <stdlib.h>
 #include <math.h>
 #include <assert.h>
+#include <time.h>
 
+#include "msi.h"
 #include "rtp.h"
 #include "codec.h"
 
-JitterBuffer *create_queue(unsigned int capacity)
+/* 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)
 {
-    unsigned int size = 1;
+    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 clear_queue(JitterBuffer *q)
+static void jbuf_clear(JitterBuffer *q)
 {
     for (; q->bottom != q->top; ++q->bottom) {
         if (q->queue[q->bottom % q->size]) {
@@ -68,48 +159,50 @@ static void clear_queue(JitterBuffer *q)
     }
 }
 
-void terminate_queue(JitterBuffer *q)
+static void jbuf_free(JitterBuffer *q)
 {
     if (!q) return;
-
-    clear_queue(q);
+    
+    jbuf_clear(q);
     free(q->queue);
     free(q);
 }
 
-void queue(JitterBuffer *q, RTPMessage *pk)
+static void jbuf_write(JitterBuffer *q, RTPMessage *m)
 {
-    uint16_t sequnum = pk->header->sequnum;
-
+    uint16_t sequnum = m->header->sequnum;
+    
     unsigned int num = sequnum % q->size;
-
+    
     if ((uint32_t)(sequnum - q->bottom) > q->size) {
-        clear_queue(q);
+        jbuf_clear(q);
         q->bottom = sequnum;
-        q->queue[num] = pk;
+        q->queue[num] = m;
         q->top = sequnum + 1;
         return;
     }
-
+    
     if (q->queue[num])
         return;
-
-    q->queue[num] = pk;
-
+    
+    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 */
-RTPMessage *dequeue(JitterBuffer *q, int *success)
+/* 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;
@@ -117,19 +210,18 @@ RTPMessage *dequeue(JitterBuffer *q, int *success)
         *success = 1;
         return ret;
     }
-
+    
     if ((uint32_t)(q->top - q->bottom) > q->capacity) {
         ++q->bottom;
         *success = 2;
         return NULL;
     }
-
+    
     *success = 0;
     return NULL;
 }
 
-
-int init_video_decoder(CodecState *cs)
+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);
 
@@ -141,72 +233,28 @@ int init_video_decoder(CodecState *cs)
     return 0;
 }
 
-int init_audio_decoder(CodecState *cs, uint32_t audio_channels)
+static int init_audio_decoder(CSSession *cs)
 {
     int rc;
-    cs->audio_decoder = opus_decoder_create(cs->audio_sample_rate, audio_channels, &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;
     }
-
-    cs->audio_decoder_channels = audio_channels;
     return 0;
 }
 
-int reconfigure_video_encoder_resolution(CodecState *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 reconfigure_video_encoder_bitrate(CodecState *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;
-}
-
-int init_video_encoder(CodecState *cs, uint16_t max_width, uint16_t max_height, uint32_t video_bitrate)
+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;
@@ -216,130 +264,375 @@ int init_video_encoder(CodecState *cs, uint16_t max_width, uint16_t max_height,
     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;
-    cs->bitrate = video_bitrate;
-
+    
     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;
 }
 
-int init_audio_encoder(CodecState *cs, uint32_t audio_channels)
+static int init_audio_encoder(CSSession *cs)
 {
     int rc = OPUS_OK;
-    cs->audio_encoder = opus_encoder_create(cs->audio_sample_rate, audio_channels, OPUS_APPLICATION_AUDIO, &rc);
-
+    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_bitrate));
-
+    
+    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;
     }
-
-    cs->audio_encoder_channels = audio_channels;
     return 0;
 }
 
-
-CodecState *codec_init_session ( uint32_t audio_bitrate,
-                                 uint16_t audio_frame_duration,
-                                 uint32_t audio_sample_rate,
-                                 uint32_t encoder_audio_channels,
-                                 uint32_t decoder_audio_channels,
-                                 uint32_t audio_VAD_tolerance_ms,
-                                 uint16_t max_video_width,
-                                 uint16_t max_video_height,
-                                 uint32_t video_bitrate )
+static float calculate_sum_sq (int16_t *n, uint16_t k)
 {
-    CodecState *retu = calloc(sizeof(CodecState), 1);
+    float result = 0;
+    uint16_t i = 0;
+    
+    for ( ; i < k; i ++) result += (float) (n[i] * n[i]);
+    
+    return result;
+}
 
-    if (!retu) return NULL;
 
-    retu->audio_bitrate = audio_bitrate;
-    retu->audio_sample_rate = audio_sample_rate;
 
-    /* Encoders */
-    if (!max_video_width || !max_video_height) { /* Disable video */
-        /*video_width = 320;
-        video_height = 240; */
+/* 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 {
-        retu->capabilities |= ( 0 == init_video_encoder(retu, max_video_width, max_video_height,
-                                video_bitrate) ) ? v_encoding : 0;
-        retu->capabilities |= ( 0 == init_video_decoder(retu) ) ? v_decoding : 0;
+        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);
+}
 
-    retu->capabilities |= ( 0 == init_audio_encoder(retu, encoder_audio_channels) ) ? a_encoding : 0;
-    retu->capabilities |= ( 0 == init_audio_decoder(retu, decoder_audio_channels) ) ? a_decoding : 0;
+void cs_register_audio_callback(CSAudioCallback cb, void* data)
+{
+    apcallback.first = cb;
+    apcallback.second = data;
+}
 
-    if ( retu->capabilities == 0  ) { /* everything failed */
-        free (retu);
-        return NULL;
+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;
+}
 
-    retu->EVAD_tolerance = audio_VAD_tolerance_ms > audio_frame_duration ?
-                           audio_VAD_tolerance_ms / audio_frame_duration : audio_frame_duration;
+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;
 
-    return retu;
+    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 codec_terminate_session ( CodecState *cs )
+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_lock(cs->queue_mutex);
+    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);
 }
 
-static float calculate_sum_sq (int16_t *n, uint16_t k)
+void cs_set_vad_treshold(CSSession* cs, uint32_t treshold, uint16_t frame_duration)
 {
-    float result = 0;
-    uint16_t i = 0;
-
-    for ( ; i < k; i ++) result += (float) (n[i] * n[i]);
-
-    return result;
+    cs->EVAD_tolerance = treshold > frame_duration ? treshold / frame_duration : frame_duration;
 }
 
-int energy_VAD(CodecState *cs, int16_t *PCM, uint16_t frame_size, float energy)
+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;
 
@@ -355,3 +648,98 @@ int energy_VAD(CodecState *cs, int16_t *PCM, uint16_t frame_size, float energy)
 
     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);
+    }
+}
\ No newline at end of file