Squashed commit upgrading to mbedtls-2.16.5

Squash merging branch import/mbedtls-2.16.5

058aefb2bfa4 ("core: mbedtls: use SHA-256 crypto accelerated routines")
bcef9baed8f1 ("core: mbedtls: use SHA-1 crypto accelerated routines")
c9359f31db12 ("core: mbedtls: use AES crypto accelerated routines")
0e6c1e2642c7 ("core: merge tee_*_get_digest_size() into a single function")
0cb3c28a2f4d ("libmbedtls: mbedtls_mpi_exp_mod(): optimize mempool usage")
5abf0e6ab72e ("libmbedtls: mbedtls_mpi_exp_mod(): reduce stack usage")
2ccc08ac7fef ("libmbedtls: preserve mempool usage on reinit")
cd2a24648569 ("libmbedtls: mbedtls_mpi_exp_mod() initialize W")
7727182ecb56 ("libmbedtls: fix no CRT issue")
120737075dcf ("libmbedtls: add interfaces in mbedtls for context memory operation")
1126250b3af8 ("libmbedtls: add missing source file chachapoly.c")
23972e9f1c98 ("libmedtls: mpi_miller_rabin: increase count limit")
1fcbc05b3cd2 ("libmbedtls: add mbedtls_mpi_init_mempool()")
66e03f068078 ("libmbedtls: make mbedtls_mpi_mont*() available")
d07e0ce56236 ("libmbedtls: refine mbedtls license header")
491ee2cd0ff4 ("mbedtls: configure mbedtls to reach for config")
9b6cee685d9a ("mbedtls: remove default include/mbedtls/config.h")
84f7467a0a91 ("Import mbedtls-2.16.5")

Signed-off-by: Jerome Forissier <jerome@forissier.org>
Acked-by: Jens Wiklander <jens.wiklander@linaro.org>
diff --git a/lib/libmbedtls/mbedtls/library/asn1write.c b/lib/libmbedtls/mbedtls/library/asn1write.c
index 871477b..aba3675 100644
--- a/lib/libmbedtls/mbedtls/library/asn1write.c
+++ b/lib/libmbedtls/mbedtls/library/asn1write.c
@@ -294,22 +294,28 @@
                           const unsigned char *buf, size_t bits )
 {
     int ret;
-    size_t len = 0, size;
+    size_t len = 0;
+    size_t unused_bits, byte_len;
 
-    size = ( bits / 8 ) + ( ( bits % 8 ) ? 1 : 0 );
+    byte_len = ( bits + 7 ) / 8;
+    unused_bits = ( byte_len * 8 ) - bits;
 
-    // Calculate byte length
-    //
-    if( *p < start || (size_t)( *p - start ) < size + 1 )
+    if( *p < start || (size_t)( *p - start ) < byte_len + 1 )
         return( MBEDTLS_ERR_ASN1_BUF_TOO_SMALL );
 
-    len = size + 1;
-    (*p) -= size;
-    memcpy( *p, buf, size );
+    len = byte_len + 1;
 
-    // Write unused bits
-    //
-    *--(*p) = (unsigned char) (size * 8 - bits);
+    /* Write the bitstring. Ensure the unused bits are zeroed */
+    if( byte_len > 0 )
+    {
+        byte_len--;
+        *--( *p ) = buf[byte_len] & ~( ( 0x1 << unused_bits ) - 1 );
+        ( *p ) -= byte_len;
+        memcpy( *p, buf, byte_len );
+    }
+
+    /* Write unused bits */
+    *--( *p ) = (unsigned char)unused_bits;
 
     MBEDTLS_ASN1_CHK_ADD( len, mbedtls_asn1_write_len( p, start, len ) );
     MBEDTLS_ASN1_CHK_ADD( len, mbedtls_asn1_write_tag( p, start, MBEDTLS_ASN1_BIT_STRING ) );