diff --git a/library/ssl_cli.c b/library/ssl_cli.c
index 178393a97..699bcb6fd 100644
--- a/library/ssl_cli.c
+++ b/library/ssl_cli.c
@@ -1908,6 +1908,7 @@ static int ssl_write_client_key_exchange( ssl_context *ssl )
     if( ciphersuite_info->key_exchange == POLARSSL_KEY_EXCHANGE_ECDHE_PSK )
     {
         unsigned char *p = ssl->handshake->premaster;
+        size_t zlen;
 
         /*
          * ECDHE_PSK key exchange: RFC 5489, section 2
@@ -1922,14 +1923,14 @@ static int ssl_write_client_key_exchange( ssl_context *ssl )
             return( POLARSSL_ERR_SSL_BAD_INPUT_DATA );
 
         i = 4;
-        n = ssl->psk_identity_len;
-        ssl->out_msg[4] = (unsigned char)( n >> 8 );
-        ssl->out_msg[5] = (unsigned char)( n      );
+        ssl->out_msg[i++] = (unsigned char)( ssl->psk_identity_len >> 8 );
+        ssl->out_msg[i++] = (unsigned char)( ssl->psk_identity_len      );
 
-        memcpy( ssl->out_msg + 6, ssl->psk_identity, ssl->psk_identity_len );
+        memcpy( ssl->out_msg + i, ssl->psk_identity, ssl->psk_identity_len );
+        i += ssl->psk_identity_len;
 
         ret = ecdh_make_public( &ssl->handshake->ecdh_ctx, &n,
-                                &ssl->out_msg[8 + ssl->psk_identity_len], 512,
+                                &ssl->out_msg[i], 1000,
                                 ssl->f_rng, ssl->p_rng );
         if( ret != 0 )
         {
@@ -1937,9 +1938,6 @@ static int ssl_write_client_key_exchange( ssl_context *ssl )
             return( ret );
         }
 
-        ssl->out_msg[6 + ssl->psk_identity_len] = (unsigned char)( n >> 8 );
-        ssl->out_msg[7 + ssl->psk_identity_len] = (unsigned char)( n      );
-
         SSL_DEBUG_ECP( 3, "ECDH: Q", &ssl->handshake->ecdh_ctx.Q );
 
         /*
@@ -1949,7 +1947,7 @@ static int ssl_write_client_key_exchange( ssl_context *ssl )
          * };
          * with "other_secret" containing Z from ECDH
          */
-        if( ( ret = ecdh_calc_secret( &ssl->handshake->ecdh_ctx, &n,
+        if( ( ret = ecdh_calc_secret( &ssl->handshake->ecdh_ctx, &zlen,
                                        p + 2, POLARSSL_MPI_MAX_SIZE,
                                        ssl->f_rng, ssl->p_rng ) ) != 0 )
         {
@@ -1957,9 +1955,9 @@ static int ssl_write_client_key_exchange( ssl_context *ssl )
             return( ret );
         }
 
-        *(p++) = (unsigned char)( n >> 8 );
-        *(p++) = (unsigned char)( n      );
-        p += n;
+        *(p++) = (unsigned char)( zlen >> 8 );
+        *(p++) = (unsigned char)( zlen      );
+        p += zlen;
 
         SSL_DEBUG_MPI( 3, "ECDH: z", &ssl->handshake->ecdh_ctx.z );
 
@@ -1968,8 +1966,7 @@ static int ssl_write_client_key_exchange( ssl_context *ssl )
         memcpy( p, ssl->psk, ssl->psk_len );
         p += ssl->psk_len;
 
-        ssl->handshake->pmslen = 4 + n + ssl->psk_len;
-        n = ssl->handshake->pmslen;
+        ssl->handshake->pmslen = p - ssl->handshake->premaster;
     }
     else
 #endif /* POLARSSL_KEY_EXCHANGE_ECDHE_PSK_ENABLED */
diff --git a/library/ssl_srv.c b/library/ssl_srv.c
index b128d9dad..158877090 100644
--- a/library/ssl_srv.c
+++ b/library/ssl_srv.c
@@ -2287,41 +2287,6 @@ static int ssl_parse_client_dh_public( ssl_context *ssl, unsigned char **p,
 #endif /* POLARSSL_KEY_EXCHANGE_DHE_RSA_ENABLED ||
           POLARSSL_KEY_EXCHANGE_DHE_PSK_ENABLED */
 
-#if defined(POLARSSL_KEY_EXCHANGE_ECDHE_RSA_ENABLED) ||                     \
-    defined(POLARSSL_KEY_EXCHANGE_ECDHE_ECDSA_ENABLED) ||                   \
-    defined(POLARSSL_KEY_EXCHANGE_ECDHE_PSK_ENABLED)
-static int ssl_parse_client_ecdh_public( ssl_context *ssl )
-{
-    int ret;
-    size_t n;
-
-    /*
-     * Receive client public key and calculate premaster
-     */
-    n = ssl->in_msg[3];
-
-    if( n < 1 || n > mpi_size( &ssl->handshake->ecdh_ctx.grp.P ) * 2 + 2 ||
-        n + 4 != ssl->in_hslen )
-    {
-        SSL_DEBUG_MSG( 1, ( "bad client key exchange message" ) );
-        return( POLARSSL_ERR_SSL_BAD_HS_CLIENT_KEY_EXCHANGE );
-    }
-
-    if( ( ret = ecdh_read_public( &ssl->handshake->ecdh_ctx,
-                                   ssl->in_msg + 4, n ) ) != 0 )
-    {
-        SSL_DEBUG_RET( 1, "ecdh_read_public", ret );
-        return( POLARSSL_ERR_SSL_BAD_HS_CLIENT_KEY_EXCHANGE_RP );
-    }
-
-    SSL_DEBUG_ECP( 3, "ECDH: Qp ", &ssl->handshake->ecdh_ctx.Qp );
-
-    return( ret );
-}
-#endif /* POLARSSL_KEY_EXCHANGE_ECDHE_RSA_ENABLED ||
-          POLARSSL_KEY_EXCHANGE_ECDHE_ECDSA_ENABLED ||
-          POLARSSL_KEY_EXCHANGE_ECDHE_PSK_ENABLED */
-
 #if defined(POLARSSL_KEY_EXCHANGE_RSA_ENABLED)
 static int ssl_parse_encrypted_pms_secret( ssl_context *ssl )
 {
@@ -2522,12 +2487,24 @@ static int ssl_parse_client_key_exchange( ssl_context *ssl )
     if( ciphersuite_info->key_exchange == POLARSSL_KEY_EXCHANGE_ECDHE_RSA ||
         ciphersuite_info->key_exchange == POLARSSL_KEY_EXCHANGE_ECDHE_ECDSA )
     {
-        if( ( ret = ssl_parse_client_ecdh_public( ssl ) ) != 0 )
+        size_t n = ssl->in_msg[3];
+
+        if( n < 1 || n > mpi_size( &ssl->handshake->ecdh_ctx.grp.P ) * 2 + 2 ||
+            n + 4 != ssl->in_hslen )
         {
-            SSL_DEBUG_RET( 1, ( "ssl_parse_client_ecdh_public" ), ret );
-            return( ret );
+            SSL_DEBUG_MSG( 1, ( "bad client key exchange message" ) );
+            return( POLARSSL_ERR_SSL_BAD_HS_CLIENT_KEY_EXCHANGE );
         }
 
+        if( ( ret = ecdh_read_public( &ssl->handshake->ecdh_ctx,
+                                       ssl->in_msg + 4, n ) ) != 0 )
+        {
+            SSL_DEBUG_RET( 1, "ecdh_read_public", ret );
+            return( POLARSSL_ERR_SSL_BAD_HS_CLIENT_KEY_EXCHANGE_RP );
+        }
+
+        SSL_DEBUG_ECP( 3, "ECDH: Qp ", &ssl->handshake->ecdh_ctx.Qp );
+
         if( ( ret = ecdh_calc_secret( &ssl->handshake->ecdh_ctx,
                                       &ssl->handshake->pmslen,
                                        ssl->handshake->premaster,
@@ -2629,12 +2606,16 @@ static int ssl_parse_client_key_exchange( ssl_context *ssl )
             SSL_DEBUG_RET( 1, ( "ssl_parse_client_psk_identity" ), ret );
             return( ret );
         }
-        if( ( ret = ssl_parse_client_ecdh_public( ssl ) ) != 0 )
+
+        if( ( ret = ecdh_read_public( &ssl->handshake->ecdh_ctx,
+                                       p, end - p ) ) != 0 )
         {
-            SSL_DEBUG_RET( 1, ( "ssl_parse_client_ecdh_public" ), ret );
-            return( ret );
+            SSL_DEBUG_RET( 1, "ecdh_read_public", ret );
+            return( POLARSSL_ERR_SSL_BAD_HS_CLIENT_KEY_EXCHANGE_RP );
         }
 
+        SSL_DEBUG_ECP( 3, "ECDH: Qp ", &ssl->handshake->ecdh_ctx.Qp );
+
         // Set up the premaster secret
         //
         p = ssl->handshake->premaster;
@@ -2668,7 +2649,7 @@ static int ssl_parse_client_key_exchange( ssl_context *ssl )
     {
         if( ( ret = ssl_parse_encrypted_pms_secret( ssl ) ) != 0 )
         {
-            SSL_DEBUG_RET( 1, ( "ssl_parse_client_ecdh_public" ), ret );
+            SSL_DEBUG_RET( 1, ( "ssl_parse_parse_ecrypted_pms_secret" ), ret );
             return( ret );
         }
     }