Add mbedtls_asn1_write_len() support for 3 and 4 byte lengths

As a consequence also adds coverage for reading 3 and 4 byte lengths
(which were not covered before)
diff --git a/library/asn1write.c b/library/asn1write.c
index ef35ee4..69b61b2 100644
--- a/library/asn1write.c
+++ b/library/asn1write.c
@@ -41,11 +41,6 @@
 
 int mbedtls_asn1_write_len( unsigned char **p, unsigned char *start, size_t len )
 {
-    // We don't support lengths over 65535 for now
-    //
-    if( len > 0xFFFF )
-        return( MBEDTLS_ERR_ASN1_INVALID_LENGTH );
-
     if( len < 0x80 )
     {
         if( *p - start < 1 )
@@ -65,14 +60,43 @@
         return( 2 );
     }
 
-    if( *p - start < 3 )
-        return( MBEDTLS_ERR_ASN1_BUF_TOO_SMALL );
+    if( len <= 0xFFFF )
+    {
+        if( *p - start < 3 )
+            return( MBEDTLS_ERR_ASN1_BUF_TOO_SMALL );
 
-    *--(*p) = len % 256;
-    *--(*p) = ( len / 256 ) % 256;
-    *--(*p) = 0x82;
+        *--(*p) = ( len       ) & 0xFF;
+        *--(*p) = ( len >>  8 ) & 0xFF;
+        *--(*p) = 0x82;
+        return( 3 );
+    }
 
-    return( 3 );
+    if( len <= 0xFFFFFF )
+    {
+        if( *p - start < 4 )
+            return( MBEDTLS_ERR_ASN1_BUF_TOO_SMALL );
+
+        *--(*p) = ( len       ) & 0xFF;
+        *--(*p) = ( len >>  8 ) & 0xFF;
+        *--(*p) = ( len >> 16 ) & 0xFF;
+        *--(*p) = 0x83;
+        return( 4 );
+    }
+
+    if( len <= 0xFFFFFFFF )
+    {
+        if( *p - start < 5 )
+            return( MBEDTLS_ERR_ASN1_BUF_TOO_SMALL );
+
+        *--(*p) = ( len       ) & 0xFF;
+        *--(*p) = ( len >>  8 ) & 0xFF;
+        *--(*p) = ( len >> 16 ) & 0xFF;
+        *--(*p) = ( len >> 24 ) & 0xFF;
+        *--(*p) = 0x84;
+        return( 5 );
+    }
+
+    return( MBEDTLS_ERR_ASN1_INVALID_LENGTH );
 }
 
 int mbedtls_asn1_write_tag( unsigned char **p, unsigned char *start, unsigned char tag )