library: debug: add support for RSA keys in PSA friendly format

Signed-off-by: Valerio Setti <valerio.setti@nordicsemi.no>

library/debug.c

@@ -220,20 +220,20 @@ void mbedtls_debug_print_mpi(const mbedtls_ssl_context *ssl, int level,
 
 #if defined(MBEDTLS_X509_CRT_PARSE_C) && !defined(MBEDTLS_X509_REMOVE_INFO)
 
-#if defined(PSA_WANT_KEY_TYPE_ECC_PUBLIC_KEY)
+#if defined(PSA_WANT_KEY_TYPE_ECC_PUBLIC_KEY) || defined(MBEDTLS_PK_USE_PSA_RSA_DATA) //no-check-names
 static void mbedtls_debug_print_ec_coord(const mbedtls_ssl_context *ssl, int level,
                                          const char *file, int line, const char *text,
                                          const unsigned char *buf, size_t len)
 {
     char str[DEBUG_BUF_SIZE];
-    size_t i, idx = 0;
+    size_t i, len_bytes = PSA_BITS_TO_BYTES(len), idx = 0;
 
     mbedtls_snprintf(str + idx, sizeof(str) - idx, "value of '%s' (%u bits) is:\n",
-                     text, (unsigned int) len * 8);
+                     text, (unsigned int) len);
 
     debug_send_line(ssl, level, file, line, str);
 
-    for (i = 0; i < len; i++) {
+    for (i = 0; i < len_bytes; i++) {
         if (i >= 4096) {
             break;
         }
@@ -251,16 +251,14 @@ static void mbedtls_debug_print_ec_coord(const mbedtls_ssl_context *ssl, int lev
                                 (unsigned int) buf[i]);
     }
 
-    if (len > 0) {
+    if (len_bytes > 0) {
-        for (/* i = i */; i % 16 != 0; i++) {
-            idx += mbedtls_snprintf(str + idx, sizeof(str) - idx, "   ");
-        }
-
         mbedtls_snprintf(str + idx, sizeof(str) - idx, "\n");
         debug_send_line(ssl, level, file, line, str);
     }
 }
+#endif /* PSA_WANT_KEY_TYPE_ECC_PUBLIC_KEY || MBEDTLS_PK_USE_PSA_RSA_DATA */ //no-check-names
 
+#if defined(PSA_WANT_KEY_TYPE_ECC_PUBLIC_KEY)
 static void mbedtls_debug_print_psa_ec(const mbedtls_ssl_context *ssl, int level,
                                        const char *file, int line,
                                        const char *text, const mbedtls_pk_context *pk)
@@ -283,15 +281,99 @@ static void mbedtls_debug_print_psa_ec(const mbedtls_ssl_context *ssl, int level
     /* X coordinate */
     coord_start = pk->pub_raw + 1;
     mbedtls_snprintf(str, sizeof(str), "%s(X)", text);
-    mbedtls_debug_print_ec_coord(ssl, level, file, line, str, coord_start, coord_len);
+    mbedtls_debug_print_ec_coord(ssl, level, file, line, str, coord_start, coord_len * 8);
 
     /* Y coordinate */
     coord_start = coord_start + coord_len;
     mbedtls_snprintf(str, sizeof(str), "%s(Y)", text);
-    mbedtls_debug_print_ec_coord(ssl, level, file, line, str, coord_start, coord_len);
+    mbedtls_debug_print_ec_coord(ssl, level, file, line, str, coord_start, coord_len * 8);
 }
 #endif /* PSA_WANT_KEY_TYPE_ECC_PUBLIC_KEY */
 
+#if defined(MBEDTLS_PK_USE_PSA_RSA_DATA) //no-check-names
+static size_t debug_count_valid_bits(unsigned char **buf, size_t len)
+{
+    size_t i, bits;
+
+    /* Ignore initial null bytes (if any). */
+    while ((len > 0) && (**buf == 0x00)) {
+        (*buf)++;
+        len--;
+    }
+
+    if (len == 0) {
+        return 0;
+    }
+
+    bits = len * 8;
+
+    /* Ignore initial null bits (if any). */
+    for (i = 7; i > 0; i--) {
+        if ((**buf & (0x1 << i)) != 0) {
+            break;
+        }
+        bits--;
+    }
+
+    return bits;
+}
+
+static void mbedtls_debug_print_psa_rsa(const mbedtls_ssl_context *ssl, int level,
+                                        const char *file, int line,
+                                        const char *text, const mbedtls_pk_context *pk)
+{
+    char str[DEBUG_BUF_SIZE];
+    unsigned char key_der[MBEDTLS_PK_MAX_RSA_PUBKEY_RAW_LEN]; //no-check-names
+    unsigned char *start_cur;
+    unsigned char *end_cur;
+    size_t len, bits;
+    int ret;
+
+    if (pk->pub_raw_len > sizeof(key_der)) {
+        return;
+    }
+
+    memcpy(key_der, pk->pub_raw, pk->pub_raw_len);
+    start_cur = key_der;
+    end_cur = key_der + pk->pub_raw_len;
+
+    ret = mbedtls_asn1_get_tag(&start_cur, end_cur, &len,
+                               MBEDTLS_ASN1_SEQUENCE | MBEDTLS_ASN1_CONSTRUCTED);
+    if (ret != 0) {
+        return;
+    }
+
+    ret = mbedtls_asn1_get_tag(&start_cur, end_cur, &len, MBEDTLS_ASN1_INTEGER);
+    if (ret != 0) {
+        return;
+    }
+
+    bits = debug_count_valid_bits(&start_cur, len);
+    if (bits == 0) {
+        return;
+    }
+    len = PSA_BITS_TO_BYTES(bits);
+
+    mbedtls_snprintf(str, sizeof(str), "%s.N", text);
+    mbedtls_debug_print_ec_coord(ssl, level, file, line, str, start_cur, bits);
+
+    start_cur += len;
+
+    ret = mbedtls_asn1_get_tag(&start_cur, end_cur, &len, MBEDTLS_ASN1_INTEGER);
+    if (ret != 0) {
+        return;
+    }
+
+    bits = debug_count_valid_bits(&start_cur, len);
+    if (bits == 0) {
+        return;
+    }
+
+    mbedtls_snprintf(str, sizeof(str), "%s.E", text);
+    mbedtls_debug_print_ec_coord(ssl, level, file, line, str, start_cur, bits);
+}
+#endif /* MBEDTLS_PK_USE_PSA_RSA_DATA */ //no-check-names
+
 static void debug_print_pk(const mbedtls_ssl_context *ssl, int level,
                            const char *file, int line,
                            const char *text, const mbedtls_pk_context *pk)
@@ -321,6 +403,11 @@ static void debug_print_pk(const mbedtls_ssl_context *ssl, int level,
             mbedtls_debug_print_mpi(ssl, level, file, line, name, items[i].value);
         } else
 #endif /* MBEDTLS_RSA_C */
+#if defined(MBEDTLS_PK_USE_PSA_RSA_DATA) //no-check-names
+        if (items[i].type == MBEDTLS_PK_DEBUG_PSA_RSA) { //no-check-names
+            mbedtls_debug_print_psa_rsa(ssl, level, file, line, name, items[i].value);
+        } else
+#endif /* MBEDTLS_PK_USE_PSA_RSA_DATA */ //no-check-names
 #if defined(PSA_WANT_KEY_TYPE_ECC_PUBLIC_KEY)
         if (items[i].type == MBEDTLS_PK_DEBUG_PSA_EC) {
             mbedtls_debug_print_psa_ec(ssl, level, file, line, name, items[i].value);