Add encryption function for the crypto_backend and px4_crypto

Take into use libtomcrypt library to add RSA and some other algorithms
for SW ecnryption use case

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

platforms/common/include/px4_platform_common/crypto_backend.h

@@ -96,6 +96,37 @@ crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm);
 
 void crypto_close(crypto_session_handle_t *handle);
 
+/*
+ * Get a key from keystore, possibly encrypted
+ *
+ * handle: an open crypto context; the returned key will be encrypted
+ *   according to this context
+ * key_idx: Index of the requested key in the keystore
+ * key: The provided buffer to the key
+ * max_len: the length of the provided key buffer. Returns the actual key size
+ * encryption_key_idx: The key index in keystore to be used for encrypting
+ * returns true on success, false on failure
+ */
+bool crypto_get_encrypted_key(crypto_session_handle_t handle,
+			      uint8_t key_idx,
+			      uint8_t *key,
+			      size_t *max_len,
+			      uint8_t encryption_key_idx);
+
+/*
+ * Get the generated nonce value
+ *
+ * handle: an open crypto context; the returned nonce is the one associsated
+ *   with this context/algorithm
+ * nonce: The provided buffer to the key. If NULL, only length is returned
+ * nonce_len: the length of the nonce value
+ * encryption_key_idx: The key index in keystore to be used for encrypting
+ * returns true on success, false on failure
+ */
+bool crypto_get_nonce(crypto_session_handle_t handle,
+		      uint8_t *nonce,
+		      size_t *nonce_len);
+
 /*
  * Perform signature check using an open session to crypto
  * handle: session handle, returned by open
@@ -110,6 +141,25 @@ bool crypto_signature_check(crypto_session_handle_t handle,
 			    const uint8_t *message,
 			    size_t message_size);
 
+bool crypto_encrypt_data(crypto_session_handle_t handle,
+			 uint8_t  key_index,
+			 const uint8_t *message,
+			 size_t message_size,
+			 uint8_t *cipher,
+			 size_t *cipher_size);
+
+/*
+ * Returns a minimum data block size on which the crypto operations can be
+ *   performed. Performing encryption on sizes which are not multiple of this
+ *   are valid, but the output will be padded to the multiple of this value
+ *   Input for decryption must be multiple of this value.
+ * handle: session handle, returned by open
+ * key_idx: key to be used for encryption/decryption
+ * returs the block size
+ */
+
+size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx);
+
 #if defined(__cplusplus)
 } // extern "C"
 #endif

platforms/common/px4_sw_crypto/sw_crypto/CMakeLists.txt

@@ -41,8 +41,9 @@ target_link_libraries(crypto_backend
 
 target_link_libraries(crypto_backend
 	PRIVATE
+		px4_random
 		monocypher
+		libtomcrypt
 )
 
 target_include_directories(crypto_backend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-

platforms/common/px4_sw_crypto/sw_crypto/crypto.c

@@ -43,6 +43,9 @@
 
 #include <px4_platform_common/crypto_backend.h>
 #include <lib/crypto/monocypher/src/optional/monocypher-ed25519.h>
+#include <tomcrypt.h>
+
+extern void libtomcrypt_init(void);
 
 /* room for 16 keys */
 #define KEY_CACHE_LEN 16
@@ -67,6 +70,11 @@ typedef struct {
 
 static volatile_key_t key_cache[KEY_CACHE_LEN];
 
+typedef struct {
+	uint8_t nonce[24];
+	uint64_t ctr;
+} chacha20_context_t;
+
 /* Clear key cache */
 static void clear_key_cache(void)
 {
@@ -126,19 +134,43 @@ void crypto_init()
 {
 	keystore_init();
 	clear_key_cache();
+	libtomcrypt_init();
 }
 
 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
 {
 	crypto_session_handle_t ret;
-	ret.algorithm = algorithm;
 	ret.keystore_handle = keystore_open();
 
 	if (keystore_session_handle_valid(ret.keystore_handle)) {
+		ret.algorithm = algorithm;
 		ret.handle = ++crypto_open_count;
 
 	} else {
 		ret.handle = 0;
+		ret.context = NULL;
+		ret.algorithm = CRYPTO_NONE;
+		return ret;
+	}
+
+	switch (algorithm) {
+	case CRYPTO_XCHACHA20: {
+			chacha20_context_t *context = malloc(sizeof(chacha20_context_t));
+
+			if (!context) {
+				ret.handle = 0;
+				crypto_open_count--;
+
+			} else {
+				ret.context = context;
+				px4_get_secure_random(context->nonce, sizeof(context->nonce));
+				context->ctr = 0;
+			}
+		}
+		break;
+
+	default:
+		ret.context = NULL;
 	}
 
 	return ret;
@@ -146,9 +178,17 @@ crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
 
 void crypto_close(crypto_session_handle_t *handle)
 {
+
+	// Not open?
+	if (!crypto_session_handle_valid(*handle)) {
+		return;
+	}
+
 	crypto_open_count--;
 	handle->handle = 0;
 	keystore_close(&handle->keystore_handle);
+	free(handle->context);
+	handle->context = NULL;
 }
 
 bool crypto_signature_check(crypto_session_handle_t handle,
@@ -159,13 +199,13 @@ bool crypto_signature_check(crypto_session_handle_t handle,
 {
 	bool ret = false;
 	size_t keylen = 0;
-	const uint8_t *public_key;
+	const uint8_t *public_key = NULL;
 
 	if (crypto_session_handle_valid(handle)) {
 		public_key = crypto_get_key_ptr(handle.keystore_handle, key_index, &keylen);
 	}
 
-	if (keylen == 0) {
+	if (keylen == 0 || public_key == NULL) {
 		return false;
 	}
 
@@ -180,3 +220,170 @@ bool crypto_signature_check(crypto_session_handle_t handle,
 
 	return ret;
 }
+
+bool crypto_encrypt_data(crypto_session_handle_t handle,
+			 uint8_t  key_idx,
+			 const uint8_t *message,
+			 size_t message_size,
+			 uint8_t *cipher,
+			 size_t *cipher_size)
+{
+
+	bool ret = false;
+
+	if (!crypto_session_handle_valid(handle)) {
+		return ret;
+	}
+
+	switch (handle.algorithm) {
+	case CRYPTO_NONE:
+		if (*cipher_size >= message_size) {
+			/* In-place there is no copy needed */
+			if (message != cipher) {
+				memcpy(cipher, message, message_size);
+			}
+
+			*cipher_size = message_size;
+			ret = true;
+		}
+
+		break;
+
+	case CRYPTO_XCHACHA20: {
+			size_t key_sz;
+			uint8_t *key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
+			chacha20_context_t *context = handle.context;
+
+			if (key_sz == 32) {
+				context->ctr = crypto_xchacha20_ctr(cipher, message, *cipher_size, key, context->nonce, context->ctr);
+				ret = true;
+			}
+		}
+		break;
+
+	case CRYPTO_RSA_OAEP: {
+			rsa_key key;
+			size_t key_sz;
+			unsigned long outlen = *cipher_size;
+			uint8_t *public_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
+			*cipher_size = 0;
+
+			if (public_key &&
+			    rsa_import(public_key, key_sz, &key) == CRYPT_OK) {
+				if (outlen >= ltc_mp.unsigned_size(key.N) &&
+				    pkcs_1_oaep_encode(message, message_size,
+						       NULL, 0,
+						       ltc_mp.count_bits(key.N),
+						       NULL,
+						       0, 0,
+						       cipher, &outlen) == CRYPT_OK &&
+				    ltc_mp.rsa_me(cipher, outlen, cipher, &outlen, PK_PUBLIC, &key) == CRYPT_OK) {
+					*cipher_size = outlen;
+					ret = true;
+
+				}
+
+				rsa_free(&key);
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+bool crypto_get_encrypted_key(crypto_session_handle_t handle,
+			      uint8_t key_idx,
+			      uint8_t *key,
+			      size_t *max_len,
+			      uint8_t encryption_key_idx)
+{
+	// Retrieve the plaintext key
+	bool ret = true;
+	size_t key_sz;
+	const uint8_t *plain_key = crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
+
+	if (key_sz == 0) {
+		return false;
+	}
+
+	// Encrypt it
+	if (key != NULL) {
+		ret = crypto_encrypt_data(handle,
+					  encryption_key_idx,
+					  plain_key,
+					  key_sz,
+					  key,
+					  max_len);
+
+	} else {
+		// The key size, encrypted, is a multiple of minimum block size for the algorithm+key
+		size_t min_block = crypto_get_min_blocksize(handle, encryption_key_idx);
+		*max_len = key_sz / min_block * min_block;
+
+		if (key_sz % min_block) {
+			*max_len += min_block;
+		}
+	}
+
+	return ret;
+}
+
+
+bool crypto_get_nonce(crypto_session_handle_t handle,
+		      uint8_t *nonce,
+		      size_t *nonce_len)
+{
+	switch (handle.algorithm) {
+	case CRYPTO_XCHACHA20: {
+			chacha20_context_t *context = handle.context;
+
+			if (nonce != NULL && context != NULL) {
+				memcpy(nonce, context->nonce, sizeof(context->nonce));
+			}
+
+			*nonce_len = sizeof(context->nonce);
+		}
+		break;
+
+	default:
+		*nonce_len = 0;
+	}
+
+	return true;
+}
+
+size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx)
+{
+	size_t ret;
+
+	switch (handle.algorithm) {
+	case CRYPTO_XCHACHA20:
+		ret = 64;
+		break;
+
+	case CRYPTO_RSA_OAEP: {
+			rsa_key enc_key;
+			unsigned pub_key_sz;
+			uint8_t *pub_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &pub_key_sz);
+
+			if (pub_key &&
+			    rsa_import(pub_key, pub_key_sz, &enc_key) == CRYPT_OK) {
+				ret = ltc_mp.unsigned_size(enc_key.N);
+				rsa_free(&enc_key);
+
+			} else {
+				ret = 0;
+			}
+		}
+		break;
+
+	default:
+		ret = 1;
+	}
+
+	return ret;
+}

platforms/common/px4_sw_crypto/sw_crypto/crypto_backend_definitions.h

@@ -39,9 +39,9 @@
 typedef struct {
 	int handle;
 	px4_crypto_algorithm_t algorithm;
-	uint8_t *nonce;
 	keystore_session_handle_t keystore_handle;
+	void *context;
 } crypto_session_handle_t;
 
 static inline void crypto_session_handle_init(crypto_session_handle_t *handle) {handle->handle = 0;}
-static inline bool crypto_session_handle_valid(crypto_session_handle_t handle) {return handle.handle > 0;}
+static inline bool crypto_session_handle_valid(crypto_session_handle_t handle) {return handle.handle > 0 && keystore_session_handle_valid(handle.keystore_handle);}