Reduce size of mbedtls_asn1_get_len

Signed-off-by: Dave Rodgman <dave.rodgman@arm.com>
diff --git a/library/asn1parse.c b/library/asn1parse.c
index edc4c69..abdd0b1 100644
--- a/library/asn1parse.c
+++ b/library/asn1parse.c
@@ -47,47 +47,18 @@
     if ((**p & 0x80) == 0) {
         *len = *(*p)++;
     } else {
-        switch (**p & 0x7F) {
-            case 1:
-                if ((end - *p) < 2) {
-                    return MBEDTLS_ERR_ASN1_OUT_OF_DATA;
-                }
-
-                *len = (*p)[1];
-                (*p) += 2;
-                break;
-
-            case 2:
-                if ((end - *p) < 3) {
-                    return MBEDTLS_ERR_ASN1_OUT_OF_DATA;
-                }
-
-                *len = ((size_t) (*p)[1] << 8) | (*p)[2];
-                (*p) += 3;
-                break;
-
-            case 3:
-                if ((end - *p) < 4) {
-                    return MBEDTLS_ERR_ASN1_OUT_OF_DATA;
-                }
-
-                *len = ((size_t) (*p)[1] << 16) |
-                       ((size_t) (*p)[2] << 8) | (*p)[3];
-                (*p) += 4;
-                break;
-
-            case 4:
-                if ((end - *p) < 5) {
-                    return MBEDTLS_ERR_ASN1_OUT_OF_DATA;
-                }
-
-                *len = ((size_t) (*p)[1] << 24) | ((size_t) (*p)[2] << 16) |
-                       ((size_t) (*p)[3] << 8) |           (*p)[4];
-                (*p) += 5;
-                break;
-
-            default:
-                return MBEDTLS_ERR_ASN1_INVALID_LENGTH;
+        int n = (**p) & 0x7F;
+        if (n == 0 || n > 4) {
+            return MBEDTLS_ERR_ASN1_INVALID_LENGTH;
+        }
+        if ((end - *p) <= n) {
+            return MBEDTLS_ERR_ASN1_OUT_OF_DATA;
+        }
+        *len = 0;
+        (*p)++;
+        while (n--) {
+            *len = (*len << 8) | **p;
+            (*p)++;
         }
     }