7 files changed, 587 insertions, 347 deletions

diff --git a/toxav/codec.c b/toxav/codec.c
index fd2f9f93..43120e0f 100644
--- a/toxav/codec.c
+++ b/toxav/codec.c
@@ -229,271 +229,127 @@ static RTPMessage *jbuf_read(JitterBuffer *q, int32_t *success)
     return NULL;
 }
 
-
-
-
-
-/* PUBLIC */
-int cs_split_video_payload(CSSession *cs, const uint8_t *payload, uint16_t length)
+static int convert_bw_to_sampling_rate(int bw)
 {
-    if (!cs || !length || length > cs->max_video_frame_size) {
-        LOGGER_ERROR("Invalid  CodecState or video frame size: %u", length);
-        return cs_ErrorSplittingVideoPayload;
+    switch(bw)
+    {
+    case OPUS_BANDWIDTH_NARROWBAND: return 8000;
+    case OPUS_BANDWIDTH_MEDIUMBAND: return 12000;
+    case OPUS_BANDWIDTH_WIDEBAND: return 16000;
+    case OPUS_BANDWIDTH_SUPERWIDEBAND: return 24000;
+    case OPUS_BANDWIDTH_FULLBAND: return 48000;
+    default: 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;
-}
+/* PUBLIC */
 
 void cs_do(CSSession *cs)
 {
     /* Codec session should always be protected by call mutex so no need to check for cs validity
      */
-
-    if (!cs) return;
-
+    
+    if (!cs) 
+        return;
+    
     Payload *p;
     int rc;
-
+    
     int success = 0;
-
+    
     pthread_mutex_lock(cs->queue_mutex);
     RTPMessage *msg;
-
+    
+    uint16_t fsize = 5760; /* Max frame size for 48 kHz */
+    int16_t tmp[fsize * 2];
+    
     while ((msg = jbuf_read(cs->j_buf, &success)) || success == 2) {
         pthread_mutex_unlock(cs->queue_mutex);
-
-        uint16_t fsize = ((cs->audio_decoder_sample_rate * cs->audio_decoder_frame_duration) / 1000);
-        int16_t tmp[fsize * cs->audio_decoder_channels];
-
+        
         if (success == 2) {
             rc = opus_decode(cs->audio_decoder, 0, 0, tmp, fsize, 1);
         } else {
+            /* Get values from packet and decode.
+             * It also checks for validity of an opus packet
+             */
+            rc = convert_bw_to_sampling_rate(opus_packet_get_bandwidth(msg->data));
+            if (rc != -1) {
+                cs->last_packet_sampling_rate = rc;
+                cs->last_pack_channels = opus_packet_get_nb_channels(msg->data);
+            
+                cs->last_packet_frame_duration = 
+                    ( opus_packet_get_samples_per_frame(msg->data, cs->last_packet_sampling_rate) * 1000 )
+                    / cs->last_packet_sampling_rate;
+                    
+            } else {
+                LOGGER_WARNING("Failed to load packet values!");
+                rtp_free_msg(NULL, msg);
+                continue;
+            }
+            
             rc = opus_decode(cs->audio_decoder, msg->data, msg->length, tmp, fsize, 0);
             rtp_free_msg(NULL, msg);
         }
-
+        
         if (rc < 0) {
             LOGGER_WARNING("Decoding error: %s", opus_strerror(rc));
-        } else if (cs->acb.first) {
+        } else if (((ToxAV*)cs->agent)->acb.first) {
             /* Play */
-            cs->acb.first(cs->agent, cs->call_idx, tmp, rc, cs->acb.second);
+            ((ToxAV*)cs->agent)->acb.first(cs->agent, cs->call_idx, tmp, rc, 
+                                           ((ToxAV*)cs->agent)->acb.second);
         }
-
+        
         pthread_mutex_lock(cs->queue_mutex);
     }
-
+    
     if (cs->vbuf_raw && !buffer_empty(cs->vbuf_raw)) {
         /* Decode video */
         buffer_read(cs->vbuf_raw, &p);
-
+        
         /* Leave space for (possibly) other thread to queue more data after we read it here */
         pthread_mutex_unlock(cs->queue_mutex);
-
+        
         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)) {
-                if (cs->vcb.first)
-                    cs->vcb.first(cs->agent, cs->call_idx, dest, cs->vcb.second);
-
-                vpx_img_free(dest);
+                if (((ToxAV*)cs->agent)->vcb.first)
+                    ((ToxAV*)cs->agent)->vcb.first(cs->agent, cs->call_idx, dest, 
+                                                   ((ToxAV*)cs->agent)->vcb.second);
+                    
+                    vpx_img_free(dest);
             }
         }
-
-        return;
-    }
-
-    pthread_mutex_unlock(cs->queue_mutex);
-}
-
-int cs_set_sending_video_resolution(CSSession *cs, uint16_t width, uint16_t height)
-{
-    if (!cs->v_encoding)
-        return -1;
-    
-    /* TODO FIXME reference is safe? */
-    vpx_codec_enc_cfg_t cfg = *cs->v_encoder[0].config.enc;
-    
-    if (cfg.g_w == width && cfg.g_h == height)
-        return 0;
-/*
-    if (width * height > cs->max_width * cs->max_height) {
-        vpx_codec_ctx_t v_encoder = cs->v_encoder;
-
-        if (init_video_encoder(cs, width, height, cs->video_bitrate) == -1) {
-            cs->v_encoder = v_encoder;
-            return cs_ErrorSettingVideoResolution;
-        }
-
-        vpx_codec_destroy(&v_encoder);
-        return 0;
-    }*/
-
-    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 cs_ErrorSettingVideoResolution;
-    }
-
-    return 0;
-}
-
-int cs_set_sending_video_bitrate(CSSession *cs, uint32_t bitrate)
-{
-    if (!cs->v_encoding)
-        return -1;
-    
-    /* TODO FIXME reference is safe? */
-    vpx_codec_enc_cfg_t cfg = *cs->v_encoder[0].config.enc;
-    if (cfg.rc_target_bitrate == bitrate)
-        return 0;
-
-    LOGGER_DEBUG("New video bitrate: %u", video_bitrate);
-    cfg.rc_target_bitrate = 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 cs_ErrorSettingVideoBitrate;
-    }
-
-    return 0;
-}
-
-int cs_set_sending_audio_bitrate(CSSession *cs, int32_t rate)
-{
-    if (cs->audio_encoder == NULL)
-        return -1;
-    
-    int rc = opus_encoder_ctl(cs->audio_encoder, OPUS_SET_BITRATE(rate));
-    
-    if ( rc != OPUS_OK ) {
-        LOGGER_ERROR("Error while setting encoder ctl: %s", opus_strerror(rc));
-        return -1;
-    }
-    
-    return 0;
-}
-
-int cs_set_sending_audio_sampling_rate(CSSession* cs, int32_t rate)
-{
-    /* TODO Find a better way? */
-    if (cs->audio_encoder == NULL)
-        return -1;
-    
-    int rc = OPUS_OK;
-    int last_rate = 0;
-    
-    rc = opus_encoder_ctl(cs->audio_encoder, OPUS_SET_BITRATE(&last_rate));
-    
-    if ( rc != OPUS_OK ) {
-        LOGGER_ERROR("Error while getting encoder ctl: %s", opus_strerror(rc));
-        return -1;
-    }
-    
-    if (rate == last_rate)
-        return 0;
         
-    OpusEncoder* new_enc = opus_encoder_create(rate, cs->channels, OPUS_APPLICATION_AUDIO, &rc);
-    
-    if ( rc != OPUS_OK ) {
-        LOGGER_ERROR("Error while starting audio encoder: %s", opus_strerror(rc));
-        return -1;
-    }
-    
-    opus_encoder_destroy(cs->audio_encoder);
-    cs->audio_encoder = new_enc;
-    return 0;
-}
-
-int cs_set_sending_audio_channels(CSSession* cs, int32_t count)
-{
-    /* TODO Find a better way? */
-    if (cs->audio_encoder == NULL)
-        return -1;
-    
-    if (cs->channels == count)
-        return 0;
-    
-    int rc = OPUS_OK;
-    int bitrate = 0;
-    
-    rc = opus_encoder_ctl(cs->audio_encoder, OPUS_SET_BITRATE(&bitrate));
-    
-    if ( rc != OPUS_OK ) {
-        LOGGER_ERROR("Error while getting encoder ctl: %s", opus_strerror(rc));
-        return -1;
-    }
-    
-    cs->channels = count;
-    OpusEncoder* new_enc = opus_encoder_create(bitrate, cs->channels, OPUS_APPLICATION_AUDIO, &rc);
-    
-    if ( rc != OPUS_OK ) {
-        LOGGER_ERROR("Error while starting audio encoder: %s", opus_strerror(rc));
-        return -1;
+        return;
     }
     
-    opus_encoder_destroy(cs->audio_encoder);
-    cs->audio_encoder = new_enc;
-    return 0;
+    pthread_mutex_unlock(cs->queue_mutex);
 }
 
 CSSession *cs_new(uint32_t s_audio_b, uint32_t p_audio_b, uint32_t s_video_b, uint32_t p_video_b)
 {
     CSSession *cs = calloc(sizeof(CSSession), 1);
-
+    
     if (!cs) {
         LOGGER_WARNING("Allocation failed! Application might misbehave!");
         return NULL;
     }
-
+    
     /* TODO this has to be exchanged in msi */
     cs->max_video_frame_size = MAX_VIDEOFRAME_SIZE;
     cs->video_frame_piece_size = VIDEOFRAME_PIECE_SIZE;
     
-    if (s_audio_b > 0 && 0 != cs_enable_audio_sending(cs, s_audio_b)) { /* Sending audio enabled */
+    if (s_audio_b > 0 && 0 != cs_enable_audio_sending(cs, s_audio_b, 2)) { /* Sending audio enabled */
         LOGGER_WARNING("Failed to enable audio sending!");
         goto FAILURE;
     }
@@ -502,29 +358,29 @@ CSSession *cs_new(uint32_t s_audio_b, uint32_t p_audio_b, uint32_t s_video_b, ui
         LOGGER_WARNING("Failed to enable audio receiving!");
         goto FAILURE;
     }
-        
+    
     if (s_video_b > 0 && 0 != cs_enable_video_sending(cs, s_video_b)) { /* Sending video enabled */
         LOGGER_WARNING("Failed to enable video sending!");
         goto FAILURE;
     }
-            
+    
     if (p_video_b > 0  && 0 != cs_enable_video_receiving(cs)) { /* Receiving video enabled */
         LOGGER_WARNING("Failed to enable video receiving!");
         goto FAILURE;
     }
-
+    
     return cs;
-
-FAILURE:
+    
+    FAILURE:
     LOGGER_WARNING("Error initializing codec session! Application might misbehave!");
-
+    
     cs_disable_audio_sending(cs);
     cs_disable_audio_receiving(cs);
     cs_disable_video_sending(cs);
     cs_disable_video_receiving(cs);
     
     free(cs);
-
+    
     return NULL;
 }
 
@@ -532,7 +388,7 @@ void cs_kill(CSSession *cs)
 {
     if (!cs) 
         return;
-
+    
     /* NOTE: queue_message() will not be called since 
      * the callback is unregistered before cs_kill is called.
      */
@@ -541,78 +397,110 @@ void cs_kill(CSSession *cs)
     cs_disable_audio_receiving(cs);
     cs_disable_video_sending(cs);
     cs_disable_video_receiving(cs);
-
+    
     LOGGER_DEBUG("Terminated codec state: %p", cs);
     free(cs);
 }
 
-int cs_enable_audio_sending(CSSession* cs, uint32_t bitrate)
+
+
+void cs_init_video_splitter_cycle(CSSession* cs)
 {
-    if (cs->audio_encoder)
-        return 0;
-    
-    /**
-     * Encoder is initialized with default values. These values (Sampling rate, channel count) 
-     * change on the fly from toxav.
-     */
-    
-    int rc = OPUS_OK;
-    cs->audio_encoder = opus_encoder_create(48000, 2, 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(bitrate));
+    cs->split_video_frame[0] = cs->frameid_out++;
+    cs->split_video_frame[1] = 0;
+}
+
+int cs_update_video_splitter_cycle(CSSession *cs, const uint8_t *payload, uint16_t length)
+{
+    cs->processing_video_frame = payload;
+    cs->processing_video_frame_size = length;
     
-    if ( rc != OPUS_OK ) {
-        LOGGER_ERROR("Error while setting encoder ctl: %s", opus_strerror(rc));
-        goto FAILURE;
+    return ((length - 1) / cs->video_frame_piece_size) + 1;
+}
+
+const uint8_t *cs_iterate_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;
+}
+
+
+
+int cs_set_sending_video_resolution(CSSession *cs, uint16_t width, uint16_t height)
+{
+    if (!cs->v_encoding)
+        return -1;
     
-    rc = opus_encoder_ctl(cs->audio_encoder, OPUS_SET_COMPLEXITY(10));
+    /* TODO FIXME reference is safe? */
+    vpx_codec_enc_cfg_t cfg = *cs->v_encoder[0].config.enc;
     
-    if ( rc != OPUS_OK ) {
-        LOGGER_ERROR("Error while setting encoder ctl: %s", opus_strerror(rc));
-        goto FAILURE;
+    if (cfg.g_w == width && cfg.g_h == height)
+        return 0;
+/*
+    if (width * height > cs->max_width * cs->max_height) {
+        vpx_codec_ctx_t v_encoder = cs->v_encoder;
+
+        if (init_video_encoder(cs, width, height, cs->video_bitrate) == -1) {
+            cs->v_encoder = v_encoder;
+            return cs_ErrorSettingVideoResolution;
+        }
+
+        vpx_codec_destroy(&v_encoder);
+        return 0;
+    }*/
+
+    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 cs_ErrorSettingVideoResolution;
     }
-    
-    cs->channels = 2;
+
     return 0;
-    
-FAILURE:
-    cs_disable_audio_sending(cs);
-    return -1;
 }
 
-int cs_enable_audio_receiving(CSSession* cs)
+int cs_set_sending_video_bitrate(CSSession *cs, uint32_t bitrate)
 {
-    if (cs->audio_decoder)
-        return 0;
-    
-    /**
-     * Decoder is initialized with default values. These values (Sampling rate, channel count) 
-     * change on the fly from toxav.
-     */
-    
-    int rc;
-    cs->audio_decoder = opus_decoder_create(48000, 2, &rc );
-    
-    if ( rc != OPUS_OK ) {
-        LOGGER_ERROR("Error while starting audio decoder: %s", opus_strerror(rc));
+    if (!cs->v_encoding)
         return -1;
-    }
     
+    /* TODO FIXME reference is safe? */
+    vpx_codec_enc_cfg_t cfg = *cs->v_encoder[0].config.enc;
+    if (cfg.rc_target_bitrate == bitrate)
+        return 0;
+
+    LOGGER_DEBUG("New video bitrate: %u", video_bitrate);
+    cfg.rc_target_bitrate = bitrate;
     
-    if ( !(cs->j_buf = jbuf_new(DEFAULT_JBUF)) ) {
-        LOGGER_WARNING("Jitter buffer creaton failed!");
-        opus_decoder_destroy(cs->audio_decoder);
-        cs->audio_decoder = NULL;
-        return -1;
+    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 cs_ErrorSettingVideoBitrate;
     }
-    
-    
+
     return 0;
 }
 
@@ -704,25 +592,6 @@ FAILURE:
     return -1;
 }
 
-void cs_disable_audio_sending(CSSession* cs)
-{
-    if ( cs->audio_encoder ) {
-        opus_encoder_destroy(cs->audio_encoder);
-        cs->audio_encoder = NULL;
-        cs->channels = 0;
-    }
-}
-
-void cs_disable_audio_receiving(CSSession* cs)
-{
-    if ( cs->audio_decoder ) {
-        opus_decoder_destroy(cs->audio_decoder);
-        cs->audio_decoder = NULL;
-        jbuf_free(cs->j_buf);
-        cs->j_buf = NULL;
-    }
-}
-
 void cs_disable_video_sending(CSSession* cs)
 {
     if (cs->v_encoding) {
@@ -752,6 +621,163 @@ void cs_disable_video_receiving(CSSession* cs)
 
 
 
+int cs_set_sending_audio_bitrate(CSSession *cs, int32_t rate)
+{
+    if (cs->audio_encoder == NULL)
+        return -1;
+    
+    int rc = opus_encoder_ctl(cs->audio_encoder, OPUS_SET_BITRATE(rate));
+    
+    if ( rc != OPUS_OK ) {
+        LOGGER_ERROR("Error while setting encoder ctl: %s", opus_strerror(rc));
+        return -1;
+    }
+    
+    return 0;
+}
+
+int cs_set_sending_audio_sampling_rate(CSSession* cs, int32_t rate)
+{
+    /* TODO Find a better way? */
+    if (cs->audio_encoder == NULL)
+        return -1;
+    
+    int rc = OPUS_OK;
+    int bitrate = 0;
+    int channels = cs->encoder_channels;
+    
+    rc = opus_encoder_ctl(cs->audio_encoder, OPUS_GET_BITRATE(&bitrate));
+    
+    if ( rc != OPUS_OK ) {
+        LOGGER_ERROR("Error while getting encoder ctl: %s", opus_strerror(rc));
+        return -1;
+    }
+    
+    cs_disable_audio_sending(cs);
+    return cs_enable_audio_sending(cs, bitrate, channels);
+}
+
+int cs_set_sending_audio_channels(CSSession* cs, int32_t count)
+{
+    /* TODO Find a better way? */
+    if (cs->audio_encoder == NULL)
+        return -1;
+    
+    if (cs->encoder_channels == count)
+        return 0;
+    
+    int rc = OPUS_OK;
+    int bitrate = 0;
+    
+    rc = opus_encoder_ctl(cs->audio_encoder, OPUS_GET_BITRATE(&bitrate));
+    
+    if ( rc != OPUS_OK ) {
+        LOGGER_ERROR("Error while getting encoder ctl: %s", opus_strerror(rc));
+        return -1;
+    }
+    
+    cs_disable_audio_sending(cs);
+    return cs_enable_audio_sending(cs, bitrate, count);
+}
+
+void cs_disable_audio_sending(CSSession* cs)
+{
+    if ( cs->audio_encoder ) {
+        opus_encoder_destroy(cs->audio_encoder);
+        cs->audio_encoder = NULL;
+        cs->encoder_channels = 0;
+    }
+}
+
+void cs_disable_audio_receiving(CSSession* cs)
+{
+    if ( cs->audio_decoder ) {
+        opus_decoder_destroy(cs->audio_decoder);
+        cs->audio_decoder = NULL;
+        jbuf_free(cs->j_buf);
+        cs->j_buf = NULL;
+        
+        /* It's used for measuring iteration interval so this has to be some value.
+         * To avoid unecessary checking we set this to 500
+         */
+        cs->last_packet_frame_duration = 500;
+    }
+}
+
+int cs_enable_audio_sending(CSSession* cs, uint32_t bitrate, int channels)
+{
+    if (cs->audio_encoder)
+        return 0;
+    
+    /**
+     * Encoder is initialized with default values. These values (Sampling rate, channel count) 
+     * change on the fly from toxav.
+     */
+    
+    int rc = OPUS_OK;
+    cs->audio_encoder = opus_encoder_create(48000, 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(bitrate));
+    
+    if ( rc != OPUS_OK ) {
+        LOGGER_ERROR("Error while setting encoder ctl: %s", opus_strerror(rc));
+        goto FAILURE;
+    }
+    
+    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));
+        goto FAILURE;
+    }
+    
+    cs->encoder_channels = channels;
+    return 0;
+    
+FAILURE:
+    cs_disable_audio_sending(cs);
+    return -1;
+}
+
+int cs_enable_audio_receiving(CSSession* cs)
+{
+    if (cs->audio_decoder)
+        return 0;
+    
+    /**
+     * Decoder is initialized with default values. These values (Sampling rate, channel count) 
+     * change on the fly from toxav.
+     */
+    
+    int rc;
+    cs->audio_decoder = opus_decoder_create(48000, 2, &rc );
+    
+    if ( rc != OPUS_OK ) {
+        LOGGER_ERROR("Error while starting audio decoder: %s", opus_strerror(rc));
+        return -1;
+    }
+    
+    
+    if ( !(cs->j_buf = jbuf_new(DEFAULT_JBUF)) ) {
+        LOGGER_WARNING("Jitter buffer creaton failed!");
+        opus_decoder_destroy(cs->audio_decoder);
+        cs->audio_decoder = NULL;
+        return -1;
+    }
+    
+    /* It's used for measuring iteration interval so this has to be some value.
+     * To avoid unecessary checking we set this to 500
+     */
+    cs->last_packet_frame_duration = 500;
+    return 0;
+}
+
+
 
 /* Called from RTP */
 void queue_message(RTPSession *session, RTPMessage *msg)
diff --git a/toxav/codec.h b/toxav/codec.h
index 92262ef8..951d6d2f 100644
--- a/toxav/codec.h
+++ b/toxav/codec.h
@@ -96,7 +96,7 @@ typedef struct _CSSession {
     uint32_t video_frame_piece_size;
     uint32_t max_video_frame_size;
 
-    /* Reassembling */
+    /* Splitting */
     uint8_t *split_video_frame;
     const uint8_t *processing_video_frame;
     uint16_t processing_video_frame_size;
@@ -110,10 +110,13 @@ typedef struct _CSSession {
 
     /* audio encoding */
     OpusEncoder *audio_encoder;
-    int32_t channels;
+    int32_t encoder_channels;
     
     /* audio decoding */
     OpusDecoder *audio_decoder;
+    int32_t last_pack_channels;
+    int32_t last_packet_sampling_rate;
+    int32_t last_packet_frame_duration;
     struct _JitterBuffer *j_buf;
 
 
@@ -127,54 +130,55 @@ typedef struct _CSSession {
         *
         *
         */
-
-    /* Callbacks */
-    PAIR(CSAudioCallback, void *) acb;
-    PAIR(CSVideoCallback, void *) vcb;
-
-    void *agent; /* Pointer to ToxAv */
+    void *agent; /* Pointer to ToxAV TODO make this pointer to ToxAV*/
     int32_t call_idx;
     
     pthread_mutex_t queue_mutex[1];
 } CSSession;
 
-int cs_split_video_payload(CSSession *cs, const uint8_t *payload, uint16_t length);
-const uint8_t *cs_get_split_video_frame(CSSession *cs, uint16_t *size);
 
 /**
- * Call playback callbacks
+ * Generic
  */
 void cs_do(CSSession *cs);
 
-/** 
- * Reconfigure video settings; return 0 on success or -1 on failure. 
+/* Make sure to be called BEFORE corresponding rtp_new */
+CSSession *cs_new(uint32_t s_audio_b, uint32_t p_audio_b, uint32_t s_video_b, uint32_t p_video_b);
+/* Make sure to be called AFTER corresponding rtp_kill */
+void cs_kill(CSSession *cs);
+
+
+/**
+ * VIDEO HANDLING
  */
+void cs_init_video_splitter_cycle(CSSession *cs);
+int cs_update_video_splitter_cycle(CSSession* cs, const uint8_t* payload, uint16_t length);
+const uint8_t *cs_iterate_split_video_frame(CSSession *cs, uint16_t *size);
+
 int cs_set_sending_video_resolution(CSSession *cs, uint16_t width, uint16_t height);
 int cs_set_sending_video_bitrate(CSSession *cs, uint32_t bitrate);
 
+int cs_enable_video_sending(CSSession* cs, uint32_t bitrate);
+int cs_enable_video_receiving(CSSession* cs);
+
+void cs_disable_video_sending(CSSession* cs);
+void cs_disable_video_receiving(CSSession* cs);
+
+/**
+ * AUDIO HANDLING
+ */
 int cs_set_sending_audio_bitrate(CSSession* cs, int32_t rate);
-/* NOTE: Try not to call these a lot */
 int cs_set_sending_audio_sampling_rate(CSSession* cs, int32_t rate);
 int cs_set_sending_audio_channels(CSSession* cs, int32_t count);
 
-/** 
- * Make sure to be called BEFORE corresponding rtp_new 
- */
-CSSession *cs_new(uint32_t s_audio_b, uint32_t p_audio_b, uint32_t s_video_b, uint32_t p_video_b);
-/** 
- * Make sure to be called AFTER corresponding rtp_kill 
- */
-void cs_kill(CSSession *cs);
-
-int cs_enable_audio_sending(CSSession* cs, uint32_t bitrate);
+int cs_enable_audio_sending(CSSession* cs, uint32_t bitrate, int channels);
 int cs_enable_audio_receiving(CSSession* cs);
-int cs_enable_video_sending(CSSession* cs, uint32_t bitrate);
-int cs_enable_video_receiving(CSSession* cs);
 
 void cs_disable_audio_sending(CSSession* cs);
 void cs_disable_audio_receiving(CSSession* cs);
-void cs_disable_video_sending(CSSession* cs);
-void cs_disable_video_receiving(CSSession* cs);
+
+
+
 
 /* Internal. Called from rtp_handle_message */
 void queue_message(RTPSession *session, RTPMessage *msg);
diff --git a/toxav/rtp.c b/toxav/rtp.c
index ba93e781..a50dd7ce 100644
--- a/toxav/rtp.c
+++ b/toxav/rtp.c
@@ -488,13 +488,13 @@ int rtp_register_for_receiving(RTPSession* session)
                                                rtp_handle_packet, session);
 }
 
-int rtp_send_msg ( RTPSession *session, Messenger *messenger, const uint8_t *data, uint16_t length )
+int rtp_send_msg ( RTPSession *session, const uint8_t *data, uint16_t length )
 {
     RTPMessage *msg = rtp_new_message (session, data, length);
     
     if ( !msg ) return -1;
     
-    if ( -1 == send_custom_lossy_packet(messenger, session->dest, msg->data, msg->length) ) {
+    if ( -1 == send_custom_lossy_packet(session->m, session->dest, msg->data, msg->length) ) {
         LOGGER_WARNING("Failed to send full packet (len: %d)! std error: %s", length, strerror(errno));
         rtp_free_msg ( session, msg );
         return rtp_ErrorSending;
diff --git a/toxav/rtp.h b/toxav/rtp.h
index b25b13ba..03b44719 100644
--- a/toxav/rtp.h
+++ b/toxav/rtp.h
@@ -121,7 +121,7 @@ int rtp_register_for_receiving (RTPSession *session);
 /**
  * Sends msg to _RTPSession::dest
  */
-int rtp_send_msg ( RTPSession *session, Messenger *messenger, const uint8_t *data, uint16_t length );
+int rtp_send_msg ( RTPSession* session, const uint8_t* data, uint16_t length );
 
 /**
  * Dealloc msg.
diff --git a/toxav/toxav.c b/toxav/toxav.c
index 68402020..ee7f49a6 100644
--- a/toxav/toxav.c
+++ b/toxav/toxav.c
@@ -176,14 +176,14 @@ uint32_t toxav_do_interval(ToxAv *av)
 void toxav_do(ToxAv *av)
 {
     msi_do(av->msi_session);
-
+    
     uint64_t start = current_time_monotonic();
-
+    
     uint32_t i = 0;
-
+    
     for (; i < av->max_calls; i ++) {
         pthread_mutex_lock(av->calls[i].mutex_control);
-
+        
         if (av->calls[i].active) {
             pthread_mutex_lock(av->calls[i].mutex_do);
             pthread_mutex_unlock(av->calls[i].mutex_control);
@@ -193,12 +193,12 @@ void toxav_do(ToxAv *av)
             pthread_mutex_unlock(av->calls[i].mutex_control);
         }
     }
-
+    
     uint64_t end = current_time_monotonic();
-
+    
     /* TODO maybe use variable for sizes */
     av->dectmsstotal += end - start;
-
+    
     if (++av->dectmsscount == 3) {
         av->avgdectms = av->dectmsstotal / 3 + 2 /* NOTE Magic Offset */;
         av->dectmsscount = 0;
@@ -432,7 +432,7 @@ static int toxav_send_rtp_payload(ToxAv *av,
         int i;
 
         for (i = 0; i < parts; i++) {
-            iter = cs_get_split_video_frame(call->cs, &part_size);
+            iter = cs_iterate_split_video_frame(call->cs, &part_size);
 
             if (rtp_send_msg(call->crtps[video_index], av->messenger, iter, part_size) < 0)
                 return av_ErrorSendingPayload;
diff --git a/toxav/toxav_new.c b/toxav/toxav_new.c
index d6c1872c..857d5a83 100644
--- a/toxav/toxav_new.c
+++ b/toxav/toxav_new.c
@@ -34,6 +34,7 @@
 #include <stdlib.h>
 #include <string.h>
 
+#define MAX_ENCODE_TIME_US ((1000 / 24) * 1000)
 
 enum {
     audio_index,
@@ -75,6 +76,8 @@ struct toxAV
     int32_t dmssc; /** Measure count */
     int32_t dmsst; /** Last cycle total */
     int32_t dmssa; /** Average decoding time in ms */
+    
+    uint32_t interval; /** Calculated interval */
 };
 
 
@@ -130,6 +133,7 @@ ToxAV* toxav_new(Tox* tox, TOXAV_ERR_NEW* error)
         goto FAILURE;
     }
     
+    av->interval = 200;
     av->msi->agent_handler = av;
     
     msi_register_callback(av->msi, i_toxav_msi_callback_invite, msi_OnInvite, NULL);
@@ -144,7 +148,7 @@ ToxAV* toxav_new(Tox* tox, TOXAV_ERR_NEW* error)
     msi_register_callback(av->msi, i_toxav_msi_callback_state_change, msi_OnSelfCSChange, NULL);
     
     
-    if (error) 
+    if (error)
         *error = rc;
     
     return av;
@@ -175,12 +179,32 @@ Tox* toxav_get_tox(ToxAV* av)
 
 uint32_t toxav_iteration_interval(const ToxAV* av)
 {
-
+    return av->interval;
 }
 
 void toxav_iteration(ToxAV* av)
 {
-
+    msi_do(av->msi);
+    
+    uint64_t start = current_time_monotonic();
+    uint32_t rc = 200 + av->dmssa; /* If no call is active interval is 200 */
+    
+    IToxAVCall* i = av->calls[av->calls_head];
+    for (; i; i = i->next) {
+        if (i->active) {
+            cs_do(i->cs);
+            rc = MIN(i->cs->last_packet_frame_duration, rc);
+        }
+    }
+    
+    av->interval = rc < av->dmssa ? 0 : rc - av->dmssa;
+    av->dmsst += current_time_monotonic() - start;
+    
+    if (++av->dmssc == 3) {
+        av->dmssa = av->dmsst / 3 + 2 /* NOTE Magic Offset for precission */;
+        av->dmssc = 0;
+        av->dmsst = 0;
+    }
 }
 
 bool toxav_call(ToxAV* av, uint32_t friend_number, uint32_t audio_bit_rate, uint32_t video_bit_rate, TOXAV_ERR_CALL* error)
@@ -261,52 +285,244 @@ void toxav_callback_call_state(ToxAV* av, toxav_call_state_cb* function, void* u
 
 bool toxav_call_control(ToxAV* av, uint32_t friend_number, TOXAV_CALL_CONTROL control, TOXAV_ERR_CALL_CONTROL* error)
 {
-
+    TOXAV_ERR_CALL_CONTROL rc = TOXAV_ERR_CALL_CONTROL_OK;
+    
+    if (m_friend_exists(av->m, friend_number)) {
+        rc = TOXAV_ERR_CALL_CONTROL_FRIEND_NOT_FOUND;
+        goto END;
+    }
+    
+    
+    IToxAVCall* call = i_toxav_get_call(av, friend_number);
+    if (call == NULL) {
+        rc = TOXAV_ERR_CALL_CONTROL_FRIEND_NOT_IN_CALL;
+        goto END;
+    }
+    
+    /* TODO rest of these */
+    switch (control)
+    {
+        case TOXAV_CALL_CONTROL_RESUME: {
+            
+        } break;
+        
+        case TOXAV_CALL_CONTROL_PAUSE: {
+            
+        } break;
+        
+        case TOXAV_CALL_CONTROL_CANCEL: {
+            if (av->msi->calls[call->call_idx]->state == msi_CallActive) {
+                /* Hang up */
+                msi_hangup(av->msi, call->call_idx);
+            } else if (av->msi->calls[call->call_idx]->state == msi_CallRequested) {
+                /* Reject the call */
+                msi_reject(av->msi, call->call_idx);
+            } else if (av->msi->calls[call->call_idx]->state == msi_CallRequesting) {
+                /* Cancel the call */
+                msi_cancel(av->msi, call->call_idx);
+            }
+        } break;
+        
+        case TOXAV_CALL_CONTROL_MUTE_AUDIO: {
+            
+        } break;
+        
+        case TOXAV_CALL_CONTROL_MUTE_VIDEO: {
+            
+        } break;
+    }
+    
+END:
+    if (error)
+        *error = rc;
+    
+    return rc == TOXAV_ERR_CALL_CONTROL_OK;
 }
 
 bool toxav_set_audio_bit_rate(ToxAV* av, uint32_t friend_number, uint32_t audio_bit_rate, TOXAV_ERR_BIT_RATE* error)
 {
-
+    /* TODO */
 }
 
 bool toxav_set_video_bit_rate(ToxAV* av, uint32_t friend_number, uint32_t video_bit_rate, TOXAV_ERR_BIT_RATE* error)
 {
-
+    /* TODO */
 }
 
 void toxav_callback_request_video_frame(ToxAV* av, toxav_request_video_frame_cb* function, void* user_data)
 {
-
+    /* TODO */
 }
 
-bool toxav_send_video_frame(ToxAV* av, uint32_t friend_number, uint16_t width, uint16_t height, const uint8_t* y, const uint8_t* u, const uint8_t* v, const uint8_t* a, TOXAV_ERR_SEND_FRAME* error)
+bool toxav_send_video_frame(ToxAV* av, uint32_t friend_number, uint16_t width, uint16_t height, const uint8_t* y, const uint8_t* u, const uint8_t* v, TOXAV_ERR_SEND_FRAME* error)
 {
-
+    TOXAV_ERR_SEND_FRAME rc = TOXAV_ERR_SEND_FRAME_OK;
+    IToxAVCall* call;
+    
+    if (m_friend_exists(av->m, friend_number)) {
+        rc = TOXAV_ERR_SEND_FRAME_FRIEND_NOT_FOUND;
+        goto END;
+    }
+    
+    call = i_toxav_get_call(av, friend_number);
+    if (call == NULL) {
+        rc = TOXAV_ERR_SEND_FRAME_FRIEND_NOT_IN_CALL;
+        goto END;
+    }
+    
+    if (av->msi->calls[call->call_idx]->state != msi_CallActive) {
+        /* TODO */
+        rc = TOXAV_ERR_SEND_FRAME_NOT_REQUESTED;
+        goto END;
+    }
+    
+    if ( y == NULL || u == NULL || v == NULL ) {
+        rc = TOXAV_ERR_SEND_FRAME_NULL;
+        goto END;
+    }
+    
+    if ( cs_set_sending_video_resolution(call->cs, width, height) != 0 ) {
+        rc = TOXAV_ERR_SEND_FRAME_INVALID;
+        goto END;
+    }
+    
+    { /* Encode */
+        vpx_image_t img;
+        img.w = img.h = img.d_w = img.d_h = 0;
+        vpx_img_alloc(&img, VPX_IMG_FMT_VPXI420, width, height, 1);
+        
+        /* I420 "It comprises an NxM Y plane followed by (N/2)x(M/2) V and U planes." 
+         * http://fourcc.org/yuv.php#IYUV
+         */
+        memcpy(img.planes[VPX_PLANE_Y], y, width * height);
+        memcpy(img.planes[VPX_PLANE_U], u, (width/2) * (height/2));
+        memcpy(img.planes[VPX_PLANE_V], v, (width/2) * (height/2));
+        
+        int vrc = vpx_codec_encode(call->cs->v_encoder, &img, 
+                                   call->cs->frame_counter, 1, 0, MAX_ENCODE_TIME_US);
+        
+        vpx_img_free(&img); /* FIXME don't free? */
+        if ( vrc != VPX_CODEC_OK) {
+            LOGGER_ERROR("Could not encode video frame: %s\n", vpx_codec_err_to_string(vrc));
+            rc = TOXAV_ERR_SEND_FRAME_INVALID;
+            goto END;
+        }
+    }
+    
+    ++call->cs->frame_counter;
+    
+    { /* Split and send */
+        vpx_codec_iter_t iter = NULL;
+        const vpx_codec_cx_pkt_t *pkt;
+        
+        cs_init_video_splitter_cycle(call->cs);
+        
+        while ( (pkt = vpx_codec_get_cx_data(call->cs->v_encoder, &iter)) ) {
+            if (pkt->kind == VPX_CODEC_CX_FRAME_PKT) {
+                int parts = cs_update_video_splitter_cycle(call->cs, pkt->data.frame.buf, 
+                                                           pkt->data.frame.sz);
+                
+                if (parts < 0) /* Should never happen though */
+                    continue;
+                
+                uint16_t part_size;
+                const uint8_t *iter;
+                
+                int i;
+                for (i = 0; i < parts; i++) {
+                    iter = cs_iterate_split_video_frame(call->cs, &part_size);
+                    
+                    if (rtp_send_msg(call->rtps[video_index], iter, part_size) < 0)
+                        goto END;
+                }
+            }
+        }
+    }
+    
+END:
+    if (error)
+        *error = rc;
+    
+    return rc == TOXAV_ERR_SEND_FRAME_OK;
 }
 
 void toxav_callback_request_audio_frame(ToxAV* av, toxav_request_audio_frame_cb* function, void* user_data)
 {
-
+    /* TODO */
 }
 
 bool toxav_send_audio_frame(ToxAV* av, uint32_t friend_number, const int16_t* pcm, size_t sample_count, uint8_t channels, uint32_t sampling_rate, TOXAV_ERR_SEND_FRAME* error)
 {
-
+    TOXAV_ERR_SEND_FRAME rc = TOXAV_ERR_SEND_FRAME_OK;
+    IToxAVCall* call;
+    
+    if (m_friend_exists(av->m, friend_number)) {
+        rc = TOXAV_ERR_SEND_FRAME_FRIEND_NOT_FOUND;
+        goto END;
+    }
+    
+    call = i_toxav_get_call(av, friend_number);
+    if (call == NULL) {
+        rc = TOXAV_ERR_SEND_FRAME_FRIEND_NOT_IN_CALL;
+        goto END;
+    }
+    
+    if (av->msi->calls[call->call_idx]->state != msi_CallActive) {
+        /* TODO */
+        rc = TOXAV_ERR_SEND_FRAME_NOT_REQUESTED;
+        goto END;
+    }
+    
+    if ( pcm == NULL ) {
+        rc = TOXAV_ERR_SEND_FRAME_NULL;
+        goto END;
+    }
+    
+    if ( channels != 1 || channels != 2 ) {
+        rc = TOXAV_ERR_SEND_FRAME_INVALID;
+        goto END;
+    }
+    
+    { /* Encode and send */
+        /* TODO redundant? */
+        cs_set_sending_audio_channels(call->cs, channels);
+        cs_set_sending_audio_sampling_rate(call->cs, sampling_rate);
+        
+        uint8_t dest[sample_count * channels * 2 /* sizeof(uint16_t) */];
+        int vrc = opus_encode(call->cs->audio_encoder, pcm, sample_count, dest, sizeof (dest));
+        
+        if (vrc < 0) {
+            LOGGER_WARNING("Failed to encode frame");
+            rc = TOXAV_ERR_SEND_FRAME_INVALID;
+            goto END;
+        }
+        
+        vrc = rtp_send_msg(call->rtps[audio_index], dest, vrc);
+        /* TODO check for error? */
+    }
+    
+END:
+    if (error)
+        *error = rc;
+    
+    return rc == TOXAV_ERR_SEND_FRAME_OK;
 }
 
 void toxav_callback_receive_video_frame(ToxAV* av, toxav_receive_video_frame_cb* function, void* user_data)
 {
-
+    av->vcb.first = function;
+    av->vcb.second = user_data;
 }
 
 void toxav_callback_receive_audio_frame(ToxAV* av, toxav_receive_audio_frame_cb* function, void* user_data)
 {
-
+    av->acb.first = function;
+    av->acb.second = user_data;
 }
 
 
 /*******************************************************************************
- * 
+ *
  * :: Internal
  *
  ******************************************************************************/
@@ -616,12 +832,6 @@ bool i_toxav_prepare_transmission(ToxAV* av, IToxAVCall* call)
     call->cs->agent = av;
     call->cs->call_idx = call->call_idx;
     
-    call->cs->acb.first = av->acb.first;
-    call->cs->acb.second = av->acb.second;
-    
-    call->cs->vcb.first = av->vcb.first;
-    call->cs->vcb.second = av->vcb.second;
-    
     
     if (c_self->audio_bitrate > 0 || c_peer->audio_bitrate > 0) { /* Prepare audio rtp */
         call->rtps[audio_index] = rtp_new(msi_TypeAudio, av->m, av->msi->calls[call->call_idx]->peers[0]);
diff --git a/toxav/toxav_new.h b/toxav/toxav_new.h
index 78e79357..038ee99a 100644
--- a/toxav/toxav_new.h
+++ b/toxav/toxav_new.h
@@ -374,8 +374,9 @@ void toxav_callback_request_video_frame(ToxAV *av, toxav_request_video_frame_cb
  *
  * This is called in response to receiving the `request_video_frame` event.
  *
- * Each plane should contain (width * height) pixels. The Alpha plane can be
- * NULL, in which case every pixel is assumed fully opaque.
+ * Y - plane should be of size: height * width
+ * U - plane should be of size: (height/2) * (width/2)
+ * V - plane should be of size: (height/2) * (width/2)
  *
  * @param friend_number The friend number of the friend to which to send a video
  * frame.
@@ -384,11 +385,10 @@ void toxav_callback_request_video_frame(ToxAV *av, toxav_request_video_frame_cb
  * @param y Y (Luminance) plane data.
  * @param u U (Chroma) plane data.
  * @param v V (Chroma) plane data.
- * @param a A (Alpha) plane data.
  */
 bool toxav_send_video_frame(ToxAV *av, uint32_t friend_number,
                             uint16_t width, uint16_t height,
-                            uint8_t const *y, uint8_t const *u, uint8_t const *v, uint8_t const *a,
+                            uint8_t const *y, uint8_t const *u, uint8_t const *v,
                             TOXAV_ERR_SEND_FRAME *error);
 /**
  * The function type for the `request_audio_frame` callback.