Fix incorrect printing of OIDs

The first 2 components of an OID are combined together into the same
subidentifier via the formula:

subidentifier = (component1 * 40) + component2

The current code extracts component1 and component2 using division and
modulo as one would expect. However, there is a subtlety in the
specification[1]:

>This packing of the first two object identifier components recognizes
>that only three values are allocated from the root node, and at most
>39 subsequent values from nodes reached by X = 0 and X = 1.

If the root node (component1) is 2, the subsequent node (component2)
may be greater than 38. For example, the following are real OIDs:
* 2.40.0.25, UPU standard S25
* 2.49.0.0.826.0, Met Office
* 2.999, Allocated example OID

This has 2 implications that the current parsing code does not take
account of:
1. The second component may be > 39, so (subidentifier % 40) is not
correct in all circumstances.
2. The first subidentifier (containing the first 2 components) may be
more than one byte long. Currently we assume it is just 1 byte.

Improve parsing code to deal with these cases correctly.

[1] Rec. ITU-T X.690 (02/2021), 8.19.4

Signed-off-by: David Horstmann <david.horstmann@arm.com>
diff --git a/library/oid.c b/library/oid.c
index fcff152..d8ba773 100644
--- a/library/oid.c
+++ b/library/oid.c
@@ -796,14 +796,39 @@
     p = buf;
     n = size;
 
-    /* First byte contains first two dots */
-    if (oid->len > 0) {
-        ret = mbedtls_snprintf(p, n, "%d.%d", oid->p[0] / 40, oid->p[0] % 40);
-        OID_SAFE_SNPRINTF;
+    /* First subidentifier contains first two OID components */
+    i = 0;
+    value = 0;
+    while (i < oid->len && ((oid->p[i] & 0x80) != 0)) {
+        /* Prevent overflow in value. */
+        if (((value << 7) >> 7) != value) {
+            return MBEDTLS_ERR_OID_BUF_TOO_SMALL;
+        }
+
+        value += oid->p[i] & 0x7F;
+        value <<= 7;
+        i++;
     }
+    if (i >= oid->len) {
+        return MBEDTLS_ERR_OID_BUF_TOO_SMALL;
+    }
+    /* Last byte of first subidentifier */
+    value += oid->p[i] & 0x7F;
+    i++;
+
+    unsigned int component1 = value / 40;
+    if (component1 > 2) {
+        /* The first component can only be 0, 1 or 2.
+         * If oid->p[0] / 40 is greater than 2, the leftover belongs to
+         * the second component. */
+        component1 = 2;
+    }
+    unsigned int component2 = value - (40 * component1);
+    ret = mbedtls_snprintf(p, n, "%u.%u", component1, component2);
+    OID_SAFE_SNPRINTF;
 
     value = 0;
-    for (i = 1; i < oid->len; i++) {
+    for (; i < oid->len; i++) {
         /* Prevent overflow in value. */
         if (((value << 7) >> 7) != value) {
             return MBEDTLS_ERR_OID_BUF_TOO_SMALL;