Add server certificate selection callback

https://github.com/ARMmbed/mbedtls/issues/5430

Signed-off-by: Glenn Strauss <gstrauss@gluelogic.com>
diff --git a/ChangeLog.d/mbedtls_ssl_cert_cb.txt b/ChangeLog.d/mbedtls_ssl_cert_cb.txt
new file mode 100644
index 0000000..89e9fdf
--- /dev/null
+++ b/ChangeLog.d/mbedtls_ssl_cert_cb.txt
@@ -0,0 +1,3 @@
+Features
+   * Add server certificate selection callback near end of Client Hello.
+     Register callback with mbedtls_ssl_conf_cert_cb().
diff --git a/include/mbedtls/ssl.h b/include/mbedtls/ssl.h
index 7544f42..0e93849 100644
--- a/include/mbedtls/ssl.h
+++ b/include/mbedtls/ssl.h
@@ -1475,6 +1475,10 @@
      * access it afterwards.
      */
     mbedtls_ssl_user_data_t MBEDTLS_PRIVATE(user_data);
+
+#if defined(MBEDTLS_SSL_SRV_C)
+    int (*MBEDTLS_PRIVATE(f_cert_cb))(mbedtls_ssl_context *); /*!< certificate selection callback */
+#endif /* MBEDTLS_SSL_SRV_C */
 };
 
 struct mbedtls_ssl_context
@@ -2220,6 +2224,28 @@
                                mbedtls_ssl_set_timer_t *f_set_timer,
                                mbedtls_ssl_get_timer_t *f_get_timer );
 
+#if defined(MBEDTLS_SSL_SRV_C)
+/**
+ * \brief           Set the certificate selection callback (server-side only).
+ *
+ *                  If set, the callback is always called for each handshake,
+ *                  after `ClientHello` processing has finished.
+ *
+ *                  The callback has the following parameters:
+ *                  - \c mbedtls_ssl_context*: The SSL context to which
+ *                                             the operation applies.
+ *                  The return value of the callback is 0 if successful,
+ *                  or a specific MBEDTLS_ERR_XXX code, which will cause
+ *                  the handshake to be aborted.
+ *
+ * \param conf      The SSL configuration to register the callback with.
+ * \param f_cert_cb The callback for selecting server certificate after
+ *                  `ClientHello` processing has finished.
+ */
+void mbedtls_ssl_conf_cert_cb( mbedtls_ssl_config *conf,
+                               int (*f_cert_cb)(mbedtls_ssl_context *) );
+#endif /* MBEDTLS_SSL_SRV_C */
+
 /**
  * \brief           Callback type: generate and write session ticket
  *
diff --git a/library/ssl_srv.c b/library/ssl_srv.c
index e9febfd..bd0982c 100644
--- a/library/ssl_srv.c
+++ b/library/ssl_srv.c
@@ -1871,9 +1871,19 @@
     }
 
     /*
+     * Server certification selection (after processing TLS extensions)
+     */
+    if( ssl->conf->f_cert_cb && ( ret = ssl->conf->f_cert_cb( ssl ) ) != 0 )
+    {
+        MBEDTLS_SSL_DEBUG_RET( 1, "f_cert_cb", ret );
+        return( ret );
+    }
+
+    /*
      * Search for a matching ciphersuite
      * (At the end because we need information from the EC-based extensions
-     * and certificate from the SNI callback triggered by the SNI extension.)
+     * and certificate from the SNI callback triggered by the SNI extension
+     * or certificate from server certificate selection callback.)
      */
     got_common_suite = 0;
     ciphersuites = ssl->conf->ciphersuite_list;
diff --git a/library/ssl_tls.c b/library/ssl_tls.c
index adb18ab..9d41cb4 100644
--- a/library/ssl_tls.c
+++ b/library/ssl_tls.c
@@ -1233,6 +1233,14 @@
 }
 
 #if defined(MBEDTLS_SSL_SRV_C)
+void mbedtls_ssl_conf_cert_cb( mbedtls_ssl_config *conf,
+                               int (*f_cert_cb)(mbedtls_ssl_context *) )
+{
+    conf->f_cert_cb = f_cert_cb;
+}
+#endif /* MBEDTLS_SSL_SRV_C */
+
+#if defined(MBEDTLS_SSL_SRV_C)
 void mbedtls_ssl_conf_session_cache( mbedtls_ssl_config *conf,
                                      void *p_cache,
                                      mbedtls_ssl_cache_get_t *f_get_cache,