diff --git a/library/ssl_srv.c b/library/ssl_srv.c
index fd04e92..70fe13d 100644
--- a/library/ssl_srv.c
+++ b/library/ssl_srv.c
@@ -2829,12 +2829,11 @@
 static int ssl_write_server_key_exchange( mbedtls_ssl_context *ssl )
 {
     int ret;
-    size_t n = 0;
     const mbedtls_ssl_ciphersuite_t *ciphersuite_info =
                             ssl->transform_negotiate->ciphersuite_info;
 
-#if defined(MBEDTLS_KEY_EXCHANGE__SOME_PFS__ENABLED)
     unsigned char *p = ssl->out_msg + 4;
+#if defined(MBEDTLS_KEY_EXCHANGE__SOME_PFS__ENABLED)
     size_t len;
 #if defined(MBEDTLS_KEY_EXCHANGE__WITH_SERVER_SIGNATURE__ENABLED)
     unsigned char *dig_signed = p;
@@ -2894,7 +2893,6 @@
         }
 
         p += len;
-        n += len;
     }
 #endif /* MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED */
 
@@ -2910,8 +2908,6 @@
     {
         *(p++) = 0x00;
         *(p++) = 0x00;
-
-        n += 2;
     }
 #endif /* MBEDTLS_KEY_EXCHANGE_DHE_PSK_ENABLED ||
           MBEDTLS_KEY_EXCHANGE_ECDHE_PSK_ENABLED */
@@ -2958,7 +2954,6 @@
 #endif
 
         p += len;
-        n += len;
 
         MBEDTLS_SSL_DEBUG_MPI( 3, "DHM: X ", &ssl->handshake->dhm_ctx.X  );
         MBEDTLS_SSL_DEBUG_MPI( 3, "DHM: P ", &ssl->handshake->dhm_ctx.P  );
@@ -3006,9 +3001,10 @@
             return( ret );
         }
 
-        if( ( ret = mbedtls_ecdh_make_params( &ssl->handshake->ecdh_ctx, &len,
-                                      p, MBEDTLS_SSL_MAX_CONTENT_LEN - n,
-                                      ssl->conf->f_rng, ssl->conf->p_rng ) ) != 0 )
+        if( ( ret = mbedtls_ecdh_make_params(
+                  &ssl->handshake->ecdh_ctx, &len,
+                  p, ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN - p,
+                  ssl->conf->f_rng, ssl->conf->p_rng ) ) != 0 )
         {
             MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ecdh_make_params", ret );
             return( ret );
@@ -3020,7 +3016,6 @@
 #endif
 
         p += len;
-        n += len;
 
         MBEDTLS_SSL_DEBUG_ECP( 3, "ECDH: Q ", &ssl->handshake->ecdh_ctx.Q );
     }
@@ -3203,8 +3198,6 @@
 
             *(p++) = mbedtls_ssl_hash_from_md_alg( md_alg );
             *(p++) = mbedtls_ssl_sig_from_pk_alg( sig_alg );
-
-            n += 2;
         }
 #endif /* MBEDTLS_SSL_PROTO_TLS1_2 */
 
@@ -3217,17 +3210,16 @@
 
         *(p++) = (unsigned char)( signature_len >> 8 );
         *(p++) = (unsigned char)( signature_len      );
-        n += 2;
 
         MBEDTLS_SSL_DEBUG_BUF( 3, "my signature", p, signature_len );
 
-        n += signature_len;
+        p += signature_len;
     }
 #endif /* MBEDTLS_KEY_EXCHANGE__WITH_SERVER_SIGNATURE__ENABLED */
 
     /* Done with actual work; add header and send. */
 
-    ssl->out_msglen  = 4 + n;
+    ssl->out_msglen  = p - ssl->out_msg;
     ssl->out_msgtype = MBEDTLS_SSL_MSG_HANDSHAKE;
     ssl->out_msg[0]  = MBEDTLS_SSL_HS_SERVER_KEY_EXCHANGE;