diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml
index d381508d..54ac6dfe 100644
--- a/.github/workflows/build_and_test.yml
+++ b/.github/workflows/build_and_test.yml
@@ -436,7 +436,7 @@ jobs:
             ctest --output-on-failure -E engine
           elif [ $DOCKER_IMAGE = "centos:7" ]; then
             # we skip the ecdh_derive tests (for now) since there is an issue with generating secp224r1 keys
-            ctest --output-on-failure -E ecdh_derive\|aes
+            ctest --output-on-failure -E ecdh_derive\|aes\|ecdh_sp800
           else
             ctest --output-on-failure
           fi
diff --git a/pkcs11/pkcs11y.h b/pkcs11/pkcs11y.h
index 193c6697..7cf1aa0c 100644
--- a/pkcs11/pkcs11y.h
+++ b/pkcs11/pkcs11y.h
@@ -33,4 +33,10 @@
 #define CKM_YUBICO_AES_CCM_WRAP                                                \
   (CKM_VENDOR_DEFINED | YUBICO_BASE_VENDOR | YH_WRAP_KEY)
 
+// TODO: These values are from PKCS11 3.0 and should be removed when we upgrade
+#define CKD_YUBICO_SHA1_KDF_SP800 0x0000000EUL
+#define CKD_YUBICO_SHA256_KDF_SP800 0x00000010UL
+#define CKD_YUBICO_SHA384_KDF_SP800 0x00000011UL
+#define CKD_YUBICO_SHA512_KDF_SP800 0x00000012UL
+
 #endif
diff --git a/pkcs11/tests/CMakeLists.txt b/pkcs11/tests/CMakeLists.txt
index 7ba6b1ff..689adcad 100644
--- a/pkcs11/tests/CMakeLists.txt
+++ b/pkcs11/tests/CMakeLists.txt
@@ -203,6 +203,12 @@ set (
   common.c
   )
 
+set (
+        SOURCE_ECDH_SP800
+        ecdh_sp800_test.c
+        common.c
+)
+
 set (
   SOURCE_AES_ENCRYPT
   aes_encrypt_test.c
@@ -224,6 +230,7 @@ set (
 if(NOT ${CMAKE_SYSTEM_NAME} MATCHES "Windows")
 add_executable (aes_encrypt_test ${SOURCE_AES_ENCRYPT})
 add_executable (ecdh_derive_test ${SOURCE_ECDH_DERIVE})
+add_executable (ecdh_sp800_test ${SOURCE_ECDH_SP800})
 
 target_link_libraries(
   aes_encrypt_test
@@ -240,11 +247,21 @@ target_link_libraries (
   ${LIBCRYPTO_LDFLAGS}
   "-ldl")
 
+target_link_libraries (
+        ecdh_sp800_test
+        ${LIBCRYPTO_LDFLAGS}
+        "-ldl")
+
 add_test (
   NAME ecdh_derive_test
   COMMAND ${CMAKE_CURRENT_BINARY_DIR}/ecdh_derive_test ${CMAKE_CURRENT_BINARY_DIR}/../yubihsm_pkcs11.${LIBEXT}
   )
 
+add_test (
+        NAME ecdh_sp800_test
+        COMMAND ${CMAKE_CURRENT_BINARY_DIR}/ecdh_sp800_test ${CMAKE_CURRENT_BINARY_DIR}/../yubihsm_pkcs11.${LIBEXT}
+)
+
 if (NOT ${LIBCRYPTO_VERSION} VERSION_LESS 1.1)
   add_executable (rsa_enc_test ${SOURCE_RSA_ENC_TEST})
 
diff --git a/pkcs11/tests/ecdh_sp800_test.c b/pkcs11/tests/ecdh_sp800_test.c
new file mode 100644
index 00000000..63507f63
--- /dev/null
+++ b/pkcs11/tests/ecdh_sp800_test.c
@@ -0,0 +1,554 @@
+/*
+ * Copyright 2015-2018 Yubico AB
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifdef NDEBUG
+#undef NDEBUG
+#endif
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+
+#include <openssl/evp.h>
+#include <openssl/ec.h>
+#include <openssl/x509.h>
+
+#include "../pkcs11.h"
+#include "../pkcs11y.h"
+#include "common.h"
+
+#define BUFSIZE 1024
+
+CK_BYTE P224_PARAMS[] = {0x06, 0x05, 0x2b, 0x81, 0x04, 0x00, 0x21};
+CK_BYTE P256_PARAMS[] = {0x06, 0x08, 0x2a, 0x86, 0x48,
+                         0xce, 0x3d, 0x03, 0x01, 0x07};
+CK_BYTE P384_PARAMS[] = {0x06, 0x05, 0x2b, 0x81, 0x04, 0x00, 0x22};
+CK_BYTE P521_PARAMS[] = {0x06, 0x05, 0x2b, 0x81, 0x04, 0x00, 0x23};
+
+static CK_FUNCTION_LIST_PTR p11;
+static CK_SESSION_HANDLE session;
+
+char *CURVES[] = {"secp224r1", "prime256v1", "secp384r1", "secp521r1"};
+CK_BYTE *CURVE_PARAMS[] = {P224_PARAMS, P256_PARAMS, P384_PARAMS, P521_PARAMS};
+CK_ULONG CURVE_LENS[] = {sizeof(P224_PARAMS), sizeof(P256_PARAMS),
+                         sizeof(P384_PARAMS), sizeof(P521_PARAMS)};
+int CURVE_COUNT = sizeof(CURVE_PARAMS) / sizeof(CURVE_PARAMS[0]);
+size_t CURVE_ECDH_LEN[] = {28, 32, 48, 66};
+
+static void success(const char *message) { printf("%s. OK\n", message); }
+
+static void fail(const char *message) { printf("%s. FAIL!\n", message); }
+
+static void increment_ctr(uint8_t *ctr, size_t len) {
+  while (len > 0) {
+    if (++ctr[--len]) {
+      break;
+    }
+  }
+}
+
+static void generate_keypair_yh(CK_BYTE *curve, CK_ULONG curve_len,
+                                CK_OBJECT_HANDLE_PTR publicKeyPtr,
+                                CK_OBJECT_HANDLE_PTR privateKeyPtr) {
+  CK_MECHANISM mechanism = {CKM_EC_KEY_PAIR_GEN, NULL_PTR, 0};
+
+  CK_BBOOL ck_true = CK_TRUE;
+
+  CK_OBJECT_CLASS pubkey_class = CKO_PUBLIC_KEY;
+  CK_OBJECT_CLASS privkey_class = CKO_PRIVATE_KEY;
+  CK_KEY_TYPE key_type = CKK_EC;
+  char *label = "ecdhtest";
+
+  CK_ATTRIBUTE publicKeyTemplate[] = {{CKA_CLASS, &pubkey_class,
+                                       sizeof(pubkey_class)},
+                                      {CKA_VERIFY, &ck_true, sizeof(ck_true)},
+                                      {CKA_KEY_TYPE, &key_type,
+                                       sizeof(key_type)},
+                                      {CKA_LABEL, label, strlen(label)},
+                                      {CKA_EC_PARAMS, curve, curve_len}};
+
+  CK_ATTRIBUTE privateKeyTemplate[] = {{CKA_CLASS, &privkey_class,
+                                        sizeof(privkey_class)},
+                                       {CKA_LABEL, label, strlen(label)},
+                                       {CKA_DERIVE, &ck_true, sizeof(ck_true)}};
+
+  if ((p11->C_GenerateKeyPair(session, &mechanism, publicKeyTemplate, 5,
+                              privateKeyTemplate, 3, publicKeyPtr,
+                              privateKeyPtr)) != CKR_OK) {
+    fail("Failed to generate EC key pair on YubiHSM");
+    exit(EXIT_FAILURE);
+  }
+  success("Generated EC key pair on YubiHSM");
+}
+
+static EVP_PKEY *generate_keypair_openssl(const char *curve) {
+  EVP_PKEY *pkey = NULL;
+  EC_KEY *eckey = NULL;
+  int eccgrp = OBJ_txt2nid(curve);
+  eckey = EC_KEY_new_by_curve_name(eccgrp);
+  if (!(EC_KEY_generate_key(eckey))) {
+    fail("Failed to generate EC keypair with openssl");
+  }
+  pkey = EVP_PKEY_new();
+  if (!EVP_PKEY_assign_EC_KEY(pkey, eckey)) {
+    fail("Failed to assign ECC key to EVP_PKEY structure");
+  }
+  return pkey;
+}
+
+static CK_ULONG get_yhsize(CK_OBJECT_HANDLE object) {
+  CK_ULONG len;
+  if ((p11->C_GetObjectSize(session, object, &len)) != CKR_OK) {
+    printf("Failed to get size of object 0x%lx from yubihsm-pkcs11. FAIL\n",
+           object);
+    return 0;
+  }
+  return len;
+}
+
+static CK_ULONG get_yhvalue(CK_OBJECT_HANDLE object, unsigned char *value,
+                            CK_ULONG object_size) {
+  if (object_size > 0) {
+    CK_ATTRIBUTE template[] = {{CKA_VALUE, value, object_size}};
+    if ((p11->C_GetAttributeValue(session, object, template,
+                                  sizeof(template) / sizeof(template[0]))) ==
+        CKR_OK) {
+      return object_size;
+    } else {
+      printf("Failed to retrieve object value from yubihsm-pkcs11. 0x%lx\n",
+             object);
+    }
+  }
+  return 0;
+}
+
+static CK_RV yh_derive(unsigned char *peerkey_bytes, int peerkey_len,
+                       CK_OBJECT_HANDLE privkey, CK_ULONG kdf, char *label,
+                       CK_OBJECT_HANDLE_PTR ecdh_key, CK_ULONG value_len,
+                       CK_ULONG ecdh_len) {
+  CK_ECDH1_DERIVE_PARAMS params;
+  params.kdf = kdf;
+  params.pSharedData = NULL;
+  params.ulSharedDataLen = 0;
+  params.pPublicData = peerkey_bytes;
+  params.ulPublicDataLen = peerkey_len;
+
+  CK_MECHANISM mechanism = {CKM_ECDH1_DERIVE, &params, sizeof(params)};
+
+  CK_OBJECT_CLASS key_class = CKO_SECRET_KEY;
+  CK_KEY_TYPE key_type = CKK_GENERIC_SECRET;
+  CK_ATTRIBUTE derivedKeyTemplate[] =
+    {{CKA_CLASS, &key_class, sizeof(key_class)},
+     {CKA_KEY_TYPE, &key_type, sizeof(key_type)},
+     {CKA_VALUE_LEN, &value_len, sizeof(value_len)},
+     {CKA_LABEL, label, strlen(label)}};
+
+  CK_RV rv = CKR_OK;
+
+  rv =
+    p11->C_DeriveKey(session, &mechanism, privkey, derivedKeyTemplate,
+                     sizeof(derivedKeyTemplate) / sizeof(derivedKeyTemplate[0]),
+                     ecdh_key);
+  if (rv != CKR_OK) {
+    return rv;
+  }
+
+  CK_ULONG actual_len = get_yhsize(*ecdh_key);
+  if ((ecdh_len != actual_len)) {
+    printf("Derived ECDH is not the expected length. Expected %lu. Found %lu\n",
+           ecdh_len, actual_len);
+    rv = CKR_FUNCTION_FAILED;
+  }
+
+  return rv;
+}
+
+static unsigned int do_hash(const EVP_MD *md, uint8_t *hashed,
+                            unsigned char *raw_derived,
+                            size_t raw_derived_len) {
+
+  EVP_MD_CTX *mdctx = NULL;
+  unsigned int len = 0;
+
+  mdctx = EVP_MD_CTX_create();
+  if (mdctx == NULL) {
+    fail("Failed to create Hash context");
+    return 0;
+  }
+
+  if (EVP_DigestInit_ex(mdctx, md, NULL) == 0) {
+    fail("Failed to initialize digest");
+    goto h_free;
+  }
+
+  if (EVP_DigestUpdate(mdctx, raw_derived, raw_derived_len) != 1) {
+    fail("Failed to update digest");
+    goto h_free;
+  }
+  if (EVP_DigestFinal_ex(mdctx, hashed, &len) != 1) {
+    fail("Failed to finalize digest");
+    len = 0;
+    goto h_free;
+  }
+
+h_free:
+  if (mdctx != NULL) {
+    EVP_MD_CTX_destroy(mdctx);
+  }
+  return len;
+}
+
+static size_t openssl_derive(CK_ULONG kdf, EVP_PKEY *private_key,
+                             EVP_PKEY *peer_key, unsigned char **ecdh_key,
+                             CK_ULONG expected_ecdh_len) {
+
+  /* Create the context for the shared secret derivation */
+  EVP_PKEY_CTX *ctx = EVP_PKEY_CTX_new(private_key, NULL);
+  if (!ctx) {
+    fail("Failed to create new openssl context");
+    return 0;
+  }
+
+  EVP_MD_CTX *mdctx = NULL;
+  size_t len = 0;
+  /* Initialize derivation function*/
+  if (EVP_PKEY_derive_init(ctx) != 1) {
+    fail("Failed to initialize openssl contex");
+    goto c_free;
+  }
+
+  /* Set the peer public key */
+  if (EVP_PKEY_derive_set_peer(ctx, peer_key) != 1) {
+    fail("Failed to set the peer public key in the openssl context");
+    goto c_free;
+  }
+
+  /* Determine buffer length for shared secret */
+  if (EVP_PKEY_derive(ctx, NULL, &len) != 1) {
+    fail("Failed to determine derived key expected size with openssl");
+    goto c_free;
+  }
+
+  /* Create the buffer */
+  unsigned char *derived = OPENSSL_malloc(len);
+  if (derived == NULL) {
+    fail("Failed to allocate the buffer to hold the ECDH key derived with "
+         "openssl");
+    len = 0;
+    goto c_free;
+  }
+
+  /* Derive the shared secret */
+  if ((EVP_PKEY_derive(ctx, derived, &len)) != 1) {
+    fail("Failed to derive ECDH key with openssl");
+    len = 0;
+    goto c_free;
+  }
+
+  *ecdh_key = malloc(BUFSIZE);
+  if (*ecdh_key == NULL) {
+    fail("Failed to allocate the buffer to hold the ECDH key derived with "
+         "openssl");
+    len = 0;
+    goto c_free;
+  }
+
+  size_t output_bits = 0;
+  const EVP_MD *md = NULL;
+  switch (kdf) {
+    case CKD_NULL:
+      memcpy(*ecdh_key, derived, len);
+      goto c_truncate;
+    case CKD_YUBICO_SHA1_KDF_SP800:
+      md = EVP_sha1();
+      output_bits = 160;
+      break;
+    case CKD_YUBICO_SHA256_KDF_SP800:
+      md = EVP_sha256();
+      output_bits = 256;
+      break;
+    case CKD_YUBICO_SHA384_KDF_SP800:
+      md = EVP_sha384();
+      output_bits = 384;
+      break;
+    case CKD_YUBICO_SHA512_KDF_SP800:
+      md = EVP_sha512();
+      output_bits = 384;
+      break;
+  }
+
+  size_t l = expected_ecdh_len * 8;
+  size_t reps = 1 + l / output_bits;
+
+  uint8_t res[BUFSIZE] = {0};
+  size_t res_len = 0;
+  size_t ctr_len = 4;
+
+  uint8_t k[BUFSIZE] = {0};
+  size_t k_len = len + ctr_len;
+  memset(k, 0, ctr_len);
+  memcpy(k + ctr_len, derived, len);
+
+  size_t hashed_len = 0;
+  for (size_t i = 0; i < reps; i++) {
+    increment_ctr(k, ctr_len);
+
+    hashed_len = do_hash(md, res + res_len, k, k_len);
+    if (hashed_len == 0) {
+      fail("Failed to apply hash function");
+      len = 0;
+      goto c_free;
+    }
+    res_len += hashed_len;
+  }
+
+  if (expected_ecdh_len > res_len) {
+    fail("Derived key is too short");
+    len = 0;
+    goto c_free;
+  }
+
+  memcpy(*ecdh_key, res, expected_ecdh_len);
+  memset((*ecdh_key) + expected_ecdh_len, 0, BUFSIZE - expected_ecdh_len);
+  len = expected_ecdh_len;
+
+c_truncate:
+  if (expected_ecdh_len < len) {
+    size_t offset = len - expected_ecdh_len;
+    memmove(*ecdh_key, *ecdh_key + offset, expected_ecdh_len);
+    len = expected_ecdh_len;
+  }
+
+c_free:
+  if (len == 0) {
+    free(*ecdh_key);
+  }
+  EVP_PKEY_CTX_free(ctx);
+  if (mdctx != NULL) {
+    EVP_MD_CTX_destroy(mdctx);
+  }
+  EVP_PKEY_free(peer_key);
+  EVP_PKEY_free(private_key);
+
+  return len;
+}
+
+static unsigned char *openssl_derive_ecdh(CK_ULONG kdf, EVP_PKEY *private_key,
+                                          CK_OBJECT_HANDLE peer_key,
+                                          CK_ULONG expected_ecdh_len,
+                                          size_t *ecdh_len) {
+  CK_LONG peerkey_len = get_yhsize(peer_key);
+  if (peerkey_len == 0) {
+    fail("Failed to get peer key size");
+    return 0;
+  }
+
+  unsigned char peerkey_bytes[peerkey_len]; // public key in DER
+  if (get_yhvalue(peer_key, peerkey_bytes, peerkey_len) == 0) {
+    fail("Failed to retrieve public key from yubihsm-pkcs11");
+    return 0;
+  }
+
+  const unsigned char *p = peerkey_bytes;
+  EVP_PKEY *pkey = d2i_PUBKEY(NULL, &p, peerkey_len);
+  if (pkey == NULL) {
+    fail("Failed to parse device public key with OpenSSL");
+    return NULL;
+  }
+
+  unsigned char *derivekey_openssl = malloc(BUFSIZE);
+  *ecdh_len = openssl_derive(kdf, private_key, pkey, &derivekey_openssl,
+                             expected_ecdh_len);
+  if (*ecdh_len == 0) {
+    fail("Failed to derive key with openssl");
+  }
+  return derivekey_openssl;
+}
+
+static void run_test(void *handle, const char *curve, CK_ULONG kdf,
+                     CK_OBJECT_HANDLE yh_privkey, CK_OBJECT_HANDLE yh_pubkey,
+                     CK_ULONG value_len, CK_ULONG ecdh_len, CK_RV exp_res) {
+
+  printf("EC key %s, KDF 0x%lx, value_len %lu. derived ECDH length: %lu. "
+         "Expected error code: 0x%lx....",
+         curve, kdf, value_len, ecdh_len, exp_res);
+
+  unsigned char *peerkey_bytes = NULL;
+
+  // Generate keypair with openssl
+  EVP_PKEY *peer_keypair = generate_keypair_openssl(curve);
+  if (peer_keypair == NULL) {
+    fail("Failed to generate keypair with OpenSSL");
+    goto clean_on_fail;
+  }
+
+  EC_KEY *peerkey = EVP_PKEY_get1_EC_KEY(peer_keypair);
+
+  int peerkey_len = i2o_ECPublicKey(peerkey, &peerkey_bytes);
+  if (peerkey_len < 0) {
+    fail("Failed to extract public key from EC keypair generated with openssl");
+    goto clean_on_fail;
+  }
+
+  EC_KEY_free(peerkey);
+
+  // Derive with yubihsm
+  CK_OBJECT_HANDLE yh_ecdh_key;
+  CK_RV rv = yh_derive(peerkey_bytes, peerkey_len, yh_privkey, kdf, "ecdh",
+                       &yh_ecdh_key, value_len, ecdh_len);
+  if (rv != exp_res) {
+    fail("Wrong error code was returned");
+    goto clean_on_fail;
+  }
+  OPENSSL_free(peerkey_bytes);
+
+  // If testing error handling, no need to test further
+  if (exp_res != CKR_OK) {
+    printf("OK!\n");
+    return;
+  }
+
+  // Derive with openssl
+  size_t ecdh_openssl_len = 0;
+  unsigned char *ecdh_openssl =
+    openssl_derive_ecdh(kdf, peer_keypair, yh_pubkey, ecdh_len,
+                        &ecdh_openssl_len);
+  if (ecdh_openssl_len == 0) {
+    fail("Failed to derive key with openssl");
+    goto clean_on_fail;
+  }
+
+  // Compare sizes
+  CK_ULONG ecdh1_len = get_yhsize(yh_ecdh_key);
+  if (ecdh1_len != ecdh_openssl_len) {
+    fail(
+      "ECDH keys derived with yubihsm-pkcs11 and with openssl do not have the "
+      "same size");
+    goto clean_on_fail;
+  }
+
+  // Compare values
+  unsigned char ecdh1_bytes[BUFSIZE]; // public key in DER
+  if (get_yhvalue(yh_ecdh_key, ecdh1_bytes, ecdh1_len) == 0) {
+    fail("Failed to retrieve derived key from yubihsm-pkcs11");
+    goto clean_on_fail;
+  }
+
+  bool equal = true;
+  for (unsigned int i = 0; i < ecdh_openssl_len; i++) {
+    if (ecdh1_bytes[i] != ecdh_openssl[i]) {
+      equal = false;
+      break;
+    }
+  }
+
+  OPENSSL_free(ecdh_openssl);
+
+  if (!equal) {
+    fail(
+      "ECDH keys derived with yubihsm-pkcs11 and with openssl do not have the "
+      "same value");
+    goto clean_on_fail;
+  }
+
+  printf("OK!\n");
+  return;
+
+clean_on_fail:
+  if (peerkey_bytes != NULL) {
+    OPENSSL_free(peerkey_bytes);
+  }
+  close_session(p11, session);
+  close_module(handle);
+  exit(EXIT_FAILURE);
+}
+
+int main(int argc, char **argv) {
+
+  if (argc != 2) {
+    fprintf(stderr, "usage: /path/to/yubihsm_pkcs11/module\n");
+    exit(EXIT_FAILURE);
+  }
+
+  void *handle = open_module(argv[1]);
+  p11 = get_function_list(handle);
+  session = open_session(p11);
+  print_session_state(p11, session);
+
+  int exit_status = EXIT_SUCCESS;
+
+  CK_OBJECT_HANDLE yh_pubkey[4], yh_privkey[4];
+  for (int i = 0; i < CURVE_COUNT; i++) {
+
+    generate_keypair_yh(CURVE_PARAMS[i], CURVE_LENS[i], &yh_pubkey[i],
+                        &yh_privkey[i]);
+  }
+  printf("\n");
+
+  for (int i = 0; i < CURVE_COUNT; i++) {
+    run_test(handle, CURVES[i], CKD_NULL, yh_privkey[i], yh_pubkey[i], 128 / 8,
+             128 / 8, CKR_OK);
+    run_test(handle, CURVES[i], CKD_NULL, yh_privkey[i], yh_pubkey[i], 192 / 8,
+             192 / 8, CKR_OK);
+  }
+
+  run_test(handle, CURVES[0], CKD_NULL, yh_privkey[0], yh_pubkey[0], 256 / 8,
+           256 / 8, CKR_DATA_LEN_RANGE);
+  run_test(handle, CURVES[1], CKD_NULL, yh_privkey[1], yh_pubkey[1], 256 / 8,
+           256 / 8, CKR_OK);
+  run_test(handle, CURVES[2], CKD_NULL, yh_privkey[2], yh_pubkey[2], 256 / 8,
+           256 / 8, CKR_OK);
+  run_test(handle, CURVES[3], CKD_NULL, yh_privkey[3], yh_pubkey[3], 256 / 8,
+           256 / 8, CKR_OK);
+
+  CK_ULONG key_lens[3] = {128, 192, 256};
+
+  for (int i = 0; i < CURVE_COUNT; i++) {
+    for (size_t j = 0; j < 3; j++) {
+      run_test(handle, CURVES[i], CKD_YUBICO_SHA1_KDF_SP800, yh_privkey[i],
+               yh_pubkey[i], key_lens[j] / 8, key_lens[j] / 8, CKR_OK);
+      run_test(handle, CURVES[i], CKD_YUBICO_SHA256_KDF_SP800, yh_privkey[i],
+               yh_pubkey[i], key_lens[j] / 8, key_lens[j] / 8, CKR_OK);
+      run_test(handle, CURVES[i], CKD_YUBICO_SHA384_KDF_SP800, yh_privkey[i],
+               yh_pubkey[i], key_lens[j] / 8, key_lens[j] / 8, CKR_OK);
+      run_test(handle, CURVES[i], CKD_YUBICO_SHA512_KDF_SP800, yh_privkey[i],
+               yh_pubkey[i], key_lens[j] / 8, key_lens[j] / 8, CKR_OK);
+    }
+
+    run_test(handle, CURVES[i], CKD_NULL, yh_privkey[i], yh_pubkey[i], 0,
+             CURVE_ECDH_LEN[i], CKR_OK);
+    run_test(handle, CURVES[i], CKD_YUBICO_SHA1_KDF_SP800, yh_privkey[i],
+             yh_pubkey[i], 0, 20, CKR_OK);
+    run_test(handle, CURVES[i], CKD_YUBICO_SHA256_KDF_SP800, yh_privkey[i],
+             yh_pubkey[i], 0, 32, CKR_OK);
+    run_test(handle, CURVES[i], CKD_YUBICO_SHA384_KDF_SP800, yh_privkey[i],
+             yh_pubkey[i], 0, 48, CKR_OK);
+    run_test(handle, CURVES[i], CKD_YUBICO_SHA512_KDF_SP800, yh_privkey[i],
+             yh_pubkey[i], 0, 64, CKR_OK);
+  }
+  run_test(handle, CURVES[0], CKD_NULL, yh_privkey[0], yh_pubkey[0], 1024, 0,
+           CKR_ATTRIBUTE_VALUE_INVALID);
+
+  printf("\n");
+  for (int i = 0; i < CURVE_COUNT; i++) {
+    if (destroy_object(p11, session, yh_privkey[i])) {
+      success("Deleted key from YubiHSM");
+    }
+  }
+  close_session(p11, session);
+  close_module(handle);
+  return (exit_status);
+}
diff --git a/pkcs11/util_pkcs11.c b/pkcs11/util_pkcs11.c
index dfbb42c6..c458a2a4 100644
--- a/pkcs11/util_pkcs11.c
+++ b/pkcs11/util_pkcs11.c
@@ -23,6 +23,7 @@
 #include "../common/platform-config.h"
 #include "../common/util.h"
 #include "../common/time_win.h"
+#include "../common/hash.h"
 
 #ifdef __WIN32
 #include <winsock.h>
@@ -38,7 +39,6 @@
 
 #include "util_pkcs11.h"
 #include "debug_p11.h"
-#include "../common/util.h"
 #include "../common/openssl-compat.h"
 #include "../common/insecure_memzero.h"
 
@@ -5308,3 +5308,74 @@ bool match_meta_attributes(yubihsm_pkcs11_session *session,
   }
   return true;
 }
+
+static void increment_ctr(uint8_t *ctr, size_t len) {
+  while (len > 0) {
+    if (++ctr[--len]) {
+      break;
+    }
+  }
+}
+
+CK_RV ecdh_with_kdf(ecdh_session_key *shared_secret, uint8_t *fixed_info,
+                    size_t fixed_len, CK_ULONG kdf, size_t value_len) {
+
+  if (fixed_len > 0 && fixed_info == NULL) {
+    return CKR_MECHANISM_PARAM_INVALID;
+  }
+
+  hash_ctx hash = NULL;
+  switch (kdf) {
+    case CKD_NULL:
+      DBG_INFO("KDF is CKD_NULL");
+      // Do nothing
+      break;
+    case CKD_YUBICO_SHA1_KDF_SP800:
+      DBG_INFO("KDF is CKD_SHA1_KDF_SP800");
+      hash_create(&hash, _SHA1);
+      break;
+    case CKD_YUBICO_SHA256_KDF_SP800:
+      DBG_INFO("KDF is CKD_SHA256_KDF_SP800");
+      hash_create(&hash, _SHA256);
+      break;
+    case CKD_YUBICO_SHA384_KDF_SP800:
+      DBG_INFO("KDF is CKD_SHA384_KDF_SP800");
+      hash_create(&hash, _SHA384);
+      break;
+    case CKD_YUBICO_SHA512_KDF_SP800:
+      DBG_INFO("KDF is CKD_SHA512_KDF_SP800");
+      hash_create(&hash, _SHA512);
+      break;
+  }
+
+  if (hash) {
+    uint8_t ctr[sizeof(uint32_t)] = {0};
+    uint8_t res[ECDH_KEY_BUF_SIZE] = {0};
+    size_t res_len = 0;
+
+    do {
+      increment_ctr(ctr, sizeof(ctr));
+      hash_init(hash);
+      hash_update(hash, ctr, sizeof(ctr));
+      hash_update(hash, shared_secret->ecdh_key, shared_secret->len);
+      hash_update(hash, fixed_info, fixed_len);
+      size_t len = sizeof(res) - res_len;
+      hash_final(hash, res + res_len, &len);
+      res_len += len;
+    } while (res_len < value_len);
+
+    if (value_len == 0) {
+      value_len = res_len;
+    }
+
+    memcpy(shared_secret->ecdh_key, res, value_len);
+    memset(shared_secret->ecdh_key + value_len, 0,
+           sizeof(shared_secret->ecdh_key) - value_len);
+    shared_secret->len = value_len;
+  } else if (kdf != CKD_NULL) {
+    DBG_ERR("Unsupported KDF %lu", kdf);
+    return CKR_MECHANISM_PARAM_INVALID;
+  }
+
+  return CKR_OK;
+}
diff --git a/pkcs11/util_pkcs11.h b/pkcs11/util_pkcs11.h
index 6a0d4701..bf3f977e 100644
--- a/pkcs11/util_pkcs11.h
+++ b/pkcs11/util_pkcs11.h
@@ -122,7 +122,8 @@ bool create_session(yubihsm_pkcs11_slot *slot, CK_FLAGS flags,
 void release_session(yubihsm_pkcs11_context *ctx,
                      yubihsm_pkcs11_session *session);
 
-CK_RV set_template_attribute(yubihsm_pkcs11_attribute *attribute, CK_BBOOL *value);
+CK_RV set_template_attribute(yubihsm_pkcs11_attribute *attribute,
+                             CK_BBOOL *value);
 CK_RV parse_rsa_template(CK_ATTRIBUTE_PTR pTemplate, CK_ULONG ulCount,
                          yubihsm_pkcs11_object_template *template);
 CK_RV parse_ec_template(CK_ATTRIBUTE_PTR pTemplate, CK_ULONG ulCount,
@@ -185,4 +186,7 @@ CK_RV parse_meta_label_template(yubihsm_pkcs11_object_template *template,
                                 pkcs11_meta_object *pkcs11meta, bool public,
                                 uint8_t *value, size_t value_len);
 bool match_byte_array(uint8_t *a, uint16_t a_len, uint8_t *b, uint16_t b_len);
+
+CK_RV ecdh_with_kdf(ecdh_session_key *shared_secret, uint8_t *fixed_info,
+                    size_t fixed_len, CK_ULONG kdf, size_t value_len);
 #endif
diff --git a/pkcs11/yubihsm_pkcs11.c b/pkcs11/yubihsm_pkcs11.c
index 03f4d4f4..f3129df0 100644
--- a/pkcs11/yubihsm_pkcs11.c
+++ b/pkcs11/yubihsm_pkcs11.c
@@ -5605,33 +5605,34 @@ CK_DEFINE_FUNCTION(CK_RV, C_DeriveKey)
     goto c_drv_out;
   }
 
-  if (pMechanism->mechanism != CKM_ECDH1_DERIVE) {
+  if (pMechanism->mechanism != CKM_ECDH1_DERIVE ||
+      pMechanism->pParameter == NULL) {
     DBG_ERR("Invalid mechanism for key generation: %lu", pMechanism->mechanism);
     rv = CKR_MECHANISM_INVALID;
     goto c_drv_out;
   }
 
-  int basekey_type = hBaseKey >> 16;
+  CK_ULONG basekey_type = hBaseKey >> 16;
   if (basekey_type == ECDH_KEY_TYPE) {
-    DBG_ERR("Cannot derive an ECDH key from another ECDH key");
+    DBG_ERR("Cannot derive a session key from another session key");
     rv = CKR_ARGUMENTS_BAD;
     goto c_drv_out;
   }
 
-  char *label_buf = NULL;
+  char *label = NULL;
   size_t label_len = 0;
-  size_t expected_key_length = 0;
+  size_t value_len = 0;
   for (CK_ULONG i = 0; i < ulAttributeCount; i++) {
     switch (pTemplate[i].type) {
       case CKA_VALUE_LEN:
-        expected_key_length = *((CK_ULONG *) pTemplate[i].pValue);
+        value_len = *((CK_ULONG *) pTemplate[i].pValue);
         break;
       case CKA_LABEL:
         if (pTemplate[i].ulValueLen > YH_OBJ_LABEL_LEN) {
           rv = CKR_ATTRIBUTE_VALUE_INVALID;
           goto c_drv_out;
         }
-        label_buf = pTemplate[i].pValue;
+        label = pTemplate[i].pValue;
         label_len = pTemplate[i].ulValueLen;
         break;
       default:
@@ -5646,27 +5647,13 @@ CK_DEFINE_FUNCTION(CK_RV, C_DeriveKey)
 
   CK_ECDH1_DERIVE_PARAMS *params = pMechanism->pParameter;
 
-  if (params->kdf == CKD_NULL) {
-    if ((params->pSharedData != NULL) || (params->ulSharedDataLen != 0)) {
-      DBG_ERR("Mechanism parameters incompatible with key derivation function "
-              "CKD_NULL");
-      rv = CKR_MECHANISM_PARAM_INVALID;
-      goto c_drv_out;
-    }
-  } else {
-    DBG_ERR("Unsupported value of mechanism parameter key derivation function");
+  if (params->kdf == CKD_NULL &&
+      ((params->pSharedData != NULL) || (params->ulSharedDataLen != 0))) {
+    DBG_ERR("Mechanism parameters incompatible with key derivation function");
     rv = CKR_MECHANISM_PARAM_INVALID;
     goto c_drv_out;
   }
 
-  size_t in_len = params->ulPublicDataLen;
-  if (in_len != params->ulPublicDataLen) {
-    DBG_ERR("Invalid parameter");
-    return CKR_ARGUMENTS_BAD;
-  }
-
-  CK_BYTE_PTR pubkey = params->pPublicData;
-
   int seq = session->ecdh_session_keys.length + 1;
   if (seq > MAX_ECDH_SESSION_KEYS) {
     DBG_ERR("There are already %d ECDH keys available for this session. "
@@ -5676,32 +5663,65 @@ CK_DEFINE_FUNCTION(CK_RV, C_DeriveKey)
     goto c_drv_out;
   }
 
+  ecdh_session_key ecdh_key = {0};
+  ecdh_key.id = ECDH_KEY_TYPE << 16 | seq;
+  ecdh_key.len = sizeof(ecdh_key.ecdh_key);
+
+  DBG_INFO("ecdh_key.id = %zu", ecdh_key.id);
+
+  if (value_len > ecdh_key.len) {
+    DBG_ERR("Requested derived key is too long");
+    rv = CKR_ATTRIBUTE_VALUE_INVALID;
+    goto c_drv_out;
+  }
+
   // Read the base key as the private keyID
   uint16_t privkey_id = hBaseKey & 0xffff;
 
-  ecdh_session_key ecdh_key = {0};
-  size_t out_len = sizeof(ecdh_key.ecdh_key);
   yh_rc rc = yh_util_derive_ecdh(session->slot->device_session, privkey_id,
-                                 pubkey, in_len, ecdh_key.ecdh_key, &out_len);
+                                 params->pPublicData, params->ulPublicDataLen,
+                                 ecdh_key.ecdh_key, &ecdh_key.len);
   if (rc != YHR_SUCCESS) {
-    DBG_ERR("Unable to derive ECDH key: %s", yh_strerror(rc));
+    DBG_ERR("Unable to derive raw ECDH key: %s", yh_strerror(rc));
     rv = yrc_to_rv(rc);
     goto c_drv_out;
   }
 
-  if ((expected_key_length > 0) && (expected_key_length != out_len)) {
-    DBG_ERR("Failed to derive a key with the expected length");
-    rv = CKR_DATA_LEN_RANGE;
+  DBG_INFO("ECDH ecdh_key.len = %zu", ecdh_key.len);
+
+  rv = ecdh_with_kdf(&ecdh_key, params->pSharedData, params->ulSharedDataLen,
+                     params->kdf, value_len);
+  if (rv != CKR_OK) {
+    DBG_ERR("Failed to derive ECDH key with KDF %lu", params->kdf);
     goto c_drv_out;
   }
 
-  // Make a session variable to store the derived key
-  ecdh_key.id = ECDH_KEY_TYPE << 16 | seq;
-  ecdh_key.len = out_len;
-  memcpy(ecdh_key.label, label_buf, label_len);
+  DBG_INFO("KDF ecdh_key.len = %zu", ecdh_key.len);
+
+  if (value_len > 0) {
+    if (ecdh_key.len < value_len) {
+      DBG_ERR("Failed to derive a key with the requested length");
+      rv = CKR_DATA_LEN_RANGE;
+      goto c_drv_out;
+    }
+
+    if (ecdh_key.len > value_len) {
+      // Truncate from the left
+      size_t offset = ecdh_key.len - value_len;
+      memmove(ecdh_key.ecdh_key, ecdh_key.ecdh_key + offset, value_len);
+      memset(ecdh_key.ecdh_key + value_len, 0, offset);
+      ecdh_key.len = value_len;
+      DBG_INFO("Truncated ecdh_key.len = %zu", ecdh_key.len);
+    }
+  }
+
+  memcpy(ecdh_key.label, label, label_len);
+
+  // Copy the derived key as a session object
   list_append(&session->ecdh_session_keys, &ecdh_key);
 
-  insecure_memzero(ecdh_key.ecdh_key, out_len);
+  // Clear the derived key
+  insecure_memzero(ecdh_key.ecdh_key, sizeof(ecdh_key.ecdh_key));
 
   *phKey = ecdh_key.id;