diff --git a/src/btstack_crypto.c b/src/btstack_crypto.c
index e3f368473..27156f578 100644
--- a/src/btstack_crypto.c
+++ b/src/btstack_crypto.c
@@ -590,6 +590,27 @@ static void btstack_crypto_ecc_p256_calculate_dhkey_software(btstack_crypto_ecc_
 
 #endif
 
+static void btstack_crypto_ccm_next_block(btstack_crypto_ccm_t * btstack_crypto_ccm, btstack_crypto_ccm_state_t state_when_done){
+    uint16_t bytes_to_process = btstack_min(btstack_crypto_ccm->block_len, 16);
+    // next block
+    btstack_crypto_ccm->counter++;
+    btstack_crypto_ccm->input       += bytes_to_process;
+    btstack_crypto_ccm->output      += bytes_to_process;
+    btstack_crypto_ccm->block_len   -= bytes_to_process;
+    btstack_crypto_ccm->message_len -= bytes_to_process;
+#ifdef DEBUG_CCM
+    printf("btstack_crypto_ccm_next_block (message len %u, block_len %u)\n", btstack_crypto_ccm->message_len, btstack_crypto_ccm->block_len);
+#endif
+    if (btstack_crypto_ccm->message_len == 0){
+        btstack_crypto_ccm->state = CCM_CALCULATE_S0;
+    } else {
+        btstack_crypto_ccm->state = state_when_done;
+        if (btstack_crypto_ccm->block_len == 0){
+            btstack_crypto_done(&btstack_crypto_ccm->btstack_crypto);
+        }
+    }
+}
+
 // If Controller is used for AES128, data is little endian
 static void btstack_crypto_ccm_handle_s0(btstack_crypto_ccm_t * btstack_crypto_ccm, const uint8_t * data){
     int i;
@@ -614,29 +635,56 @@ static void btstack_crypto_ccm_handle_sn(btstack_crypto_ccm_t * btstack_crypto_c
         btstack_crypto_ccm->output[i] = btstack_crypto_ccm->input[i] ^ data[15-i];
 #endif
     }
-}
-
-static void btstack_crypto_ccm_next_block(btstack_crypto_ccm_t * btstack_crypto_ccm, btstack_crypto_ccm_state_t state_when_done){
-    uint16_t bytes_to_process = btstack_min(btstack_crypto_ccm->block_len, 16);
-    // next block
-    btstack_crypto_ccm->counter++;
-    btstack_crypto_ccm->input       += bytes_to_process;
-    btstack_crypto_ccm->output      += bytes_to_process;
-    btstack_crypto_ccm->block_len   -= bytes_to_process;
-    btstack_crypto_ccm->message_len -= bytes_to_process;
-#ifdef DEBUG_CCM
-    printf("btstack_crypto_ccm_next_block (message len %u, block_len %u)\n", btstack_crypto_ccm->message_len, btstack_crypto_ccm->block_len);
-#endif
-    if (btstack_crypto_ccm->message_len == 0){
-        btstack_crypto_ccm->state = CCM_CALCULATE_S0;
-    } else {
-        btstack_crypto_ccm->state = state_when_done;
-        if (btstack_crypto_ccm->block_len == 0){
-            btstack_crypto_done(&btstack_crypto_ccm->btstack_crypto);
-        }
+    switch (btstack_crypto_ccm->btstack_crypto.operation){
+        case BTSTACK_CRYPTO_CCM_DECRYPT_BLOCK:
+            btstack_crypto_ccm->state = CCM_CALCULATE_XN;
+            break;
+        case BTSTACK_CRYPTO_CCM_ENCRYPT_BLOCK:
+            btstack_crypto_ccm_next_block(btstack_crypto_ccm, CCM_CALCULATE_XN);
+            break;
+        default:
+            btstack_assert(false);
+            break;
     }
 }
 
+static void btstack_crypto_ccm_handle_aad_xn(btstack_crypto_ccm_t * btstack_crypto_ccm) {
+#ifdef DEBUG_CCM
+    printf("%16s: ", "Xn+1 AAD");
+    printf_hexdump(btstack_crypto_ccm->x_i, 16);
+#endif
+    // more aad?
+    if (btstack_crypto_ccm->aad_offset < (btstack_crypto_ccm->aad_len + 2)){
+        btstack_crypto_ccm->state = CCM_CALCULATE_AAD_XN;
+    } else {
+        // done
+        btstack_crypto_done((btstack_crypto_t *) btstack_crypto_ccm);
+    }
+}
+
+static void btstack_crypto_ccm_handle_x1(btstack_crypto_ccm_t * btstack_crypto_ccm) {
+#ifdef DEBUG_CCM
+    printf("%16s: ", "Xi");
+    printf_hexdump(btstack_crypto_ccm->x_i, 16);
+#endif
+    switch (btstack_crypto_ccm->btstack_crypto.operation){
+        case BTSTACK_CRYPTO_CCM_DIGEST_BLOCK:
+            btstack_crypto_ccm->aad_remainder_len = 0;
+            btstack_crypto_ccm->state = CCM_CALCULATE_AAD_XN;
+            break;
+        case BTSTACK_CRYPTO_CCM_DECRYPT_BLOCK:
+            btstack_crypto_ccm->state = CCM_CALCULATE_SN;
+            break;
+        case BTSTACK_CRYPTO_CCM_ENCRYPT_BLOCK:
+            btstack_crypto_ccm->state = CCM_CALCULATE_XN;
+            break;
+        default:
+        btstack_assert(false);
+            break;
+    }
+}
+
+
 static void btstack_crypto_ccm_calc_s0(btstack_crypto_ccm_t * btstack_crypto_ccm){
 #ifdef DEBUG_CCM
     printf("btstack_crypto_ccm_calc_s0\n");
@@ -671,11 +719,9 @@ static void btstack_crypto_ccm_calc_sn(btstack_crypto_ccm_t * btstack_crypto_ccm
 #endif
     switch (btstack_crypto_ccm->btstack_crypto.operation){
         case BTSTACK_CRYPTO_CCM_DECRYPT_BLOCK:
-            btstack_crypto_ccm_handle_sn(btstack_crypto_ccm, data);
             btstack_crypto_ccm->state = CCM_CALCULATE_XN;
             break;
         case BTSTACK_CRYPTO_CCM_ENCRYPT_BLOCK:
-            btstack_crypto_ccm_handle_sn(btstack_crypto_ccm, data);
             btstack_crypto_ccm_next_block(btstack_crypto_ccm, CCM_CALCULATE_XN);
             break;
         default:
@@ -693,28 +739,28 @@ static void btstack_crypto_ccm_calc_x1(btstack_crypto_ccm_t * btstack_crypto_ccm
     btstack_crypto_ccm_setup_b_0(btstack_crypto_ccm, btstack_crypto_ccm_buffer);
 #ifdef USE_BTSTACK_AES128
     btstack_aes128_calc(btstack_crypto_ccm->key, btstack_crypto_ccm_buffer, btstack_crypto_ccm->x_i);
+    btstack_crypto_ccm_handle_x1(btstack_crypto_ccm);
+#else
+    btstack_crypto_aes128_start(btstack_crypto_ccm->key, btstack_crypto_ccm_buffer);
+#endif
+}
+
+static void btstack_crypto_ccm_handle_xn(btstack_crypto_ccm_t * btstack_crypto_ccm) {
 #ifdef DEBUG_CCM
-    printf("%16s: ", "Xi");
+    printf("%16s: ", "Xn+1");
     printf_hexdump(btstack_crypto_ccm->x_i, 16);
 #endif
     switch (btstack_crypto_ccm->btstack_crypto.operation){
-        case BTSTACK_CRYPTO_CCM_DIGEST_BLOCK:
-            btstack_crypto_ccm->aad_remainder_len = 0;
-            btstack_crypto_ccm->state = CCM_CALCULATE_AAD_XN;
-            break;
         case BTSTACK_CRYPTO_CCM_DECRYPT_BLOCK:
-            btstack_crypto_ccm->state = CCM_CALCULATE_SN;
+            btstack_crypto_ccm_next_block(btstack_crypto_ccm, CCM_CALCULATE_SN);
             break;
         case BTSTACK_CRYPTO_CCM_ENCRYPT_BLOCK:
-            btstack_crypto_ccm->state = CCM_CALCULATE_XN;
+            btstack_crypto_ccm->state = CCM_CALCULATE_SN;
             break;
         default:
             btstack_assert(false);
             break;
     }
-#else
-    btstack_crypto_aes128_start(btstack_crypto_ccm->key, btstack_crypto_ccm_buffer);
-#endif
 }
 
 static void btstack_crypto_ccm_calc_xn(btstack_crypto_ccm_t * btstack_crypto_ccm, const uint8_t * plaintext){
@@ -743,21 +789,7 @@ static void btstack_crypto_ccm_calc_xn(btstack_crypto_ccm_t * btstack_crypto_ccm
 
 #ifdef USE_BTSTACK_AES128
     btstack_aes128_calc(btstack_crypto_ccm->key, btstack_crypto_ccm_buffer, btstack_crypto_ccm->x_i);
-#ifdef DEBUG_CCM
-    printf("%16s: ", "Xn+1");
-    printf_hexdump(btstack_crypto_ccm->x_i, 16);
-#endif
-    switch (btstack_crypto_ccm->btstack_crypto.operation){
-        case BTSTACK_CRYPTO_CCM_DECRYPT_BLOCK:
-            btstack_crypto_ccm_next_block(btstack_crypto_ccm, CCM_CALCULATE_SN);
-            break;
-        case BTSTACK_CRYPTO_CCM_ENCRYPT_BLOCK:
-            btstack_crypto_ccm->state = CCM_CALCULATE_SN;
-            break;
-        default:
-            btstack_assert(false);
-            break;
-    }
+    btstack_crypto_ccm_handle_xn(btstack_crypto_ccm);
 #else
     btstack_crypto_aes128_start(btstack_crypto_ccm->key, btstack_crypto_ccm_buffer);
 #endif
@@ -805,17 +837,7 @@ static void btstack_crypto_ccm_calc_aad_xn(btstack_crypto_ccm_t * btstack_crypto
     btstack_crypto_ccm->state = CCM_W4_AAD_XN;
 #ifdef USE_BTSTACK_AES128
     btstack_aes128_calc(btstack_crypto_ccm->key, btstack_crypto_ccm->x_i, btstack_crypto_ccm->x_i);
-#ifdef DEBUG_CCM
-    printf("%16s: ", "Xn+1 AAD");
-    printf_hexdump(btstack_crypto_ccm->x_i, 16);
-#endif
-    // more aad?
-    if (btstack_crypto_ccm->aad_offset < (btstack_crypto_ccm->aad_len + 2)){
-        btstack_crypto_ccm->state = CCM_CALCULATE_AAD_XN;
-    } else {
-        // done
-        btstack_crypto_done((btstack_crypto_t *) btstack_crypto_ccm);
-    }
+    btstack_crypto_ccm_handle_aad_xn(btstack_crypto_ccm);
 #else
     btstack_crypto_aes128_start(btstack_crypto_ccm->key, btstack_crypto_ccm->x_i);
 #endif
@@ -868,7 +890,7 @@ static void btstack_crypto_run(void){
 #ifdef USE_BTSTACK_AES128
                 btstack_crypto_cmac_calc( btstack_crypto_cmac );
                 btstack_crypto_done(btstack_crypto);
-#else                
+#else
     			btstack_crypto_wait_for_hci_result = 1;
     			if (btstack_crypto_cmac_state == CMAC_IDLE){
     				btstack_crypto_cmac_start(btstack_crypto_cmac);
@@ -884,23 +906,33 @@ static void btstack_crypto_run(void){
                 btstack_crypto_ccm = (btstack_crypto_ccm_t *) btstack_crypto;
                 switch (btstack_crypto_ccm->state){
                     case CCM_CALCULATE_AAD_XN:
+#ifdef DEBUG_CCM
                         printf("CCM_CALCULATE_AAD_XN\n");
+#endif
                         btstack_crypto_ccm_calc_aad_xn(btstack_crypto_ccm);
                         break;
                     case CCM_CALCULATE_X1:
+#ifdef DEBUG_CCM
                         printf("CCM_CALCULATE_X1\n");
+#endif
                         btstack_crypto_ccm_calc_x1(btstack_crypto_ccm);
                         break;
                     case CCM_CALCULATE_S0:
+#ifdef DEBUG_CCM
                         printf("CCM_CALCULATE_S0\n");
+#endif
                         btstack_crypto_ccm_calc_s0(btstack_crypto_ccm);
                         break;
                     case CCM_CALCULATE_SN:
+#ifdef DEBUG_CCM
                         printf("CCM_CALCULATE_SN\n");
+#endif
                         btstack_crypto_ccm_calc_sn(btstack_crypto_ccm);
                         break;
                     case CCM_CALCULATE_XN:
+#ifdef DEBUG_CCM
                         printf("CCM_CALCULATE_XN\n");
+#endif
                         btstack_crypto_ccm_calc_xn(btstack_crypto_ccm, (btstack_crypto->operation == BTSTACK_CRYPTO_CCM_ENCRYPT_BLOCK) ? btstack_crypto_ccm->input : btstack_crypto_ccm->output);
                         break;
                     default:
@@ -918,7 +950,7 @@ static void btstack_crypto_run(void){
                         (void)memcpy(btstack_crypto_ec_p192->public_key,
                                      btstack_crypto_ecc_p256_public_key, 64);
                         btstack_linked_list_pop(&btstack_crypto_operations);
-                        (*btstack_crypto_ec_p192->btstack_crypto.context_callback.callback)(btstack_crypto_ec_p192->btstack_crypto.context_callback.context);                    
+                        (*btstack_crypto_ec_p192->btstack_crypto.context_callback.callback)(btstack_crypto_ec_p192->btstack_crypto.context_callback.context);
                         break;
                     case ECC_P256_KEY_GENERATION_IDLE:
 #ifdef USE_SOFTWARE_ECC_P256_IMPLEMENTATION
@@ -950,7 +982,7 @@ static void btstack_crypto_run(void){
                 btstack_crypto_ecc_p256_calculate_dhkey_software(btstack_crypto_ec_p192);
                 // done
                 btstack_linked_list_pop(&btstack_crypto_operations);
-                (*btstack_crypto_ec_p192->btstack_crypto.context_callback.callback)(btstack_crypto_ec_p192->btstack_crypto.context_callback.context);                    
+                (*btstack_crypto_ec_p192->btstack_crypto.context_callback.callback)(btstack_crypto_ec_p192->btstack_crypto.context_callback.context);
 #else
                 btstack_crypto_wait_for_hci_result = 1;
                 hci_send_cmd(&hci_le_generate_dhkey, &btstack_crypto_ec_p192->public_key[0], &btstack_crypto_ec_p192->public_key[32]);
@@ -1025,100 +1057,27 @@ static void btstack_crypto_handle_encryption_result(const uint8_t * data){
 		    btstack_crypto_cmac_handle_encryption_result(btstack_crypto_cmac, result);
 			break;
         case BTSTACK_CRYPTO_CCM_DIGEST_BLOCK:
-            btstack_crypto_ccm = (btstack_crypto_ccm_t*) btstack_linked_list_get_first_item(&btstack_crypto_operations);
-            switch (btstack_crypto_ccm->state){
-                case CCM_W4_X1:
-                    reverse_128(data, btstack_crypto_ccm->x_i);
-#ifdef DEBUG_CCM
-                    printf("%16s: ", "Xi");
-                    printf_hexdump(btstack_crypto_ccm->x_i, 16);
-#endif
-                    btstack_crypto_ccm->aad_remainder_len = 0;
-                    btstack_crypto_ccm->state = CCM_CALCULATE_AAD_XN;
-                    break;
-                case CCM_W4_AAD_XN:
-                    reverse_128(data, btstack_crypto_ccm->x_i);
-#ifdef DEBUG_CCM
-                    printf("%16s: ", "Xn+1 AAD");
-                    printf_hexdump(btstack_crypto_ccm->x_i, 16);
-#endif
-                    // more aad?
-                    if (btstack_crypto_ccm->aad_offset < (btstack_crypto_ccm->aad_len + 2)){
-                        btstack_crypto_ccm->state = CCM_CALCULATE_AAD_XN;
-                    } else {
-                        // done
-                        btstack_crypto_done(btstack_crypto);
-                    }
-                    break;
-                default:
-                    break;
-            }
-            break;                
         case BTSTACK_CRYPTO_CCM_ENCRYPT_BLOCK:
-            btstack_crypto_ccm = (btstack_crypto_ccm_t*) btstack_linked_list_get_first_item(&btstack_crypto_operations);
-            switch (btstack_crypto_ccm->state){
-                case CCM_W4_X1:
-                    reverse_128(data, btstack_crypto_ccm->x_i);
-#ifdef DEBUG_CCM
-                    printf("%16s: ", "Xi");
-                    printf_hexdump(btstack_crypto_ccm->x_i, 16);
-#endif
-                    btstack_crypto_ccm->state = CCM_CALCULATE_XN;
-                    break;           
-                case CCM_W4_XN:
-                    reverse_128(data, btstack_crypto_ccm->x_i);
-#ifdef DEBUG_CCM
-                    printf("%16s: ", "Xn+1");
-                    printf_hexdump(btstack_crypto_ccm->x_i, 16);
-#endif
-                    btstack_crypto_ccm->state = CCM_CALCULATE_SN;
-                    break;
-                case CCM_W4_S0:
-#ifdef DEBUG_CCM
-                    reverse_128(data, result);
-                    printf("%16s: ", "S0");
-                    printf_hexdump(result, 16);
-#endif
-                    btstack_crypto_ccm_handle_s0(btstack_crypto_ccm, data);
-                    break;
-                case CCM_W4_SN:
-#ifdef DEBUG_CCM
-                    reverse_128(data, result);
-                    printf("%16s: ", "Sn");
-                    printf_hexdump(result, 16);
-#endif
-                    btstack_crypto_ccm_handle_sn(btstack_crypto_ccm, data);
-                    btstack_crypto_ccm_next_block(btstack_crypto_ccm, CCM_CALCULATE_XN);
-                    break;
-                default:
-                    break;
-            }  
-            break;      
         case BTSTACK_CRYPTO_CCM_DECRYPT_BLOCK:
             btstack_crypto_ccm = (btstack_crypto_ccm_t*) btstack_linked_list_get_first_item(&btstack_crypto_operations);
             switch (btstack_crypto_ccm->state){
                 case CCM_W4_X1:
                     reverse_128(data, btstack_crypto_ccm->x_i);
-#ifdef DEBUG_CCM
-    printf("%16s: ", "X1");
-    printf_hexdump(btstack_crypto_ccm->x_i, 16);
-#endif
-                    btstack_crypto_ccm->state = CCM_CALCULATE_SN;
-                    break;           
+                    btstack_crypto_ccm_handle_x1(btstack_crypto_ccm);
+                    break;
                 case CCM_W4_XN:
                     reverse_128(data, btstack_crypto_ccm->x_i);
-#ifdef DEBUG_CCM
-    printf("%16s: ", "Xn+1");
-    printf_hexdump(btstack_crypto_ccm->x_i, 16);
-#endif
-                    btstack_crypto_ccm_next_block(btstack_crypto_ccm, CCM_CALCULATE_SN);
+                    btstack_crypto_ccm_handle_xn(btstack_crypto_ccm);
+                    break;
+                case CCM_W4_AAD_XN:
+                    reverse_128(data, btstack_crypto_ccm->x_i);
+                    btstack_crypto_ccm_handle_aad_xn(btstack_crypto_ccm);
                     break;
                 case CCM_W4_S0:
                     btstack_crypto_ccm_handle_s0(btstack_crypto_ccm, data);
                     break;
                 case CCM_W4_SN:
                     btstack_crypto_ccm_handle_sn(btstack_crypto_ccm, data);
-                    btstack_crypto_ccm->state = CCM_CALCULATE_XN;
                     break;
                 default:
                     break;