diff --git a/demo/fw/src/cage.c b/demo/fw/src/cage.c
index 5452ac0..a741298 100644
--- a/demo/fw/src/cage.c
+++ b/demo/fw/src/cage.c
@@ -21,6 +21,19 @@
 #include "con_usart.h"
 #define LOG_PRINTF con_printf
 
+#define MBEDTLS_CHECK_MSG( fun_call, err_value, msg, ... ) \
+    do { \
+        int mbedtls_rc = fun_call; \
+        if (mbedtls_rc) { \
+            LOG_PRINTF("Error: " #fun_call ": " msg ": rc=-0x%x\n", __VA_ARGS__ -mbedtls_rc); \
+            err = err_value; \
+            goto cleanup; \
+        } \
+    } while (false);
+
+#define MBEDTLS_CHECK_VAL(fun_call, err_value) MBEDTLS_CHECK_MSG(fun_call, err_value, "")
+#define MBEDTLS_CHECK(fun_call) MBEDTLS_CHECK_VAL(fun_call, CA_ERR_MBEDTLS_ERROR)
+
 
 static enum ca_error parse_stanza(struct ca_keystore *ks, const char *stanza_head, size_t len, unsigned char file_key[16]);
 static enum ca_error parse_stanza_x25519(struct ca_keystore *ks, size_t nargs, const char **args, size_t body_len, const unsigned char *body, unsigned char file_key[16]);
@@ -35,7 +48,7 @@ void ca_keystore_free(struct ca_keystore *ks) {
 }
 
 enum ca_error ca_keystore_load_x25519_private_key(struct ca_keystore *ks, const unsigned char buf[32]) {
-
+    enum ca_error err = CA_ERR_CORRUPTED_STATE;
     /*
     printf("x25519: private key: ");
     for (size_t i=0; i<32; i++) {
@@ -44,22 +57,17 @@ enum ca_error ca_keystore_load_x25519_private_key(struct ca_keystore *ks, const
     printf("\n");
     */
 
-    int mbedtls_rc = mbedtls_ecp_read_key(MBEDTLS_ECP_DP_CURVE25519, &(ks->x25519_kp), buf, 32);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_ecp_read_key: rc=-0x%x\n", -mbedtls_rc);
-        return CA_ERR_MBEDTLS_ERROR;
-    }
+    MBEDTLS_CHECK( mbedtls_ecp_read_key(MBEDTLS_ECP_DP_CURVE25519, &(ks->x25519_kp), buf, 32) );
 
     /* Reconstruct public key from private key */
-    mbedtls_rc = mbedtls_ecp_mul(&(ks->x25519_kp.grp), &(ks->x25519_kp.Q),
+    MBEDTLS_CHECK( mbedtls_ecp_mul(&(ks->x25519_kp.grp), &(ks->x25519_kp.Q),
              &(ks->x25519_kp.d), &(ks->x25519_kp.grp.G),
-             mbedtls_ctr_drbg_random, g_drbg_ctx);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_ecp_read_key: rc=-0x%x\n", -mbedtls_rc);
-        return CA_ERR_MBEDTLS_ERROR;
-    }
-    
-    return CA_ERR_SUCCESS;
+             mbedtls_ctr_drbg_random, g_drbg_ctx) );
+
+    err = CA_ERR_SUCCESS;
+
+cleanup:
+    return err;
 }
 
 enum ca_error parse_age_buf(struct ca_keystore *ks, const char *buf, size_t buflen, unsigned char file_key[16]) {
@@ -238,7 +246,6 @@ errout:
 }
 
 enum ca_error parse_stanza_x25519(struct ca_keystore *ks, size_t nargs, const char **args, size_t body_len, const unsigned char *body, unsigned char file_key[16]) {
-    int mbedtls_rc = -1;
     enum ca_error err = CA_ERR_CORRUPTED_STATE;
 
     /* body length: 16 bytes ciphertext of 128 bit file key + 16 bytes Poly1305 tag */
@@ -270,19 +277,17 @@ enum ca_error parse_stanza_x25519(struct ca_keystore *ks, size_t nargs, const ch
     mbedtls_ecp_point_init(&ecp_ciphertext_pt);
     mbedtls_ecp_group x25519_grp;
     mbedtls_ecp_group_init(&x25519_grp);
-    mbedtls_rc = mbedtls_ecp_group_load(&x25519_grp, MBEDTLS_ECP_DP_CURVE25519);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_ecp_group_load: rc=-0x%x\n", -mbedtls_rc);
-        err = CA_ERR_KEY_WRAPPING_DECRYPTION_FAILURE;
-        goto err_free_grp;
-    }
+    mbedtls_ecp_point ecp_dec_res_pt;
+    mbedtls_ecp_point_init(&ecp_dec_res_pt);
+    mbedtls_chachapoly_context cp_ctx;
+    mbedtls_chachapoly_init(&cp_ctx);
 
-    mbedtls_rc = mbedtls_ecp_point_read_binary(&x25519_grp, &ecp_ciphertext_pt, ecp_ciphertext, sizeof(ecp_ciphertext));
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_ecp_point_read_binary: rc=-0x%x\n", -mbedtls_rc);
-        err = CA_ERR_KEY_WRAPPING_DECRYPTION_FAILURE;
-        goto err_free_grp;
-    }
+    MBEDTLS_CHECK_VAL( mbedtls_ecp_group_load(&x25519_grp, MBEDTLS_ECP_DP_CURVE25519),
+            CA_ERR_KEY_WRAPPING_DECRYPTION_FAILURE );
+
+    MBEDTLS_CHECK_VAL( mbedtls_ecp_point_read_binary(&x25519_grp,
+                &ecp_ciphertext_pt, ecp_ciphertext, sizeof(ecp_ciphertext)),
+        CA_ERR_KEY_WRAPPING_DECRYPTION_FAILURE );
 
     /*
     printf("x25519: derived_secret: ");
@@ -292,23 +297,16 @@ enum ca_error parse_stanza_x25519(struct ca_keystore *ks, size_t nargs, const ch
     printf("\n");
     */
 
-    mbedtls_ecp_point ecp_dec_res_pt;
-    mbedtls_ecp_point_init(&ecp_dec_res_pt);
-
-    mbedtls_rc = mbedtls_ecp_mul(&x25519_grp, &ecp_dec_res_pt, &(ks->x25519_kp.d), &ecp_ciphertext_pt,
-             mbedtls_ctr_drbg_random, g_drbg_ctx);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_ecp_mul: rc=-0x%x\n", -mbedtls_rc);
-        err = CA_ERR_MBEDTLS_ERROR;
-        goto err_free_dec;
-    }
+    MBEDTLS_CHECK( mbedtls_ecp_mul(&x25519_grp, &ecp_dec_res_pt,
+                &(ks->x25519_kp.d), &ecp_ciphertext_pt,
+             mbedtls_ctr_drbg_random, g_drbg_ctx) );
 
 
     unsigned char ecp_plaintext_buf[32];
     size_t ecp_plaintext_len = 0;
-    mbedtls_rc = mbedtls_ecp_point_write_binary(&x25519_grp, &ecp_dec_res_pt,
+    MBEDTLS_CHECK( mbedtls_ecp_point_write_binary(&x25519_grp, &ecp_dec_res_pt,
                                     MBEDTLS_ECP_PF_UNCOMPRESSED, &ecp_plaintext_len,
-                                    ecp_plaintext_buf, sizeof(ecp_plaintext_buf));
+                                    ecp_plaintext_buf, sizeof(ecp_plaintext_buf)) );
 
     /*
     printf("x25519: key_material: ");
@@ -318,39 +316,28 @@ enum ca_error parse_stanza_x25519(struct ca_keystore *ks, size_t nargs, const ch
     printf("\n");
     */
 
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_ecp_point_write_binary: rc=-0x%x\n", -mbedtls_rc);
-        err = CA_ERR_MBEDTLS_ERROR;
-        goto err_free_dec;
-    }
-
     if (ecp_plaintext_len != 32) {
         LOG_PRINTF("Error: parse_stanza_x25519: (ecp_plaintext_len == %d) != 32 (bug, should not happen!)\n", ecp_plaintext_len);
         err = CA_ERR_CORRUPTED_STATE;
-        goto err_free_dec;
+        goto cleanup;
     }
 
     const mbedtls_md_info_t *sha256_md = mbedtls_md_info_from_type(MBEDTLS_MD_SHA256);
     if (!sha256_md) {
         LOG_PRINTF("Error: mbedtls_md_info_from_type: cannot load SHA256\n");
         err = CA_ERR_MBEDTLS_ERROR;
-        goto err_free_dec;
+        goto cleanup;
     }
 
     unsigned char salt[64];
     memcpy(salt, ecp_ciphertext, 32);
     size_t pbin_olen = 0;
-    mbedtls_rc = mbedtls_ecp_point_write_binary(&x25519_grp, &(ks->x25519_kp.Q),
-                                    MBEDTLS_ECP_PF_UNCOMPRESSED, &pbin_olen, salt+32, 32);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_ecp_point_write_binary: rc=-0x%x\n", -mbedtls_rc);
-        err = CA_ERR_MBEDTLS_ERROR;
-        goto err_free_dec;
-    }
+    MBEDTLS_CHECK( mbedtls_ecp_point_write_binary(&x25519_grp, &(ks->x25519_kp.Q),
+                                    MBEDTLS_ECP_PF_UNCOMPRESSED, &pbin_olen, salt+32, 32) );
     if (pbin_olen != 32) {
         LOG_PRINTF("Error: mbedtls_ecp_point_write_binary returned invalid output len: %d != 32\n", pbin_olen);
         err = CA_ERR_CORRUPTED_STATE;
-        goto err_free_dec;
+        goto cleanup;
     }
 
     /*
@@ -363,14 +350,10 @@ enum ca_error parse_stanza_x25519(struct ca_keystore *ks, size_t nargs, const ch
 
     const char *info_str = "age-encryption.org/v1/X25519";
     unsigned char wrap_key[32];
-    mbedtls_rc = mbedtls_hkdf(sha256_md, salt, sizeof(salt),
+    MBEDTLS_CHECK( mbedtls_hkdf(sha256_md, salt, sizeof(salt),
                   ecp_plaintext_buf, sizeof(ecp_plaintext_buf),
                   (const unsigned char *)info_str, strlen(info_str),
-                  wrap_key, sizeof(wrap_key));
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_hkdf: rc=-0x%x\n", -mbedtls_rc);
-        return CA_ERR_MBEDTLS_ERROR;
-    }
+                  wrap_key, sizeof(wrap_key)) );
 
     /*
     printf("x25519: key: ");
@@ -380,40 +363,24 @@ enum ca_error parse_stanza_x25519(struct ca_keystore *ks, size_t nargs, const ch
     printf("\n");
     */
 
-    mbedtls_chachapoly_context cp_ctx;
-    mbedtls_chachapoly_init(&cp_ctx);
-    mbedtls_rc = mbedtls_chachapoly_setkey(&cp_ctx, wrap_key);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_chachapoly_setkey: rc=-0x%x\n", -mbedtls_rc);
-        err = CA_ERR_MBEDTLS_ERROR;
-        goto err_free_cp;
-    }
+    MBEDTLS_CHECK( mbedtls_chachapoly_setkey(&cp_ctx, wrap_key) );
 
     const unsigned char nonce[12] = { 0 };
-    mbedtls_rc = mbedtls_chachapoly_auth_decrypt(&cp_ctx, 16, nonce, NULL, 0, (const unsigned char*)(body + body_len - 16), body, file_key);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_chachapoly_auth_decrypt: rc=-0x%x\n", -mbedtls_rc);
-        err = CA_ERR_MBEDTLS_ERROR;
-        goto err_free_cp;
-    }
+    MBEDTLS_CHECK( mbedtls_chachapoly_auth_decrypt(&cp_ctx, 16, nonce,
+                NULL, 0, (const unsigned char*)(body + body_len - 16), body, file_key) );
 
     err = CA_ERR_SUCCESS;
 
-err_free_cp:
+cleanup:
     mbedtls_chachapoly_free(&cp_ctx);
-
-err_free_dec:
     mbedtls_ecp_point_free(&ecp_dec_res_pt);
-
-err_free_grp:
     mbedtls_ecp_group_free(&x25519_grp);
     mbedtls_ecp_point_free(&ecp_ciphertext_pt);
-
     return err;
 }
 
 enum ca_error check_file_key(const unsigned char *buf, size_t buflen, const unsigned char file_key[16]) {
-    int mbedtls_rc;
+    enum ca_error err = CA_ERR_CORRUPTED_STATE;
 
     const mbedtls_md_info_t *sha256_md = mbedtls_md_info_from_type(MBEDTLS_MD_SHA256);
     if (!sha256_md) {
@@ -430,23 +397,15 @@ enum ca_error check_file_key(const unsigned char *buf, size_t buflen, const unsi
     size_t header_len = found - (const char *)buf;
 
     unsigned char check_value[32];
-    mbedtls_rc = mbedtls_hkdf(sha256_md, (const unsigned char *)"", 0,
+    MBEDTLS_CHECK( mbedtls_hkdf(sha256_md, (const unsigned char *)"", 0,
                   file_key, 16,
                   (const unsigned char *)"header", strlen("header"),
-                  check_value, sizeof(check_value));
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_hkdf: rc=-0x%x\n", -mbedtls_rc);
-        return CA_ERR_MBEDTLS_ERROR;
-    }
+                  check_value, sizeof(check_value)) );
 
     unsigned char hmac_calculated[32];
-    mbedtls_rc = mbedtls_md_hmac(sha256_md, check_value, sizeof(check_value),
+    MBEDTLS_CHECK( mbedtls_md_hmac(sha256_md, check_value, sizeof(check_value),
                 buf, header_len,
-                hmac_calculated);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_md_hmac: rc=-0x%x\n", -mbedtls_rc);
-        return CA_ERR_MBEDTLS_ERROR;
-    }
+                hmac_calculated) );
 
     unsigned char mac[32];
     size_t mac_osize = 0;
@@ -465,7 +424,10 @@ enum ca_error check_file_key(const unsigned char *buf, size_t buflen, const unsi
         return CA_ERR_MAC_MISMATCH;
     }
 
-    return CA_ERR_SUCCESS;
+    err = CA_ERR_SUCCESS;
+
+cleanup:
+    return err;
 }
 
 enum ca_error stream_decrypt(unsigned char *out, size_t outlen, size_t *out_written, const unsigned char *in, size_t inlen, const unsigned char file_key[16]) {
@@ -501,6 +463,9 @@ enum ca_error stream_decrypt(unsigned char *out, size_t outlen, size_t *out_writ
         return CA_ERR_NOT_ENOUGH_SPACE;
     }
 
+    mbedtls_chachapoly_context cp_ctx;
+    mbedtls_chachapoly_init(&cp_ctx);
+
     /*
     printf("file key: ");
     for (size_t i=0; i<16; i++) {
@@ -511,7 +476,7 @@ enum ca_error stream_decrypt(unsigned char *out, size_t outlen, size_t *out_writ
 
     const mbedtls_md_info_t *sha256_md = mbedtls_md_info_from_type(MBEDTLS_MD_SHA256);
     if (!sha256_md) {
-        LOG_PRINTF("Error: mbedtls_md_info_from_type: cannot load SHA256\n");
+        LOG_PRINTF("Error: mbedtls_md_info_from_type: cannot load SHA256 MD info\n");
         return CA_ERR_MBEDTLS_ERROR;
     }
 
@@ -524,14 +489,10 @@ enum ca_error stream_decrypt(unsigned char *out, size_t outlen, size_t *out_writ
     */
 
     unsigned char stream_key[32];
-    int mbedtls_rc = mbedtls_hkdf(sha256_md, in, 16,
+    MBEDTLS_CHECK( mbedtls_hkdf(sha256_md, in, 16,
                   file_key, 16,
                   (const unsigned char *)"payload", strlen("payload"),
-                  stream_key, sizeof(stream_key));
-    if (!sha256_md) {
-        LOG_PRINTF("Error: mbedtls_md_info_from_type: cannot load SHA256\n");
-        return CA_ERR_MBEDTLS_ERROR;
-    }
+                  stream_key, sizeof(stream_key)) );
 
     /*
     printf("stream key: ");
@@ -541,14 +502,7 @@ enum ca_error stream_decrypt(unsigned char *out, size_t outlen, size_t *out_writ
     printf("\n");
     */
 
-    mbedtls_chachapoly_context cp_ctx;
-    mbedtls_chachapoly_init(&cp_ctx);
-    mbedtls_rc = mbedtls_chachapoly_setkey(&cp_ctx, stream_key);
-    if (mbedtls_rc) {
-        LOG_PRINTF("Error: mbedtls_chachapoly_setkey: rc=-0x%x\n", -mbedtls_rc);
-        err = CA_ERR_MBEDTLS_ERROR;
-        goto err_free_cp;
-    }
+    MBEDTLS_CHECK( mbedtls_chachapoly_setkey(&cp_ctx, stream_key) );
 
     in += 16;
     size_t block_num = 0;
@@ -578,13 +532,10 @@ enum ca_error stream_decrypt(unsigned char *out, size_t outlen, size_t *out_writ
         LOG_PRINTF("inlen=%zu block_len=%zu inp=%p@%zu outp=%p@%zu data_len=%zu tag=%p\n",
                 inlen, block_len, inp, i, outp, block_num*out_blocksize, data_len, tag);
         */
-        mbedtls_rc = mbedtls_chachapoly_auth_decrypt(&cp_ctx,
-                data_len, (unsigned char *)&nonce, NULL, 0, tag, inp, outp);
-        if (mbedtls_rc) {
-            LOG_PRINTF("Error: mbedtls_chachapoly_auth_decrypt: i=%zx rc=-0x%x\n", i, -mbedtls_rc);
-            err = CA_ERR_MBEDTLS_ERROR;
-            goto err_free_cp;
-        }
+        MBEDTLS_CHECK_MSG( mbedtls_chachapoly_auth_decrypt(&cp_ctx,
+                data_len, (unsigned char *)&nonce, NULL, 0, tag, inp, outp),
+                CA_ERR_MBEDTLS_ERROR,
+                "i=%zx", i );
 
         *out_written += data_len;
         block_num += 1;
@@ -592,7 +543,7 @@ enum ca_error stream_decrypt(unsigned char *out, size_t outlen, size_t *out_writ
 
     err = CA_ERR_SUCCESS;
 
-err_free_cp:
+cleanup:
     mbedtls_chachapoly_free(&cp_ctx);
 
     return err;