Merge remote-tracking branch 'origin/pr/558' into baremetal
diff --git a/include/mbedtls/check_config.h b/include/mbedtls/check_config.h
index e73beac..bc94b77 100644
--- a/include/mbedtls/check_config.h
+++ b/include/mbedtls/check_config.h
@@ -595,6 +595,11 @@
 #error "MBEDTLS_SSL_DTLS_ANTI_REPLAY  defined, but not all prerequisites"
 #endif
 
+#if defined(MBEDTLS_SSL_CID) &&                              \
+    ( !defined(MBEDTLS_SSL_TLS_C) || !defined(MBEDTLS_SSL_PROTO_DTLS) )
+#error "MBEDTLS_SSL_CID  defined, but not all prerequisites"
+#endif
+
 #if defined(MBEDTLS_SSL_DTLS_BADMAC_LIMIT) &&                              \
     ( !defined(MBEDTLS_SSL_TLS_C) || !defined(MBEDTLS_SSL_PROTO_DTLS) )
 #error "MBEDTLS_SSL_DTLS_BADMAC_LIMIT  defined, but not all prerequisites"
diff --git a/include/mbedtls/config.h b/include/mbedtls/config.h
index 4d82f62..24501ab 100644
--- a/include/mbedtls/config.h
+++ b/include/mbedtls/config.h
@@ -1270,6 +1270,30 @@
 #define MBEDTLS_SSL_ALL_ALERT_MESSAGES
 
 /**
+ * \def MBEDTLS_SSL_CID
+ *
+ * Enable support for the DTLS Connection ID extension
+ * (version draft-ietf-tls-dtls-connection-id-04)
+ * which allows to identify DTLS connections across changes
+ * in the underlying transport.
+ *
+ * Setting this option enables the SSL APIs `mbedtls_ssl_set_cid()`
+ * and `mbedtls_ssl_get_peer_cid()`. See their documentation for more
+ * information.
+ *
+ * \warning The Connection ID extension is still in draft state.
+ *          We make no stability promises for the availability
+ *          or the shape of the API controlled by this option.
+ *
+ * See also MBEDTLS_SSL_CID_OUT_LEN_MAX and MBEDTLS_SSL_CID_IN_LEN_MAX.
+ *
+ * Requires: MBEDTLS_SSL_PROTO_DTLS
+ *
+ * Uncomment to enable the Connection ID extension.
+ */
+#define MBEDTLS_SSL_CID
+
+/**
  * \def MBEDTLS_SSL_ASYNC_PRIVATE
  *
  * Enable asynchronous external private key operations in SSL. This allows
@@ -3159,6 +3183,20 @@
  */
 //#define MBEDTLS_SSL_IN_CONTENT_LEN              16384
 
+/** \def MBEDTLS_SSL_CID_IN_LEN_MAX
+ *
+ * The maximum length of CIDs used for incoming DTLS messages.
+ *
+ */
+//#define MBEDTLS_SSL_CID_IN_LEN_MAX 32
+
+/** \def MBEDTLS_SSL_CID_OUT_LEN_MAX
+ *
+ * The maximum length of CIDs used for outgoing DTLS messages.
+ *
+ */
+//#define MBEDTLS_SSL_CID_OUT_LEN_MAX 32
+
 /** \def MBEDTLS_SSL_OUT_CONTENT_LEN
  *
  * Maximum length (in bytes) of outgoing plaintext fragments.
diff --git a/include/mbedtls/ssl.h b/include/mbedtls/ssl.h
index 8106bb4..dae4558 100644
--- a/include/mbedtls/ssl.h
+++ b/include/mbedtls/ssl.h
@@ -156,6 +156,9 @@
 #define MBEDTLS_SSL_EXTENDED_MS_DISABLED        0
 #define MBEDTLS_SSL_EXTENDED_MS_ENABLED         1
 
+#define MBEDTLS_SSL_CID_DISABLED                0
+#define MBEDTLS_SSL_CID_ENABLED                 1
+
 #define MBEDTLS_SSL_ETM_DISABLED                0
 #define MBEDTLS_SSL_ETM_ENABLED                 1
 
@@ -252,6 +255,17 @@
 #define MBEDTLS_SSL_DTLS_MAX_BUFFERING 32768
 #endif
 
+/*
+ * Maximum length of CIDs for incoming and outgoing messages.
+ */
+#if !defined(MBEDTLS_SSL_CID_IN_LEN_MAX)
+#define MBEDTLS_SSL_CID_IN_LEN_MAX         32
+#endif
+
+#if !defined(MBEDTLS_SSL_CID_OUT_LEN_MAX)
+#define MBEDTLS_SSL_CID_OUT_LEN_MAX         32
+#endif
+
 /* \} name SECTION: Module settings */
 
 /*
@@ -1396,6 +1410,135 @@
                           mbedtls_ssl_recv_timeout_t *f_recv_timeout );
 
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
+
+#if defined(MBEDTLS_SSL_CID)
+
+
+/**
+ * \brief             (STUB) Configure the use of the Connection ID (CID)
+ *                    extension in the next handshake.
+ *
+ *                    Reference:
+ *                    https://tools.ietf.org/html/draft-ietf-tls-dtls-connection-id-04
+ *
+ *                    The DTLS CID extension allows to reliably associate
+ *                    DTLS records to DTLS connections across changes in the
+ *                    underlying transport (changed IP+Port metadata) by adding
+ *                    explicit connection identifiers (CIDs) to the headers of
+ *                    encrypted DTLS records. The desired CIDs are configured
+ *                    by the application layer and are exchanged in new
+ *                    `ClientHello` / `ServerHello` extensions during the
+ *                    handshake, where each side indicates the CID it wants the
+ *                    peer to use when writing encrypted messages. The CIDs are
+ *                    put to use once records get encrypted: the stack discards
+ *                    any incoming records that don't include the configured CID
+ *                    in their header, and adds the peer's requested CID to the
+ *                    headers of outgoing messages.
+ *
+ *                    This API allows to enable/disable the use of the CID
+ *                    extension in the next handshake and to set the value of
+ *                    the CID to be used for incoming messages.
+ *
+ * \warning           The current implementation of this API does nothing!
+ *                    It is included solely to allow review and coding against
+ *                    the new Connection CID API.
+ *                    The actual implementation will be added in the future.
+ *
+ * \param ssl         The SSL context to configure. This must be initialized.
+ * \param enable      This value determines whether the CID extension should
+ *                    be used or not. Possible values are:
+ *                    - MBEDTLS_SSL_CID_ENABLED to enable the use of the CID.
+ *                    - MBEDTLS_SSL_CID_DISABLED (default) to disable the use
+ *                      of the CID.
+ * \param own_cid     The address of the readable buffer holding the CID we want
+ *                    the peer to use when sending encrypted messages to us.
+ *                    This may be \c NULL if \p own_cid_len is \c 0.
+ *                    This parameter is unused if \p enabled is set to
+ *                    MBEDTLS_SSL_CID_DISABLED.
+ * \param own_cid_len The length of \p own_cid.
+ *                    This parameter is unused if \p enabled is set to
+ *                    MBEDTLS_SSL_CID_DISABLED.
+ *
+ * \note              This CID configuration applies to subsequent handshakes
+ *                    performed on the SSL context \p ssl, but does not trigger
+ *                    one. You still have to call `mbedtls_ssl_handshake()`
+ *                    (for the initial handshake) or `mbedtls_ssl_renegotiate()`
+ *                    (for a renegotiation handshake) explicitly after a
+ *                    successful call to this function to run the handshake.
+ *
+ * \note              This call cannot guarantee that the use of the CID
+ *                    will be successfully negotiated in the next handshake,
+ *                    because the peer might not support it. Specifically:
+ *                    - On the Client, enabling the use of the CID through
+ *                      this call implies that the `ClientHello` in the next
+ *                      handshake will include the CID extension, thereby
+ *                      offering the use of the CID to the server. Only if
+ *                      the `ServerHello` contains the CID extension, too,
+ *                      the CID extension will actually be put to use.
+ *                    - On the Server, enabling the use of the CID through
+ *                      this call implies that that the server will look for
+ *                      the CID extension in a `ClientHello` from the client,
+ *                      and, if present, reply with a CID extension in its
+ *                      `ServerHello`.
+ *
+ * \note              To check whether the use of the CID was negotiated
+ *                    after the subsequent handshake has completed, please
+ *                    use the API mbedtls_ssl_get_peer_cid().
+ *
+ * \warning           If the use of the CID extension is enabled in this call
+ *                    and the subsequent handshake negotiates its use, Mbed TLS
+ *                    will silently drop every packet whose CID does not match
+ *                    the CID configured in \p own_cid. It is the responsibility
+ *                    of the user to adapt the underlying transport to take care
+ *                    of CID-based demultiplexing before handing datagrams to
+ *                    Mbed TLS.
+ *
+ * \return            \c 0 on success. In this case, the CID configuration
+ *                    applies to the next handshake.
+ * \return            A negative error code on failure.
+ */
+int mbedtls_ssl_set_cid( mbedtls_ssl_context *ssl,
+                         int enable,
+                         unsigned char const *own_cid,
+                         size_t own_cid_len );
+
+/**
+ * \brief              (STUB) Get information about the current use of the
+ *                     CID extension.
+ *
+ * \warning            The current implementation of this API does nothing
+ *                     except setting `*enabled` to MBEDTLS_SSL_CID_DISABLED!
+ *                     It is included solely to allow review and coding against
+ *                     the new Connection CID API.
+ *                     The actual implementation will be added in the future.
+ *
+ * \param ssl          The SSL context to query.
+ * \param enabled      The address at which to store whether the CID extension
+ *                     is currently in use or not. If the CID is in use,
+ *                     `*enabled` is set to MBEDTLS_SSL_CID_ENABLED;
+ *                     otherwise, it is set to MBEDTLS_SSL_CID_DISABLED.
+ * \param peer_cid     The address of the buffer in which to store the CID
+ *                     chosen by the peer (if the CID extension is used).
+ * \param peer_cid_len The address at which to store the size of the CID
+ *                     chosen by the peer (if the CID extension is used).
+ *                     This is also the number of Bytes in \p peer_cid that
+ *                     have been written.
+ *
+ * \note               This applies to the state of the CID negotiated in
+ *                     the last complete handshake. If a handshake is in
+ *                     progress, this function will attempt to complete
+ *                     the handshake first.
+ *
+ * \return            \c 0 on success.
+ * \return            A negative error code on failure.
+ */
+int mbedtls_ssl_get_peer_cid( mbedtls_ssl_context *ssl,
+                     int *enabled,
+                     unsigned char peer_cid[ MBEDTLS_SSL_CID_OUT_LEN_MAX ],
+                     size_t *peer_cid_len );
+
+#endif /* MBEDTLS_SSL_CID */
+
 /**
  * \brief          Set the Maximum Tranport Unit (MTU).
  *                 Special value: 0 means unset (no limit).
diff --git a/library/ssl_tls.c b/library/ssl_tls.c
index 38690fa..a8c140c 100644
--- a/library/ssl_tls.c
+++ b/library/ssl_tls.c
@@ -109,6 +109,41 @@
 
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
 
+#if defined(MBEDTLS_SSL_CID)
+/* Top-level Connection ID API */
+
+/* WARNING: This implementation is a stub and doesn't do anything!
+ *          It is included solely to allow review and coding against
+ *          the new Connection CID API. */
+int mbedtls_ssl_set_cid( mbedtls_ssl_context *ssl,
+                         int enable,
+                         unsigned char const *own_cid,
+                         size_t own_cid_len )
+{
+    ((void) ssl);
+    ((void) enable);
+    ((void) own_cid);
+    ((void) own_cid_len);
+    return( 0 );
+}
+
+/* WARNING: This implementation is a stub and doesn't do anything!
+ *          It is included solely to allow review and coding against
+ *          the new Connection CID API. */
+int mbedtls_ssl_get_peer_cid( mbedtls_ssl_context *ssl,
+                     int *enabled,
+                     unsigned char peer_cid[ MBEDTLS_SSL_CID_OUT_LEN_MAX ],
+                     size_t *peer_cid_len )
+{
+    ((void) ssl);
+    ((void) peer_cid);
+    ((void) peer_cid_len);
+
+    *enabled = MBEDTLS_SSL_CID_DISABLED;
+    return( 0 );
+}
+#endif /* MBEDTLS_SSL_CID */
+
 /* Forward declarations for functions related to message buffering. */
 static void ssl_buffering_free( mbedtls_ssl_context *ssl );
 static void ssl_buffering_free_slot( mbedtls_ssl_context *ssl,
diff --git a/library/version_features.c b/library/version_features.c
index 7dbe107..74a0a0e 100644
--- a/library/version_features.c
+++ b/library/version_features.c
@@ -435,6 +435,9 @@
 #if defined(MBEDTLS_SSL_ALL_ALERT_MESSAGES)
     "MBEDTLS_SSL_ALL_ALERT_MESSAGES",
 #endif /* MBEDTLS_SSL_ALL_ALERT_MESSAGES */
+#if defined(MBEDTLS_SSL_CID)
+    "MBEDTLS_SSL_CID",
+#endif /* MBEDTLS_SSL_CID */
 #if defined(MBEDTLS_SSL_ASYNC_PRIVATE)
     "MBEDTLS_SSL_ASYNC_PRIVATE",
 #endif /* MBEDTLS_SSL_ASYNC_PRIVATE */
diff --git a/programs/ssl/query_config.c b/programs/ssl/query_config.c
index 266216f..6c76f47 100644
--- a/programs/ssl/query_config.c
+++ b/programs/ssl/query_config.c
@@ -1202,6 +1202,14 @@
     }
 #endif /* MBEDTLS_SSL_ALL_ALERT_MESSAGES */
 
+#if defined(MBEDTLS_SSL_CID)
+    if( strcmp( "MBEDTLS_SSL_CID", config ) == 0 )
+    {
+        MACRO_EXPANSION_TO_STR( MBEDTLS_SSL_CID );
+        return( 0 );
+    }
+#endif /* MBEDTLS_SSL_CID */
+
 #if defined(MBEDTLS_SSL_ASYNC_PRIVATE)
     if( strcmp( "MBEDTLS_SSL_ASYNC_PRIVATE", config ) == 0 )
     {
@@ -2426,6 +2434,22 @@
     }
 #endif /* MBEDTLS_SSL_IN_CONTENT_LEN */
 
+#if defined(MBEDTLS_SSL_CID_IN_LEN_MAX)
+    if( strcmp( "MBEDTLS_SSL_CID_IN_LEN_MAX", config ) == 0 )
+    {
+        MACRO_EXPANSION_TO_STR( MBEDTLS_SSL_CID_IN_LEN_MAX );
+        return( 0 );
+    }
+#endif /* MBEDTLS_SSL_CID_IN_LEN_MAX */
+
+#if defined(MBEDTLS_SSL_CID_OUT_LEN_MAX)
+    if( strcmp( "MBEDTLS_SSL_CID_OUT_LEN_MAX", config ) == 0 )
+    {
+        MACRO_EXPANSION_TO_STR( MBEDTLS_SSL_CID_OUT_LEN_MAX );
+        return( 0 );
+    }
+#endif /* MBEDTLS_SSL_CID_OUT_LEN_MAX */
+
 #if defined(MBEDTLS_SSL_OUT_CONTENT_LEN)
     if( strcmp( "MBEDTLS_SSL_OUT_CONTENT_LEN", config ) == 0 )
     {
diff --git a/programs/ssl/ssl_client2.c b/programs/ssl/ssl_client2.c
index 1343970..db99ef6 100644
--- a/programs/ssl/ssl_client2.c
+++ b/programs/ssl/ssl_client2.c
@@ -103,6 +103,8 @@
 #define DFL_DHMLEN              -1
 #define DFL_RECONNECT           0
 #define DFL_RECO_DELAY          0
+#define DFL_CID_ENABLED         0
+#define DFL_CID_VALUE           ""
 #define DFL_RECONNECT_HARD      0
 #define DFL_TICKETS             MBEDTLS_SSL_SESSION_TICKETS_ENABLED
 #define DFL_ALPN_STRING         NULL
@@ -137,6 +139,16 @@
 #define USAGE_IO ""
 #endif /* MBEDTLS_X509_CRT_PARSE_C */
 
+#if defined(MBEDTLS_SSL_CID)
+#define USAGE_CID \
+    "    cid=%%d             Disable (0) or enable (1) the use of the DTLS Connection ID extension.\n" \
+    "                       default: 0 (disabled)\n"     \
+    "    cid_val=%%s          The CID to use for incoming messages (in hex, without 0x).\n"  \
+    "                        default: \"\"\n"
+#else /* MBEDTLS_SSL_CID */
+#define USAGE_CID ""
+#endif /* MBEDTLS_SSL_CID */
+
 #if defined(MBEDTLS_KEY_EXCHANGE__SOME__PSK_ENABLED)
 #define USAGE_PSK                                                   \
     "    psk=%%s              default: \"\" (in hex, without 0x)\n" \
@@ -278,6 +290,7 @@
     "    max_resend=%%d       default: 0 (no resend on timeout)\n" \
     "\n"                                                    \
     USAGE_DTLS                                              \
+    USAGE_CID                                               \
     "\n"                                                    \
     "    auth_mode=%%s        default: (library default: none)\n" \
     "                        options: none, optional, required\n" \
@@ -385,6 +398,8 @@
     int dgram_packing;          /* allow/forbid datagram packing            */
     int extended_ms;            /* negotiate extended master secret?        */
     int etm;                    /* negotiate encrypt then mac?              */
+    int cid_enabled;            /* whether to use the CID extension or not  */
+    const char *cid_val;        /* the CID to use for incoming messages     */
 } opt;
 
 int query_config( const char *config );
@@ -536,6 +551,45 @@
     return( 0 );
 }
 
+/* Unhexify `hex` into `dst`. `dst` must have
+ * size at least `strlen( hex ) / 2`. */
+int unhexify( char const *hex, unsigned char *dst )
+{
+    unsigned char c;
+    size_t j;
+    size_t len = strlen( hex );
+
+    if( len % 2 != 0 )
+        return( -1 );
+
+    for( j = 0; j < len; j += 2 )
+    {
+        c = hex[j];
+        if( c >= '0' && c <= '9' )
+            c -= '0';
+        else if( c >= 'a' && c <= 'f' )
+            c -= 'a' - 10;
+        else if( c >= 'A' && c <= 'F' )
+            c -= 'A' - 10;
+        else
+            return( -1 );
+        dst[ j / 2 ] = c << 4;
+
+        c = hex[j + 1];
+        if( c >= '0' && c <= '9' )
+            c -= '0';
+        else if( c >= 'a' && c <= 'f' )
+            c -= 'a' - 10;
+        else if( c >= 'A' && c <= 'F' )
+            c -= 'A' - 10;
+        else
+            return( -1 );
+        dst[ j / 2 ] |= c;
+    }
+
+    return( 0 );
+}
+
 int main( int argc, char *argv[] )
 {
     int ret = 0, len, tail_len, i, written, frags, retry_left;
@@ -547,6 +601,12 @@
     unsigned char psk[MBEDTLS_PSK_MAX_LEN];
     size_t psk_len = 0;
 #endif
+
+#if defined(MBEDTLS_SSL_CID)
+    unsigned char cid[MBEDTLS_SSL_CID_IN_LEN_MAX];
+    size_t cid_len = 0;
+#endif
+
 #if defined(MBEDTLS_SSL_ALPN)
     const char *alpn_list[ALPN_LIST_SIZE];
 #endif
@@ -620,6 +680,8 @@
     opt.server_addr         = DFL_SERVER_ADDR;
     opt.server_port         = DFL_SERVER_PORT;
     opt.debug_level         = DFL_DEBUG_LEVEL;
+    opt.cid_enabled         = DFL_CID_ENABLED;
+    opt.cid_val             = DFL_CID_VALUE;
     opt.nbio                = DFL_NBIO;
     opt.event               = DFL_EVENT;
     opt.read_timeout        = DFL_READ_TIMEOUT;
@@ -729,6 +791,18 @@
             opt.crt_file = q;
         else if( strcmp( p, "key_file" ) == 0 )
             opt.key_file = q;
+#if defined(MBEDTLS_SSL_CID)
+        else if( strcmp( p, "cid" ) == 0 )
+        {
+            opt.cid_enabled = atoi( q );
+            if( opt.cid_enabled != 0 && opt.cid_enabled != 1 )
+                goto usage;
+        }
+        else if( strcmp( p, "cid_val" ) == 0 )
+        {
+            opt.cid_val = q;
+        }
+#endif /* MBEDTLS_SSL_CID */
         else if( strcmp( p, "psk" ) == 0 )
             opt.psk = q;
         else if( strcmp( p, "psk_identity" ) == 0 )
@@ -1070,52 +1144,41 @@
         }
     }
 
+#if defined(MBEDTLS_SSL_CID)
+   if( strlen( opt.cid_val ) )
+   {
+       cid_len = strlen( opt.cid_val ) / 2;
+       if( cid_len > sizeof( cid ) )
+       {
+           mbedtls_printf( "CID too long\n" );
+           goto exit;
+       }
+
+       if( unhexify( opt.cid_val, cid ) != 0 )
+       {
+           mbedtls_printf( "CID not valid hex\n" );
+           goto exit;
+       }
+   }
+#endif /* MBEDTLS_SSL_CID */
+
 #if defined(MBEDTLS_KEY_EXCHANGE__SOME__PSK_ENABLED)
     /*
      * Unhexify the pre-shared key if any is given
      */
     if( strlen( opt.psk ) )
     {
-        unsigned char c;
-        size_t j;
-
-        if( strlen( opt.psk ) % 2 != 0 )
+        psk_len = strlen( opt.psk ) / 2;
+        if( psk_len > sizeof( psk ) )
         {
-            mbedtls_printf( "pre-shared key not valid hex\n" );
+            mbedtls_printf( "pre-shared key too long\n" );
             goto exit;
         }
 
-        psk_len = strlen( opt.psk ) / 2;
-
-        for( j = 0; j < strlen( opt.psk ); j += 2 )
+        if( unhexify( opt.psk, psk ) != 0 )
         {
-            c = opt.psk[j];
-            if( c >= '0' && c <= '9' )
-                c -= '0';
-            else if( c >= 'a' && c <= 'f' )
-                c -= 'a' - 10;
-            else if( c >= 'A' && c <= 'F' )
-                c -= 'A' - 10;
-            else
-            {
-                mbedtls_printf( "pre-shared key not valid hex\n" );
-                goto exit;
-            }
-            psk[ j / 2 ] = c << 4;
-
-            c = opt.psk[j + 1];
-            if( c >= '0' && c <= '9' )
-                c -= '0';
-            else if( c >= 'a' && c <= 'f' )
-                c -= 'a' - 10;
-            else if( c >= 'A' && c <= 'F' )
-                c -= 'A' - 10;
-            else
-            {
-                mbedtls_printf( "pre-shared key not valid hex\n" );
-                goto exit;
-            }
-            psk[ j / 2 ] |= c;
+            mbedtls_printf( "pre-shared key not valid hex\n" );
+            goto exit;
         }
     }
 #endif /* MBEDTLS_KEY_EXCHANGE__SOME__PSK_ENABLED */
@@ -1551,6 +1614,19 @@
                              mbedtls_net_send, mbedtls_net_recv,
                              opt.nbio == 0 ? mbedtls_net_recv_timeout : NULL );
 
+#if defined(MBEDTLS_SSL_CID)
+    if( opt.transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    {
+        if( ( ret = mbedtls_ssl_set_cid( &ssl, opt.cid_enabled,
+                                         cid, cid_len ) ) != 0 )
+        {
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_set_cid returned %d\n\n",
+                            ret );
+            goto exit;
+        }
+    }
+#endif /* MBEDTLS_SSL_CID */
+
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
     if( opt.dtls_mtu != DFL_DTLS_MTU )
         mbedtls_ssl_set_mtu( &ssl, opt.dtls_mtu );
@@ -1679,6 +1755,46 @@
     }
 #endif /* MBEDTLS_X509_CRT_PARSE_C */
 
+#if defined(MBEDTLS_SSL_CID)
+    if( opt.transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    {
+        unsigned char peer_cid[ MBEDTLS_SSL_CID_OUT_LEN_MAX ];
+        size_t peer_cid_len;
+        int cid_negotiated;
+
+        /* Check if the use of a CID has been negotiated */
+        ret = mbedtls_ssl_get_peer_cid( &ssl, &cid_negotiated,
+                                        peer_cid, &peer_cid_len );
+        if( ret != 0 )
+        {
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_get_peer_cid returned -0x%x\n\n",
+                            -ret );
+            goto exit;
+        }
+
+        if( cid_negotiated == MBEDTLS_SSL_CID_DISABLED )
+        {
+            if( opt.cid_enabled == MBEDTLS_SSL_CID_ENABLED )
+            {
+                mbedtls_printf( "Use of Connection ID was rejected by the server.\n" );
+            }
+        }
+        else
+        {
+            size_t idx=0;
+            mbedtls_printf( "Use of Connection ID has been negotiated.\n" );
+            mbedtls_printf( "Peer CID (length %u Bytes): ",
+                            (unsigned) peer_cid_len );
+            while( idx < peer_cid_len )
+            {
+                mbedtls_printf( "%#02x ", peer_cid[ idx ] );
+                idx++;
+            }
+            mbedtls_printf( "\n" );
+        }
+    }
+#endif /* MBEDTLS_SSL_CID */
+
 #if defined(MBEDTLS_SSL_RENEGOTIATION)
     if( opt.renegotiate )
     {
diff --git a/programs/ssl/ssl_server2.c b/programs/ssl/ssl_server2.c
index 2a499ad..dff899f 100644
--- a/programs/ssl/ssl_server2.c
+++ b/programs/ssl/ssl_server2.c
@@ -137,6 +137,8 @@
 #define DFL_MAX_VERSION         -1
 #define DFL_ARC4                -1
 #define DFL_SHA1                -1
+#define DFL_CID_ENABLED         0
+#define DFL_CID_VALUE           ""
 #define DFL_AUTH_MODE           -1
 #define DFL_CERT_REQ_CA_LIST    MBEDTLS_SSL_CERT_REQ_CA_LIST_ENABLED
 #define DFL_MFL_CODE            MBEDTLS_SSL_MAX_FRAG_LEN_NONE
@@ -222,6 +224,16 @@
 #define USAGE_SSL_ASYNC ""
 #endif /* MBEDTLS_SSL_ASYNC_PRIVATE */
 
+#if defined(MBEDTLS_SSL_CID)
+#define USAGE_CID \
+    "    cid=%%d             Disable (0) or enable (1) the use of the DTLS Connection ID extension.\n" \
+    "                       default: 0 (disabled)\n"     \
+    "    cid_val=%%s          The CID to use for incoming messages (in hex, without 0x).\n"  \
+    "                        default: \"\"\n"
+#else /* MBEDTLS_SSL_CID */
+#define USAGE_CID ""
+#endif /* MBEDTLS_SSL_CID */
+
 #if defined(MBEDTLS_KEY_EXCHANGE__SOME__PSK_ENABLED)
 #define USAGE_PSK                                                       \
     "    psk=%%s              default: \"\" (in hex, without 0x)\n"     \
@@ -510,6 +522,8 @@
     int dtls_mtu;               /* UDP Maximum tranport unit for DTLS       */
     int dgram_packing;          /* allow/forbid datagram packing            */
     int badmac_limit;           /* Limit of records with bad MAC            */
+    int cid_enabled;            /* whether to use the CID extension or not  */
+    const char *cid_val;        /* the CID to use for incoming messages     */
 } opt;
 
 int query_config( const char *config );
@@ -1260,6 +1274,11 @@
     unsigned char alloc_buf[MEMORY_HEAP_SIZE];
 #endif
 
+#if defined(MBEDTLS_SSL_CID)
+    unsigned char cid[MBEDTLS_SSL_CID_IN_LEN_MAX];
+    size_t cid_len = 0;
+#endif
+
     int i;
     char *p, *q;
     const int *list;
@@ -1337,6 +1356,8 @@
     opt.event               = DFL_EVENT;
     opt.response_size       = DFL_RESPONSE_SIZE;
     opt.nbio                = DFL_NBIO;
+    opt.cid_enabled         = DFL_CID_ENABLED;
+    opt.cid_val             = DFL_CID_VALUE;
     opt.read_timeout        = DFL_READ_TIMEOUT;
     opt.ca_file             = DFL_CA_FILE;
     opt.ca_path             = DFL_CA_PATH;
@@ -1475,6 +1496,18 @@
             opt.async_private_error = n;
         }
 #endif /* MBEDTLS_SSL_ASYNC_PRIVATE */
+#if defined(MBEDTLS_SSL_CID)
+        else if( strcmp( p, "cid" ) == 0 )
+        {
+            opt.cid_enabled = atoi( q );
+            if( opt.cid_enabled != 0 && opt.cid_enabled != 1 )
+                goto usage;
+        }
+        else if( strcmp( p, "cid_val" ) == 0 )
+        {
+            opt.cid_val = q;
+        }
+#endif /* MBEDTLS_SSL_CID */
         else if( strcmp( p, "psk" ) == 0 )
             opt.psk = q;
         else if( strcmp( p, "psk_identity" ) == 0 )
@@ -1882,6 +1915,24 @@
         }
     }
 
+#if defined(MBEDTLS_SSL_CID)
+   if( strlen( opt.cid_val ) )
+   {
+       cid_len = strlen( opt.cid_val ) / 2;
+       if( cid_len > sizeof( cid ) )
+       {
+           mbedtls_printf( "CID too long\n" );
+           goto exit;
+       }
+
+       if( unhexify( cid, opt.cid_val, &cid_len ) != 0 )
+       {
+           mbedtls_printf( "CID not valid hex\n" );
+           goto exit;
+       }
+   }
+#endif /* MBEDTLS_SSL_CID */
+
 #if defined(MBEDTLS_KEY_EXCHANGE__SOME__PSK_ENABLED)
     /*
      * Unhexify the pre-shared key and parse the list if any given
@@ -2561,6 +2612,19 @@
         mbedtls_ssl_set_bio( &ssl, &client_fd, mbedtls_net_send, mbedtls_net_recv,
                              opt.nbio == 0 ? mbedtls_net_recv_timeout : NULL );
 
+#if defined(MBEDTLS_SSL_CID)
+    if( opt.transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    {
+        if( ( ret = mbedtls_ssl_set_cid( &ssl, opt.cid_enabled,
+                                         cid, cid_len ) ) != 0 )
+        {
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_set_cid returned %d\n\n",
+                            ret );
+            goto exit;
+        }
+    }
+#endif /* MBEDTLS_SSL_CID */
+
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
     if( opt.dtls_mtu != DFL_DTLS_MTU )
         mbedtls_ssl_set_mtu( &ssl, opt.dtls_mtu );
@@ -2786,6 +2850,46 @@
     }
 #endif /* MBEDTLS_X509_CRT_PARSE_C */
 
+#if defined(MBEDTLS_SSL_CID)
+    if( opt.transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    {
+        unsigned char peer_cid[ MBEDTLS_SSL_CID_OUT_LEN_MAX ];
+        size_t peer_cid_len;
+        int cid_negotiated;
+
+        /* Check if the use of a CID has been negotiated */
+        ret = mbedtls_ssl_get_peer_cid( &ssl, &cid_negotiated,
+                                        peer_cid, &peer_cid_len );
+        if( ret != 0 )
+        {
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_get_peer_cid returned -0x%x\n\n",
+                            -ret );
+            goto exit;
+        }
+
+        if( cid_negotiated == MBEDTLS_SSL_CID_DISABLED )
+        {
+            if( opt.cid_enabled == MBEDTLS_SSL_CID_ENABLED )
+            {
+                mbedtls_printf( "Use of Connection ID was not offered by the client.\n" );
+            }
+        }
+        else
+        {
+            size_t idx=0;
+            mbedtls_printf( "Use of Connection ID has been negotiated.\n" );
+            mbedtls_printf( "Peer CID (length %u Bytes): ",
+                            (unsigned) peer_cid_len );
+            while( idx < peer_cid_len )
+            {
+                mbedtls_printf( "%#02x ", peer_cid[ idx ] );
+                idx++;
+            }
+            mbedtls_printf( "\n" );
+        }
+    }
+#endif /* MBEDTLS_SSL_CID */
+
     if( opt.exchanges == 0 )
         goto close_notify;
 
diff --git a/tests/ssl-opt.sh b/tests/ssl-opt.sh
index 9a96294..bd4f21d 100755
--- a/tests/ssl-opt.sh
+++ b/tests/ssl-opt.sh
@@ -1112,6 +1112,103 @@
             -S "dumping 'expected mac' (20 bytes)" \
             -s "dumping 'expected mac' (10 bytes)"
 
+# Tests for DTLS Connection ID extension
+
+# So far, the CID API isn't implemented, so we can't
+# grep for output witnessing its use. This needs to be
+# changed once the CID extension is implemented.
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client enabled, server disabled" \
+            "$P_SRV dtls=1 cid=0" \
+            "$P_CLI dtls=1 cid=1 cid_val=deadbeef" \
+             0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client disabled, server enabled" \
+            "$P_SRV dtls=1 cid=1 cid_val=deadbeef" \
+            "$P_CLI dtls=1 cid=0" \
+             0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client+Server CID nonempty" \
+            "$P_SRV dtls=1 cid=1 cid_val=dead" \
+            "$P_CLI dtls=1 cid=1 cid_val=beef" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client CID empty" \
+            "$P_SRV dtls=1 cid=1 cid_val=deadbeef" \
+            "$P_CLI dtls=1 cid=1" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Server CID empty" \
+            "$P_SRV dtls=1 cid=1" \
+            "$P_CLI dtls=1 cid=1 cid_val=deadbeef" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client+Server CID empty" \
+            "$P_SRV dtls=1 cid=1" \
+            "$P_CLI dtls=1 cid=1" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client+Server CID nonempty, AES-128-CCM-8" \
+            "$P_SRV dtls=1 cid=1 cid_val=dead" \
+            "$P_CLI dtls=1 cid=1 cid_val=beef force_ciphersuite=TLS-ECDHE-ECDSA-WITH-AES-128-CCM-8" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client CID empty, AES-128-CCM-8" \
+            "$P_SRV dtls=1 cid=1 cid_val=deadbeef" \
+            "$P_CLI dtls=1 cid=1 force_ciphersuite=TLS-ECDHE-ECDSA-WITH-AES-128-CCM-8" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Server CID empty, AES-128-CCM-8" \
+            "$P_SRV dtls=1 cid=1" \
+            "$P_CLI dtls=1 cid=1 cid_val=deadbeef force_ciphersuite=TLS-ECDHE-ECDSA-WITH-AES-128-CCM-8" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client+Server CID empty, AES-128-CCM-8" \
+            "$P_SRV dtls=1 cid=1" \
+            "$P_CLI dtls=1 cid=1 force_ciphersuite=TLS-ECDHE-ECDSA-WITH-AES-128-CCM-8" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client+Server CID nonempty, AES-128-CBC" \
+            "$P_SRV dtls=1 cid=1 cid_val=dead" \
+            "$P_CLI dtls=1 cid=1 cid_val=beef force_ciphersuite=TLS-ECDHE-ECDSA-WITH-AES-128-CBC-SHA256" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client CID empty, AES-128-CBC" \
+            "$P_SRV dtls=1 cid=1 cid_val=deadbeef" \
+            "$P_CLI dtls=1 cid=1 force_ciphersuite=TLS-ECDHE-ECDSA-WITH-AES-128-CBC-SHA256" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Server CID empty, AES-128-CBC" \
+            "$P_SRV dtls=1 cid=1" \
+            "$P_CLI dtls=1 cid=1 cid_val=deadbeef force_ciphersuite=TLS-ECDHE-ECDSA-WITH-AES-128-CBC-SHA256" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+run_test    "(STUB) Connection ID: Client+Server enabled, Client+Server CID empty, AES-128-CBC" \
+            "$P_SRV dtls=1 cid=1" \
+            "$P_CLI dtls=1 cid=1 force_ciphersuite=TLS-ECDHE-ECDSA-WITH-AES-128-CBC-SHA256" \
+            0
+
+requires_config_enabled MBEDTLS_SSL_CID
+requires_config_enabled MBEDTLS_SSL_RENEGOTIATION
+run_test    "(STUB) Connection ID: Client+Server enabled, renegotiate" \
+            "$P_SRV dtls=1 cid=1 cid_val=dead renegotiation=1" \
+            "$P_CLI dtls=1 cid=1 cid_val=beef renegotiation=1 renegotiate=1" \
+            0
+
 # Tests for Encrypt-then-MAC extension
 
 run_test    "Encrypt then MAC: default" \