Tidy up mbedtls_asn1_write_len

Signed-off-by: Dave Rodgman <dave.rodgman@arm.com>
diff --git a/library/asn1write.c b/library/asn1write.c
index 4887f15..7d53349 100644
--- a/library/asn1write.c
+++ b/library/asn1write.c
@@ -37,9 +37,8 @@
 #endif
 
     int required = 1;
-    if (len < 0x80) {
-        required = 1;
-    } else {
+
+    if (len >= 0x80) {
         for (size_t l = len; l != 0; l >>= 8) {
             required++;
         }
@@ -55,7 +54,7 @@
     } while (len);
 
     if (required > 1) {
-        *--(*p) = (unsigned char) (required + 0x7f);
+        *--(*p) = (unsigned char) (0x80 + required - 1);
     }
 
     return required;