From bdaa7ba4afe163bdaf66c839fc75811075304e82 Mon Sep 17 00:00:00 2001
From: Vinayak Chettimada <vinayak.kariappa.chettimada@nordicsemi.no>
Date: Wed, 3 May 2017 10:02:23 +0200
Subject: [PATCH 1/2] Bluetooth: Controller: Allow multiple ctrl pkt enqueue

Scale the ctrl pkt enqueue implementation to allow multiple
ctrl packets to be enqueued. This will aid in the graceful
implementation of parallel control procedure collisions.

Signed-off-by: Vinayak Kariappa Chettimada <vich@nordicsemi.no>
---
 subsys/bluetooth/controller/ll_sw/ctrl.c | 29 +++++++++++++++---------
 1 file changed, 18 insertions(+), 11 deletions(-)

diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.c b/subsys/bluetooth/controller/ll_sw/ctrl.c
index f833316ccc9c07..7fd8312db0588a 100644
--- a/subsys/bluetooth/controller/ll_sw/ctrl.c
+++ b/subsys/bluetooth/controller/ll_sw/ctrl.c
@@ -6440,6 +6440,21 @@ static struct pdu_data *empty_tx_enqueue(struct connection *conn)
 	return pdu_data_tx;
 }
 
+static void ctrl_tx_enqueue_tail(struct connection *conn,
+			    struct radio_pdu_node_tx *node_tx)
+{
+	struct radio_pdu_node_tx *p;
+
+	/* TODO: optimise by having a ctrl_last member. */
+	p = conn->pkt_tx_ctrl;
+	while (p->next != conn->pkt_tx_data) {
+		p = p->next;
+	}
+
+	node_tx->next = p->next;
+	p->next = node_tx;
+}
+
 static void ctrl_tx_enqueue(struct connection *conn,
 			    struct radio_pdu_node_tx *node_tx)
 {
@@ -6468,16 +6483,12 @@ static void ctrl_tx_enqueue(struct connection *conn,
 		/* if no ctrl packet already queued, new ctrl added will be
 		 * the ctrl pointer and is inserted after head.
 		 */
-		if (conn->pkt_tx_ctrl == 0) {
+		if (!conn->pkt_tx_ctrl) {
 			node_tx->next = conn->pkt_tx_head->next;
 			conn->pkt_tx_head->next = node_tx;
 			conn->pkt_tx_ctrl = node_tx;
 		} else {
-			/* TODO support for more than 2 pending ctrl packets. */
-			LL_ASSERT(conn->pkt_tx_ctrl->next == conn->pkt_tx_data);
-
-			node_tx->next = conn->pkt_tx_ctrl->next;
-			conn->pkt_tx_ctrl->next = node_tx;
+			ctrl_tx_enqueue_tail(conn, node_tx);
 		}
 	} else {
 		/* No packet needing ACK. */
@@ -6490,11 +6501,7 @@ static void ctrl_tx_enqueue(struct connection *conn,
 			conn->pkt_tx_head = node_tx;
 			conn->pkt_tx_ctrl = node_tx;
 		} else {
-			/* TODO support for more than 2 pending ctrl packets. */
-			LL_ASSERT(conn->pkt_tx_ctrl->next == conn->pkt_tx_data);
-
-			node_tx->next = conn->pkt_tx_ctrl->next;
-			conn->pkt_tx_ctrl->next = node_tx;
+			ctrl_tx_enqueue_tail(conn, node_tx);
 		}
 	}
 

From 3fe6eafd117d99e3fd5c6db8dfdb7ce71c5643e7 Mon Sep 17 00:00:00 2001
From: Vinayak Chettimada <vinayak.kariappa.chettimada@nordicsemi.no>
Date: Sat, 29 Apr 2017 06:50:52 +0200
Subject: [PATCH 2/2] Bluetooth: controller: PHY Update Procedure

Add support for Bluetooth v5.0 PHY Update Procedure.

Jira: ZEP-2086

Signed-off-by: Vinayak Kariappa Chettimada <vich@nordicsemi.no>
---
 subsys/bluetooth/controller/Kconfig           |   8 +
 subsys/bluetooth/controller/hal/nrf5/radio.c  |  88 ++-
 subsys/bluetooth/controller/hal/radio.h       |   2 +-
 subsys/bluetooth/controller/hci/hci.c         |  36 +
 subsys/bluetooth/controller/include/ll.h      |   4 +
 subsys/bluetooth/controller/ll_sw/ctrl.c      | 681 +++++++++++++++---
 subsys/bluetooth/controller/ll_sw/ctrl.h      |   9 +
 .../controller/ll_sw/ctrl_internal.h          |  38 +
 subsys/bluetooth/controller/ll_sw/pdu.h       |   6 +-
 9 files changed, 742 insertions(+), 130 deletions(-)

diff --git a/subsys/bluetooth/controller/Kconfig b/subsys/bluetooth/controller/Kconfig
index 7c9338e18c9276..3d788922c96381 100644
--- a/subsys/bluetooth/controller/Kconfig
+++ b/subsys/bluetooth/controller/Kconfig
@@ -130,6 +130,13 @@ config BLUETOOTH_CONTROLLER_CHAN_SEL_2
 	  Enable support for Bluetooth 5.0 LE Channel Selection Algorithm #2 in
 	  the Controller.
 
+config BLUETOOTH_CONTROLLER_PHY
+	bool "PHY Update"
+	default y
+	help
+	  Enable support for Bluetooth 5.0 PHY Update Procedure in the
+	  Controller.
+
 config BLUETOOTH_CONTROLLER_ADVANCED_FEATURES
 	bool "Show advanced features"
 	help
@@ -211,6 +218,7 @@ config BLUETOOTH_CONTROLLER_SCHED_ADVANCED
 
 config BLUETOOTH_CONTROLLER_TIFS_HW
 	bool "H/w Accelerated tIFS Trx switching"
+	depends on !BLUETOOTH_CONTROLLER_PHY
 	default y
 	help
 	  Enable use of hardware accelerated tIFS Trx switching.
diff --git a/subsys/bluetooth/controller/hal/nrf5/radio.c b/subsys/bluetooth/controller/hal/nrf5/radio.c
index 70de81687b2554..8a1a6eb83b33c6 100644
--- a/subsys/bluetooth/controller/hal/nrf5/radio.c
+++ b/subsys/bluetooth/controller/hal/nrf5/radio.c
@@ -60,11 +60,42 @@ void radio_reset(void)
 	     RADIO_POWER_POWER_Msk);
 }
 
-void radio_phy_set(u8_t phy)
+void radio_phy_set(u8_t phy, u8_t flags)
 {
-	NRF_RADIO->MODE =
-	    (((phy) ? (u32_t)phy : RADIO_MODE_MODE_Ble_1Mbit) <<
-	     RADIO_MODE_MODE_Pos) & RADIO_MODE_MODE_Msk;
+	u32_t mode;
+
+	switch (phy) {
+	case BIT(0):
+	default:
+		mode = RADIO_MODE_MODE_Ble_1Mbit;
+		break;
+
+#if defined(CONFIG_SOC_SERIES_NRF51X)
+	case BIT(1):
+		mode = RADIO_MODE_MODE_Nrf_2Mbit;
+		break;
+
+#elif defined(CONFIG_SOC_SERIES_NRF52X)
+	case BIT(1):
+		mode = RADIO_MODE_MODE_Ble_2Mbit;
+		break;
+
+#else /* !CONFIG_SOC_SERIES_NRF52X */
+	case BIT(1):
+		mode = RADIO_MODE_MODE_Ble_2Mbit;
+		break;
+
+	case BIT(2):
+		if (flags & 0x01) {
+			mode = RADIO_MODE_MODE_Ble_LR500Kbit;
+		} else {
+			mode = RADIO_MODE_MODE_Ble_LR125Kbit;
+		}
+		break;
+#endif /* !CONFIG_SOC_SERIES_NRF52X */
+	}
+
+	NRF_RADIO->MODE = (mode << RADIO_MODE_MODE_Pos) & RADIO_MODE_MODE_Msk;
 }
 
 void radio_tx_power_set(u32_t power)
@@ -96,12 +127,12 @@ void radio_aa_set(u8_t *aa)
 
 void radio_pkt_configure(u8_t bits_len, u8_t max_len, u8_t flags)
 {
-	u8_t p16 = (flags >> 1) & 0x01; /* 16-bit preamble */
 	u8_t dc = flags & 0x01; /* Adv or Data channel */
 	u32_t extra;
+	u8_t phy;
 
 #if defined(CONFIG_SOC_SERIES_NRF51X)
-	ARG_UNUSED(p16);
+	ARG_UNUSED(phy);
 
 	extra = 0;
 
@@ -110,8 +141,28 @@ void radio_pkt_configure(u8_t bits_len, u8_t max_len, u8_t flags)
 		bits_len = 5;
 	}
 #else /* !CONFIG_SOC_SERIES_NRF51X */
-	extra = (((p16) ? RADIO_PCNF0_PLEN_16bit : RADIO_PCNF0_PLEN_8bit) <<
-		 RADIO_PCNF0_PLEN_Pos) & RADIO_PCNF0_PLEN_Msk;
+	extra = 0;
+
+	phy = (flags >> 1) & 0x07; /* phy */
+	switch (phy) {
+	case BIT(0):
+	default:
+		extra |= (RADIO_PCNF0_PLEN_8bit << RADIO_PCNF0_PLEN_Pos) &
+			 RADIO_PCNF0_PLEN_Msk;
+		break;
+
+	case BIT(1):
+		extra |= (RADIO_PCNF0_PLEN_16bit << RADIO_PCNF0_PLEN_Pos) &
+			 RADIO_PCNF0_PLEN_Msk;
+		break;
+
+#if !defined(CONFIG_SOC_SERIES_NRF52X)
+	case BIT(2):
+		extra |= (RADIO_PCNF0_PLEN_LongRange << RADIO_PCNF0_PLEN_Pos) &
+			 RADIO_PCNF0_PLEN_Msk;
+		break;
+#endif
+	}
 
 	/* To use same Data Channel PDU structure with nRF5 specific overhead
 	 * byte, include the S1 field in radio packet configuration.
@@ -555,28 +606,7 @@ void *radio_ccm_tx_pkt_set(struct ccm *ccm, void *pkt)
 	NRF_CCM->EVENTS_ENDCRYPT = 0;
 	NRF_CCM->EVENTS_ERROR = 0;
 
-#if defined(CONFIG_SOC_SERIES_NRF51X)
-	/* set up PPI to enable CCM */
-	NRF_PPI->CH[6].EEP = (u32_t)&(NRF_RADIO->EVENTS_READY);
-	NRF_PPI->CH[6].TEP = (u32_t)&(NRF_CCM->TASKS_KSGEN);
-	NRF_PPI->CHENSET = PPI_CHEN_CH6_Msk;
-#elif 0
-	/* encrypt tx packet */
-	NRF_CCM->INTENSET = CCM_INTENSET_ENDCRYPT_Msk;
-	NRF_CCM->TASKS_KSGEN = 1;
-	while (NRF_CCM->EVENTS_ENDCRYPT == 0) {
-		__WFE();
-		__SEV();
-		__WFE();
-	}
-	NRF_CCM->INTENCLR = CCM_INTENCLR_ENDCRYPT_Msk;
-	NVIC_ClearPendingIRQ(CCM_AAR_IRQn);
-
-	LL_ASSERT(NRF_CCM->EVENTS_ERROR == 0);
-#else
-	/* start KSGEN early, but dont wait for ENDCRYPT */
 	NRF_CCM->TASKS_KSGEN = 1;
-#endif
 
 	return _pkt_scratch;
 }
diff --git a/subsys/bluetooth/controller/hal/radio.h b/subsys/bluetooth/controller/hal/radio.h
index d66e26995ee671..6790082ced419f 100644
--- a/subsys/bluetooth/controller/hal/radio.h
+++ b/subsys/bluetooth/controller/hal/radio.h
@@ -26,7 +26,7 @@ void isr_radio(void);
 void radio_isr_set(radio_isr_fp fp_radio_isr);
 
 void radio_reset(void);
-void radio_phy_set(u8_t phy);
+void radio_phy_set(u8_t phy, u8_t flags);
 void radio_tx_power_set(u32_t power);
 void radio_freq_chan_set(u32_t chan);
 void radio_whiten_iv_set(u32_t iv);
diff --git a/subsys/bluetooth/controller/hci/hci.c b/subsys/bluetooth/controller/hci/hci.c
index 7d333f989dd9ed..add5e5d55f2aa0 100644
--- a/subsys/bluetooth/controller/hci/hci.c
+++ b/subsys/bluetooth/controller/hci/hci.c
@@ -1339,6 +1339,33 @@ static void le_chan_sel_algo(struct pdu_data *pdu_data, u16_t handle,
 }
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2 */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+static void le_phy_upd_complete(struct pdu_data *pdu_data, u16_t handle,
+				struct net_buf *buf)
+{
+	struct bt_hci_evt_le_phy_update_complete *sep;
+	struct radio_le_phy_upd_cmplt *radio_le_phy_upd_cmplt;
+
+	radio_le_phy_upd_cmplt = (struct radio_le_phy_upd_cmplt *)
+				 pdu_data->payload.lldata;
+
+	if (!(event_mask & BT_EVT_MASK_LE_META_EVENT) ||
+	    !(le_event_mask & BT_EVT_MASK_LE_PHY_UPDATE_COMPLETE)) {
+		BT_WARN("handle: 0x%04x, tx: %x, rx: %x.", handle,
+			radio_le_phy_upd_cmplt->tx,
+			radio_le_phy_upd_cmplt->rx);
+		return;
+	}
+
+	sep = meta_evt(buf, BT_HCI_EVT_LE_PHY_UPDATE_COMPLETE, sizeof(*sep));
+
+	sep->status = 0x00;
+	sep->handle = sys_cpu_to_le16(handle);
+	sep->tx_phy = radio_le_phy_upd_cmplt->tx;
+	sep->rx_phy = radio_le_phy_upd_cmplt->rx;
+}
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2 */
+
 static void encode_control(struct radio_pdu_node_rx *node_rx,
 			   struct pdu_data *pdu_data, struct net_buf *buf)
 {
@@ -1382,6 +1409,12 @@ static void encode_control(struct radio_pdu_node_rx *node_rx,
 		break;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2 */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	case NODE_RX_TYPE_PHY_UPDATE:
+		le_phy_upd_complete(pdu_data, handle, buf);
+		return;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_CONN_RSSI)
 	case NODE_RX_TYPE_RSSI:
 		BT_INFO("handle: 0x%04x, rssi: -%d dB.", handle,
@@ -1717,6 +1750,9 @@ s8_t hci_get_class(struct radio_pdu_node_rx *node_rx)
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2)
 		case NODE_RX_TYPE_CHAN_SEL_ALGO:
 #endif
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		case NODE_RX_TYPE_PHY_UPDATE:
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
 			return HCI_CLASS_EVT_CONNECTION;
 		default:
 			return -1;
diff --git a/subsys/bluetooth/controller/include/ll.h b/subsys/bluetooth/controller/include/ll.h
index e67f0cec140b7a..53eebd3de00ee4 100644
--- a/subsys/bluetooth/controller/include/ll.h
+++ b/subsys/bluetooth/controller/include/ll.h
@@ -55,4 +55,8 @@ void ll_length_max_get(u16_t *max_tx_octets, u16_t *max_tx_time,
 		       u16_t *max_rx_octets, u16_t *max_rx_time);
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+u32_t ll_phy_req_send(u16_t handle, u8_t tx, u8_t rx);
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 #endif /* _LL_H_ */
diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.c b/subsys/bluetooth/controller/ll_sw/ctrl.c
index 7fd8312db0588a..66f38d5f1a552a 100644
--- a/subsys/bluetooth/controller/ll_sw/ctrl.c
+++ b/subsys/bluetooth/controller/ll_sw/ctrl.c
@@ -36,10 +36,7 @@
 #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BLUETOOTH_DEBUG_HCI_DRIVER)
 #include <bluetooth/log.h>
 
-#define RADIO_PREAMBLE_TO_ADDRESS_US	40
 #define RADIO_TIFS                      150
-#define RADIO_HCTO_US			(RADIO_TIFS + 2 + 2 + \
-					 RADIO_PREAMBLE_TO_ADDRESS_US)
 #define RADIO_CONN_EVENTS(x, y)		((u16_t)((x) / (y)))
 
 #define RADIO_TICKER_JITTER_US			16
@@ -58,9 +55,6 @@
 
 #define SILENT_CONNECTION	0
 
-#define RADIO_PHY_ADV		0
-#define RADIO_PHY_CONN		0
-
 enum role {
 	ROLE_NONE,
 	ROLE_ADV,
@@ -268,6 +262,11 @@ static u32_t is_peer_compatible(struct connection *conn);
 static u32_t conn_update_req(struct connection *conn);
 static u32_t chan_map_update(struct connection *conn,
 			     struct pdu_data *pdu_data_rx);
+
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+static u32_t phy_upd_ind(struct connection *conn, struct pdu_data *pdu_data_rx);
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 static void enc_req_reused_send(struct connection *conn,
 				struct radio_pdu_node_tx *node_tx);
 static void terminate_ind_rx_enqueue(struct connection *conn, u8_t reason);
@@ -291,6 +290,10 @@ static void length_resp_send(struct connection *conn, u16_t eff_rx_octets,
 			     u16_t eff_tx_octets);
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+static void phy_rsp_send(struct connection *conn);
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 static u32_t role_disable(u8_t ticker_id_primary, u8_t ticker_id_stop);
 static void rx_fc_lock(u16_t handle);
 
@@ -508,17 +511,46 @@ static void common_init(void)
 	packet_rx_allocate(0xFF);
 }
 
+static inline u32_t addr_us_get(u8_t phy)
+{
+	switch (phy) {
+	default:
+	case BIT(0):
+		return 40;
+	case BIT(1):
+		return 20;
+	case BIT(2):
+		return 336;
+	}
+}
+
+#if defined(SILENT_CONNECTION)
+static inline u32_t empty_pkt_us_get(u8_t phy)
+{
+	switch (phy) {
+	default:
+	case BIT(0):
+		return 80;
+	case BIT(1):
+		return 40;
+	case BIT(2):
+		return 720;
+	}
+}
+#endif
+
 static inline void isr_radio_state_tx(void)
 {
+	u32_t hcto;
+
 	_radio.state = STATE_RX;
 
+	hcto = radio_tmr_end_get() + RADIO_RX_CHAIN_DELAY_US + RADIO_TIFS -
+	       RADIO_TX_CHAIN_DELAY_US + 4;
+
 	radio_tmr_tifs_set(RADIO_TIFS);
 	radio_switch_complete_and_tx();
 
-	radio_tmr_hcto_configure(radio_tmr_end_get() +
-				 RADIO_RX_CHAIN_DELAY_US + RADIO_HCTO_US -
-				 RADIO_TX_CHAIN_DELAY_US);
-
 	switch (_radio.role) {
 	case ROLE_ADV:
 		radio_pkt_rx_set(radio_pkt_scratch_get());
@@ -530,6 +562,8 @@ static inline void isr_radio_state_tx(void)
 			radio_ar_configure(_radio.nirk, _radio.irk);
 		}
 
+		hcto += addr_us_get(0);
+		radio_tmr_hcto_configure(hcto);
 		radio_tmr_end_capture();
 		break;
 
@@ -540,6 +574,8 @@ static inline void isr_radio_state_tx(void)
 		/* assert if radio packet ptr is not set and radio started rx */
 		LL_ASSERT(!radio_is_ready());
 
+		hcto += addr_us_get(0);
+		radio_tmr_hcto_configure(hcto);
 		radio_rssi_measure();
 		break;
 
@@ -560,6 +596,13 @@ static inline void isr_radio_state_tx(void)
 		/* assert if radio packet ptr is not set and radio started rx */
 		LL_ASSERT(!radio_is_ready());
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		hcto += addr_us_get(_radio.conn_curr->phy_rx);
+#else /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+		hcto += addr_us_get(0);
+#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
+		radio_tmr_hcto_configure(hcto);
 		radio_tmr_end_capture();
 
 		/* Route the tx packet to respective connections */
@@ -1277,6 +1320,12 @@ static inline u8_t isr_rx_conn_pkt_ack(struct pdu_data *pdu_data_tx,
 		break;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	case PDU_DATA_LLCTRL_TYPE_PHY_REQ:
+		_radio.conn_curr->llcp_phy.state = LLCP_PHY_STATE_RSP_WAIT;
+		break;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 	default:
 		/* Do nothing for other ctrl packet ack */
 		break;
@@ -1332,61 +1381,71 @@ static inline u32_t feat_get(u8_t *features)
 }
 
 static inline void
-isr_rx_conn_pkt_ctrl_rej(struct radio_pdu_node_rx *radio_pdu_node_rx,
-			 u8_t *rx_enqueue)
+isr_rx_conn_pkt_ctrl_rej_conn_upd(struct radio_pdu_node_rx *radio_pdu_node_rx,
+				  u8_t *rx_enqueue)
 {
-	/* reset ctrl procedure */
-	_radio.conn_curr->llcp_ack = _radio.conn_curr->llcp_req;
+	LL_ASSERT(_radio.conn_upd == _radio.conn_curr);
 
-	switch (_radio.conn_curr->llcp_type) {
-	case LLCP_CONNECTION_UPDATE:
-		LL_ASSERT(_radio.conn_upd == _radio.conn_curr);
+	/* reset mutex */
+	_radio.conn_upd = NULL;
 
-		/* reset mutex */
-		_radio.conn_upd = NULL;
+	/* update to next ticks offsets */
+	if (_radio.conn_curr->role.slave.role != 0) {
+		_radio.conn_curr->role.slave.ticks_to_offset =
+		    _radio.conn_curr->llcp.connection_update.
+			ticks_to_offset_next;
+	}
 
-		/* update to next ticks offsets */
-		if (_radio.conn_curr->role.slave.role != 0) {
-			_radio.conn_curr->role.slave.ticks_to_offset =
-			    _radio.conn_curr->llcp.connection_update.
-			    ticks_to_offset_next;
-		}
+	/* conn param req procedure, if any, is complete */
+	_radio.conn_curr->procedure_expire = 0;
 
-		/* conn param req procedure, if any, is complete */
-		_radio.conn_curr->procedure_expire = 0;
+	/* enqueue the reject ind ext */
+	if (!_radio.conn_curr->llcp.connection_update.is_internal) {
+		struct radio_le_conn_update_cmplt
+			*radio_le_conn_update_cmplt;
+		struct pdu_data *pdu_data_rx;
 
-		/* enqueue the reject ind ext */
-		if (!_radio.conn_curr->llcp.connection_update.is_internal) {
-			struct radio_le_conn_update_cmplt
-				*radio_le_conn_update_cmplt;
-			struct pdu_data *pdu_data_rx;
+		radio_pdu_node_rx->hdr.type = NODE_RX_TYPE_CONN_UPDATE;
+
+		/* prepare connection update complete structure */
+		pdu_data_rx = (struct pdu_data *)radio_pdu_node_rx->pdu_data;
+		radio_le_conn_update_cmplt =
+			(struct radio_le_conn_update_cmplt *)
+			&pdu_data_rx->payload;
+		radio_le_conn_update_cmplt->status = 0x00;
+		radio_le_conn_update_cmplt->interval =
+			_radio.conn_curr->conn_interval;
+		radio_le_conn_update_cmplt->latency = _radio.conn_curr->latency;
+		radio_le_conn_update_cmplt->timeout =
+			_radio.conn_curr->supervision_reload *
+			_radio.conn_curr->conn_interval * 125 / 1000;
 
-			radio_pdu_node_rx->hdr.type = NODE_RX_TYPE_CONN_UPDATE;
+		*rx_enqueue = 1;
+	}
+}
 
-			/* prepare connection update complete structure */
-			pdu_data_rx =
-				(struct pdu_data *)radio_pdu_node_rx->pdu_data;
-			radio_le_conn_update_cmplt =
-				(struct radio_le_conn_update_cmplt *)
-				&pdu_data_rx->payload;
-			radio_le_conn_update_cmplt->status = 0x00;
-			radio_le_conn_update_cmplt->interval =
-				_radio.conn_curr->conn_interval;
-			radio_le_conn_update_cmplt->latency =
-				_radio.conn_curr->latency;
-			radio_le_conn_update_cmplt->timeout =
-				_radio.conn_curr->supervision_reload *
-				_radio.conn_curr->conn_interval * 125 / 1000;
+static inline void
+isr_rx_conn_pkt_ctrl_rej(struct radio_pdu_node_rx *radio_pdu_node_rx,
+			 u8_t *rx_enqueue)
+{
+	if (_radio.conn_curr->llcp_ack != _radio.conn_curr->llcp_req) {
+		/* reset ctrl procedure */
+		_radio.conn_curr->llcp_ack = _radio.conn_curr->llcp_req;
 
-			*rx_enqueue = 1;
-		}
-		break;
+		switch (_radio.conn_curr->llcp_type) {
+		case LLCP_CONNECTION_UPDATE:
+			isr_rx_conn_pkt_ctrl_rej_conn_upd(radio_pdu_node_rx,
+							  rx_enqueue);
+			break;
 
-	default:
-		LL_ASSERT(0);
-		break;
+		default:
+			LL_ASSERT(0);
+			break;
+		}
+	} else if (_radio.conn_curr->llcp_phy.ack !=
+		   _radio.conn_curr->llcp_phy.req) {
+		/* Ignore, procedure timeout will continue until phy upd ind */
 	}
-
 }
 
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH)
@@ -1865,15 +1924,7 @@ isr_rx_conn_pkt_ctrl(struct radio_pdu_node_rx *radio_pdu_node_rx,
 		break;
 
 	case PDU_DATA_LLCTRL_TYPE_REJECT_EXT_IND:
-		if (_radio.conn_curr->llcp_req != _radio.conn_curr->llcp_ack) {
-			isr_rx_conn_pkt_ctrl_rej(radio_pdu_node_rx, rx_enqueue);
-
-		} else {
-			/* By spec. slave shall not generate a conn update
-			 * complete on reject from master.
-			 */
-			LL_ASSERT(_radio.conn_curr->role.slave.role);
-		}
+		isr_rx_conn_pkt_ctrl_rej(radio_pdu_node_rx, rx_enqueue);
 		break;
 
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_LE_PING)
@@ -1902,7 +1953,6 @@ isr_rx_conn_pkt_ctrl(struct radio_pdu_node_rx *radio_pdu_node_rx,
 		} else if (_radio.conn_curr->llcp_length.req !=
 			   _radio.conn_curr->llcp_length.ack) {
 			/* Procedure complete */
-			_radio.conn_curr->procedure_expire = 0;
 			_radio.conn_curr->llcp_length.ack =
 				_radio.conn_curr->llcp_length.req;
 
@@ -1915,6 +1965,27 @@ isr_rx_conn_pkt_ctrl(struct radio_pdu_node_rx *radio_pdu_node_rx,
 			*rx_enqueue = 1;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		} else if (_radio.conn_curr->llcp_phy.req !=
+			   _radio.conn_curr->llcp_phy.ack) {
+			struct radio_le_phy_upd_cmplt *p;
+
+			/* Procedure complete */
+			_radio.conn_curr->llcp_phy.ack =
+				_radio.conn_curr->llcp_phy.req;
+
+			/* generate phy update complete event */
+			radio_pdu_node_rx->hdr.type = NODE_RX_TYPE_PHY_UPDATE;
+
+			p = (struct radio_le_phy_upd_cmplt *)
+			    &pdu_data_rx->payload;
+			p->tx = _radio.conn_curr->phy_tx;
+			p->rx = _radio.conn_curr->phy_rx;
+
+			/* enqueue the phy update complete */
+			*rx_enqueue = 1;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 		} else {
 			struct pdu_data_llctrl *llctrl;
 
@@ -1935,10 +2006,10 @@ isr_rx_conn_pkt_ctrl(struct radio_pdu_node_rx *radio_pdu_node_rx,
 				*rx_enqueue = 1;
 				break;
 			}
-
-			/* Procedure complete */
-			_radio.conn_curr->procedure_expire = 0;
 		}
+
+		/* Procedure complete */
+		_radio.conn_curr->procedure_expire = 0;
 		break;
 
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH)
@@ -1948,6 +2019,76 @@ isr_rx_conn_pkt_ctrl(struct radio_pdu_node_rx *radio_pdu_node_rx,
 		break;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	case PDU_DATA_LLCTRL_TYPE_PHY_REQ:
+		if (_radio.role == ROLE_MASTER) {
+			if ((_radio.conn_curr->llcp_phy.ack !=
+			     _radio.conn_curr->llcp_phy.req) &&
+			    ((_radio.conn_curr->llcp_phy.state ==
+			      LLCP_PHY_STATE_ACK_WAIT) ||
+			     (_radio.conn_curr->llcp_phy.state ==
+			      LLCP_PHY_STATE_RSP_WAIT) ||
+			     (_radio.conn_curr->llcp_phy.state ==
+			      LLCP_PHY_STATE_UPD))) {
+				/* cross-over */
+				reject_ind_ext_send(_radio.conn_curr,
+					PDU_DATA_LLCTRL_TYPE_PHY_REQ,
+					0x23);
+			} else {
+				struct pdu_data_llctrl *c =
+					&pdu_data_rx->payload.llctrl;
+				struct pdu_data_llctrl_phy_req_rsp *p =
+					&c->ctrldata.phy_req;
+
+				_radio.conn_curr->llcp_phy.state =
+					LLCP_PHY_STATE_UPD;
+
+				if (_radio.conn_curr->llcp_phy.ack ==
+				    _radio.conn_curr->llcp_phy.req) {
+					_radio.conn_curr->llcp_phy.ack--;
+
+					_radio.conn_curr->llcp_phy.cmd = 0;
+
+					_radio.conn_curr->llcp_phy.tx =
+						_radio.conn_curr->phy_pref_tx;
+					_radio.conn_curr->llcp_phy.rx =
+						_radio.conn_curr->phy_pref_rx;
+				}
+
+				_radio.conn_curr->llcp_phy.tx &= p->rx_phys;
+				_radio.conn_curr->llcp_phy.rx &= p->tx_phys;
+			}
+		} else {
+			phy_rsp_send(_radio.conn_curr);
+		}
+		break;
+
+	case PDU_DATA_LLCTRL_TYPE_PHY_RSP:
+		if ((_radio.role == ROLE_MASTER) &&
+		    (_radio.conn_curr->llcp_phy.ack !=
+		     _radio.conn_curr->llcp_phy.req) &&
+		    (_radio.conn_curr->llcp_phy.state ==
+		     LLCP_PHY_STATE_RSP_WAIT)) {
+			struct pdu_data_llctrl_phy_req_rsp *p =
+				&pdu_data_rx->payload.llctrl.ctrldata.phy_rsp;
+
+			_radio.conn_curr->llcp_phy.state = LLCP_PHY_STATE_UPD;
+
+			_radio.conn_curr->llcp_phy.tx &= p->rx_phys;
+			_radio.conn_curr->llcp_phy.rx &= p->tx_phys;
+
+			/* Procedure timeout is stopped */
+			_radio.conn_curr->procedure_expire = 0;
+		}
+		break;
+
+	case PDU_DATA_LLCTRL_TYPE_PHY_UPD_IND:
+		if (phy_upd_ind(_radio.conn_curr, pdu_data_rx)) {
+			_radio.conn_curr->llcp_terminate.reason_peer = 0x28;
+		}
+		break;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 	default:
 		unknown_rsp_send(_radio.conn_curr,
 				 pdu_data_rx->payload.llctrl.opcode);
@@ -2538,17 +2679,24 @@ static inline void isr_close_conn(void)
 	if ((_radio.packet_counter != 0) && ((!SILENT_CONNECTION) ||
 					     (_radio.packet_counter != 0xFF))) {
 		if (_radio.role == ROLE_SLAVE) {
-			u32_t start_to_address_actual_us;
 			u32_t start_to_address_expected_us;
+			u32_t start_to_address_actual_us;
 			u32_t window_widening_event_us;
+			u32_t preamble_to_addr_us;
 
 			/* calculate the drift in ticks */
 			start_to_address_actual_us = radio_tmr_aa_get();
 			window_widening_event_us =
 				_radio.conn_curr->role.slave.window_widening_event_us;
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+			preamble_to_addr_us =
+				addr_us_get(_radio.conn_curr->phy_rx);
+#else /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+			preamble_to_addr_us = addr_us_get(0);
+#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
 			start_to_address_expected_us =
 				(RADIO_TICKER_JITTER_US << 1) +
-				RADIO_PREAMBLE_TO_ADDRESS_US +
+				preamble_to_addr_us +
 				window_widening_event_us;
 			if (start_to_address_actual_us <=
 			    start_to_address_expected_us) {
@@ -2562,7 +2710,7 @@ static inline void isr_close_conn(void)
 					TICKER_US_TO_TICKS(start_to_address_actual_us);
 				ticks_drift_minus =
 					TICKER_US_TO_TICKS((RADIO_TICKER_JITTER_US << 1) +
-							   RADIO_PREAMBLE_TO_ADDRESS_US);
+							   preamble_to_addr_us);
 			}
 
 
@@ -4298,19 +4446,19 @@ static u32_t access_addr_get(void)
 	return access_addr;
 }
 
-static void adv_obs_conn_configure(u8_t phy)
+static void adv_obs_conn_configure(void)
 {
 	radio_reset();
-	radio_phy_set(phy);
 	radio_tx_power_set(0);
 	radio_isr_set(isr);
 }
 
-static void adv_obs_configure(u8_t phy)
+static void adv_obs_configure(u8_t phy, u8_t flags)
 {
 	u32_t aa = 0x8e89bed6;
 
-	adv_obs_conn_configure(phy);
+	adv_obs_conn_configure();
+	radio_phy_set(phy, flags);
 	radio_aa_set((u8_t *)&aa);
 	radio_pkt_configure(8, PDU_AC_PAYLOAD_SIZE_MAX, (phy << 1));
 	radio_crc_configure(((0x5bUL) | ((0x06UL) << 8) | ((0x00UL) << 16)),
@@ -4393,7 +4541,7 @@ static void event_adv(u32_t ticks_at_expire, u32_t remainder,
 	_radio.ticker_id_event = RADIO_TICKER_ID_ADV;
 	_radio.ticks_anchor = ticks_at_expire;
 
-	adv_obs_configure(RADIO_PHY_ADV);
+	adv_obs_configure(0, 0); /* TODO: Advertisement PHY */
 
 	_radio.advertiser.chl_map_current = _radio.advertiser.chl_map;
 	adv_setup();
@@ -4572,7 +4720,7 @@ static void event_obs(u32_t ticks_at_expire, u32_t remainder, u16_t lazy,
 	_radio.ticks_anchor = ticks_at_expire;
 	_radio.observer.scan_state = 0;
 
-	adv_obs_configure(RADIO_PHY_ADV);
+	adv_obs_configure(0, 0); /* TODO: Advertisement PHY */
 
 	chan_set(37 + _radio.observer.scan_chan++);
 	if (_radio.observer.scan_chan == 3) {
@@ -5686,6 +5834,182 @@ static inline void event_len_prep(struct connection *conn)
 }
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+static inline void event_phy_req_prep(struct connection *conn)
+{
+	switch (conn->llcp_phy.state) {
+	case LLCP_PHY_STATE_REQ:
+	{
+		struct pdu_data_llctrl_phy_req_rsp *pr;
+		struct radio_pdu_node_tx *node_tx;
+		struct pdu_data *pdu_ctrl_tx;
+
+		node_tx = mem_acquire(&_radio.pkt_tx_ctrl_free);
+		if (!node_tx) {
+			break;
+		}
+
+		conn->llcp_phy.state = LLCP_PHY_STATE_ACK_WAIT;
+
+		/* update preferred phy */
+		conn->phy_pref_tx = conn->llcp_phy.tx;
+		conn->phy_pref_rx = conn->llcp_phy.rx;
+
+		/* place the phy req packet as next in tx queue */
+		pdu_ctrl_tx = (struct pdu_data *) node_tx->pdu_data;
+		pdu_ctrl_tx->ll_id = PDU_DATA_LLID_CTRL;
+		pdu_ctrl_tx->len = offsetof(struct pdu_data_llctrl, ctrldata) +
+				   sizeof(struct pdu_data_llctrl_phy_req_rsp);
+		pdu_ctrl_tx->payload.llctrl.opcode =
+			PDU_DATA_LLCTRL_TYPE_PHY_REQ;
+
+		pr = (struct pdu_data_llctrl_phy_req_rsp *)
+		     &pdu_ctrl_tx->payload.llctrl.ctrldata.phy_req;
+		pr->tx_phys = conn->llcp_phy.tx;
+		pr->rx_phys = conn->llcp_phy.rx;
+
+		ctrl_tx_enqueue(conn, node_tx);
+
+		/* Start Procedure Timeout (TODO: this shall not replace
+		 * terminate procedure).
+		 */
+		conn->procedure_expire = conn->procedure_reload;
+	}
+	break;
+
+	case LLCP_PHY_STATE_UPD:
+	{
+		u8_t tx, rx;
+
+		/* Procedure complete */
+		conn->llcp_phy.ack = conn->llcp_phy.req;
+
+		/* select the phy */
+		if (conn->llcp_phy.tx) {
+			tx = conn->llcp_phy.tx;
+		} else {
+			tx = conn->phy_tx;
+		}
+		if (tx & BIT(1)) {
+			tx = BIT(1);
+		} else if (tx & BIT(0)) {
+			tx = BIT(0);
+		} else if (tx & BIT(2)) {
+			tx = BIT(2);
+		} else {
+			tx = BIT(0);
+		}
+		if (conn->llcp_phy.rx) {
+			rx = conn->llcp_phy.rx;
+		} else {
+			rx = conn->phy_rx;
+		}
+		if (rx & BIT(1)) {
+			rx = BIT(1);
+		} else if (rx & BIT(0)) {
+			rx = BIT(0);
+		} else if (rx & BIT(2)) {
+			rx = BIT(2);
+		} else {
+			rx = BIT(0);
+		}
+
+		/* Intiate PHY Update Ind */
+		conn->llcp.phy_upd_ind.m_s = tx;
+		conn->llcp.phy_upd_ind.s_m = rx;
+		/* conn->llcp.phy_upd_ind.instant = 0; */
+		conn->llcp.phy_upd_ind.initiate = 1;
+		conn->llcp.phy_upd_ind.cmd = conn->llcp_phy.cmd;
+
+		conn->llcp_type = LLCP_PHY_UPD;
+		conn->llcp_ack--;
+	}
+	break;
+
+	case LLCP_PHY_STATE_ACK_WAIT:
+	case LLCP_PHY_STATE_RSP_WAIT:
+		/* no nothing */
+		break;
+
+	default:
+		LL_ASSERT(0);
+		break;
+	}
+}
+
+static inline void event_phy_upd_ind_prep(struct connection *conn,
+					  u16_t event_counter)
+{
+	if (conn->llcp.phy_upd_ind.initiate) {
+		struct radio_pdu_node_tx *node_tx;
+
+		node_tx = mem_acquire(&_radio.pkt_tx_ctrl_free);
+		if (node_tx) {
+			struct pdu_data *pdu_ctrl_tx = (struct pdu_data *)
+						       node_tx->pdu_data;
+			struct pdu_data_llctrl_phy_upd_ind *p;
+
+			/* reset initiate flag */
+			conn->llcp.phy_upd_ind.initiate = 0;
+
+			/* set instant */
+			conn->llcp.phy_upd_ind.instant = event_counter +
+							 conn->latency + 6;
+
+			/* place the phy update ind packet as next in
+			 * tx queue
+			 */
+			pdu_ctrl_tx->ll_id = PDU_DATA_LLID_CTRL;
+			pdu_ctrl_tx->len = offsetof(struct pdu_data_llctrl,
+						    ctrldata) +
+				sizeof(struct pdu_data_llctrl_phy_upd_ind);
+			pdu_ctrl_tx->payload.llctrl.opcode =
+				PDU_DATA_LLCTRL_TYPE_PHY_UPD_IND;
+			p = &pdu_ctrl_tx->payload.llctrl.ctrldata.phy_upd_ind;
+			p->m_to_s_phy = conn->llcp.phy_upd_ind.m_s;
+			p->s_to_m_phy = conn->llcp.phy_upd_ind.s_m;
+			p->instant = conn->llcp.phy_upd_ind.instant;
+
+			ctrl_tx_enqueue(conn, node_tx);
+		}
+	} else if (((event_counter - conn->llcp.phy_upd_ind.instant) & 0xFFFF)
+			    <= 0x7FFF) {
+		struct radio_pdu_node_rx *node_rx;
+		struct radio_le_phy_upd_cmplt *p;
+		struct pdu_data *pdu_data;
+		u8_t old_tx, old_rx;
+
+		/* procedure request acked */
+		conn->llcp_ack = conn->llcp_req;
+
+		/* apply new phy */
+		old_tx = conn->phy_tx;
+		old_rx = conn->phy_rx;
+		conn->phy_tx = conn->llcp.phy_upd_ind.m_s;
+		conn->phy_rx = conn->llcp.phy_upd_ind.s_m;
+
+		/* generate event if phy changed or initiated by cmd */
+		if (!conn->llcp.phy_upd_ind.cmd && (conn->phy_tx == old_tx) &&
+		    (conn->phy_rx == old_rx)) {
+			return;
+		}
+
+		node_rx = packet_rx_reserve_get(2);
+		LL_ASSERT(node_rx);
+
+		node_rx->hdr.handle = conn->handle;
+		node_rx->hdr.type = NODE_RX_TYPE_PHY_UPDATE;
+
+		pdu_data = (struct pdu_data *)&node_rx->pdu_data;
+		p = (struct radio_le_phy_upd_cmplt *)&pdu_data->payload;
+		p->tx = conn->phy_tx;
+		p->rx = conn->phy_rx;
+
+		packet_rx_enqueue();
+	}
+}
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 static void event_connection_prepare(u32_t ticks_at_expire,
 				     u32_t remainder, u16_t lazy,
 				     struct connection *conn)
@@ -5748,6 +6072,12 @@ static void event_connection_prepare(u32_t ticks_at_expire,
 			break;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_LE_PING */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		case LLCP_PHY_UPD:
+			event_phy_upd_ind_prep(conn, event_counter);
+			break;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 		default:
 			LL_ASSERT(0);
 			break;
@@ -5803,6 +6133,22 @@ static void event_connection_prepare(u32_t ticks_at_expire,
 	}
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	/* check if PHY Req procedure is requested and no other procedure
+	 * using instant is active.
+	 */
+	if ((conn->llcp_ack == conn->llcp_req) &&
+	    (conn->llcp_phy.ack != conn->llcp_phy.req)) {
+		/* Stop previous event, to avoid Radio DMA corrupting the
+		 * rx queue
+		 */
+		event_stop(0, 0, 0, (void *)STATE_ABORT);
+
+		/* handle PHY Upd state machine */
+		event_phy_req_prep(conn);
+	}
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 	/* Setup XTAL startup and radio active events */
 	event_common_prepare(ticks_at_expire, remainder,
 			     &conn->hdr.ticks_xtal_to_start,
@@ -5818,10 +6164,7 @@ static void event_connection_prepare(u32_t ticks_at_expire,
 
 static void connection_configure(struct connection *conn)
 {
-	u8_t phy;
-
-	phy = RADIO_PHY_CONN;
-	adv_obs_conn_configure(phy);
+	adv_obs_conn_configure();
 	radio_aa_set(conn->access_addr);
 	radio_crc_configure(((0x5bUL) | ((0x06UL) << 8) | ((0x00UL) << 16)),
 			    (((u32_t)conn->crc_init[2] << 16) |
@@ -5842,9 +6185,10 @@ static void event_slave_prepare(u32_t ticks_at_expire, u32_t remainder,
 static void event_slave(u32_t ticks_at_expire, u32_t remainder, u16_t lazy,
 			void *context)
 {
-	u8_t data_chan_use = 0;
 	struct connection *conn;
+	u8_t data_chan_use = 0;
 	u32_t remainder_us;
+	u32_t hcto;
 
 	ARG_UNUSED(remainder);
 	ARG_UNUSED(lazy);
@@ -5922,12 +6266,18 @@ static void event_slave(u32_t ticks_at_expire, u32_t remainder, u16_t lazy,
 				TICKER_US_TO_TICKS(RADIO_TICKER_START_PART_US),
 				_radio.remainder_anchor);
 	radio_tmr_aa_capture();
-	radio_tmr_hcto_configure(0 + remainder_us + RADIO_RX_READY_DELAY_US +
-				 (conn->role.slave.window_widening_event_us << 1) +
-/* +/- 16 us of BLE jitter plus own implementation drift unit of 30.51 us. */
-			       (RADIO_TICKER_JITTER_US << 2) +
-			       RADIO_PREAMBLE_TO_ADDRESS_US +
-			       conn->role.slave.window_size_event_us);
+	hcto = remainder_us + (RADIO_TICKER_JITTER_US << 2) +
+	       RADIO_RX_READY_DELAY_US +
+	       (conn->role.slave.window_widening_event_us << 1) +
+	       conn->role.slave.window_size_event_us;
+
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	hcto += addr_us_get(conn->phy_rx);
+#else /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+	hcto += addr_us_get(0);
+#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
+	radio_tmr_hcto_configure(hcto);
 	radio_tmr_end_capture();
 
 #if (defined(CONFIG_BLUETOOTH_CONTROLLER_XTAL_ADVANCED) && \
@@ -6054,6 +6404,7 @@ static void event_master(u32_t ticks_at_expire, u32_t remainder, u16_t lazy,
 	/* silent connection! */
 	else {
 		u32_t remainder_us;
+		u32_t hcto;
 
 		/* start in RX state */
 		_radio.state = STATE_RX;
@@ -6070,12 +6421,20 @@ static void event_master(u32_t ticks_at_expire, u32_t remainder, u16_t lazy,
 					TICKER_US_TO_TICKS(RADIO_TICKER_START_PART_US),
 					_radio.remainder_anchor);
 		radio_tmr_aa_capture();
-#define QUICK_FIX_EXTRA_WINDOW 230
-		radio_tmr_hcto_configure(0 + remainder_us +
-					 RADIO_TX_READY_DELAY_US + 230 +
-					 RADIO_PREAMBLE_TO_ADDRESS_US +
-					 QUICK_FIX_EXTRA_WINDOW);
-#undef QUICK_FIX_EXTRA_WINDOW
+
+		hcto = remainder_us + RADIO_TX_READY_DELAY_US + RADIO_TIFS;
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		hcto += empty_pkt_us_get(conn->phy_rx);
+		hcto += addr_us_get(conn->phy_rx);
+#else /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+		hcto += empty_pkt_us_get(0);
+		hcto += addr_us_get(0);
+#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
+		/* TODO: account for slave window widening */
+		hcto += 256;
+
+		radio_tmr_hcto_configure(hcto);
 	}
 #endif
 
@@ -6109,8 +6468,8 @@ static void event_master(u32_t ticks_at_expire, u32_t remainder, u16_t lazy,
 
 static void rx_packet_set(struct connection *conn, struct pdu_data *pdu_data_rx)
 {
-	u8_t phy;
 	u16_t max_rx_octets;
+	u8_t phy;
 
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH)
 	max_rx_octets = conn->max_rx_octets;
@@ -6118,7 +6477,14 @@ static void rx_packet_set(struct connection *conn, struct pdu_data *pdu_data_rx)
 	max_rx_octets = RADIO_LL_LENGTH_OCTETS_RX_MIN;
 #endif /* !CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
-	phy = RADIO_PHY_CONN;
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	phy = conn->phy_rx;
+#else /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+	phy = 0;
+#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
+	radio_phy_set(phy, 0);
+
 	if (conn->enc_rx) {
 		radio_pkt_configure(8, (max_rx_octets + 4), (phy << 1) | 0x01);
 
@@ -6133,8 +6499,8 @@ static void rx_packet_set(struct connection *conn, struct pdu_data *pdu_data_rx)
 
 static void tx_packet_set(struct connection *conn, struct pdu_data *pdu_data_tx)
 {
-	u8_t phy;
 	u16_t max_tx_octets;
+	u8_t phy, flags;
 
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH)
 	max_tx_octets = conn->max_tx_octets;
@@ -6142,7 +6508,16 @@ static void tx_packet_set(struct connection *conn, struct pdu_data *pdu_data_tx)
 	max_tx_octets = RADIO_LL_LENGTH_OCTETS_RX_MIN;
 #endif /* !CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
-	phy = RADIO_PHY_CONN;
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	phy = conn->phy_tx;
+	flags = conn->phy_flags;
+#else /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+	phy = 0;
+	flags = 0;
+#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
+	radio_phy_set(phy, flags);
+
 	if (conn->enc_tx) {
 		radio_pkt_configure(8, (max_tx_octets + 4), (phy << 1) | 0x01);
 
@@ -6766,6 +7141,39 @@ static u32_t chan_map_update(struct connection *conn,
 	return 0;
 }
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+static u32_t phy_upd_ind(struct connection *conn, struct pdu_data *pdu_data_rx)
+{
+	struct pdu_data_llctrl_phy_upd_ind *p;
+
+	p = &pdu_data_rx->payload.llctrl.ctrldata.phy_upd_ind;
+	if (((p->instant - conn->event_counter) & 0xffff) > 0x7fff) {
+		return 1;
+	}
+
+	LL_ASSERT(conn->llcp_req == conn->llcp_ack);
+
+	if ((conn->llcp_phy.ack != conn->llcp_phy.req) &&
+	    (conn->llcp_phy.state == LLCP_PHY_STATE_RSP_WAIT)) {
+		conn->llcp_phy.ack = conn->llcp_phy.req;
+		conn->llcp.phy_upd_ind.cmd = conn->llcp_phy.cmd;
+
+		/* Procedure complete, just wait for instant */
+		_radio.conn_curr->procedure_expire = 0;
+	}
+
+	conn->llcp.phy_upd_ind.m_s = p->m_to_s_phy;
+	conn->llcp.phy_upd_ind.s_m = p->s_to_m_phy;
+	conn->llcp.phy_upd_ind.instant = p->instant;
+	conn->llcp.phy_upd_ind.initiate = 0;
+
+	conn->llcp_type = LLCP_PHY_UPD;
+	conn->llcp_ack--;
+
+	return 0;
+}
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 static void enc_req_reused_send(struct connection *conn,
 				struct radio_pdu_node_tx *node_tx)
 {
@@ -7007,6 +7415,30 @@ static void length_resp_send(struct connection *conn, u16_t eff_rx_octets,
 }
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+static void phy_rsp_send(struct connection *conn)
+{
+	struct radio_pdu_node_tx *node_tx;
+	struct pdu_data *pdu_ctrl_tx;
+
+	/* acquire tx mem */
+	node_tx = mem_acquire(&_radio.pkt_tx_ctrl_free);
+	LL_ASSERT(node_tx);
+
+	pdu_ctrl_tx = (struct pdu_data *)node_tx->pdu_data;
+	pdu_ctrl_tx->ll_id = PDU_DATA_LLID_CTRL;
+	pdu_ctrl_tx->len = offsetof(struct pdu_data_llctrl, ctrldata) +
+			   sizeof(struct pdu_data_llctrl_phy_req_rsp);
+	pdu_ctrl_tx->payload.llctrl.opcode = PDU_DATA_LLCTRL_TYPE_PHY_RSP;
+	pdu_ctrl_tx->payload.llctrl.ctrldata.phy_rsp.tx_phys =
+		conn->phy_pref_tx;
+	pdu_ctrl_tx->payload.llctrl.ctrldata.phy_rsp.rx_phys =
+		conn->phy_pref_rx;
+
+	ctrl_tx_enqueue(conn, node_tx);
+}
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 void radio_ticks_active_to_start_set(u32_t ticks_active_to_start)
 {
 	_radio.ticks_active_to_start = ticks_active_to_start;
@@ -7372,6 +7804,14 @@ u32_t radio_adv_enable(u16_t interval, u8_t chl_map, u8_t filter_policy)
 		conn->max_rx_octets = RADIO_LL_LENGTH_OCTETS_RX_MIN;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		conn->phy_pref_tx = 0; /* TODO: pick from a global hci value */
+		conn->phy_tx = 0;
+		conn->phy_flags = 0;
+		conn->phy_pref_rx = 0; /* TODO: pick from a global hci value */
+		conn->phy_rx = 0;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 		conn->role.slave.role = 1;
 		conn->role.slave.latency_cancel = 0;
 		conn->role.slave.window_widening_prepare_us = 0;
@@ -7399,6 +7839,11 @@ u32_t radio_adv_enable(u16_t interval, u8_t chl_map, u8_t filter_policy)
 		conn->llcp_length.ack = 0;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		conn->llcp_phy.req = 0;
+		conn->llcp_phy.ack = 0;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 		conn->sn = 0;
 		conn->nesn = 0;
 		conn->pause_rx = 0;
@@ -7713,6 +8158,14 @@ u32_t radio_connect_enable(u8_t adv_addr_type, u8_t *adv_addr, u16_t interval,
 	conn->max_rx_octets = RADIO_LL_LENGTH_OCTETS_RX_MIN;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	conn->phy_pref_tx = 0; /* TODO: pick from a global hci value */
+	conn->phy_tx = 0;
+	conn->phy_flags = 0;
+	conn->phy_pref_rx = 0; /* TODO: pick from a global hci value */
+	conn->phy_rx = 0;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 	conn->role.master.role = 0;
 	conn->role.master.connect_expire = 6;
 	conn_interval_us =
@@ -7754,6 +8207,11 @@ u32_t radio_connect_enable(u8_t adv_addr_type, u8_t *adv_addr, u16_t interval,
 	conn->llcp_length.ack = 0;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	conn->llcp_phy.req = 0;
+	conn->llcp_phy.ack = 0;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 	conn->sn = 0;
 	conn->nesn = 0;
 	conn->pause_rx = 0;
@@ -8092,6 +8550,27 @@ void ll_length_max_get(u16_t *max_tx_octets, u16_t *max_tx_time,
 }
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+u32_t ll_phy_req_send(u16_t handle, u8_t tx, u8_t rx)
+{
+	struct connection *conn;
+
+	conn = connection_get(handle);
+	if (!conn || (conn->llcp_req != conn->llcp_ack) ||
+	    (conn->llcp_phy.req != conn->llcp_phy.ack)) {
+		return 1;
+	}
+
+	conn->llcp_phy.state = LLCP_PHY_STATE_REQ;
+	conn->llcp_phy.cmd = 1;
+	conn->llcp_phy.tx = tx;
+	conn->llcp_phy.rx = rx;
+	conn->llcp_phy.req++;
+
+	return 0;
+}
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 static u8_t tx_cmplt_get(u16_t *handle, u8_t *first, u8_t last)
 {
 	u8_t _first;
@@ -8213,6 +8692,10 @@ void radio_rx_dequeue(void)
 
 	case NODE_RX_TYPE_CHAN_SEL_ALGO:
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	case NODE_RX_TYPE_PHY_UPDATE:
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_CONN_RSSI)
 	case NODE_RX_TYPE_RSSI:
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_CONN_RSSI */
@@ -8267,6 +8750,10 @@ void radio_rx_mem_release(struct radio_pdu_node_rx **radio_pdu_node_rx)
 
 		case NODE_RX_TYPE_CHAN_SEL_ALGO:
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		case NODE_RX_TYPE_PHY_UPDATE:
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_CONN_RSSI)
 		case NODE_RX_TYPE_RSSI:
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_CONN_RSSI */
diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.h b/subsys/bluetooth/controller/ll_sw/ctrl.h
index fcb0351cdd0708..2d0c71fdbebcbe 100644
--- a/subsys/bluetooth/controller/ll_sw/ctrl.h
+++ b/subsys/bluetooth/controller/ll_sw/ctrl.h
@@ -210,6 +210,10 @@ enum radio_pdu_node_rx_type {
 
 	NODE_RX_TYPE_CHAN_SEL_ALGO,
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	NODE_RX_TYPE_PHY_UPDATE,
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_CONN_RSSI)
 	NODE_RX_TYPE_RSSI,
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_CONN_RSSI */
@@ -248,6 +252,11 @@ struct radio_le_chan_sel_algo {
 	u8_t chan_sel_algo;
 } __packed;
 
+struct radio_le_phy_upd_cmplt {
+	u8_t tx;
+	u8_t rx;
+} __packed;
+
 struct radio_pdu_node_rx_hdr {
 	union {
 		sys_snode_t node; /* used by slist */
diff --git a/subsys/bluetooth/controller/ll_sw/ctrl_internal.h b/subsys/bluetooth/controller/ll_sw/ctrl_internal.h
index 838c2f35edb6e5..6e8a97a4150f0a 100644
--- a/subsys/bluetooth/controller/ll_sw/ctrl_internal.h
+++ b/subsys/bluetooth/controller/ll_sw/ctrl_internal.h
@@ -17,6 +17,10 @@ enum llcp {
 #if defined(CONFIG_BLUETOOTH_CONTROLLER_LE_PING)
 	LLCP_PING,
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_LE_PING */
+
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	LLCP_PHY_UPD,
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
 };
 
 
@@ -60,6 +64,14 @@ struct connection {
 	u16_t max_rx_octets;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	u8_t phy_pref_tx:3;
+	u8_t phy_tx:3;
+	u8_t phy_flags:1;
+	u8_t phy_pref_rx:3;
+	u8_t phy_rx:3;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 	u16_t supervision_reload;
 	u16_t supervision_expire;
 	u16_t procedure_reload;
@@ -131,6 +143,17 @@ struct connection {
 			u8_t  chm[5];
 			u16_t instant;
 		} chan_map;
+
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+		struct {
+			u8_t initiate:1;
+			u8_t cmd:1;
+			u8_t m_s:3;
+			u8_t s_m:3;
+			u16_t instant;
+		} phy_upd_ind;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 		struct {
 			u8_t  error_code;
 			u8_t  rand[8];
@@ -175,6 +198,21 @@ struct connection {
 	} llcp_length;
 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */
 
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY)
+	struct {
+		u8_t req;
+		u8_t ack;
+		u8_t state:2;
+#define LLCP_PHY_STATE_REQ      0
+#define LLCP_PHY_STATE_ACK_WAIT 1
+#define LLCP_PHY_STATE_RSP_WAIT 2
+#define LLCP_PHY_STATE_UPD      3
+		u8_t tx:3;
+		u8_t rx:3;
+		u8_t cmd:1;
+	} llcp_phy;
+#endif /* CONFIG_BLUETOOTH_CONTROLLER_PHY */
+
 	u8_t  sn:1;
 	u8_t  nesn:1;
 	u8_t  pause_rx:1;
diff --git a/subsys/bluetooth/controller/ll_sw/pdu.h b/subsys/bluetooth/controller/ll_sw/pdu.h
index 4ba91905cc48df..7764aa5e121ee0 100644
--- a/subsys/bluetooth/controller/ll_sw/pdu.h
+++ b/subsys/bluetooth/controller/ll_sw/pdu.h
@@ -121,7 +121,7 @@ enum pdu_data_llctrl_type {
 	PDU_DATA_LLCTRL_TYPE_LENGTH_RSP = 0x15,
 	PDU_DATA_LLCTRL_TYPE_PHY_REQ = 0x16,
 	PDU_DATA_LLCTRL_TYPE_PHY_RSP = 0x17,
-	PDU_DATA_LLCTRL_TYPE_PHY_UPDATE_IND = 0x18,
+	PDU_DATA_LLCTRL_TYPE_PHY_UPD_IND = 0x18,
 	PDU_DATA_LLCTRL_TYPE_MIN_USED_CHAN_IND = 0x19,
 };
 
@@ -224,7 +224,7 @@ struct pdu_data_llctrl_phy_req_rsp {
 	u8_t rx_phys;
 } __packed;
 
-struct pdu_data_llctrl_phy_update_ind {
+struct pdu_data_llctrl_phy_upd_ind {
 	u8_t  m_to_s_phy;
 	u8_t  s_to_m_phy;
 	u16_t instant;
@@ -256,7 +256,7 @@ struct pdu_data_llctrl {
 		struct pdu_data_llctrl_length_req_rsp length_rsp;
 		struct pdu_data_llctrl_phy_req_rsp phy_req;
 		struct pdu_data_llctrl_phy_req_rsp phy_rsp;
-		struct pdu_data_llctrl_phy_update_ind phy_update_ind;
+		struct pdu_data_llctrl_phy_upd_ind phy_upd_ind;
 		struct pdu_data_llctrl_min_used_chans_ind min_used_chans_ind;
 	} __packed ctrldata;
 } __packed;