[MFI] Adds decryption of mode 2 packets (zephyrproject-rtos#11)

Adds in detection of entering apple's MFI mode 2 state and then decrypts mode 2 packets and passes them along to higher levels of the stack.

Mode 2 is entered by the iphone first pausing encryption and then re-starting it. After this, all encrypted packets coming from the iphone use mode 2 regardless of whether they're standard BLE packets or audio packets.

Author: kevin-whisper

subsys/bluetooth/controller/hal/ccm.h

@@ -12,3 +12,13 @@ struct ccm {
 	uint8_t  resv1:7;
 	uint8_t  iv[8];
 } __packed;
+
+// Whisper added for MFI
+struct __attribute__ ((packed, aligned(1))) ccm_mode2_nonce {
+	uint16_t counter;
+	uint8_t  resv1;
+	uint8_t  resv2;
+	uint8_t  resv3:7;
+	uint8_t  direction:1;
+	uint8_t  iv[8];
+};

subsys/bluetooth/controller/ll_sw/lll_conn.h

@@ -130,6 +130,13 @@ struct lll_conn {
 
 	struct ccm ccm_rx;
 	struct ccm ccm_tx;
+
+	// Whisper added for MFI
+	struct  ccm_mode2_nonce ccm_mode2_nonce_rx;
+	struct  ccm_mode2_nonce ccm_mode2_nonce_tx;
+	uint8_t mode2_rx_enabled;
+	uint8_t mode2_tx_enabled;
+	uint8_t has_paused;
 #endif /* CONFIG_BT_CTLR_LE_ENC */
 
 #if defined(CONFIG_BT_CTLR_CONN_RSSI)

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

@@ -42,6 +42,9 @@
 #include "common/log.h"
 #include "hal/debug.h"
 
+// Whisper added for MFI
+#include "hal/nrf5/ccm_mode2_soft.h"
+
 static int init_reset(void);
 static void isr_done(void *param);
 static inline int isr_rx_pdu(struct lll_conn *lll, struct pdu_data *pdu_data_rx,
@@ -970,6 +973,37 @@ static inline int isr_rx_pdu(struct lll_conn *lll, struct pdu_data *pdu_data_rx,
 
 				bool mic_failure = !radio_ccm_mic_is_valid();
 
+				// Whisper added for MFI
+				if (lll->mode2_rx_enabled) {
+					// decrypt apple's mode 2 encryption packet via a soft decrypt routine
+					// first get the encrypted packet
+					struct pdu_data *scratch_pkt = radio_pkt_scratch_get();
+
+					// next set up the structure for the soft decryption. NOTE: The event counter
+					// has already been incremented elsewhere in the stack prior to this code running
+					// so the event_counter we actually want is (event_counter - 1)
+					ccm_soft_data_t ccm_params;
+					lll->ccm_mode2_nonce_rx.counter = lll->event_counter - 1;
+					ccm_params.p_nonce = (uint8_t *)&lll->ccm_mode2_nonce_rx;
+					ccm_params.p_m = scratch_pkt->lldata;
+					ccm_params.m_len = scratch_pkt->len;
+					ccm_params.p_out = pdu_data_rx->lldata;
+					ccm_params.p_key = lll->ccm_rx.key;
+
+					ccm_mode2_soft_decrypt(&ccm_params);
+
+					// finally finish setting up pdu_data_rx
+					pdu_data_rx->ll_id = scratch_pkt->ll_id;
+					pdu_data_rx->nesn = scratch_pkt->nesn;
+					pdu_data_rx->sn = scratch_pkt->sn;
+					pdu_data_rx->md = scratch_pkt->md;
+					pdu_data_rx->rfu = scratch_pkt->rfu;
+					pdu_data_rx->len = scratch_pkt->len;
+
+					// there's no MIC with mode 2 encryption so set this to false
+					mic_failure = false;
+				}
+
 				if (mic_failure &&
 				    lll->ccm_rx.counter == 0 &&
 				    (pdu_data_rx->ll_id ==

subsys/bluetooth/controller/ll_sw/ull_adv.c

@@ -1081,6 +1081,11 @@ uint8_t ll_adv_enable(uint8_t enable)
 		conn->llcp_enc.pause_tx = conn->llcp_enc.pause_rx = 0U;
 		conn->llcp_enc.refresh = 0U;
 		conn->periph.llcp_type = 0U;
+
+		// Whisper added for MFI
+		conn_lll->mode2_rx_enabled = 0;
+		conn_lll->mode2_tx_enabled = 0;
+		conn_lll->has_paused = 0;
 #endif /* CONFIG_BT_CTLR_LE_ENC */
 
 #if defined(CONFIG_BT_CTLR_CONN_PARAM_REQ)

subsys/bluetooth/controller/ll_sw/ull_conn.c

@@ -3584,6 +3584,19 @@ static inline void event_enc_prep(struct ll_conn *conn)
 			 */
 			lll->enc_rx = 1U;
 
+			// Whisper added for MFI
+			if(lll->has_paused) {
+				lll->mode2_rx_enabled = 1;
+				
+				// set up the Rx nonce
+				lll->ccm_mode2_nonce_rx.counter = lll->event_counter;
+				lll->ccm_mode2_nonce_rx.resv1 = 0;
+				lll->ccm_mode2_nonce_rx.resv2 = 0;
+				lll->ccm_mode2_nonce_rx.resv3 = 0;
+				lll->ccm_mode2_nonce_rx.direction = lll->ccm_rx.direction;
+				memcpy(lll->ccm_mode2_nonce_rx.iv, lll->ccm_rx.iv, sizeof(lll->ccm_mode2_nonce_rx.iv));
+			}
+
 			/* prepare the start enc req */
 			pdu_ctrl_tx->ll_id = PDU_DATA_LLID_CTRL;
 			pdu_ctrl_tx->len = offsetof(struct pdu_data_llctrl,
@@ -4957,6 +4970,19 @@ static int start_enc_rsp_send(struct ll_conn *conn,
 
 	/* enable transmit encryption */
 	conn->lll.enc_tx = 1;
+	
+	// Whisper added for MFI
+	if(conn->lll.has_paused) {
+		conn->lll.mode2_tx_enabled = 1;
+
+		// set up the Tx nonce
+		conn->lll.ccm_mode2_nonce_tx.counter = conn->lll.event_counter;
+		conn->lll.ccm_mode2_nonce_tx.resv1 = 0;
+		conn->lll.ccm_mode2_nonce_tx.resv2 = 0;
+		conn->lll.ccm_mode2_nonce_tx.resv3 = 0;
+		conn->lll.ccm_mode2_nonce_tx.direction = conn->lll.ccm_tx.direction;
+		memcpy(conn->lll.ccm_mode2_nonce_tx.iv, conn->lll.ccm_tx.iv, sizeof(conn->lll.ccm_mode2_nonce_tx.iv));
+	}
 
 	ull_pdu_data_init(pdu_ctrl_tx);
 
@@ -6361,6 +6387,9 @@ static inline void ctrl_tx_ack(struct ll_conn *conn, struct node_tx **tx,
 		{
 			/* pause data packet tx */
 			conn->llcp_enc.pause_tx = 1U;
+
+			// Whisper added for MFI:
+			conn->lll.has_paused = 1U;
 		}
 		break;