core: crypto: introduce struct crypto_mac_ops

Uses struct crypto_mac_ops pointer in crypto context for MACs as a
glue layer instead of a switch(algo) in each crypto_mac_*() function.

Moves CBC-MAC implementation from LTC wrapper to core/crypto.

Reviewed-by: Jerome Forissier <jerome.forissier@linaro.org>
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

8 files changed, 595 insertions, 376 deletions

diff --git a/core/crypto/cbc-mac.c b/core/crypto/cbc-mac.c
new file mode 100644
index 00000000..36c8036e
--- /dev/null
+++ b/core/crypto/cbc-mac.c
@@ -0,0 +1,220 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2014-2019, Linaro Limited
+ */
+
+/*
+ * This is implemented here as being the plain text which is encoded with IV=0.
+ * Result of the CBC-MAC is the last 16-bytes cipher.
+ */
+
+#include <assert.h>
+#include <crypto/crypto.h>
+#include <crypto/crypto_impl.h>
+#include <stdlib.h>
+#include <string.h>
+#include <types_ext.h>
+#include <util.h>
+
+#define CBCMAC_MAX_BLOCK_LEN 16
+
+struct crypto_cbc_mac_ctx {
+	struct crypto_mac_ctx ctx;
+	void *cbc_ctx;
+	uint32_t cbc_algo;
+	uint8_t block[CBCMAC_MAX_BLOCK_LEN];
+	uint8_t digest[CBCMAC_MAX_BLOCK_LEN];
+	unsigned char current_block_len;
+	unsigned char block_len;
+	bool is_computed;
+	bool pkcs5_pad;
+};
+
+static const struct crypto_mac_ops crypto_cbc_mac_ops;
+
+static struct crypto_cbc_mac_ctx *to_cbc_mac_ctx(struct crypto_mac_ctx *ctx)
+{
+	assert(ctx && ctx->ops == &crypto_cbc_mac_ops);
+
+	return container_of(ctx, struct crypto_cbc_mac_ctx, ctx);
+}
+
+static TEE_Result crypto_cbc_mac_init(struct crypto_mac_ctx *ctx,
+				      const uint8_t *key, size_t len)
+{
+	struct crypto_cbc_mac_ctx *mc = to_cbc_mac_ctx(ctx);
+
+	memset(mc->block, 0, sizeof(mc->block));
+	memset(mc->digest, 0, sizeof(mc->digest));
+	mc->current_block_len = 0;
+	mc->is_computed = false;
+
+	/* IV should be zero and mc->block happens to be zero at this stage */
+	return crypto_cipher_init(mc->cbc_ctx, mc->cbc_algo, TEE_MODE_ENCRYPT,
+				  key, len, NULL, 0, mc->block, mc->block_len);
+}
+
+static TEE_Result crypto_cbc_mac_update(struct crypto_mac_ctx *ctx,
+					const uint8_t *data, size_t len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct crypto_cbc_mac_ctx *mc = to_cbc_mac_ctx(ctx);
+
+	if ((mc->current_block_len > 0) &&
+	    (len + mc->current_block_len >= mc->block_len)) {
+		size_t pad_len = mc->block_len - mc->current_block_len;
+
+		memcpy(mc->block + mc->current_block_len, data, pad_len);
+		data += pad_len;
+		len -= pad_len;
+		res = crypto_cipher_update(mc->cbc_ctx, mc->cbc_algo,
+					   TEE_MODE_ENCRYPT, false, mc->block,
+					   mc->block_len, mc->digest);
+		if (res)
+			return res;
+		mc->is_computed = 1;
+		mc->current_block_len = 0;
+	}
+
+	while (len >= mc->block_len) {
+		res = crypto_cipher_update(mc->cbc_ctx, mc->cbc_algo,
+					   TEE_MODE_ENCRYPT, false, data,
+					   mc->block_len, mc->digest);
+		if (res)
+			return res;
+		mc->is_computed = 1;
+		data += mc->block_len;
+		len -= mc->block_len;
+	}
+
+	if (len > 0) {
+		assert(mc->current_block_len + len < mc->block_len);
+		memcpy(mc->block + mc->current_block_len, data, len);
+		mc->current_block_len += len;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result crypto_cbc_mac_final(struct crypto_mac_ctx *ctx,
+				       uint8_t *digest, size_t digest_len)
+{
+	struct crypto_cbc_mac_ctx *mc = to_cbc_mac_ctx(ctx);
+
+	if (mc->pkcs5_pad) {
+		/*
+		 * Padding is in whole bytes. The value of each added
+		 * byte is the number of bytes that are added, i.e. N
+		 * bytes, each of value N are added
+		 */
+		size_t pad_len = mc->block_len - mc->current_block_len;
+
+		memset(mc->block + mc->current_block_len, pad_len, pad_len);
+		mc->current_block_len = 0;
+		if (crypto_cbc_mac_update(ctx, mc->block, mc->block_len))
+			return TEE_ERROR_BAD_STATE;
+	}
+
+	if (!mc->is_computed || mc->current_block_len)
+		return TEE_ERROR_BAD_STATE;
+
+	memcpy(digest, mc->digest, MIN(digest_len, mc->block_len));
+	crypto_cipher_final(mc->cbc_ctx, mc->cbc_algo);
+
+	return TEE_SUCCESS;
+}
+
+static void crypto_cbc_mac_free_ctx(struct crypto_mac_ctx *ctx)
+{
+	struct crypto_cbc_mac_ctx *mc = to_cbc_mac_ctx(ctx);
+
+	crypto_cipher_free_ctx(mc->cbc_ctx, mc->cbc_algo);
+	free(mc);
+}
+
+static void crypto_cbc_mac_copy_state(struct crypto_mac_ctx *dst_ctx,
+				      struct crypto_mac_ctx *src_ctx)
+{
+	struct crypto_cbc_mac_ctx *dst = to_cbc_mac_ctx(dst_ctx);
+	struct crypto_cbc_mac_ctx *src = to_cbc_mac_ctx(src_ctx);
+
+	assert(dst->block_len == src->block_len);
+	assert(dst->pkcs5_pad == src->pkcs5_pad);
+	assert(dst->cbc_algo == src->cbc_algo);
+
+	crypto_cipher_copy_state(dst->cbc_ctx, src->cbc_ctx, src->cbc_algo);
+	memcpy(dst->block, src->block, sizeof(dst->block));
+	memcpy(dst->digest, src->digest, sizeof(dst->digest));
+	dst->current_block_len = src->current_block_len;
+	dst->is_computed = src->is_computed;
+}
+
+static const struct crypto_mac_ops crypto_cbc_mac_ops = {
+	.init = crypto_cbc_mac_init,
+	.update = crypto_cbc_mac_update,
+	.final = crypto_cbc_mac_final,
+	.free_ctx = crypto_cbc_mac_free_ctx,
+	.copy_state = crypto_cbc_mac_copy_state,
+};
+
+static TEE_Result crypto_cbc_mac_alloc_ctx(struct crypto_mac_ctx **ctx_ret,
+					   uint32_t cbc_algo, bool pkcs5_pad)
+{
+	TEE_Result res;
+	void *cbc_ctx = NULL;
+	struct crypto_cbc_mac_ctx *ctx = NULL;
+	size_t block_size = 0;
+
+	res = crypto_cipher_get_block_size(cbc_algo, &block_size);
+	if (res)
+		return res;
+
+	res = crypto_cipher_alloc_ctx(&cbc_ctx, cbc_algo);
+	if (res)
+		return res;
+
+	ctx = calloc(1, sizeof(*ctx));
+	if (!ctx) {
+		crypto_cipher_free_ctx(cbc_ctx, cbc_algo);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	ctx->cbc_ctx = cbc_ctx;
+	ctx->cbc_algo = cbc_algo;
+	ctx->pkcs5_pad = pkcs5_pad;
+	ctx->block_len = block_size;
+	ctx->ctx.ops = &crypto_cbc_mac_ops;
+	*ctx_ret = &ctx->ctx;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result crypto_aes_cbc_mac_nopad_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return crypto_cbc_mac_alloc_ctx(ctx, TEE_ALG_AES_CBC_NOPAD, false);
+}
+
+TEE_Result crypto_aes_cbc_mac_pkcs5_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return crypto_cbc_mac_alloc_ctx(ctx, TEE_ALG_AES_CBC_NOPAD, true);
+}
+
+TEE_Result crypto_des_cbc_mac_nopad_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return crypto_cbc_mac_alloc_ctx(ctx, TEE_ALG_DES_CBC_NOPAD, false);
+}
+
+TEE_Result crypto_des_cbc_mac_pkcs5_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return crypto_cbc_mac_alloc_ctx(ctx, TEE_ALG_DES_CBC_NOPAD, true);
+}
+
+TEE_Result crypto_des3_cbc_mac_nopad_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return crypto_cbc_mac_alloc_ctx(ctx, TEE_ALG_DES3_CBC_NOPAD, false);
+}
+
+TEE_Result crypto_des3_cbc_mac_pkcs5_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return crypto_cbc_mac_alloc_ctx(ctx, TEE_ALG_DES3_CBC_NOPAD, true);
+}
diff --git a/core/crypto/crypto.c b/core/crypto/crypto.c
index 29a90784..4e3fc620 100644
--- a/core/crypto/crypto.c
+++ b/core/crypto/crypto.c
@@ -151,43 +151,101 @@ TEE_Result crypto_cipher_get_block_size(uint32_t algo, size_t *size)
 	}
 }
 
-#if !defined(_CFG_CRYPTO_WITH_MAC)
-TEE_Result crypto_mac_alloc_ctx(void **ctx __unused, uint32_t algo __unused)
+TEE_Result crypto_mac_alloc_ctx(void **ctx, uint32_t algo)
 {
-	return TEE_ERROR_NOT_IMPLEMENTED;
+	TEE_Result res = TEE_SUCCESS;
+	struct crypto_mac_ctx *c = NULL;
+
+	switch (algo) {
+	case TEE_ALG_HMAC_MD5:
+		res = crypto_hmac_md5_alloc_ctx(&c);
+		break;
+	case TEE_ALG_HMAC_SHA1:
+		res = crypto_hmac_sha1_alloc_ctx(&c);
+		break;
+	case TEE_ALG_HMAC_SHA224:
+		res = crypto_hmac_sha224_alloc_ctx(&c);
+		break;
+	case TEE_ALG_HMAC_SHA256:
+		res = crypto_hmac_sha256_alloc_ctx(&c);
+		break;
+	case TEE_ALG_HMAC_SHA384:
+		res = crypto_hmac_sha384_alloc_ctx(&c);
+		break;
+	case TEE_ALG_HMAC_SHA512:
+		res = crypto_hmac_sha512_alloc_ctx(&c);
+		break;
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+		res = crypto_aes_cbc_mac_nopad_alloc_ctx(&c);
+		break;
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+		res = crypto_aes_cbc_mac_pkcs5_alloc_ctx(&c);
+		break;
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+		res = crypto_des_cbc_mac_nopad_alloc_ctx(&c);
+		break;
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+		res = crypto_des_cbc_mac_pkcs5_alloc_ctx(&c);
+		break;
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+		res = crypto_des3_cbc_mac_nopad_alloc_ctx(&c);
+		break;
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+		res = crypto_des3_cbc_mac_pkcs5_alloc_ctx(&c);
+		break;
+	case TEE_ALG_AES_CMAC:
+		res = crypto_aes_cmac_alloc_ctx(&c);
+		break;
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (!res)
+		*ctx = c;
+
+	return res;
+}
+
+static const struct crypto_mac_ops *mac_ops(void *ctx)
+{
+	struct crypto_mac_ctx *c = ctx;
+
+	assert(c && c->ops);
+
+	return c->ops;
 }
 
 void crypto_mac_free_ctx(void *ctx, uint32_t algo __unused)
 {
 	if (ctx)
-		assert(0);
+		mac_ops(ctx)->free_ctx(ctx);
 }
 
-void crypto_mac_copy_state(void *dst_ctx __unused, void *src_ctx __unused,
-			   uint32_t algo __unused)
+void crypto_mac_copy_state(void *dst_ctx, void *src_ctx, uint32_t algo __unused)
 {
-	assert(0);
+	mac_ops(dst_ctx)->copy_state(dst_ctx, src_ctx);
 }
 
-TEE_Result crypto_mac_init(void *ctx __unused, uint32_t algo __unused,
-			   const uint8_t *key __unused, size_t len __unused)
+TEE_Result crypto_mac_init(void *ctx, uint32_t algo __unused,
+			   const uint8_t *key, size_t len)
 {
-	return TEE_ERROR_NOT_IMPLEMENTED;
+	return mac_ops(ctx)->init(ctx, key, len);
 }
 
-TEE_Result crypto_mac_update(void *ctx __unused, uint32_t algo __unused,
-			     const uint8_t *data __unused, size_t len __unused)
+TEE_Result crypto_mac_update(void *ctx, uint32_t algo __unused,
+			     const uint8_t *data, size_t len)
 {
-	return TEE_ERROR_NOT_IMPLEMENTED;
+	if (!len)
+		return TEE_SUCCESS;
+
+	return mac_ops(ctx)->update(ctx, data, len);
 }
 
-TEE_Result crypto_mac_final(void *ctx __unused, uint32_t algo __unused,
-			    uint8_t *digest __unused,
-			    size_t digest_len __unused)
+TEE_Result crypto_mac_final(void *ctx, uint32_t algo __unused,
+			    uint8_t *digest, size_t digest_len)
 {
-	return TEE_ERROR_NOT_IMPLEMENTED;
+	return mac_ops(ctx)->final(ctx, digest, digest_len);
 }
-#endif /*_CFG_CRYPTO_WITH_MAC*/
 
 TEE_Result crypto_authenc_alloc_ctx(void **ctx, uint32_t algo)
 {
diff --git a/core/crypto/sub.mk b/core/crypto/sub.mk
index bb16a3eb..25e3ed11 100644
--- a/core/crypto/sub.mk
+++ b/core/crypto/sub.mk
@@ -13,3 +13,7 @@ srcs-y += rng_fortuna.c
 else
 srcs-y += rng_hw.c
 endif
+
+ifneq ($(CFG_CRYPTO_CBC_MAC_FROM_CRYPTOLIB),y)
+srcs-$(CFG_CRYPTO_CBC_MAC) += cbc-mac.c
+endif
diff --git a/core/include/crypto/crypto_impl.h b/core/include/crypto/crypto_impl.h
index 38e6dd3d..6a5cf337 100644
--- a/core/include/crypto/crypto_impl.h
+++ b/core/include/crypto/crypto_impl.h
@@ -68,4 +68,61 @@ TEE_Result crypto_sha512_alloc_ctx(struct crypto_hash_ctx **ctx);
 CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(sha512, hash)
 #endif
 
+/*
+ * The crypto context used by the crypto_mac_*() functions is defined by
+ * struct crypto_mac_ctx.
+ */
+struct crypto_mac_ctx {
+	const struct crypto_mac_ops *ops;
+};
+
+struct crypto_mac_ops {
+	TEE_Result (*init)(struct crypto_mac_ctx *ctx, const uint8_t *key,
+			   size_t len);
+	TEE_Result (*update)(struct crypto_mac_ctx *ctx, const uint8_t *data,
+			     size_t len);
+	TEE_Result (*final)(struct crypto_mac_ctx *ctx, uint8_t *digest,
+			    size_t len);
+	void (*free_ctx)(struct crypto_mac_ctx *ctx);
+	void (*copy_state)(struct crypto_mac_ctx *dst_ctx,
+			   struct crypto_mac_ctx *src_ctx);
+};
+
+#if defined(CFG_CRYPTO_HMAC)
+TEE_Result crypto_hmac_md5_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_hmac_sha1_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_hmac_sha224_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_hmac_sha256_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_hmac_sha384_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_hmac_sha512_alloc_ctx(struct crypto_mac_ctx **ctx);
+#else
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(hmac_md5, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(hmac_sha1, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(hmac_sha224, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(hmac_sha256, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(hmac_sha384, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(hmac_sha512, mac)
+#endif
+
+#if defined(CFG_CRYPTO_CBC_MAC)
+TEE_Result crypto_aes_cbc_mac_nopad_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_aes_cbc_mac_pkcs5_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_des_cbc_mac_nopad_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_des_cbc_mac_pkcs5_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_des3_cbc_mac_nopad_alloc_ctx(struct crypto_mac_ctx **ctx);
+TEE_Result crypto_des3_cbc_mac_pkcs5_alloc_ctx(struct crypto_mac_ctx **ctx);
+#else
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(aes_cbc_mac_nopad, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(aes_cbc_mac_pkcs5, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(des_cbc_mac_nopad, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(des_cbc_mac_pkcs5, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(des3_cbc_mac_nopad, mac)
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(des3_cbc_mac_pkcs5, mac)
+#endif
+
+#if defined(CFG_CRYPTO_CMAC)
+TEE_Result crypto_aes_cmac_alloc_ctx(struct crypto_mac_ctx **ctx);
+#else
+CRYPTO_ALLOC_CTX_NOT_IMPLEMENTED(aes_cmac, mac)
+#endif
 #endif /*__CRYPTO_CRYPTO_IMPL_H*/
diff --git a/core/lib/libtomcrypt/cmac.c b/core/lib/libtomcrypt/cmac.c
new file mode 100644
index 00000000..3062e2b5
--- /dev/null
+++ b/core/lib/libtomcrypt/cmac.c
@@ -0,0 +1,102 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2014-2019, Linaro Limited
+ */
+
+#include <assert.h>
+#include <crypto/crypto.h>
+#include <crypto/crypto_impl.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <tomcrypt.h>
+#include <utee_defines.h>
+#include <util.h>
+
+struct ltc_omac_ctx {
+	struct crypto_mac_ctx ctx;
+	int cipher_idx;
+	omac_state state;
+};
+
+static const struct crypto_mac_ops ltc_omac_ops;
+
+static struct ltc_omac_ctx *to_omac_ctx(struct crypto_mac_ctx *ctx)
+{
+	assert(ctx && ctx->ops == &ltc_omac_ops);
+
+	return container_of(ctx, struct ltc_omac_ctx, ctx);
+}
+
+static TEE_Result ltc_omac_init(struct crypto_mac_ctx *ctx, const uint8_t *key,
+				size_t len)
+{
+	struct ltc_omac_ctx *hc = to_omac_ctx(ctx);
+
+	if (omac_init(&hc->state, hc->cipher_idx, key, len) == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static TEE_Result ltc_omac_update(struct crypto_mac_ctx *ctx,
+				  const uint8_t *data, size_t len)
+{
+	if (omac_process(&to_omac_ctx(ctx)->state, data, len) == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static TEE_Result ltc_omac_final(struct crypto_mac_ctx *ctx, uint8_t *digest,
+				 size_t len)
+{
+	unsigned long l = len;
+
+	if (omac_done(&to_omac_ctx(ctx)->state, digest, &l) == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static void ltc_omac_free_ctx(struct crypto_mac_ctx *ctx)
+{
+	free(to_omac_ctx(ctx));
+}
+
+static void ltc_omac_copy_state(struct crypto_mac_ctx *dst_ctx,
+				struct crypto_mac_ctx *src_ctx)
+{
+	struct ltc_omac_ctx *src = to_omac_ctx(src_ctx);
+	struct ltc_omac_ctx *dst = to_omac_ctx(dst_ctx);
+
+	assert(src->cipher_idx == dst->cipher_idx);
+	dst->state = src->state;
+}
+
+static const struct crypto_mac_ops ltc_omac_ops = {
+	.init = ltc_omac_init,
+	.update = ltc_omac_update,
+	.final = ltc_omac_final,
+	.free_ctx = ltc_omac_free_ctx,
+	.copy_state = ltc_omac_copy_state,
+};
+
+TEE_Result crypto_aes_cmac_alloc_ctx(struct crypto_mac_ctx **ctx_ret)
+{
+	struct ltc_omac_ctx *ctx = NULL;
+	int cipher_idx = find_cipher("aes");
+
+	if (cipher_idx < 0)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	ctx = calloc(1, sizeof(*ctx));
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	ctx->ctx.ops = &ltc_omac_ops;
+	ctx->cipher_idx = cipher_idx;
+	*ctx_ret = &ctx->ctx;
+
+	return TEE_SUCCESS;
+}
diff --git a/core/lib/libtomcrypt/hmac.c b/core/lib/libtomcrypt/hmac.c
new file mode 100644
index 00000000..6bcf74c8
--- /dev/null
+++ b/core/lib/libtomcrypt/hmac.c
@@ -0,0 +1,132 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2014-2019, Linaro Limited
+ */
+
+#include <assert.h>
+#include <crypto/crypto.h>
+#include <crypto/crypto_impl.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <tomcrypt.h>
+#include <utee_defines.h>
+#include <util.h>
+
+struct ltc_hmac_ctx {
+	struct crypto_mac_ctx ctx;
+	int hash_idx;
+	hmac_state state;
+};
+
+static const struct crypto_mac_ops ltc_hmac_ops;
+
+static struct ltc_hmac_ctx *to_hmac_ctx(struct crypto_mac_ctx *ctx)
+{
+	assert(ctx && ctx->ops == &ltc_hmac_ops);
+
+	return container_of(ctx, struct ltc_hmac_ctx, ctx);
+}
+
+static TEE_Result ltc_hmac_init(struct crypto_mac_ctx *ctx, const uint8_t *key,
+				size_t len)
+{
+	struct ltc_hmac_ctx *hc = to_hmac_ctx(ctx);
+
+	if (hmac_init(&hc->state, hc->hash_idx, key, len) == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static TEE_Result ltc_hmac_update(struct crypto_mac_ctx *ctx,
+				  const uint8_t *data, size_t len)
+{
+	if (hmac_process(&to_hmac_ctx(ctx)->state, data, len) == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static TEE_Result ltc_hmac_final(struct crypto_mac_ctx *ctx, uint8_t *digest,
+				 size_t len)
+{
+	unsigned long l = len;
+
+	if (hmac_done(&to_hmac_ctx(ctx)->state, digest, &l) == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static void ltc_hmac_free_ctx(struct crypto_mac_ctx *ctx)
+{
+	free(to_hmac_ctx(ctx));
+}
+
+static void ltc_hmac_copy_state(struct crypto_mac_ctx *dst_ctx,
+				struct crypto_mac_ctx *src_ctx)
+{
+	struct ltc_hmac_ctx *src = to_hmac_ctx(src_ctx);
+	struct ltc_hmac_ctx *dst = to_hmac_ctx(dst_ctx);
+
+	assert(src->hash_idx == dst->hash_idx);
+	dst->state = src->state;
+}
+
+static const struct crypto_mac_ops ltc_hmac_ops = {
+	.init = ltc_hmac_init,
+	.update = ltc_hmac_update,
+	.final = ltc_hmac_final,
+	.free_ctx = ltc_hmac_free_ctx,
+	.copy_state = ltc_hmac_copy_state,
+};
+
+static TEE_Result ltc_hmac_alloc_ctx(struct crypto_mac_ctx **ctx_ret,
+				     int hash_idx)
+{
+	struct ltc_hmac_ctx *ctx = NULL;
+
+	if (hash_idx < 0)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	ctx = calloc(1, sizeof(*ctx));
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	ctx->ctx.ops = &ltc_hmac_ops;
+	ctx->hash_idx = hash_idx;
+	*ctx_ret = &ctx->ctx;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result crypto_hmac_md5_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return ltc_hmac_alloc_ctx(ctx, find_hash("md5"));
+}
+
+TEE_Result crypto_hmac_sha1_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return ltc_hmac_alloc_ctx(ctx, find_hash("sha1"));
+}
+
+TEE_Result crypto_hmac_sha224_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return ltc_hmac_alloc_ctx(ctx, find_hash("sha224"));
+}
+
+TEE_Result crypto_hmac_sha256_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return ltc_hmac_alloc_ctx(ctx, find_hash("sha256"));
+}
+
+TEE_Result crypto_hmac_sha384_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return ltc_hmac_alloc_ctx(ctx, find_hash("sha384"));
+}
+
+TEE_Result crypto_hmac_sha512_alloc_ctx(struct crypto_mac_ctx **ctx)
+{
+	return ltc_hmac_alloc_ctx(ctx, find_hash("sha512"));
+}
diff --git a/core/lib/libtomcrypt/src/tee_ltc_provider.c b/core/lib/libtomcrypt/src/tee_ltc_provider.c
index 5609d278..580386b2 100644
--- a/core/lib/libtomcrypt/src/tee_ltc_provider.c
+++ b/core/lib/libtomcrypt/src/tee_ltc_provider.c
@@ -128,8 +128,7 @@ static void tee_ltc_reg_algs(void)
 }
 
 
-#if defined(_CFG_CRYPTO_WITH_HASH) || defined(CFG_CRYPTO_RSA) || \
-	defined(CFG_CRYPTO_HMAC)
+#if defined(CFG_CRYPTO_RSA)
 
 /*
  * Compute the LibTomCrypt "hashindex" given a TEE Algorithm "algo"
@@ -146,16 +145,11 @@ static TEE_Result tee_algo_to_ltc_hashindex(uint32_t algo, int *ltc_hashindex)
 	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
 	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
 	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA1:
-	case TEE_ALG_SHA1:
-	case TEE_ALG_DSA_SHA1:
-	case TEE_ALG_HMAC_SHA1:
 		*ltc_hashindex = find_hash("sha1");
 		break;
 #endif
 #if defined(CFG_CRYPTO_MD5)
 	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
-	case TEE_ALG_MD5:
-	case TEE_ALG_HMAC_MD5:
 		*ltc_hashindex = find_hash("md5");
 		break;
 #endif
@@ -163,9 +157,6 @@ static TEE_Result tee_algo_to_ltc_hashindex(uint32_t algo, int *ltc_hashindex)
 	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
 	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
 	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA224:
-	case TEE_ALG_SHA224:
-	case TEE_ALG_DSA_SHA224:
-	case TEE_ALG_HMAC_SHA224:
 		*ltc_hashindex = find_hash("sha224");
 		break;
 #endif
@@ -173,9 +164,6 @@ static TEE_Result tee_algo_to_ltc_hashindex(uint32_t algo, int *ltc_hashindex)
 	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
 	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
 	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA256:
-	case TEE_ALG_SHA256:
-	case TEE_ALG_DSA_SHA256:
-	case TEE_ALG_HMAC_SHA256:
 		*ltc_hashindex = find_hash("sha256");
 		break;
 #endif
@@ -183,8 +171,6 @@ static TEE_Result tee_algo_to_ltc_hashindex(uint32_t algo, int *ltc_hashindex)
 	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
 	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
 	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA384:
-	case TEE_ALG_SHA384:
-	case TEE_ALG_HMAC_SHA384:
 		*ltc_hashindex = find_hash("sha384");
 		break;
 #endif
@@ -192,8 +178,6 @@ static TEE_Result tee_algo_to_ltc_hashindex(uint32_t algo, int *ltc_hashindex)
 	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
 	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
 	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA512:
-	case TEE_ALG_SHA512:
-	case TEE_ALG_HMAC_SHA512:
 		*ltc_hashindex = find_hash("sha512");
 		break;
 #endif
@@ -212,8 +196,7 @@ static TEE_Result tee_algo_to_ltc_hashindex(uint32_t algo, int *ltc_hashindex)
 	else
 		return TEE_SUCCESS;
 }
-#endif /* defined(_CFG_CRYPTO_WITH_HASH) ||
-	  defined(_CFG_CRYPTO_WITH_ACIPHER) || defined(_CFG_CRYPTO_WITH_MAC) */
+#endif /* defined(CFG_CRYPTO_RSA) */
 
 #if defined(_CFG_CRYPTO_WITH_CIPHER) || defined(_CFG_CRYPTO_WITH_MAC) || \
 	defined(_CFG_CRYPTO_WITH_AUTHENC)
@@ -1844,345 +1827,6 @@ void crypto_cipher_final(void *ctx, uint32_t algo)
 }
 #endif /* _CFG_CRYPTO_WITH_CIPHER */
 
-/*****************************************************************************
- * Message Authentication Code functions
- *****************************************************************************/
-
-#if defined(_CFG_CRYPTO_WITH_MAC)
-
-#if defined(CFG_CRYPTO_CBC_MAC)
-/*
- * CBC-MAC is not implemented in Libtomcrypt
- * This is implemented here as being the plain text which is encoded with IV=0.
- * Result of the CBC-MAC is the last 16-bytes cipher.
- */
-
-#define CBCMAC_MAX_BLOCK_LEN 16
-struct cbc_state {
-	symmetric_CBC cbc;
-	uint8_t block[CBCMAC_MAX_BLOCK_LEN];
-	uint8_t digest[CBCMAC_MAX_BLOCK_LEN];
-	size_t current_block_len, block_len;
-	int is_computed;
-};
-#endif
-
-static TEE_Result mac_get_ctx_size(uint32_t algo, size_t *size)
-{
-	switch (algo) {
-#if defined(CFG_CRYPTO_HMAC)
-	case TEE_ALG_HMAC_MD5:
-	case TEE_ALG_HMAC_SHA224:
-	case TEE_ALG_HMAC_SHA1:
-	case TEE_ALG_HMAC_SHA256:
-	case TEE_ALG_HMAC_SHA384:
-	case TEE_ALG_HMAC_SHA512:
-		*size = sizeof(hmac_state);
-		break;
-#endif
-#if defined(CFG_CRYPTO_CBC_MAC)
-	case TEE_ALG_AES_CBC_MAC_NOPAD:
-	case TEE_ALG_AES_CBC_MAC_PKCS5:
-	case TEE_ALG_DES_CBC_MAC_NOPAD:
-	case TEE_ALG_DES_CBC_MAC_PKCS5:
-	case TEE_ALG_DES3_CBC_MAC_NOPAD:
-	case TEE_ALG_DES3_CBC_MAC_PKCS5:
-		*size = sizeof(struct cbc_state);
-		break;
-#endif
-#if defined(CFG_CRYPTO_CMAC)
-	case TEE_ALG_AES_CMAC:
-		*size = sizeof(omac_state);
-		break;
-#endif
-	default:
-		return TEE_ERROR_NOT_SUPPORTED;
-	}
-
-	return TEE_SUCCESS;
-}
-
-TEE_Result crypto_mac_alloc_ctx(void **ctx_ret, uint32_t algo)
-{
-	TEE_Result res;
-	size_t ctx_size;
-	void *ctx;
-
-	res = mac_get_ctx_size(algo, &ctx_size);
-	if (res)
-		return res;
-
-	ctx = calloc(1, ctx_size);
-	if (!ctx)
-		return TEE_ERROR_OUT_OF_MEMORY;
-
-	*ctx_ret = ctx;
-	return TEE_SUCCESS;
-}
-
-void crypto_mac_free_ctx(void *ctx, uint32_t algo __maybe_unused)
-{
-	size_t ctx_size __maybe_unused;
-
-	/*
-	 * Check that it's a supported algo, or crypto_mac_alloc_ctx()
-	 * could never have succeded above.
-	 */
-	if (ctx)
-		assert(!mac_get_ctx_size(algo, &ctx_size));
-	free(ctx);
-}
-
-void crypto_mac_copy_state(void *dst_ctx, void *src_ctx, uint32_t algo)
-{
-	TEE_Result res __maybe_unused;
-	size_t ctx_size = 0;
-
-	res = mac_get_ctx_size(algo, &ctx_size);
-	assert(!res);
-	memcpy(dst_ctx, src_ctx, ctx_size);
-}
-
-TEE_Result crypto_mac_init(void *ctx, uint32_t algo, const uint8_t *key,
-			   size_t len)
-{
-	TEE_Result res;
-#if defined(CFG_CRYPTO_HMAC)
-	int ltc_hashindex;
-#endif
-#if defined(CFG_CRYPTO_CBC_MAC) || defined(CFG_CRYPTO_CMAC)
-	int ltc_cipherindex;
-#endif
-#if defined(CFG_CRYPTO_CBC_MAC)
-	uint8_t *real_key;
-	uint8_t key_array[24];
-	size_t real_key_len;
-	uint8_t iv[CBCMAC_MAX_BLOCK_LEN];
-	struct cbc_state *cbc;
-#endif
-
-	switch (algo) {
-#if defined(CFG_CRYPTO_HMAC)
-	case TEE_ALG_HMAC_MD5:
-	case TEE_ALG_HMAC_SHA224:
-	case TEE_ALG_HMAC_SHA1:
-	case TEE_ALG_HMAC_SHA256:
-	case TEE_ALG_HMAC_SHA384:
-	case TEE_ALG_HMAC_SHA512:
-		res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
-		if (res != TEE_SUCCESS)
-			return res;
-		if (CRYPT_OK !=
-		    hmac_init((hmac_state *)ctx, ltc_hashindex, key, len))
-			return TEE_ERROR_BAD_STATE;
-		break;
-#endif
-#if defined(CFG_CRYPTO_CBC_MAC)
-	case TEE_ALG_AES_CBC_MAC_NOPAD:
-	case TEE_ALG_AES_CBC_MAC_PKCS5:
-	case TEE_ALG_DES_CBC_MAC_NOPAD:
-	case TEE_ALG_DES_CBC_MAC_PKCS5:
-	case TEE_ALG_DES3_CBC_MAC_NOPAD:
-	case TEE_ALG_DES3_CBC_MAC_PKCS5:
-		cbc = (struct cbc_state *)ctx;
-
-		res = tee_algo_to_ltc_cipherindex(algo, &ltc_cipherindex);
-		if (res != TEE_SUCCESS)
-			return res;
-
-		cbc->block_len =
-			cipher_descriptor[ltc_cipherindex]->block_length;
-		if (CBCMAC_MAX_BLOCK_LEN < cbc->block_len)
-			return TEE_ERROR_BAD_PARAMETERS;
-		memset(iv, 0, cbc->block_len);
-
-		if (algo == TEE_ALG_DES3_CBC_MAC_NOPAD ||
-		    algo == TEE_ALG_DES3_CBC_MAC_PKCS5) {
-			get_des2_key(key, len, key_array,
-				     &real_key, &real_key_len);
-			key = real_key;
-			len = real_key_len;
-		}
-		if (CRYPT_OK != cbc_start(
-			ltc_cipherindex, iv, key, len, 0, &cbc->cbc))
-				return TEE_ERROR_BAD_STATE;
-		cbc->is_computed = 0;
-		cbc->current_block_len = 0;
-		break;
-#endif
-#if defined(CFG_CRYPTO_CMAC)
-	case TEE_ALG_AES_CMAC:
-		res = tee_algo_to_ltc_cipherindex(algo, &ltc_cipherindex);
-		if (res != TEE_SUCCESS)
-			return res;
-		if (CRYPT_OK != omac_init((omac_state *)ctx, ltc_cipherindex,
-					  key, len))
-			return TEE_ERROR_BAD_STATE;
-		break;
-#endif
-	default:
-		return TEE_ERROR_NOT_SUPPORTED;
-	}
-
-	return TEE_SUCCESS;
-}
-
-TEE_Result crypto_mac_update(void *ctx, uint32_t algo, const uint8_t *data,
-			     size_t len)
-{
-#if defined(CFG_CRYPTO_CBC_MAC)
-	int ltc_res;
-	struct cbc_state *cbc;
-	size_t pad_len;
-#endif
-
-	if (!data || !len)
-		return TEE_SUCCESS;
-
-	switch (algo) {
-#if defined(CFG_CRYPTO_HMAC)
-	case TEE_ALG_HMAC_MD5:
-	case TEE_ALG_HMAC_SHA224:
-	case TEE_ALG_HMAC_SHA1:
-	case TEE_ALG_HMAC_SHA256:
-	case TEE_ALG_HMAC_SHA384:
-	case TEE_ALG_HMAC_SHA512:
-		if (CRYPT_OK != hmac_process((hmac_state *)ctx, data, len))
-			return TEE_ERROR_BAD_STATE;
-		break;
-#endif
-#if defined(CFG_CRYPTO_CBC_MAC)
-	case TEE_ALG_AES_CBC_MAC_NOPAD:
-	case TEE_ALG_AES_CBC_MAC_PKCS5:
-	case TEE_ALG_DES_CBC_MAC_NOPAD:
-	case TEE_ALG_DES_CBC_MAC_PKCS5:
-	case TEE_ALG_DES3_CBC_MAC_NOPAD:
-	case TEE_ALG_DES3_CBC_MAC_PKCS5:
-		cbc = ctx;
-
-		if ((cbc->current_block_len > 0) &&
-		    (len + cbc->current_block_len >= cbc->block_len)) {
-			pad_len = cbc->block_len - cbc->current_block_len;
-			memcpy(cbc->block + cbc->current_block_len,
-			       data, pad_len);
-			data += pad_len;
-			len -= pad_len;
-			ltc_res = cbc_encrypt(cbc->block, cbc->digest,
-					      cbc->block_len, &cbc->cbc);
-			if (CRYPT_OK != ltc_res)
-				return TEE_ERROR_BAD_STATE;
-			cbc->is_computed = 1;
-			cbc->current_block_len = 0;
-		}
-
-		while (len >= cbc->block_len) {
-			ltc_res = cbc_encrypt(data, cbc->digest,
-					      cbc->block_len, &cbc->cbc);
-			if (CRYPT_OK != ltc_res)
-				return TEE_ERROR_BAD_STATE;
-			cbc->is_computed = 1;
-			data += cbc->block_len;
-			len -= cbc->block_len;
-		}
-
-		if (len > 0) {
-			assert(cbc->current_block_len + len < cbc->block_len);
-			memcpy(cbc->block + cbc->current_block_len, data, len);
-			cbc->current_block_len += len;
-		}
-		break;
-#endif
-#if defined(CFG_CRYPTO_CMAC)
-	case TEE_ALG_AES_CMAC:
-		if (CRYPT_OK != omac_process((omac_state *)ctx, data, len))
-			return TEE_ERROR_BAD_STATE;
-		break;
-#endif
-	default:
-		return TEE_ERROR_NOT_SUPPORTED;
-	}
-
-	return TEE_SUCCESS;
-}
-
-TEE_Result crypto_mac_final(void *ctx, uint32_t algo, uint8_t *digest,
-			    size_t digest_len)
-{
-#if defined(CFG_CRYPTO_CBC_MAC)
-	struct cbc_state *cbc;
-	size_t pad_len;
-#endif
-	unsigned long ltc_digest_len = digest_len;
-
-	switch (algo) {
-#if defined(CFG_CRYPTO_HMAC)
-	case TEE_ALG_HMAC_MD5:
-	case TEE_ALG_HMAC_SHA224:
-	case TEE_ALG_HMAC_SHA1:
-	case TEE_ALG_HMAC_SHA256:
-	case TEE_ALG_HMAC_SHA384:
-	case TEE_ALG_HMAC_SHA512:
-		if (CRYPT_OK != hmac_done((hmac_state *)ctx, digest,
-					  &ltc_digest_len))
-			return TEE_ERROR_BAD_STATE;
-		break;
-#endif
-#if defined(CFG_CRYPTO_CBC_MAC)
-	case TEE_ALG_AES_CBC_MAC_NOPAD:
-	case TEE_ALG_AES_CBC_MAC_PKCS5:
-	case TEE_ALG_DES_CBC_MAC_NOPAD:
-	case TEE_ALG_DES_CBC_MAC_PKCS5:
-	case TEE_ALG_DES3_CBC_MAC_NOPAD:
-	case TEE_ALG_DES3_CBC_MAC_PKCS5:
-		cbc = (struct cbc_state *)ctx;
-
-		/* Padding is required */
-		switch (algo) {
-		case TEE_ALG_AES_CBC_MAC_PKCS5:
-		case TEE_ALG_DES_CBC_MAC_PKCS5:
-		case TEE_ALG_DES3_CBC_MAC_PKCS5:
-			/*
-			 * Padding is in whole bytes. The value of each added
-			 * byte is the number of bytes that are added, i.e. N
-			 * bytes, each of value N are added
-			 */
-			pad_len = cbc->block_len - cbc->current_block_len;
-			memset(cbc->block+cbc->current_block_len,
-			       pad_len, pad_len);
-			cbc->current_block_len = 0;
-			if (TEE_SUCCESS != crypto_mac_update(ctx, algo,
-							     cbc->block,
-							     cbc->block_len))
-					return TEE_ERROR_BAD_STATE;
-			break;
-		default:
-			/* nothing to do */
-			break;
-		}
-
-		if ((!cbc->is_computed) || (cbc->current_block_len != 0))
-			return TEE_ERROR_BAD_STATE;
-
-		memcpy(digest, cbc->digest, MIN(ltc_digest_len,
-						cbc->block_len));
-		crypto_cipher_final(&cbc->cbc, algo);
-		break;
-#endif
-#if defined(CFG_CRYPTO_CMAC)
-	case TEE_ALG_AES_CMAC:
-		if (CRYPT_OK != omac_done((omac_state *)ctx, digest,
-					  &ltc_digest_len))
-			return TEE_ERROR_BAD_STATE;
-		break;
-#endif
-	default:
-		return TEE_ERROR_NOT_SUPPORTED;
-	}
-
-	return TEE_SUCCESS;
-}
-#endif /* _CFG_CRYPTO_WITH_MAC */
-
 /******************************************************************************
  * Authenticated encryption
  ******************************************************************************/
diff --git a/core/lib/libtomcrypt/sub.mk b/core/lib/libtomcrypt/sub.mk
index 3e338995..7bc2a73e 100644
--- a/core/lib/libtomcrypt/sub.mk
+++ b/core/lib/libtomcrypt/sub.mk
@@ -7,3 +7,5 @@ global-incdirs-y += include
 subdirs-y += src
 
 srcs-$(_CFG_CRYPTO_WITH_HASH) += hash.c
+srcs-$(CFG_CRYPTO_HMAC) += hmac.c
+srcs-$(CFG_CRYPTO_CMAC) += cmac.c