diff --git a/ta/crypt/arith_taf.c b/ta/crypt/arith_taf.c
new file mode 100644
index 0000000..9c42faa
--- /dev/null
+++ b/ta/crypt/arith_taf.c
@@ -0,0 +1,563 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/* Copyright (c) 2018, Linaro Limited */
+
+#include <arith_taf.h>
+#include <ta_crypt.h>
+#include <tee_internal_api.h>
+#include <util.h>
+
+#include "handle.h"
+
+#define HT_BIGINT	BIT(31)
+#define HT_FMMCTX	BIT(30)
+#define HT_FMMVAR	BIT(29)
+#define HT_MASK		(HT_BIGINT | HT_FMMCTX | HT_FMMVAR)
+
+#define CHECK_PT(t0, t1, t2, t3) do { \
+		const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_##t0, \
+							TEE_PARAM_TYPE_##t1, \
+							TEE_PARAM_TYPE_##t2, \
+							TEE_PARAM_TYPE_##t3); \
+		\
+		if ((param_types) != exp_pt) { \
+			EMSG("Unpexteded param_types 0x%" PRIx32 ", exptected 0x%" PRIx32, (param_types), exp_pt); \
+			return TEE_ERROR_BAD_PARAMETERS; \
+		} \
+	} while (0)
+
+static struct handle_db hdb = HANDLE_DB_INITIALIZER;
+
+static void *lookup_handle(uint32_t type, uint32_t handle)
+{
+	void *ptr = NULL;
+
+	if ((handle & HT_MASK) == type)
+		ptr = handle_lookup(&hdb, handle & ~HT_MASK);
+
+	if (!ptr)
+		EMSG("Invalid handle 0x%" PRIx32, handle);
+
+	return ptr;
+}
+
+static bool get_handle(uint32_t type, void *ptr, uint32_t *handle)
+{
+	switch (type) {
+	case HT_BIGINT:
+	case HT_FMMCTX:
+	case HT_FMMVAR:
+		break;
+	default:
+		EMSG("Invalid type 0x%" PRIx32, type);
+		return false;
+	}
+
+	int h = handle_get(&hdb, ptr);
+
+	if (h < 0) {
+		EMSG("Failed to allocate handle");
+		return false;
+	}
+
+	*handle = (uint32_t)h | type;
+	return true;
+}
+
+static void *put_handle(uint32_t handle)
+{
+	void *ptr = handle_put(&hdb, handle & ~HT_MASK);
+
+	if (!ptr)
+		EMSG("Invalid handle 0x%" PRIx32, handle);
+
+	return ptr;
+}
+
+TEE_Result ta_entry_arith_new_var(uint32_t param_types,
+				  TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	size_t len = TEE_BigIntSizeInU32(params[0].value.a);
+	TEE_BigInt *big_int = TEE_Malloc(len * sizeof(TEE_BigInt), 0);
+
+	if (!big_int)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	if (!get_handle(HT_BIGINT, big_int, &params[1].value.a)) {
+		TEE_Free(big_int);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	TEE_BigIntInit(big_int, len);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_new_fmm_ctx(uint32_t param_types,
+				      TEE_Param params[TEE_NUM_PARAMS])
+{
+	TEE_BigInt *modulus = NULL;
+	uint32_t len = 0;
+	TEE_BigIntFMMContext *ctx = NULL;
+
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	modulus = lookup_handle(HT_BIGINT, params[0].value.b);
+	if (!modulus)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	len = TEE_BigIntFMMContextSizeInU32(params[0].value.a);
+	ctx = TEE_Malloc(len * sizeof(*ctx), 0);
+
+	if (!get_handle(HT_FMMCTX, ctx, &params[1].value.a)) {
+		TEE_Free(ctx);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	TEE_BigIntInitFMMContext(ctx, len, modulus);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_new_fmm_var(uint32_t param_types,
+				      TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	size_t len = TEE_BigIntFMMSizeInU32(params[0].value.a);
+	TEE_BigIntFMM *fmm = TEE_Malloc(len * sizeof(*fmm), 0);
+
+	if (!fmm)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	if (!get_handle(HT_FMMVAR, fmm, &params[1].value.a)) {
+		TEE_Free(fmm);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	TEE_BigIntInitFMM(fmm, len);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_free_handle(uint32_t param_types,
+				      TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, NONE, NONE, NONE);
+
+	void *ptr = put_handle(params[0].value.a & ~HT_MASK);
+
+	if (!ptr)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TEE_Free(ptr);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_from_octet_string(uint32_t param_types,
+					    TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, MEMREF_INPUT, NONE, NONE);
+
+	TEE_BigInt *big_int = lookup_handle(HT_BIGINT, params[0].value.a);
+
+	if (!big_int)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return TEE_BigIntConvertFromOctetString(big_int,
+						params[1].memref.buffer,
+						params[1].memref.size,
+						params[0].value.b);
+}
+
+TEE_Result ta_entry_arith_from_s32(uint32_t param_types,
+				   TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, NONE, NONE, NONE);
+
+	TEE_BigInt *big_int = lookup_handle(HT_BIGINT, params[0].value.a);
+
+	if (!big_int)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TEE_BigIntConvertFromS32(big_int, params[0].value.b);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_get_value(uint32_t param_types,
+				    TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, MEMREF_OUTPUT, NONE);
+
+	TEE_BigInt *big_int = lookup_handle(HT_BIGINT, params[0].value.a);
+
+	if (!big_int)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (TEE_BigIntCmpS32(big_int, 0) < 0)
+		params[1].value.a = -1;
+	else
+		params[1].value.a = 1;
+
+	return TEE_BigIntConvertToOctetString(params[2].memref.buffer,
+					      &params[2].memref.size, big_int);
+}
+
+TEE_Result ta_entry_arith_get_value_s32(uint32_t param_types,
+					TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	TEE_BigInt *big_int = lookup_handle(HT_BIGINT, params[0].value.a);
+
+	if (!big_int)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	int32_t v = 0;
+	TEE_Result res = TEE_BigIntConvertToS32(&v, big_int);
+
+	if (!res)
+		params[1].value.a = v;
+
+	return res;
+}
+
+TEE_Result ta_entry_arith_get_bit(uint32_t param_types,
+				  TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	TEE_BigInt *big_int = lookup_handle(HT_BIGINT, params[0].value.a);
+
+	if (!big_int)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	params[1].value.a = TEE_BigIntGetBit(big_int, params[0].value.b);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_get_bit_count(uint32_t param_types,
+					TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	TEE_BigInt *big_int = lookup_handle(HT_BIGINT, params[0].value.a);
+
+	if (!big_int)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	params[1].value.a = TEE_BigIntGetBitCount(big_int);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_shift_right(uint32_t param_types,
+				      TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_INPUT, NONE, NONE);
+
+	TEE_BigInt *op = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *dest = lookup_handle(HT_BIGINT, params[1].value.a);
+
+	if (!op || !dest)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TEE_BigIntShiftRight(dest, op, params[0].value.b);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_cmp(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	TEE_BigInt *op1 = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *op2 = lookup_handle(HT_BIGINT, params[0].value.b);
+
+	if (!op1 || !op2)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	params[1].value.a = TEE_BigIntCmp(op1, op2);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_cmp_s32(uint32_t param_types,
+				  TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	TEE_BigInt *op = lookup_handle(HT_BIGINT, params[0].value.a);
+
+	if (!op)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	params[1].value.a = TEE_BigIntCmpS32(op, params[0].value.b);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result ternary_func(uint32_t param_types,
+			       TEE_Param params[TEE_NUM_PARAMS],
+			       void (*func)(TEE_BigInt *dest,
+					    const TEE_BigInt *op1,
+					    const TEE_BigInt *op2,
+					    const TEE_BigInt *n))
+{
+	CHECK_PT(VALUE_INPUT, VALUE_INPUT, NONE, NONE);
+
+	TEE_BigInt *op1 = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *op2 = lookup_handle(HT_BIGINT, params[0].value.b);
+	TEE_BigInt *n = lookup_handle(HT_BIGINT, params[1].value.a);
+	TEE_BigInt *dest = lookup_handle(HT_BIGINT, params[1].value.b);
+
+	if (!op1 || !op2 || !n || !dest)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	func(dest, op1, op2, n);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result binary_func(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS],
+			      void (*func)(TEE_BigInt *dest,
+					   const TEE_BigInt *op1,
+					   const TEE_BigInt *op2))
+{
+	CHECK_PT(VALUE_INPUT, VALUE_INPUT, NONE, NONE);
+
+	TEE_BigInt *op1 = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *op2 = lookup_handle(HT_BIGINT, params[0].value.b);
+	TEE_BigInt *dest = lookup_handle(HT_BIGINT, params[1].value.a);
+
+	if (!op1 || !op2 || !dest)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	func(dest, op1, op2);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result unary_func(uint32_t param_types,
+			     TEE_Param params[TEE_NUM_PARAMS],
+			     void (*func)(TEE_BigInt *dest,
+					  const TEE_BigInt *op))
+{
+	CHECK_PT(VALUE_INPUT, NONE, NONE, NONE);
+
+	TEE_BigInt *op = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *dest = lookup_handle(HT_BIGINT, params[0].value.b);
+
+	if (!op || !dest)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	func(dest, op);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_add(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	return binary_func(param_types, params,  TEE_BigIntAdd);
+}
+
+TEE_Result ta_entry_arith_sub(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	return binary_func(param_types, params,  TEE_BigIntSub);
+}
+
+TEE_Result ta_entry_arith_mul(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	return binary_func(param_types, params,  TEE_BigIntMul);
+}
+
+TEE_Result ta_entry_arith_neg(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	return unary_func(param_types, params, TEE_BigIntNeg);
+}
+
+TEE_Result ta_entry_arith_sqr(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	return unary_func(param_types, params, TEE_BigIntSquare);
+}
+
+TEE_Result ta_entry_arith_div(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_INPUT, NONE, NONE);
+
+	TEE_BigInt *op1 = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *op2 = lookup_handle(HT_BIGINT, params[0].value.b);
+	TEE_BigInt *dest_q = lookup_handle(HT_BIGINT, params[1].value.a);
+	TEE_BigInt *dest_r = lookup_handle(HT_BIGINT, params[1].value.b);
+
+	if (!op1 || !op2 || !dest_q || !dest_r)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TEE_BigIntDiv(dest_q, dest_r, op1, op2);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_mod(uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	return binary_func(param_types, params, TEE_BigIntMod);
+}
+
+TEE_Result ta_entry_arith_addmod(uint32_t param_types,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	return ternary_func(param_types, params, TEE_BigIntAddMod);
+}
+
+TEE_Result ta_entry_arith_submod(uint32_t param_types,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	return ternary_func(param_types, params, TEE_BigIntSubMod);
+}
+
+TEE_Result ta_entry_arith_mulmod(uint32_t param_types,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	return ternary_func(param_types, params, TEE_BigIntMulMod);
+}
+
+TEE_Result ta_entry_arith_sqrmod(uint32_t param_types,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	return binary_func(param_types, params, TEE_BigIntSquareMod);
+}
+
+TEE_Result ta_entry_arith_invmod(uint32_t param_types,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	return binary_func(param_types, params, TEE_BigIntInvMod);
+}
+
+TEE_Result ta_entry_arith_is_rel_prime(uint32_t param_types,
+				       TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	TEE_BigInt *op1 = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *op2 = lookup_handle(HT_BIGINT, params[0].value.b);
+
+	if (!op1 || !op2)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	params[1].value.a = TEE_BigIntRelativePrime(op1, op2);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_compute_egcd(uint32_t param_types,
+				       TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_INPUT, VALUE_INPUT, NONE);
+
+	TEE_BigInt *op1 = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *op2 = lookup_handle(HT_BIGINT, params[0].value.b);
+	TEE_BigInt *dest_gcd = lookup_handle(HT_BIGINT, params[2].value.a);
+	TEE_BigInt *dest_u = NULL;
+	TEE_BigInt *dest_v = NULL;
+
+	if (!op1 || !op2 || !dest_gcd)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (params[1].value.a != TA_CRYPT_ARITH_INVALID_HANDLE) {
+		dest_u = lookup_handle(HT_BIGINT, params[1].value.a);
+		if (!dest_u)
+			return TEE_ERROR_BAD_PARAMETERS;
+	}
+	if (params[1].value.b != TA_CRYPT_ARITH_INVALID_HANDLE) {
+		dest_v = lookup_handle(HT_BIGINT, params[1].value.b);
+		if (!dest_v)
+			return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	TEE_BigIntComputeExtendedGcd(dest_gcd, dest_u, dest_v, op1, op2);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_is_prime(uint32_t param_types,
+				   TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_OUTPUT, NONE, NONE);
+
+	TEE_BigInt *op = lookup_handle(HT_BIGINT, params[0].value.a);
+
+	if (!op)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	params[1].value.a = TEE_BigIntIsProbablePrime(op, params[0].value.b);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_to_fmm(uint32_t param_types,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_INPUT, NONE, NONE);
+
+	TEE_BigInt *src = lookup_handle(HT_BIGINT, params[0].value.a);
+	TEE_BigInt *n = lookup_handle(HT_BIGINT, params[0].value.b);
+	TEE_BigIntFMMContext *ctx = lookup_handle(HT_FMMCTX, params[1].value.a);
+	TEE_BigIntFMM *dest = lookup_handle(HT_FMMVAR, params[1].value.b);
+
+	if (!src || !n | !ctx || !dest)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TEE_BigIntConvertToFMM(dest, src, n, ctx);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_from_fmm(uint32_t param_types,
+				   TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_INPUT, NONE, NONE);
+
+	TEE_BigIntFMM *src = lookup_handle(HT_FMMVAR, params[0].value.a);
+	TEE_BigInt *n = lookup_handle(HT_BIGINT, params[0].value.b);
+	TEE_BigIntFMMContext *ctx = lookup_handle(HT_FMMCTX, params[1].value.a);
+	TEE_BigInt *dest = lookup_handle(HT_BIGINT, params[1].value.b);
+
+	if (!src || !n | !ctx || !dest)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TEE_BigIntConvertFromFMM(dest, src, n, ctx);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result ta_entry_arith_compute_fmm(uint32_t param_types,
+				      TEE_Param params[TEE_NUM_PARAMS])
+{
+	CHECK_PT(VALUE_INPUT, VALUE_INPUT, VALUE_INPUT, NONE);
+
+	TEE_BigIntFMM *op1 = lookup_handle(HT_FMMVAR, params[0].value.a);
+	TEE_BigIntFMM *op2 = lookup_handle(HT_FMMVAR, params[0].value.b);
+	TEE_BigInt *n = lookup_handle(HT_BIGINT, params[1].value.a);
+	TEE_BigIntFMMContext *ctx = lookup_handle(HT_FMMCTX, params[1].value.b);
+	TEE_BigIntFMM *dest = lookup_handle(HT_FMMVAR, params[2].value.a);
+
+	if (!op1 || !op2 || !n | !ctx || !dest)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TEE_BigIntComputeFMM(dest, op1, op2, n, ctx);
+
+	return TEE_SUCCESS;
+}
diff --git a/ta/crypt/handle.c b/ta/crypt/handle.c
new file mode 100644
index 0000000..0ebb85a
--- /dev/null
+++ b/ta/crypt/handle.c
@@ -0,0 +1,79 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2014, Linaro Limited
+ */
+
+#include <tee_internal_api.h>
+#include "handle.h"
+
+/*
+ * Define the initial capacity of the database. It should be a low number
+ * multiple of 2 since some databases a likely to only use a few handles.
+ * Since the algorithm doubles up when growing it shouldn't cause a
+ * noticeable overhead on large databases.
+ */
+#define HANDLE_DB_INITIAL_MAX_PTRS	4
+
+void handle_db_destroy(struct handle_db *db)
+{
+	if (db) {
+		TEE_Free(db->ptrs);
+		db->ptrs = NULL;
+		db->max_ptrs = 0;
+	}
+}
+
+int handle_get(struct handle_db *db, void *ptr)
+{
+	size_t n;
+	void *p;
+	size_t new_max_ptrs;
+
+	if (!db || !ptr)
+		return -1;
+
+	/* Try to find an empty location */
+	for (n = 0; n < db->max_ptrs; n++) {
+		if (!db->ptrs[n]) {
+			db->ptrs[n] = ptr;
+			return n;
+		}
+	}
+
+	/* No location available, grow the ptrs array */
+	if (db->max_ptrs)
+		new_max_ptrs = db->max_ptrs * 2;
+	else
+		new_max_ptrs = HANDLE_DB_INITIAL_MAX_PTRS;
+	p = TEE_Realloc(db->ptrs, new_max_ptrs * sizeof(void *));
+	if (!p)
+		return -1;
+	db->ptrs = p;
+	TEE_MemFill(db->ptrs + db->max_ptrs, 0,
+		    (new_max_ptrs - db->max_ptrs) * sizeof(void *));
+	db->max_ptrs = new_max_ptrs;
+
+	/* Since n stopped at db->max_ptrs there is an empty location there */
+	db->ptrs[n] = ptr;
+	return n;
+}
+
+void *handle_put(struct handle_db *db, int handle)
+{
+	void *p;
+
+	if (!db || handle < 0 || (size_t)handle >= db->max_ptrs)
+		return NULL;
+
+	p = db->ptrs[handle];
+	db->ptrs[handle] = NULL;
+	return p;
+}
+
+void *handle_lookup(struct handle_db *db, int handle)
+{
+	if (!db || handle < 0 || (size_t)handle >= db->max_ptrs)
+		return NULL;
+
+	return db->ptrs[handle];
+}
diff --git a/ta/crypt/handle.h b/ta/crypt/handle.h
new file mode 100644
index 0000000..e3fad39
--- /dev/null
+++ b/ta/crypt/handle.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (c) 2014, Linaro Limited
+ */
+#ifndef __HANDLE_H
+#define __HANDLE_H
+
+#include <tee_internal_api.h>
+
+struct handle_db {
+	void **ptrs;
+	size_t max_ptrs;
+};
+
+#define HANDLE_DB_INITIALIZER	{ NULL, 0 }
+
+/*
+ * Frees all internal data structures of the database, but does not free
+ * the db pointer. The database is safe to reuse after it's destroyed, it
+ * will just be empty again.
+ */
+void handle_db_destroy(struct handle_db *db);
+
+/*
+ * Allocates a new handle and assigns the supplied pointer to it,
+ * ptr must not be NULL.
+ * The function returns
+ * >= 0 on success and
+ * -1 on failure
+ */
+int handle_get(struct handle_db *db, void *ptr);
+
+/*
+ * Deallocates a handle. Returns the assiciated pointer of the handle
+ * the the handle was valid or NULL if it's invalid.
+ */
+void *handle_put(struct handle_db *db, int handle);
+
+/*
+ * Returns the assiciated pointer of the handle if the handle is a valid
+ * handle.
+ * Returns NULL on failure.
+ */
+void *handle_lookup(struct handle_db *db, int handle);
+
+#endif /*__HANDLE_H*/
diff --git a/ta/crypt/include/arith_taf.h b/ta/crypt/include/arith_taf.h
new file mode 100644
index 0000000..048a1c2
--- /dev/null
+++ b/ta/crypt/include/arith_taf.h
@@ -0,0 +1,72 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/* Copyright (c) 2018, Linaro Limited */
+
+#ifndef __ARITH_TAF_H
+#define __ARITH_TAF_H
+
+#include <tee_internal_api.h>
+
+TEE_Result ta_entry_arith_new_var(uint32_t param_type,
+				  TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_new_fmm_ctx(uint32_t param_type,
+				      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_new_fmm_var(uint32_t param_type,
+				      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_free_handle(uint32_t param_type,
+				      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_from_octet_string(uint32_t param_type,
+					    TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_from_s32(uint32_t param_type,
+				   TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_get_value(uint32_t param_type,
+				    TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_get_value_s32(uint32_t param_type,
+					TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_get_bit(uint32_t param_type,
+				  TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_get_bit_count(uint32_t param_type,
+					TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_shift_right(uint32_t param_type,
+				      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_cmp(uint32_t param_type,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_cmp_s32(uint32_t param_type,
+				  TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_add(uint32_t param_type,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_sub(uint32_t param_type,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_mul(uint32_t param_type,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_neg(uint32_t param_type,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_sqr(uint32_t param_type,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_div(uint32_t param_type,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_mod(uint32_t param_type,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_addmod(uint32_t param_type,
+				 TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_submod(uint32_t param_type,
+				 TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_mulmod(uint32_t param_type,
+				 TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_sqrmod(uint32_t param_type,
+				 TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_invmod(uint32_t param_type,
+				 TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_is_rel_prime(uint32_t param_type,
+				       TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_compute_egcd(uint32_t param_type,
+				       TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_is_prime(uint32_t param_type,
+				   TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_to_fmm(uint32_t param_type,
+				 TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_from_fmm(uint32_t param_type,
+				   TEE_Param params[TEE_NUM_PARAMS]);
+TEE_Result ta_entry_arith_compute_fmm(uint32_t param_type,
+				      TEE_Param params[TEE_NUM_PARAMS]);
+
+#endif /*__ARITH_TAF_H*/
diff --git a/ta/crypt/include/ta_crypt.h b/ta/crypt/include/ta_crypt.h
index 17288e1..43bea67 100644
--- a/ta/crypt/include/ta_crypt.h
+++ b/ta/crypt/include/ta_crypt.h
@@ -382,4 +382,233 @@
  * system pTA is used for adding entropy to RNG pool */
 #define TA_CRYPT_CMD_SEED_RNG_POOL 45
 
+/*
+ * Testing arithmetical interface.
+ *
+ * Coding of signed 32-bit values:
+ * a int32_t with its bit pattern stored in a 32-bit value
+ */
+
+#define TA_CRYPT_ARITH_INVALID_HANDLE	0xffffffff
+
+/*
+ * in	params[0].value.a:	Number of bits
+ * out	params[1].value.b:	Handle to bignum variable
+ */
+#define TA_CRYPT_CMD_ARITH_NEW_VAR		46
+
+/*
+ * in	params[0].value.a:	Number of bits
+ * in	params[0].value.b:	Handle to bignum variable modulus
+ * out	params[1].value.a:	Handle to FMM context
+ */
+#define TA_CRYPT_CMD_ARITH_NEW_FMM_CTX		47
+
+/*
+ * in	params[0].value.a:	Number of bits
+ * out	params[1].value.a:	Handle to FMM variable
+ */
+#define TA_CRYPT_CMD_ARITH_NEW_FMM_VAR		48
+
+/*
+ * in	params[0].value.a:	Handle to bignum variable, FMM context, or
+ *				FMM variable
+ */
+#define TA_CRYPT_CMD_ARITH_FREE_HANDLE		49
+
+
+/*
+ * in	params[0].value.a:	Handle to bignum variable
+ * in	params[0].value.b:	S32 representing the sign of the value
+ * in	params[1].memref:	octet string representing the value
+ */
+#define TA_CRYPT_CMD_ARITH_FROM_OCTET_STRING	50
+
+/*
+ * in	params[0].value.a:	Handle to bignum variable
+ * in	params[0].value.b:	S32 representing the value
+ */
+#define TA_CRYPT_CMD_ARITH_FROM_S32		51
+
+/*
+ * in	params[0].value.a:	Handle to bignum variable
+ * out	params[1].value.a:	S32 representing the sign of the value
+ * out	params[2].memref:	octet string representing the value
+ */
+#define TA_CRYPT_CMD_ARITH_GET_VALUE		52
+
+/*
+ * in	params[0].value.a:	Handle to bignum variable
+ * out	params[1].value.a:	S32 the value
+ */
+#define TA_CRYPT_CMD_ARITH_GET_VALUE_S32	53
+
+/*
+ * in	params[0].value.a:	Handle to bignum variable
+ * in	params[0].value.b:	Bit number
+ * out	params[1].value.a:	Bit value
+ */
+#define TA_CRYPT_CMD_ARITH_GET_BIT		54
+
+/*
+ * in	params[0].value.a:	Handle to bignum variable
+ * out	params[1].value.a:	Bit count
+ */
+#define TA_CRYPT_CMD_ARITH_GET_BIT_COUNT	55
+
+/*
+ * in	params[0].value.a:	handle op
+ * in	params[0].value.b:	number of bits
+ * in	params[1].value.a:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_SHIFT_RIGHT		56
+
+/*
+ * in	params[0].value.a:	handle op1
+ * in	params[0].value.b:	handle op2
+ * out	params[1].value.a:	result
+ */
+#define TA_CRYPT_CMD_ARITH_CMP			57
+
+/*
+ * in	params[0].value.a:	handle op
+ * in	params[0].value.b:	S32 shortVal
+ * out	params[1].value.a:	result
+ */
+#define TA_CRYPT_CMD_ARITH_CMP_S32		58
+
+/*
+ * in	params[0].value.a:	handle a
+ * in	params[0].value.b:	handle b
+ * in	params[1].value.a:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_ADD			59
+
+/*
+ * in	params[0].value.a:	handle a
+ * in	params[0].value.b:	handle b
+ * in	params[1].value.a:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_SUB			60
+
+/*
+ * in	params[0].value.a:	handle a
+ * in	params[0].value.b:	handle b
+ * in	params[1].value.a:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_MUL			61
+
+/*
+ * in	params[0].value.a:	handle a
+ * in	params[0].value.b:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_NEG			62
+
+/*
+ * in	params[0].value.a:	handle a
+ * in	params[0].value.b:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_SQR			63
+
+/*
+ * in	params[0].value.a:	handle op1
+ * in	params[0].value.b:	handle op2
+ * in	params[1].value.a:	handle result Q
+ * in	params[1].value.b:	handle result R
+ */
+#define TA_CRYPT_CMD_ARITH_DIV			64
+
+/*
+ * in	params[0].value.a:	handle op
+ * in	params[0].value.b:	handle n
+ * in	params[1].value.a:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_MOD			65
+
+/*
+ * in	params[0].value.a:	handle op1
+ * in	params[0].value.b:	handle op2
+ * in	params[1].value.a:	handle n
+ * in	params[1].value.b:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_ADDMOD		66
+
+/*
+ * in	params[0].value.a:	handle op1
+ * in	params[0].value.b:	handle op2
+ * in	params[1].value.a:	handle n
+ * in	params[1].value.b:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_SUBMOD		67
+
+/*
+ * in	params[0].value.a:	handle op1
+ * in	params[0].value.b:	handle op2
+ * in	params[1].value.a:	handle n
+ * in	params[1].value.b:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_MULMOD		68
+
+/*
+ * in	params[0].value.a:	handle op
+ * in	params[0].value.b:	handle n
+ * in	params[1].value.a:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_SQRMOD		69
+
+/*
+ * in	params[0].value.a:	handle op
+ * in	params[0].value.b:	handle n
+ * in	params[1].value.a:	handle result
+ */
+#define TA_CRYPT_CMD_ARITH_INVMOD		70
+
+/*
+ * in	params[0].value.a:	handle op
+ * in	params[0].value.b:	handle n
+ * in	params[1].value.a:	bool result
+ */
+#define TA_CRYPT_CMD_ARITH_IS_RELATIVE_PRIME	71
+
+/*
+ * in	params[0].value.a:	handle op1
+ * in	params[0].value.b:	handle op2
+ * in	params[1].value.a:	handle result u
+ * in	params[1].value.b:	handle result v
+ * in	params[2].value.a:	handle result gcd
+ */
+#define TA_CRYPT_CMD_ARITH_COMPUTE_EGCD		72
+
+/*
+ * in	params[0].value.a:	handle op
+ * in	params[0].value.b:	confidence level
+ * out	params[1].value.a:	S32 result
+ */
+#define TA_CRYPT_CMD_ARITH_IS_PRIME		73
+
+/*
+ * in	params[0].value.a:	handle src
+ * in	params[0].value.b:	handle n
+ * in	params[1].value.a:	handle FMM context
+ * in	params[1].value.b:	handle result FMM variable
+ */
+#define TA_CRYPT_CMD_ARITH_TO_FMM		74
+
+/*
+ * in	params[0].value.a:	handle FMM src
+ * in	params[0].value.b:	handle bigint n
+ * in	params[1].value.a:	handle FMM context
+ * in	params[1].value.b:	handle result bigint
+ */
+#define TA_CRYPT_CMD_ARITH_FROM_FMM		75
+
+/*
+ * in	params[0].value.a:	handle FMM op1
+ * in	params[0].value.b:	handle FMM op2
+ * in	params[1].value.a:	handle bigint n
+ * in	params[1].value.b:	handle FMM context
+ * in	params[2].value.a:	handle FMM result
+ */
+#define TA_CRYPT_CMD_ARITH_COMPUTE_FMM		76
+
 #endif /*TA_CRYPT_H */
diff --git a/ta/crypt/sub.mk b/ta/crypt/sub.mk
index e9fb0a6..dcdde55 100644
--- a/ta/crypt/sub.mk
+++ b/ta/crypt/sub.mk
@@ -7,6 +7,8 @@
 srcs-$(CFG_SYSTEM_PTA) += seed_rng_taf.c
 srcs-y += ta_entry.c
 srcs-$(CFG_TA_MBEDTLS) += mbedtls_taf.c
+srcs-y += arith_taf.c
+srcs-y += handle.c
 
 CRYPT_FILE_TO_C_SCRIPT = ../../scripts/file_to_c.py
 CRYPT_CA_CRT = ../../certs/ca.crt
diff --git a/ta/crypt/ta_entry.c b/ta/crypt/ta_entry.c
index 6d1a3cb..acb1aee 100644
--- a/ta/crypt/ta_entry.c
+++ b/ta/crypt/ta_entry.c
@@ -26,6 +26,7 @@
  */
 
 #include <aes_taf.h>
+#include <arith_taf.h>
 #include <cryp_taf.h>
 #include <mbedtls_taf.h>
 #include <seed_rng_taf.h>
@@ -234,6 +235,69 @@
 	case TA_CRYPT_CMD_SEED_RNG_POOL:
 		return seed_rng_pool(nParamTypes, pParams);
 #endif
+	case TA_CRYPT_CMD_ARITH_NEW_VAR:
+		return ta_entry_arith_new_var(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_NEW_FMM_CTX:
+		return ta_entry_arith_new_fmm_ctx(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_NEW_FMM_VAR:
+		return ta_entry_arith_new_fmm_var(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_FREE_HANDLE:
+		return ta_entry_arith_free_handle(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_FROM_OCTET_STRING:
+		return ta_entry_arith_from_octet_string(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_FROM_S32:
+		return ta_entry_arith_from_s32(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_GET_VALUE:
+		return ta_entry_arith_get_value(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_GET_VALUE_S32:
+		return ta_entry_arith_get_value_s32(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_GET_BIT:
+		return ta_entry_arith_get_bit(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_GET_BIT_COUNT:
+		return ta_entry_arith_get_bit_count(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_SHIFT_RIGHT:
+		return ta_entry_arith_shift_right(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_CMP:
+		return ta_entry_arith_cmp(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_CMP_S32:
+		return ta_entry_arith_cmp_s32(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_ADD:
+		return ta_entry_arith_add(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_SUB:
+		return ta_entry_arith_sub(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_MUL:
+		return ta_entry_arith_mul(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_NEG:
+		return ta_entry_arith_neg(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_SQR:
+		return ta_entry_arith_sqr(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_DIV:
+		return ta_entry_arith_div(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_MOD:
+		return ta_entry_arith_mod(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_ADDMOD:
+		return ta_entry_arith_addmod(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_SUBMOD:
+		return ta_entry_arith_submod(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_MULMOD:
+		return ta_entry_arith_mulmod(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_SQRMOD:
+		return ta_entry_arith_sqrmod(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_INVMOD:
+		return ta_entry_arith_invmod(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_IS_RELATIVE_PRIME:
+		return ta_entry_arith_is_rel_prime(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_COMPUTE_EGCD:
+		return ta_entry_arith_compute_egcd(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_IS_PRIME:
+		return ta_entry_arith_is_prime(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_TO_FMM:
+		return ta_entry_arith_to_fmm(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_FROM_FMM:
+		return ta_entry_arith_from_fmm(nParamTypes, pParams);
+	case TA_CRYPT_CMD_ARITH_COMPUTE_FMM:
+		return ta_entry_arith_compute_fmm(nParamTypes, pParams);
+
 	default:
 		return TEE_ERROR_BAD_PARAMETERS;
 	}