Almost done

1 files changed, 144 insertions, 132 deletions

diff --git a/toxav/rtp.c b/toxav/rtp.c
index 9657da67..a48eba20 100644
--- a/toxav/rtp.c
+++ b/toxav/rtp.c
@@ -29,6 +29,7 @@
 
 #include "rtp.h"
 #include <stdlib.h>
+#include <assert.h>
 
 #define size_32 4
 #define RTCP_REPORT_INTERVAL_MS 500
@@ -37,8 +38,8 @@
 #define ADD_FLAG_PADDING(_h, _v) do { if ( _v > 0 ) _v = 1; ( _h->flags ) &= 0xDF; ( _h->flags ) |= ( ( ( _v ) << 5 ) & 0x20 ); } while(0)
 #define ADD_FLAG_EXTENSION(_h, _v) do { if ( _v > 0 ) _v = 1; ( _h->flags ) &= 0xEF;( _h->flags ) |= ( ( ( _v ) << 4 ) & 0x10 ); } while(0)
 #define ADD_FLAG_CSRCC(_h, _v) do { ( _h->flags ) &= 0xF0; ( _h->flags ) |= ( ( _v ) & 0x0F ); } while(0)
-#define ADD_SETTING_MARKER(_h, _v) do { if ( _v > 1 ) _v = 1; ( _h->marker_payloadt ) &= 0x7F; ( _h->marker_payloadt ) |= ( ( ( _v ) << 7 ) /*& 0x80 */ ); } while(0)
-#define ADD_SETTING_PAYLOAD(_h, _v) do { if ( _v > 127 ) _v = 127; ( _h->marker_payloadt ) &= 0x80; ( _h->marker_payloadt ) |= ( ( _v ) /* & 0x7F */ ); } while(0)
+#define ADD_SETTING_MARKER(_h, _v) do { ( _h->marker_payloadt ) &= 0x7F; ( _h->marker_payloadt ) |= ( ( ( _v ) << 7 ) /*& 0x80 */ ); } while(0)
+#define ADD_SETTING_PAYLOAD(_h, _v) do { ( _h->marker_payloadt ) &= 0x80; ( _h->marker_payloadt ) |= ( ( _v ) /* & 0x7F */ ); } while(0)
 
 #define GET_FLAG_VERSION(_h) (( _h->flags & 0xd0 ) >> 6)
 #define GET_FLAG_PADDING(_h) (( _h->flags & 0x20 ) >> 5)
@@ -70,17 +71,19 @@ typedef struct RTCPSession_s {
 
 RTPHeader *parse_header_in ( const uint8_t *payload, int length );
 RTPExtHeader *parse_ext_header_in ( const uint8_t *payload, uint16_t length );
-RTPMessage *msg_parse ( const uint8_t *data, int length );
 uint8_t *parse_header_out ( const RTPHeader* header, uint8_t* payload );
 uint8_t *parse_ext_header_out ( const RTPExtHeader* header, uint8_t* payload );
-void build_header ( RTPSession* session, RTPHeader* header );
-void send_rtcp_report ( RTCPSession* session, Messenger* m, uint32_t friendnumber );
 int handle_rtp_packet ( Messenger *m, uint32_t friendnumber, const uint8_t *data, uint16_t length, void *object );
 int handle_rtcp_packet ( Messenger *m, uint32_t friendnumber, const uint8_t *data, uint16_t length, void *object );
+void send_rtcp_report ( RTCPSession* session, Messenger* m, uint32_t friendnumber );
 
 
-RTPSession *rtp_new ( int payload_type, Messenger *messenger, int friend_num )
+RTPSession *rtp_new ( int payload_type, Messenger *messenger, int friend_num, void* cs, int (*mcb) (void*, RTPMessage*) )
 {
+    assert(mcb);
+    assert(cs);
+    assert(messenger);
+    
     RTPSession *retu = calloc(1, sizeof(RTPSession));
 
     if ( !retu ) {
@@ -107,6 +110,8 @@ RTPSession *rtp_new ( int payload_type, Messenger *messenger, int friend_num )
     /* Also set payload type as prefix */
     retu->prefix = payload_type;
     
+    retu->cs = cs;
+    retu->mcb = mcb;
     
     /* Initialize rtcp session */
     if (!(retu->rtcp_session = calloc(1, sizeof(RTCPSession)))) {
@@ -120,6 +125,14 @@ RTPSession *rtp_new ( int payload_type, Messenger *messenger, int friend_num )
     retu->rtcp_session->pl_stats = rb_new(4);
     retu->rtcp_session->rtp_session = retu;
     
+    if (-1 == rtp_start_receiving(retu)) {
+        LOGGER_WARNING("Failed to start rtp receiving mode");
+        free(retu->rtcp_session);
+        free(retu->csrc);
+        free(retu);
+        return NULL;
+    }
+    
     return retu;
 }
 void rtp_kill ( RTPSession *session )
@@ -156,11 +169,12 @@ void rtp_do(RTPSession *session)
         RTCPReport* reports[4];
         
         int i = 0;
-        for (; rb_read(session->rtcp_session->pl_stats, (void**) reports + i); i++);
+        for (; i < 4; i++)
+            rb_read(session->rtcp_session->pl_stats, (void**) reports + i);
         
         /* Check for timed out reports (> 6 sec) */
         uint64_t now = current_time_monotonic();
-        for (i = 0; i < 4 && now - reports[i]->timestamp < 6000; i ++);
+        for (i = 0; i < 4 && (now - reports[i]->timestamp) < 6000; i ++);
         for (; i < 4; i ++) {
             rb_write(session->rtcp_session->pl_stats, reports[i]);
             reports[i] = NULL;
@@ -168,18 +182,18 @@ void rtp_do(RTPSession *session)
         if (!rb_empty(session->rtcp_session->pl_stats)) {
             for (i = 0; reports[i] != NULL; i ++)
                 free(reports[i]);
-            return; /* As some reports are timed out, we need more... */
+            return; /* As some reports are timed out, we need more */
         }
         
         /* We have 4 on-time reports so we can proceed */
         uint32_t quality = 100;
         for (i = 0; i < 4; i++) {
-            uint32_t idx = reports[i]->received_packets * 100 / reports[i]->expected_packets;
-            quality = MIN(quality, idx);
+            uint32_t current = reports[i]->received_packets * 100 / reports[i]->expected_packets;
+            quality = MIN(quality, current);
             free(reports[i]);
         }
         
-        if (quality <= 70) {
+        if (quality <= 90) {
             session->tstate = rtp_StateBad;
             LOGGER_WARNING("Stream quality: BAD");
         } else if (quality >= 99) {
@@ -220,7 +234,7 @@ int rtp_stop_receiving(RTPSession* session)
     
     return 0;
 }
-int rtp_send_msg ( RTPSession *session, const uint8_t *data, uint16_t length )
+int rtp_send_data ( RTPSession *session, const uint8_t *data, uint16_t length, bool dummy )
 {
     if ( !session ) {
         LOGGER_WARNING("No session!");
@@ -231,12 +245,31 @@ int rtp_send_msg ( RTPSession *session, const uint8_t *data, uint16_t length )
     uint8_t *it;
 
     RTPHeader header[1];
-    build_header(session, header);
+    ADD_FLAG_VERSION ( header, session->version );
+    ADD_FLAG_PADDING ( header, session->padding );
+    ADD_FLAG_EXTENSION ( header, session->extension );
+    ADD_FLAG_CSRCC ( header, session->cc );
+    ADD_SETTING_MARKER ( header, session->marker );
+    
+    if (dummy)
+        ADD_SETTING_PAYLOAD ( header, (session->payload_type + 2) % 128 );
+    else
+        ADD_SETTING_PAYLOAD ( header, session->payload_type );
 
+    header->sequnum = session->sequnum;
+    header->timestamp = current_time_monotonic();
+    header->ssrc = session->ssrc;
+
+    int i;
+    for ( i = 0; i < session->cc; i++ )
+        header->csrc[i] = session->csrc[i];
+
+    header->length = 12 /* Minimum header len */ + ( session->cc * size_32 );
+    
     uint32_t parsed_len = length + header->length + 1;
+    assert(parsed_len + (session->ext_header ? session->ext_header->length * size_32 : 0) > MAX_RTP_SIZE );
 
     parsed[0] = session->prefix;
-
     it = parse_header_out ( header, parsed + 1 );
     
     if ( session->ext_header ) {
@@ -244,8 +277,7 @@ int rtp_send_msg ( RTPSession *session, const uint8_t *data, uint16_t length )
         it = parse_ext_header_out ( session->ext_header, it );
     }
 
-    memcpy ( it, data, length );
-    
+    memcpy(it, data, length);
     
     if ( -1 == send_custom_lossy_packet(session->m, session->friend_id, parsed, parsed_len) ) {
         LOGGER_WARNING("Failed to send full packet (len: %d)! std error: %s", length, strerror(errno));
@@ -256,18 +288,11 @@ int rtp_send_msg ( RTPSession *session, const uint8_t *data, uint16_t length )
     session->sequnum = session->sequnum >= MAX_SEQU_NUM ? 0 : session->sequnum + 1;
     return 0;
 }
-void rtp_free_msg ( RTPSession *session, RTPMessage *msg )
+void rtp_free_msg ( RTPMessage *msg )
 {
-    if ( !session ) {
-        if ( msg->ext_header ) {
-            free ( msg->ext_header->table );
-            free ( msg->ext_header );
-        }
-    } else {
-        if ( msg->ext_header && session->ext_header != msg->ext_header ) {
-            free ( msg->ext_header->table );
-            free ( msg->ext_header );
-        }
+    if ( msg->ext_header ) {
+        free ( msg->ext_header->table );
+        free ( msg->ext_header );
     }
     
     free ( msg->header );
@@ -389,50 +414,6 @@ RTPExtHeader *parse_ext_header_in ( const uint8_t *payload, uint16_t length )
 
     return retu;
 }
-RTPMessage *msg_parse ( const uint8_t *data, int length )
-{
-    /* TODO: data dynamic, [0] 
-     * TODO: dummy payload type
-     * TODO: parse header before allocating message
-     */
-    RTPMessage *retu = calloc(1, sizeof (RTPMessage));
-
-    retu->header = parse_header_in ( data, length ); /* It allocates memory and all */
-
-    if ( !retu->header ) {
-        LOGGER_WARNING("Header failed to extract!");
-        free(retu);
-        return NULL;
-    }
-
-    uint16_t from_pos = retu->header->length;
-    retu->length = length - from_pos;
-
-    if ( GET_FLAG_EXTENSION ( retu->header ) ) {
-        retu->ext_header = parse_ext_header_in ( data + from_pos, length );
-
-        if ( retu->ext_header ) {
-            retu->length -= ( 4 /* Minimum ext header len */ + retu->ext_header->length * size_32 );
-            from_pos += ( 4 /* Minimum ext header len */ + retu->ext_header->length * size_32 );
-        } else { /* Error */
-            LOGGER_WARNING("Ext Header failed to extract!");
-            rtp_free_msg(NULL, retu);
-            return NULL;
-        }
-    } else {
-        retu->ext_header = NULL;
-    }
-
-    if ( length - from_pos <= MAX_RTP_SIZE )
-        memcpy ( retu->data, data + from_pos, length - from_pos );
-    else {
-        LOGGER_WARNING("Invalid length!");
-        rtp_free_msg(NULL, retu);
-        return NULL;
-    }
-
-    return retu;
-}
 uint8_t *parse_header_out ( const RTPHeader *header, uint8_t *payload )
 {
     uint8_t cc = GET_FLAG_CSRCC ( header );
@@ -495,88 +476,95 @@ uint8_t *parse_ext_header_out ( const RTPExtHeader *header, uint8_t *payload )
 
     return it + 4;
 }
-void build_header ( RTPSession *session, RTPHeader *header )
-{
-    ADD_FLAG_VERSION ( header, session->version );
-    ADD_FLAG_PADDING ( header, session->padding );
-    ADD_FLAG_EXTENSION ( header, session->extension );
-    ADD_FLAG_CSRCC ( header, session->cc );
-    ADD_SETTING_MARKER ( header, session->marker );
-    ADD_SETTING_PAYLOAD ( header, session->payload_type );
-
-    header->sequnum = session->sequnum;
-    header->timestamp = current_time_monotonic(); /* milliseconds */
-    header->ssrc = session->ssrc;
-
-    int i;
-    for ( i = 0; i < session->cc; i++ )
-        header->csrc[i] = session->csrc[i];
-
-    header->length = 12 /* Minimum header len */ + ( session->cc * size_32 );
-}
-void send_rtcp_report(RTCPSession* session, Messenger* m, uint32_t friendnumber)
-{
-    if (session->last_expected_packets == 0)
-        return;
-    
-    uint8_t parsed[9];
-    parsed[0] = session->prefix;
-    
-    uint32_t received_packets = htonl(session->last_received_packets);
-    uint32_t expected_packets = htonl(session->last_expected_packets);
-    
-    memcpy(parsed + 1, &received_packets, 4);
-    memcpy(parsed + 5, &expected_packets, 4);
-    
-    if (-1 == send_custom_lossy_packet(m, friendnumber, parsed, sizeof(parsed)))
-        LOGGER_WARNING("Failed to send full packet (len: %d)! std error: %s", sizeof(parsed), strerror(errno));
-    else {
-        LOGGER_DEBUG("Sent rtcp report: ex: %d rc: %d", session->last_expected_packets, session->last_received_packets);
-        
-        session->last_received_packets = 0;
-        session->last_expected_packets = 0;
-        session->last_sent_report_ts = current_time_monotonic();
-    }
-}
 int handle_rtp_packet ( Messenger* m, uint32_t friendnumber, const uint8_t* data, uint16_t length, void* object )
 {
     RTPSession *session = object;
-    RTPMessage *msg;
 
-    if ( !session || length < 13 ) { /* 12 is the minimum length for rtp + desc. byte */
+    if ( !session || length < 13 || length > MAX_RTP_SIZE ) {
         LOGGER_WARNING("No session or invalid length of received buffer!");
         return -1;
     }
+    
+    RTPHeader* header = parse_header_in ( data, length );
 
-    msg = msg_parse ( data + 1, length - 1 );
-
-    if ( !msg ) {
-        LOGGER_WARNING("Could not parse message!");
+    if ( !header ) {
+        LOGGER_WARNING("Could not parse message: Header failed to extract!");
         return -1;
     }
+    
+    RTPExtHeader* ext_header = NULL;
+    
+    uint16_t from_pos = header->length;
+    uint16_t msg_length = length - from_pos;
 
+    if ( GET_FLAG_EXTENSION ( header ) ) {
+        ext_header = parse_ext_header_in ( data + from_pos, length );
+
+        if ( ext_header ) {
+            msg_length -= ( 4 /* Minimum ext header len */ + ext_header->length * size_32 );
+            from_pos += ( 4 /* Minimum ext header len */ + ext_header->length * size_32 );
+        } else { /* Error */
+            LOGGER_WARNING("Could not parse message: Ext Header failed to extract!");
+            free(header);
+            return -1;
+        }
+    }
+    
+    if (msg_length > MAX_RTP_SIZE) {
+        LOGGER_WARNING("Could not parse message: Invalid length!");
+        free(header);
+        free(ext_header);
+        return -1;
+    }
+    
     /* Check if message came in late */
-    if ( msg->header->sequnum > session->rsequnum || msg->header->timestamp > session->rtimestamp ) {
+    if ( header->sequnum > session->rsequnum || header->timestamp > session->rtimestamp ) {
         /* Not late */
-        if (msg->header->sequnum > session->rsequnum)
-            session->rtcp_session->last_expected_packets += msg->header->sequnum - session->rsequnum;
-        else if (msg->header->sequnum < session->rsequnum)
-            session->rtcp_session->last_expected_packets += (msg->header->sequnum + 65535) - session->rsequnum;
+        if (header->sequnum > session->rsequnum)
+            session->rtcp_session->last_expected_packets += header->sequnum - session->rsequnum;
+        else if (header->sequnum < session->rsequnum)
+            session->rtcp_session->last_expected_packets += (header->sequnum + 65535) - session->rsequnum;
         else /* Usual case when transmission starts */
             session->rtcp_session->last_expected_packets ++;
         
-        session->rsequnum = msg->header->sequnum;
-        session->rtimestamp = msg->header->timestamp;
+        session->rsequnum = header->sequnum;
+        session->rtimestamp = header->timestamp;
     }
 
     session->rtcp_session->last_received_packets ++;
     
-    if (session->mcb)
-        return session->mcb (session->cs, msg);
-    else {
-        rtp_free_msg(session, msg);
+    /* Check if the message is dummy. We don't keep dummy messages */
+    if (GET_SETTING_PAYLOAD(header) == (session->payload_type + 2) % 128) {
+        LOGGER_DEBUG("Received dummy rtp message");
+        free(header);
+        free(ext_header);
+        return 0;
+    }
+    
+    /* Otherwise we will store the message if we have an appropriate handler */
+    if (!session->mcb) {
+        LOGGER_DEBUG("No handler for the message of %d payload", GET_SETTING_PAYLOAD(header));
+        free(header);
+        free(ext_header);
         return 0;
     }
+    
+    RTPMessage *msg = calloc(1, sizeof (RTPMessage) + msg_length);
+    
+    if ( !msg ) {
+        LOGGER_WARNING("Could not parse message: Allocation failed!");
+        free(header);
+        free(ext_header);
+        return -1;
+    }
+    
+    msg->header = header;
+    msg->ext_header = ext_header;
+    msg->length = msg_length;
+    
+    memcpy ( msg->data, data + from_pos, msg_length );
+    
+    return session->mcb (session->cs, msg);
 }
 int handle_rtcp_packet ( Messenger* m, uint32_t friendnumber, const uint8_t* data, uint16_t length, void* object )
 {
@@ -605,4 +593,28 @@ int handle_rtcp_packet ( Messenger* m, uint32_t friendnumber, const uint8_t* dat
     
     LOGGER_DEBUG("Got rtcp report: ex: %d rc: %d", report->expected_packets, report->received_packets);
     return 0;
+}
+void send_rtcp_report(RTCPSession* session, Messenger* m, uint32_t friendnumber)
+{
+    if (session->last_expected_packets == 0)
+        return;
+    
+    uint8_t parsed[9];
+    parsed[0] = session->prefix;
+    
+    uint32_t received_packets = htonl(session->last_received_packets);
+    uint32_t expected_packets = htonl(session->last_expected_packets);
+    
+    memcpy(parsed + 1, &received_packets, 4);
+    memcpy(parsed + 5, &expected_packets, 4);
+    
+    if (-1 == send_custom_lossy_packet(m, friendnumber, parsed, sizeof(parsed)))
+        LOGGER_WARNING("Failed to send full packet (len: %d)! std error: %s", sizeof(parsed), strerror(errno));
+    else {
+        LOGGER_DEBUG("Sent rtcp report: ex: %d rc: %d", session->last_expected_packets, session->last_received_packets);
+        
+        session->last_received_packets = 0;
+        session->last_expected_packets = 0;
+        session->last_sent_report_ts = current_time_monotonic();
+    }
 }
\ No newline at end of file