Use radio scratch area when doing soft encryption for tx (zephyrproject-rtos#16)

Author: kevin-whisper

subsys/bluetooth/controller/ll_sw/nordic/lll/lll_conn.c

@@ -703,28 +703,43 @@ void lll_conn_tx_pkt_set(struct lll_conn *lll, struct pdu_data *pdu_data_tx)
 #endif /* CONFIG_BT_CTLR_LE_ENC */
 	} else {
 		// Whisper added for MFI.
+		struct pdu_data *tx_pkt = pdu_data_tx;
+
 #if defined(CONFIG_BT_CTLR_LE_ENC)
-		if(lll->mode2_tx_enabled) {
+		if (lll->mode2_tx_enabled) {
 			// if mode 2 is enabled we need to encrypt the packet using mode 2
 			// encryption before transmitting
 			ccm_soft_data_t ccm_params;
 
+			// the output of the encryption needs to go to the radio's scratch packet area
+			// so that we don't overwrite the non-encrypted pdu_tx which still needs to be
+			// used later for processing acks
+			struct pdu_data *scratch_pkt = radio_pkt_scratch_get();
+			scratch_pkt->ll_id = pdu_data_tx->ll_id;
+			scratch_pkt->nesn = pdu_data_tx->nesn;
+			scratch_pkt->sn = pdu_data_tx->sn;
+			scratch_pkt->md = pdu_data_tx->md;
+			scratch_pkt->rfu = pdu_data_tx->rfu;
+			scratch_pkt->len = pdu_data_tx->len;
+
 			// Note that the event counter is already incremented before this code is called
 			// so the event counter we want is actually (event_counter - 1)
 			lll->ccm_mode2_nonce_tx.counter = lll->event_counter - 1;
 			ccm_params.p_nonce = (uint8_t *)&lll->ccm_mode2_nonce_tx;
 			ccm_params.p_m = pdu_data_tx->lldata;
 			ccm_params.m_len = pdu_data_tx->len;
-			ccm_params.p_out = pdu_data_tx->lldata;
+			ccm_params.p_out = scratch_pkt->lldata;
 			ccm_params.p_key = lll->ccm_tx.key;
 
 			ccm_mode2_soft_encrypt(&ccm_params);
+
+			tx_pkt = scratch_pkt;
 		}
 #endif /* CONFIG_BT_CTLR_LE_ENC */
 
 		radio_pkt_configure(RADIO_PKT_CONF_LENGTH_8BIT, max_tx_octets, pkt_flags);
 
-		radio_pkt_tx_set(pdu_data_tx);
+		radio_pkt_tx_set(tx_pkt);
 	}
 }