From 28f4beab1c3f2df6a45000fa8985bf46736700b6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Manuel=20P=C3=A9gouri=C3=A9-Gonnard?=
 <manuel.pegourie-gonnard@arm.com>
Date: Wed, 13 Sep 2017 14:00:05 +0200
Subject: [PATCH] Start implementing fragmentation

---
 include/mbedtls/ssl_internal.h |  5 ++-
 library/ssl_tls.c              | 72 ++++++++++++++++++++++++++++++----
 2 files changed, 68 insertions(+), 9 deletions(-)

diff --git a/include/mbedtls/ssl_internal.h b/include/mbedtls/ssl_internal.h
index 501202bb3..18982f89a 100644
--- a/include/mbedtls/ssl_internal.h
+++ b/include/mbedtls/ssl_internal.h
@@ -298,8 +298,9 @@ struct mbedtls_ssl_handshake_params
 
     uint32_t retransmit_timeout;        /*!<  Current value of timeout       */
     unsigned char retransmit_state;     /*!<  Retransmission state           */
-    mbedtls_ssl_flight_item *flight;            /*!<  Current outgoing flight        */
-    mbedtls_ssl_flight_item *cur_msg;           /*!<  Current message in flight      */
+    mbedtls_ssl_flight_item *flight;    /*!<  Current outgoing flight        */
+    mbedtls_ssl_flight_item *cur_msg;   /*!<  Current message in flight      */
+    unsigned char *cur_msg_p;           /*!<  Position in current message    */
     unsigned int in_flight_start_seq;   /*!<  Minimum message sequence in the
                                               flight being received          */
     mbedtls_ssl_transform *alt_transform_out;   /*!<  Alternative transform for
diff --git a/library/ssl_tls.c b/library/ssl_tls.c
index 5f032232a..6e0f6b604 100644
--- a/library/ssl_tls.c
+++ b/library/ssl_tls.c
@@ -2852,16 +2852,23 @@ int mbedtls_ssl_flight_transmit( mbedtls_ssl_context *ssl )
         MBEDTLS_SSL_DEBUG_MSG( 2, ( "initialise fligh transmission" ) );
 
         ssl->handshake->cur_msg = ssl->handshake->flight;
+        ssl->handshake->cur_msg_p = ssl->handshake->flight->p + 12;
         ssl_swap_epochs( ssl );
 
         ssl->handshake->retransmit_state = MBEDTLS_SSL_RETRANS_SENDING;
     }
 
+    /*
+     * XXX: this should not be hardcoded.
+     * Currently UDP limit - HS header - Record header
+     * (Should account for encryption overhead (renegotiation, finished)?)
+     */
+#define HS_LIMIT    ( 512 - 12 - 13 )
+
     while( ssl->handshake->cur_msg != NULL )
     {
         int ret;
-        mbedtls_ssl_flight_item *cur = ssl->handshake->cur_msg;
-
+        const mbedtls_ssl_flight_item * const cur = ssl->handshake->cur_msg;
         /* Swap epochs before sending Finished: we can't do it after
          * sending ChangeCipherSpec, in case write returns WANT_READ.
          * Must be done before copying, may change out_msg pointer */
@@ -2871,14 +2878,64 @@ int mbedtls_ssl_flight_transmit( mbedtls_ssl_context *ssl )
             ssl_swap_epochs( ssl );
         }
 
-        memcpy( ssl->out_msg, cur->p, cur->len );
-        ssl->out_msglen = cur->len;
-        ssl->out_msgtype = cur->type;
+        /* CCS is copied as is, while HS messages may need fragmentation */
+        if( cur->type == MBEDTLS_SSL_MSG_CHANGE_CIPHER_SPEC )
+        {
+            memcpy( ssl->out_msg, cur->p, cur->len );
+            ssl->out_msglen = cur->len;
+            ssl->out_msgtype = cur->type;
 
-        ssl->handshake->cur_msg = cur->next;
+            /* Update position inside current message */
+            ssl->handshake->cur_msg_p += cur->len;
+        }
+        else
+        {
+            const unsigned char * const p = ssl->handshake->cur_msg_p;
+            const size_t hs_len = cur->len - 12;
+            const size_t frag_off = p - ( cur->p + 12 );
+            const size_t rem_len = hs_len - frag_off;
+            const size_t frag_len = rem_len > HS_LIMIT ? HS_LIMIT : rem_len;
 
-        MBEDTLS_SSL_DEBUG_BUF( 3, "handshake header", ssl->out_msg, 12 );
+            /* Messages are stored with handshake headers as if not fragmented,
+             * copy beginning of headers then fill fragmentation fields.
+             * Handshake headers: type(1) len(3) seq(2) f_off(3) f_len(3) */
+            memcpy( ssl->out_msg, cur->p, 6 );
 
+            ssl->out_msg[6] = ( ( frag_off >> 16 ) & 0xff );
+            ssl->out_msg[7] = ( ( frag_off >>  8 ) & 0xff );
+            ssl->out_msg[8] = ( ( frag_off       ) & 0xff );
+
+            ssl->out_msg[ 9] = ( ( frag_len >> 16 ) & 0xff );
+            ssl->out_msg[10] = ( ( frag_len >>  8 ) & 0xff );
+            ssl->out_msg[11] = ( ( frag_len       ) & 0xff );
+
+            MBEDTLS_SSL_DEBUG_BUF( 3, "handshake header", ssl->out_msg, 12 );
+
+            /* Copy the handshame message content and set records fields */
+            memcpy( ssl->out_msg + 12, p, frag_len );
+            ssl->out_msglen = frag_len + 12;
+            ssl->out_msgtype = cur->type;
+
+            /* Update position inside current message */
+            ssl->handshake->cur_msg_p += frag_len;
+        }
+
+        /* If done with the current message move to the next one if any */
+        if( ssl->handshake->cur_msg_p >= cur->p + cur->len )
+        {
+            if( cur->next != NULL )
+            {
+                ssl->handshake->cur_msg = cur->next;
+                ssl->handshake->cur_msg_p = cur->next->p + 12;
+            }
+            else
+            {
+                ssl->handshake->cur_msg = NULL;
+                ssl->handshake->cur_msg_p = NULL;
+            }
+        }
+
+        /* Actually send the message out */
         if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
         {
             MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
@@ -2886,6 +2943,7 @@ int mbedtls_ssl_flight_transmit( mbedtls_ssl_context *ssl )
         }
     }
 
+    /* Update state and set timer */
     if( ssl->state == MBEDTLS_SSL_HANDSHAKE_OVER )
         ssl->handshake->retransmit_state = MBEDTLS_SSL_RETRANS_FINISHED;
     else