diff --git a/subsys/bluetooth/host/hci_core.c b/subsys/bluetooth/host/hci_core.c index fc91d230b7a..30823c6c67b 100644 --- a/subsys/bluetooth/host/hci_core.c +++ b/subsys/bluetooth/host/hci_core.c @@ -668,6 +668,27 @@ static int hci_le_read_remote_features(struct bt_conn *conn) return 0; } +static int hci_le_set_phy(struct bt_conn *conn) +{ + struct bt_hci_cp_le_set_phy *cp; + struct net_buf *buf; + + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_PHY, sizeof(*cp)); + if (!buf) { + return -ENOBUFS; + } + + cp = net_buf_add(buf, sizeof(*cp)); + cp->handle = sys_cpu_to_le16(conn->handle); + cp->all_phys = 0; + cp->tx_phys = BT_HCI_LE_PHY_PREFER_2M; + cp->rx_phys = BT_HCI_LE_PHY_PREFER_2M; + cp->phy_opts = BT_HCI_LE_PHY_CODED_ANY; + bt_hci_cmd_send(BT_HCI_OP_LE_SET_PHY, buf); + + return 0; +} + static void update_conn_param(struct bt_conn *conn) { /* @@ -805,6 +826,14 @@ static void le_conn_complete(struct net_buf *buf) } } + if (BT_FEAT_LE_PHY_2M(bt_dev.le.features) || + BT_FEAT_LE_PHY_CODED(bt_dev.le.features)) { + err = hci_le_set_phy(conn); + if (!err) { + goto done; + } + } + update_conn_param(conn); done: @@ -829,6 +858,37 @@ static void le_remote_feat_complete(struct net_buf *buf) sizeof(conn->le.features)); } + if (BT_FEAT_LE_PHY_2M(bt_dev.le.features) || + BT_FEAT_LE_PHY_CODED(bt_dev.le.features)) { + int err; + + err = hci_le_set_phy(conn); + if (!err) { + goto done; + } + } + + update_conn_param(conn); +done: + bt_conn_unref(conn); +} + +static void le_phy_update_complete(struct net_buf *buf) +{ + struct bt_hci_evt_le_phy_update_complete *evt = (void *)buf->data; + u16_t handle = sys_le16_to_cpu(evt->handle); + struct bt_conn *conn; + + conn = bt_conn_lookup_handle(handle); + if (!conn) { + BT_ERR("Unable to lookup conn for handle %u", handle); + return; + } + + if (!evt->status) { + BT_DBG("PHY updated: tx: %u, rx: %u", evt->tx_phy, evt->rx_phy); + } + update_conn_param(conn); bt_conn_unref(conn); @@ -2643,6 +2703,9 @@ static void hci_le_meta_event(struct net_buf *buf) case BT_HCI_EVT_LE_CONN_PARAM_REQ: le_conn_param_req(buf); break; + case BT_HCI_EVT_LE_PHY_UPDATE_COMPLETE: + le_phy_update_complete(buf); + break; #endif /* CONFIG_BLUETOOTH_CONN */ #if defined(CONFIG_BLUETOOTH_SMP) case BT_HCI_EVT_LE_LTK_REQUEST: @@ -3103,6 +3166,10 @@ static int le_set_event_mask(void) if (BT_FEAT_LE_CONN_PARAM_REQ_PROC(bt_dev.le.features)) { mask |= BT_EVT_MASK_LE_CONN_PARAM_REQ; } + if (BT_FEAT_LE_PHY_2M(bt_dev.le.features) || + BT_FEAT_LE_PHY_CODED(bt_dev.le.features)) { + mask |= BT_EVT_MASK_LE_PHY_UPDATE_COMPLETE; + } } if (IS_ENABLED(CONFIG_BLUETOOTH_SMP) &&