Bluetooth: Add initial implementation of CIS connections

+32 -1

include/net/bluetooth/bluetooth.h

··· 55 55 #define BTPROTO_CMTP 5 56 56 #define BTPROTO_HIDP 6 57 57 #define BTPROTO_AVDTP 7 58 + #define BTPROTO_ISO 8 59 + #define BTPROTO_LAST BTPROTO_ISO 58 60 59 61 #define SOL_HCI 0 60 62 #define SOL_L2CAP 6 ··· 151 149 #define BT_MODE_LE_FLOWCTL 0x03 152 150 #define BT_MODE_EXT_FLOWCTL 0x04 153 151 154 - #define BT_PKT_STATUS 16 152 + #define BT_PKT_STATUS 16 155 153 156 154 #define BT_SCM_PKT_STATUS 0x03 155 + 156 + #define BT_ISO_QOS 17 157 + 158 + #define BT_ISO_QOS_CIG_UNSET 0xff 159 + #define BT_ISO_QOS_CIS_UNSET 0xff 160 + 161 + struct bt_iso_io_qos { 162 + __u32 interval; 163 + __u16 latency; 164 + __u16 sdu; 165 + __u8 phy; 166 + __u8 rtn; 167 + }; 168 + 169 + struct bt_iso_qos { 170 + __u8 cig; 171 + __u8 cis; 172 + __u8 sca; 173 + __u8 packing; 174 + __u8 framing; 175 + struct bt_iso_io_qos in; 176 + struct bt_iso_io_qos out; 177 + }; 178 + 179 + #define BT_ISO_PHY_1M 0x01 180 + #define BT_ISO_PHY_2M 0x02 181 + #define BT_ISO_PHY_CODED 0x04 182 + #define BT_ISO_PHY_ANY (BT_ISO_PHY_1M | BT_ISO_PHY_2M | \ 183 + BT_ISO_PHY_CODED) 157 184 158 185 #define BT_CODEC 19 159 186

+26 -2

include/net/bluetooth/hci.h

··· 1989 1989 struct hci_cis_params { 1990 1990 __u8 cis_id; 1991 1991 __le16 c_sdu; 1992 - __le16 p_pdu; 1992 + __le16 p_sdu; 1993 1993 __u8 c_phy; 1994 1994 __u8 p_phy; 1995 1995 __u8 c_rtn; ··· 2000 2000 __u8 cig_id; 2001 2001 __u8 c_interval[3]; 2002 2002 __u8 p_interval[3]; 2003 - __u8 wc_sca; 2003 + __u8 sca; 2004 2004 __u8 packing; 2005 2005 __u8 framing; 2006 2006 __le16 c_latency; ··· 2041 2041 struct hci_cp_le_reject_cis { 2042 2042 __le16 handle; 2043 2043 __u8 reason; 2044 + } __packed; 2045 + 2046 + #define HCI_OP_LE_SETUP_ISO_PATH 0x206e 2047 + struct hci_cp_le_setup_iso_path { 2048 + __le16 handle; 2049 + __u8 direction; 2050 + __u8 path; 2051 + __u8 codec; 2052 + __le16 codec_cid; 2053 + __le16 codec_vid; 2054 + __u8 delay[3]; 2055 + __u8 codec_cfg_len; 2056 + __u8 codec_cfg[0]; 2057 + } __packed; 2058 + 2059 + struct hci_rp_le_setup_iso_path { 2060 + __u8 status; 2061 + __le16 handle; 2062 + } __packed; 2063 + 2064 + #define HCI_OP_LE_SET_HOST_FEATURE 0x2074 2065 + struct hci_cp_le_set_host_feature { 2066 + __u8 bit_number; 2067 + __u8 bit_value; 2044 2068 } __packed; 2045 2069 2046 2070 /* ---- HCI Events ---- */

+106 -1

include/net/bluetooth/hci_core.h

··· 126 126 unsigned int acl_num; 127 127 unsigned int amp_num; 128 128 unsigned int sco_num; 129 + unsigned int iso_num; 129 130 unsigned int le_num; 130 131 unsigned int le_num_peripheral; 131 132 }; ··· 475 474 unsigned int acl_cnt; 476 475 unsigned int sco_cnt; 477 476 unsigned int le_cnt; 477 + unsigned int iso_cnt; 478 478 479 479 unsigned int acl_mtu; 480 480 unsigned int sco_mtu; 481 481 unsigned int le_mtu; 482 + unsigned int iso_mtu; 482 483 unsigned int acl_pkts; 483 484 unsigned int sco_pkts; 484 485 unsigned int le_pkts; 486 + unsigned int iso_pkts; 485 487 486 488 __u16 block_len; 487 489 __u16 block_mtu; ··· 661 657 CONN_REASON_PAIR_DEVICE, 662 658 CONN_REASON_L2CAP_CHAN, 663 659 CONN_REASON_SCO_CONNECT, 660 + CONN_REASON_ISO_CONNECT, 664 661 }; 665 662 666 663 struct hci_conn { ··· 714 709 __s8 rssi; 715 710 __s8 tx_power; 716 711 __s8 max_tx_power; 712 + struct bt_iso_qos iso_qos; 717 713 unsigned long flags; 718 714 719 715 enum conn_reasons conn_reason; ··· 745 739 struct hci_dev *hdev; 746 740 void *l2cap_data; 747 741 void *sco_data; 742 + void *iso_data; 748 743 struct amp_mgr *amp_mgr; 749 744 750 745 struct hci_conn *link; ··· 754 747 void (*connect_cfm_cb) (struct hci_conn *conn, u8 status); 755 748 void (*security_cfm_cb) (struct hci_conn *conn, u8 status); 756 749 void (*disconn_cfm_cb) (struct hci_conn *conn, u8 reason); 750 + 751 + void (*cleanup)(struct hci_conn *conn); 757 752 }; 758 753 759 754 struct hci_chan { ··· 963 954 case ESCO_LINK: 964 955 h->sco_num++; 965 956 break; 957 + case ISO_LINK: 958 + h->iso_num++; 959 + break; 966 960 } 967 961 } 968 962 ··· 992 980 case ESCO_LINK: 993 981 h->sco_num--; 994 982 break; 983 + case ISO_LINK: 984 + h->iso_num--; 985 + break; 995 986 } 996 987 } 997 988 ··· 1011 996 case SCO_LINK: 1012 997 case ESCO_LINK: 1013 998 return h->sco_num; 999 + case ISO_LINK: 1000 + return h->iso_num; 1014 1001 default: 1015 1002 return 0; 1016 1003 } ··· 1022 1005 { 1023 1006 struct hci_conn_hash *c = &hdev->conn_hash; 1024 1007 1025 - return c->acl_num + c->amp_num + c->sco_num + c->le_num; 1008 + return c->acl_num + c->amp_num + c->sco_num + c->le_num + c->iso_num; 1026 1009 } 1027 1010 1028 1011 static inline __u8 hci_conn_lookup_type(struct hci_dev *hdev, __u16 handle) ··· 1108 1091 return NULL; 1109 1092 } 1110 1093 1094 + static inline struct hci_conn *hci_conn_hash_lookup_cis(struct hci_dev *hdev, 1095 + bdaddr_t *ba, 1096 + __u8 ba_type) 1097 + { 1098 + struct hci_conn_hash *h = &hdev->conn_hash; 1099 + struct hci_conn *c; 1100 + 1101 + rcu_read_lock(); 1102 + 1103 + list_for_each_entry_rcu(c, &h->list, list) { 1104 + if (c->type != ISO_LINK) 1105 + continue; 1106 + 1107 + if (ba_type == c->dst_type && !bacmp(&c->dst, ba)) { 1108 + rcu_read_unlock(); 1109 + return c; 1110 + } 1111 + } 1112 + 1113 + rcu_read_unlock(); 1114 + 1115 + return NULL; 1116 + } 1117 + 1118 + static inline struct hci_conn *hci_conn_hash_lookup_cig(struct hci_dev *hdev, 1119 + __u8 handle) 1120 + { 1121 + struct hci_conn_hash *h = &hdev->conn_hash; 1122 + struct hci_conn *c; 1123 + 1124 + rcu_read_lock(); 1125 + 1126 + list_for_each_entry_rcu(c, &h->list, list) { 1127 + if (c->type != ISO_LINK) 1128 + continue; 1129 + 1130 + if (handle == c->iso_qos.cig) { 1131 + rcu_read_unlock(); 1132 + return c; 1133 + } 1134 + } 1135 + 1136 + rcu_read_unlock(); 1137 + 1138 + return NULL; 1139 + } 1140 + 1111 1141 static inline struct hci_conn *hci_conn_hash_lookup_state(struct hci_dev *hdev, 1112 1142 __u8 type, __u16 state) 1113 1143 { ··· 1173 1109 rcu_read_unlock(); 1174 1110 1175 1111 return NULL; 1112 + } 1113 + 1114 + typedef void (*hci_conn_func_t)(struct hci_conn *conn, void *data); 1115 + static inline void hci_conn_hash_list_state(struct hci_dev *hdev, 1116 + hci_conn_func_t func, __u8 type, 1117 + __u16 state, void *data) 1118 + { 1119 + struct hci_conn_hash *h = &hdev->conn_hash; 1120 + struct hci_conn *c; 1121 + 1122 + if (!func) 1123 + return; 1124 + 1125 + rcu_read_lock(); 1126 + 1127 + list_for_each_entry_rcu(c, &h->list, list) { 1128 + if (c->type == type && c->state == state) 1129 + func(c, data); 1130 + } 1131 + 1132 + rcu_read_unlock(); 1176 1133 } 1177 1134 1178 1135 static inline struct hci_conn *hci_lookup_le_connect(struct hci_dev *hdev) ··· 1219 1134 int hci_disconnect(struct hci_conn *conn, __u8 reason); 1220 1135 bool hci_setup_sync(struct hci_conn *conn, __u16 handle); 1221 1136 void hci_sco_setup(struct hci_conn *conn, __u8 status); 1137 + bool hci_iso_setup_path(struct hci_conn *conn); 1138 + int hci_le_create_cis(struct hci_conn *conn); 1222 1139 1223 1140 struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst, 1224 1141 u8 role); ··· 1245 1158 enum conn_reasons conn_reason); 1246 1159 struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type, bdaddr_t *dst, 1247 1160 __u16 setting, struct bt_codec *codec); 1161 + struct hci_conn *hci_bind_cis(struct hci_dev *hdev, bdaddr_t *dst, 1162 + __u8 dst_type, struct bt_iso_qos *qos); 1163 + struct hci_conn *hci_connect_cis(struct hci_dev *hdev, bdaddr_t *dst, 1164 + __u8 dst_type, struct bt_iso_qos *qos); 1248 1165 int hci_conn_check_link_mode(struct hci_conn *conn); 1249 1166 int hci_conn_check_secure(struct hci_conn *conn, __u8 sec_level); 1250 1167 int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type, ··· 1616 1525 #define use_enhanced_conn_complete(dev) (ll_privacy_capable(dev) || \ 1617 1526 ext_adv_capable(dev)) 1618 1527 1528 + /* CIS Master/Slave support */ 1529 + #define iso_capable(dev) (cis_capable(dev)) 1530 + #define cis_capable(dev) \ 1531 + (cis_central_capable(dev) || cis_peripheral_capable(dev)) 1532 + #define cis_central_capable(dev) \ 1533 + ((dev)->le_features[3] & HCI_LE_CIS_CENTRAL) 1534 + #define cis_peripheral_capable(dev) \ 1535 + ((dev)->le_features[3] & HCI_LE_CIS_PERIPHERAL) 1536 + 1619 1537 /* ----- HCI protocols ----- */ 1620 1538 #define HCI_PROTO_DEFER 0x01 1621 1539 ··· 1638 1538 case SCO_LINK: 1639 1539 case ESCO_LINK: 1640 1540 return sco_connect_ind(hdev, bdaddr, flags); 1541 + 1542 + case ISO_LINK: 1543 + /* TODO: Handle connection indication */ 1544 + return -EINVAL; 1641 1545 1642 1546 default: 1643 1547 BT_ERR("unknown link type %d", type); ··· 1850 1746 const void *param); 1851 1747 void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags); 1852 1748 void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb); 1749 + void hci_send_iso(struct hci_conn *conn, struct sk_buff *skb); 1853 1750 1854 1751 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode); 1855 1752 void *hci_recv_event_data(struct hci_dev *hdev, __u8 event);

+2

include/net/bluetooth/hci_sock.h

··· 124 124 __u16 acl_pkts; 125 125 __u16 sco_mtu; 126 126 __u16 sco_pkts; 127 + __u16 iso_mtu; 128 + __u16 iso_pkts; 127 129 128 130 struct hci_dev_stats stat; 129 131 };

+3

include/net/bluetooth/hci_sync.h

··· 109 109 int hci_abort_conn_sync(struct hci_dev *hdev, struct hci_conn *conn, u8 reason); 110 110 111 111 int hci_le_create_conn_sync(struct hci_dev *hdev, struct hci_conn *conn); 112 + 113 + int hci_le_remove_cig_sync(struct hci_dev *hdev, u8 handle); 114 + int hci_le_remove_cig(struct hci_dev *hdev, u8 handle);

+1

net/bluetooth/Kconfig

··· 29 29 SCO audio links 30 30 L2CAP (Logical Link Control and Adaptation Protocol) 31 31 SMP (Security Manager Protocol) on LE (Low Energy) links 32 + ISO isochronous links 32 33 HCI Device drivers (Interface to the hardware) 33 34 RFCOMM Module (RFCOMM Protocol) 34 35 BNEP Module (Bluetooth Network Encapsulation Protocol)

+440

net/bluetooth/hci_conn.c

··· 125 125 126 126 hci_conn_hash_del(hdev, conn); 127 127 128 + if (conn->cleanup) 129 + conn->cleanup(conn); 130 + 128 131 if (conn->type == SCO_LINK || conn->type == ESCO_LINK) { 129 132 switch (conn->setting & SCO_AIRMODE_MASK) { 130 133 case SCO_AIRMODE_CVSD: ··· 725 722 conn->pkt_type = hdev->pkt_type & ACL_PTYPE_MASK; 726 723 break; 727 724 case LE_LINK: 725 + case ISO_LINK: 728 726 /* conn->src should reflect the local identity address */ 729 727 hci_copy_identity_address(hdev, &conn->src, &conn->src_type); 730 728 break; ··· 1234 1230 } 1235 1231 1236 1232 return sco; 1233 + } 1234 + 1235 + struct iso_list_data { 1236 + u8 cig; 1237 + u8 cis; 1238 + int count; 1239 + struct { 1240 + struct hci_cp_le_set_cig_params cp; 1241 + struct hci_cis_params cis[0x11]; 1242 + } pdu; 1243 + }; 1244 + 1245 + static void cis_add(struct iso_list_data *d, struct bt_iso_qos *qos) 1246 + { 1247 + struct hci_cis_params *cis = &d->pdu.cis[d->pdu.cp.num_cis]; 1248 + 1249 + cis->cis_id = qos->cis; 1250 + cis->c_sdu = cpu_to_le16(qos->out.sdu); 1251 + cis->p_sdu = cpu_to_le16(qos->in.sdu); 1252 + cis->c_phy = qos->out.phy; 1253 + cis->p_phy = qos->in.phy; 1254 + cis->c_rtn = qos->out.rtn; 1255 + cis->p_rtn = qos->in.rtn; 1256 + 1257 + d->pdu.cp.num_cis++; 1258 + } 1259 + 1260 + static void cis_list(struct hci_conn *conn, void *data) 1261 + { 1262 + struct iso_list_data *d = data; 1263 + 1264 + if (d->cig != conn->iso_qos.cig || d->cis == BT_ISO_QOS_CIS_UNSET || 1265 + d->cis != conn->iso_qos.cis) 1266 + return; 1267 + 1268 + d->count++; 1269 + 1270 + if (d->pdu.cp.cig_id == BT_ISO_QOS_CIG_UNSET || 1271 + d->count >= ARRAY_SIZE(d->pdu.cis)) 1272 + return; 1273 + 1274 + cis_add(d, &conn->iso_qos); 1275 + } 1276 + 1277 + static bool hci_le_set_cig_params(struct hci_conn *conn, struct bt_iso_qos *qos) 1278 + { 1279 + struct hci_dev *hdev = conn->hdev; 1280 + struct iso_list_data data; 1281 + 1282 + memset(&data, 0, sizeof(data)); 1283 + 1284 + /* Allocate a CIG if not set */ 1285 + if (qos->cig == BT_ISO_QOS_CIG_UNSET) { 1286 + for (data.cig = 0x00; data.cig < 0xff; data.cig++) { 1287 + data.count = 0; 1288 + data.cis = 0xff; 1289 + 1290 + hci_conn_hash_list_state(hdev, cis_list, ISO_LINK, 1291 + BT_BOUND, &data); 1292 + if (data.count) 1293 + continue; 1294 + 1295 + hci_conn_hash_list_state(hdev, cis_list, ISO_LINK, 1296 + BT_CONNECTED, &data); 1297 + if (!data.count) 1298 + break; 1299 + } 1300 + 1301 + if (data.cig == 0xff) 1302 + return false; 1303 + 1304 + /* Update CIG */ 1305 + qos->cig = data.cig; 1306 + } 1307 + 1308 + data.pdu.cp.cig_id = qos->cig; 1309 + hci_cpu_to_le24(qos->out.interval, data.pdu.cp.c_interval); 1310 + hci_cpu_to_le24(qos->in.interval, data.pdu.cp.p_interval); 1311 + data.pdu.cp.sca = qos->sca; 1312 + data.pdu.cp.packing = qos->packing; 1313 + data.pdu.cp.framing = qos->framing; 1314 + data.pdu.cp.c_latency = cpu_to_le16(qos->out.latency); 1315 + data.pdu.cp.p_latency = cpu_to_le16(qos->in.latency); 1316 + 1317 + if (qos->cis != BT_ISO_QOS_CIS_UNSET) { 1318 + data.count = 0; 1319 + data.cig = qos->cig; 1320 + data.cis = qos->cis; 1321 + 1322 + hci_conn_hash_list_state(hdev, cis_list, ISO_LINK, BT_BOUND, 1323 + &data); 1324 + if (data.count) 1325 + return false; 1326 + 1327 + cis_add(&data, qos); 1328 + } 1329 + 1330 + /* Reprogram all CIS(s) with the same CIG */ 1331 + for (data.cig = qos->cig, data.cis = 0x00; data.cis < 0x11; 1332 + data.cis++) { 1333 + data.count = 0; 1334 + 1335 + hci_conn_hash_list_state(hdev, cis_list, ISO_LINK, BT_BOUND, 1336 + &data); 1337 + if (data.count) 1338 + continue; 1339 + 1340 + /* Allocate a CIS if not set */ 1341 + if (qos->cis == BT_ISO_QOS_CIS_UNSET) { 1342 + /* Update CIS */ 1343 + qos->cis = data.cis; 1344 + cis_add(&data, qos); 1345 + } 1346 + } 1347 + 1348 + if (qos->cis == BT_ISO_QOS_CIS_UNSET || !data.pdu.cp.num_cis) 1349 + return false; 1350 + 1351 + if (hci_send_cmd(hdev, HCI_OP_LE_SET_CIG_PARAMS, 1352 + sizeof(data.pdu.cp) + 1353 + (data.pdu.cp.num_cis * sizeof(*data.pdu.cis)), 1354 + &data.pdu) < 0) 1355 + return false; 1356 + 1357 + return true; 1358 + } 1359 + 1360 + static void find_cis(struct hci_conn *conn, void *data) 1361 + { 1362 + struct iso_list_data *d = data; 1363 + 1364 + /* Ignore broadcast */ 1365 + if (!bacmp(&conn->dst, BDADDR_ANY)) 1366 + return; 1367 + 1368 + d->count++; 1369 + } 1370 + 1371 + static int remove_cig_sync(struct hci_dev *hdev, void *data) 1372 + { 1373 + u8 handle = PTR_ERR(data); 1374 + 1375 + return hci_le_remove_cig_sync(hdev, handle); 1376 + } 1377 + 1378 + int hci_le_remove_cig(struct hci_dev *hdev, u8 handle) 1379 + { 1380 + bt_dev_dbg(hdev, "handle 0x%2.2x", handle); 1381 + 1382 + return hci_cmd_sync_queue(hdev, remove_cig_sync, ERR_PTR(handle), NULL); 1383 + } 1384 + 1385 + static void cis_cleanup(struct hci_conn *conn) 1386 + { 1387 + struct hci_dev *hdev = conn->hdev; 1388 + struct iso_list_data d; 1389 + 1390 + memset(&d, 0, sizeof(d)); 1391 + d.cig = conn->iso_qos.cig; 1392 + 1393 + /* Check if ISO connection is a CIS and remove CIG if there are 1394 + * no other connections using it. 1395 + */ 1396 + hci_conn_hash_list_state(hdev, find_cis, ISO_LINK, BT_CONNECTED, &d); 1397 + if (d.count) 1398 + return; 1399 + 1400 + hci_le_remove_cig(hdev, conn->iso_qos.cig); 1401 + } 1402 + 1403 + struct hci_conn *hci_bind_cis(struct hci_dev *hdev, bdaddr_t *dst, 1404 + __u8 dst_type, struct bt_iso_qos *qos) 1405 + { 1406 + struct hci_conn *cis; 1407 + 1408 + cis = hci_conn_hash_lookup_cis(hdev, dst, dst_type); 1409 + if (!cis) { 1410 + cis = hci_conn_add(hdev, ISO_LINK, dst, HCI_ROLE_MASTER); 1411 + if (!cis) 1412 + return ERR_PTR(-ENOMEM); 1413 + cis->cleanup = cis_cleanup; 1414 + } 1415 + 1416 + if (cis->state == BT_CONNECTED) 1417 + return cis; 1418 + 1419 + /* Check if CIS has been set and the settings matches */ 1420 + if (cis->state == BT_BOUND && 1421 + !memcmp(&cis->iso_qos, qos, sizeof(*qos))) 1422 + return cis; 1423 + 1424 + /* Update LINK PHYs according to QoS preference */ 1425 + cis->le_tx_phy = qos->out.phy; 1426 + cis->le_rx_phy = qos->in.phy; 1427 + 1428 + /* If output interval is not set use the input interval as it cannot be 1429 + * 0x000000. 1430 + */ 1431 + if (!qos->out.interval) 1432 + qos->out.interval = qos->in.interval; 1433 + 1434 + /* If input interval is not set use the output interval as it cannot be 1435 + * 0x000000. 1436 + */ 1437 + if (!qos->in.interval) 1438 + qos->in.interval = qos->out.interval; 1439 + 1440 + /* If output latency is not set use the input latency as it cannot be 1441 + * 0x0000. 1442 + */ 1443 + if (!qos->out.latency) 1444 + qos->out.latency = qos->in.latency; 1445 + 1446 + /* If input latency is not set use the output latency as it cannot be 1447 + * 0x0000. 1448 + */ 1449 + if (!qos->in.latency) 1450 + qos->in.latency = qos->out.latency; 1451 + 1452 + /* Mirror PHYs that are disabled as SDU will be set to 0 */ 1453 + if (!qos->in.phy) 1454 + qos->in.phy = qos->out.phy; 1455 + 1456 + if (!qos->out.phy) 1457 + qos->out.phy = qos->in.phy; 1458 + 1459 + if (!hci_le_set_cig_params(cis, qos)) { 1460 + hci_conn_drop(cis); 1461 + return ERR_PTR(-EINVAL); 1462 + } 1463 + 1464 + cis->iso_qos = *qos; 1465 + cis->state = BT_BOUND; 1466 + 1467 + return cis; 1468 + } 1469 + 1470 + bool hci_iso_setup_path(struct hci_conn *conn) 1471 + { 1472 + struct hci_dev *hdev = conn->hdev; 1473 + struct hci_cp_le_setup_iso_path cmd; 1474 + 1475 + memset(&cmd, 0, sizeof(cmd)); 1476 + 1477 + if (conn->iso_qos.out.sdu) { 1478 + cmd.handle = cpu_to_le16(conn->handle); 1479 + cmd.direction = 0x00; /* Input (Host to Controller) */ 1480 + cmd.path = 0x00; /* HCI path if enabled */ 1481 + cmd.codec = 0x03; /* Transparent Data */ 1482 + 1483 + if (hci_send_cmd(hdev, HCI_OP_LE_SETUP_ISO_PATH, sizeof(cmd), 1484 + &cmd) < 0) 1485 + return false; 1486 + } 1487 + 1488 + if (conn->iso_qos.in.sdu) { 1489 + cmd.handle = cpu_to_le16(conn->handle); 1490 + cmd.direction = 0x01; /* Output (Controller to Host) */ 1491 + cmd.path = 0x00; /* HCI path if enabled */ 1492 + cmd.codec = 0x03; /* Transparent Data */ 1493 + 1494 + if (hci_send_cmd(hdev, HCI_OP_LE_SETUP_ISO_PATH, sizeof(cmd), 1495 + &cmd) < 0) 1496 + return false; 1497 + } 1498 + 1499 + return true; 1500 + } 1501 + 1502 + static int hci_create_cis_sync(struct hci_dev *hdev, void *data) 1503 + { 1504 + struct { 1505 + struct hci_cp_le_create_cis cp; 1506 + struct hci_cis cis[0x1f]; 1507 + } cmd; 1508 + struct hci_conn *conn = data; 1509 + u8 cig; 1510 + 1511 + memset(&cmd, 0, sizeof(cmd)); 1512 + cmd.cis[0].acl_handle = cpu_to_le16(conn->link->handle); 1513 + cmd.cis[0].cis_handle = cpu_to_le16(conn->handle); 1514 + cmd.cp.num_cis++; 1515 + cig = conn->iso_qos.cig; 1516 + 1517 + hci_dev_lock(hdev); 1518 + 1519 + rcu_read_lock(); 1520 + 1521 + list_for_each_entry_rcu(conn, &hdev->conn_hash.list, list) { 1522 + struct hci_cis *cis = &cmd.cis[cmd.cp.num_cis]; 1523 + 1524 + if (conn == data || conn->type != ISO_LINK || 1525 + conn->state == BT_CONNECTED || conn->iso_qos.cig != cig) 1526 + continue; 1527 + 1528 + /* Check if all CIS(s) belonging to a CIG are ready */ 1529 + if (conn->link->state != BT_CONNECTED || 1530 + conn->state != BT_CONNECT) { 1531 + cmd.cp.num_cis = 0; 1532 + break; 1533 + } 1534 + 1535 + /* Group all CIS with state BT_CONNECT since the spec don't 1536 + * allow to send them individually: 1537 + * 1538 + * BLUETOOTH CORE SPECIFICATION Version 5.3 | Vol 4, Part E 1539 + * page 2566: 1540 + * 1541 + * If the Host issues this command before all the 1542 + * HCI_LE_CIS_Established events from the previous use of the 1543 + * command have been generated, the Controller shall return the 1544 + * error code Command Disallowed (0x0C). 1545 + */ 1546 + cis->acl_handle = cpu_to_le16(conn->link->handle); 1547 + cis->cis_handle = cpu_to_le16(conn->handle); 1548 + cmd.cp.num_cis++; 1549 + } 1550 + 1551 + rcu_read_unlock(); 1552 + 1553 + hci_dev_unlock(hdev); 1554 + 1555 + if (!cmd.cp.num_cis) 1556 + return 0; 1557 + 1558 + return hci_send_cmd(hdev, HCI_OP_LE_CREATE_CIS, sizeof(cmd.cp) + 1559 + sizeof(cmd.cis[0]) * cmd.cp.num_cis, &cmd); 1560 + } 1561 + 1562 + int hci_le_create_cis(struct hci_conn *conn) 1563 + { 1564 + struct hci_conn *cis; 1565 + struct hci_dev *hdev = conn->hdev; 1566 + int err; 1567 + 1568 + switch (conn->type) { 1569 + case LE_LINK: 1570 + if (!conn->link || conn->state != BT_CONNECTED) 1571 + return -EINVAL; 1572 + cis = conn->link; 1573 + break; 1574 + case ISO_LINK: 1575 + cis = conn; 1576 + break; 1577 + default: 1578 + return -EINVAL; 1579 + } 1580 + 1581 + if (cis->state == BT_CONNECT) 1582 + return 0; 1583 + 1584 + /* Queue Create CIS */ 1585 + err = hci_cmd_sync_queue(hdev, hci_create_cis_sync, cis, NULL); 1586 + if (err) 1587 + return err; 1588 + 1589 + cis->state = BT_CONNECT; 1590 + 1591 + return 0; 1592 + } 1593 + 1594 + static void hci_iso_qos_setup(struct hci_dev *hdev, struct hci_conn *conn, 1595 + struct bt_iso_io_qos *qos, __u8 phy) 1596 + { 1597 + /* Only set MTU if PHY is enabled */ 1598 + if (!qos->sdu && qos->phy) { 1599 + if (hdev->iso_mtu > 0) 1600 + qos->sdu = hdev->iso_mtu; 1601 + else if (hdev->le_mtu > 0) 1602 + qos->sdu = hdev->le_mtu; 1603 + else 1604 + qos->sdu = hdev->acl_mtu; 1605 + } 1606 + 1607 + /* Use the same PHY as ACL if set to any */ 1608 + if (qos->phy == BT_ISO_PHY_ANY) 1609 + qos->phy = phy; 1610 + 1611 + /* Use LE ACL connection interval if not set */ 1612 + if (!qos->interval) 1613 + /* ACL interval unit in 1.25 ms to us */ 1614 + qos->interval = conn->le_conn_interval * 1250; 1615 + 1616 + /* Use LE ACL connection latency if not set */ 1617 + if (!qos->latency) 1618 + qos->latency = conn->le_conn_latency; 1619 + } 1620 + 1621 + struct hci_conn *hci_connect_cis(struct hci_dev *hdev, bdaddr_t *dst, 1622 + __u8 dst_type, struct bt_iso_qos *qos) 1623 + { 1624 + struct hci_conn *le; 1625 + struct hci_conn *cis; 1626 + 1627 + /* Convert from ISO socket address type to HCI address type */ 1628 + if (dst_type == BDADDR_LE_PUBLIC) 1629 + dst_type = ADDR_LE_DEV_PUBLIC; 1630 + else 1631 + dst_type = ADDR_LE_DEV_RANDOM; 1632 + 1633 + if (hci_dev_test_flag(hdev, HCI_ADVERTISING)) 1634 + le = hci_connect_le(hdev, dst, dst_type, false, 1635 + BT_SECURITY_LOW, 1636 + HCI_LE_CONN_TIMEOUT, 1637 + HCI_ROLE_SLAVE); 1638 + else 1639 + le = hci_connect_le_scan(hdev, dst, dst_type, 1640 + BT_SECURITY_LOW, 1641 + HCI_LE_CONN_TIMEOUT, 1642 + CONN_REASON_ISO_CONNECT); 1643 + if (IS_ERR(le)) 1644 + return le; 1645 + 1646 + hci_iso_qos_setup(hdev, le, &qos->out, 1647 + le->le_tx_phy ? le->le_tx_phy : hdev->le_tx_def_phys); 1648 + hci_iso_qos_setup(hdev, le, &qos->in, 1649 + le->le_rx_phy ? le->le_rx_phy : hdev->le_rx_def_phys); 1650 + 1651 + cis = hci_bind_cis(hdev, dst, dst_type, qos); 1652 + if (IS_ERR(cis)) { 1653 + hci_conn_drop(le); 1654 + return cis; 1655 + } 1656 + 1657 + le->link = cis; 1658 + cis->link = le; 1659 + 1660 + hci_conn_hold(cis); 1661 + 1662 + /* If LE is already connected and CIS handle is already set proceed to 1663 + * Create CIS immediately. 1664 + */ 1665 + if (le->state == BT_CONNECTED && cis->handle != HCI_CONN_HANDLE_UNSET) 1666 + hci_le_create_cis(le); 1667 + 1668 + return cis; 1237 1669 } 1238 1670 1239 1671 /* Check link security requirement */

+183 -47

net/bluetooth/hci_core.c

··· 616 616 hci_dev_clear_flag(hdev, HCI_CMD_DRAIN_WORKQUEUE); 617 617 618 618 atomic_set(&hdev->cmd_cnt, 1); 619 - hdev->acl_cnt = 0; hdev->sco_cnt = 0; hdev->le_cnt = 0; 619 + hdev->acl_cnt = 0; 620 + hdev->sco_cnt = 0; 621 + hdev->le_cnt = 0; 622 + hdev->iso_cnt = 0; 620 623 621 624 ret = hci_reset_sync(hdev); 622 625 ··· 3160 3157 queue_work(hdev->workqueue, &hdev->tx_work); 3161 3158 } 3162 3159 3160 + /* Send ISO data */ 3161 + static void hci_add_iso_hdr(struct sk_buff *skb, __u16 handle, __u8 flags) 3162 + { 3163 + struct hci_iso_hdr *hdr; 3164 + int len = skb->len; 3165 + 3166 + skb_push(skb, HCI_ISO_HDR_SIZE); 3167 + skb_reset_transport_header(skb); 3168 + hdr = (struct hci_iso_hdr *)skb_transport_header(skb); 3169 + hdr->handle = cpu_to_le16(hci_handle_pack(handle, flags)); 3170 + hdr->dlen = cpu_to_le16(len); 3171 + } 3172 + 3173 + static void hci_queue_iso(struct hci_conn *conn, struct sk_buff_head *queue, 3174 + struct sk_buff *skb) 3175 + { 3176 + struct hci_dev *hdev = conn->hdev; 3177 + struct sk_buff *list; 3178 + __u16 flags; 3179 + 3180 + skb->len = skb_headlen(skb); 3181 + skb->data_len = 0; 3182 + 3183 + hci_skb_pkt_type(skb) = HCI_ISODATA_PKT; 3184 + 3185 + list = skb_shinfo(skb)->frag_list; 3186 + 3187 + flags = hci_iso_flags_pack(list ? ISO_START : ISO_SINGLE, 0x00); 3188 + hci_add_iso_hdr(skb, conn->handle, flags); 3189 + 3190 + if (!list) { 3191 + /* Non fragmented */ 3192 + BT_DBG("%s nonfrag skb %p len %d", hdev->name, skb, skb->len); 3193 + 3194 + skb_queue_tail(queue, skb); 3195 + } else { 3196 + /* Fragmented */ 3197 + BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len); 3198 + 3199 + skb_shinfo(skb)->frag_list = NULL; 3200 + 3201 + __skb_queue_tail(queue, skb); 3202 + 3203 + do { 3204 + skb = list; list = list->next; 3205 + 3206 + hci_skb_pkt_type(skb) = HCI_ISODATA_PKT; 3207 + flags = hci_iso_flags_pack(list ? ISO_CONT : ISO_END, 3208 + 0x00); 3209 + hci_add_iso_hdr(skb, conn->handle, flags); 3210 + 3211 + BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len); 3212 + 3213 + __skb_queue_tail(queue, skb); 3214 + } while (list); 3215 + } 3216 + } 3217 + 3218 + void hci_send_iso(struct hci_conn *conn, struct sk_buff *skb) 3219 + { 3220 + struct hci_dev *hdev = conn->hdev; 3221 + 3222 + BT_DBG("%s len %d", hdev->name, skb->len); 3223 + 3224 + hci_queue_iso(conn, &conn->data_q, skb); 3225 + 3226 + queue_work(hdev->workqueue, &hdev->tx_work); 3227 + } 3228 + 3163 3229 /* ---- HCI TX task (outgoing data) ---- */ 3164 3230 3165 3231 /* HCI Connection scheduler */ 3232 + static inline void hci_quote_sent(struct hci_conn *conn, int num, int *quote) 3233 + { 3234 + struct hci_dev *hdev; 3235 + int cnt, q; 3236 + 3237 + if (!conn) { 3238 + *quote = 0; 3239 + return; 3240 + } 3241 + 3242 + hdev = conn->hdev; 3243 + 3244 + switch (conn->type) { 3245 + case ACL_LINK: 3246 + cnt = hdev->acl_cnt; 3247 + break; 3248 + case AMP_LINK: 3249 + cnt = hdev->block_cnt; 3250 + break; 3251 + case SCO_LINK: 3252 + case ESCO_LINK: 3253 + cnt = hdev->sco_cnt; 3254 + break; 3255 + case LE_LINK: 3256 + cnt = hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt; 3257 + break; 3258 + case ISO_LINK: 3259 + cnt = hdev->iso_mtu ? hdev->iso_cnt : 3260 + hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt; 3261 + break; 3262 + default: 3263 + cnt = 0; 3264 + bt_dev_err(hdev, "unknown link type %d", conn->type); 3265 + } 3266 + 3267 + q = cnt / num; 3268 + *quote = q ? q : 1; 3269 + } 3270 + 3166 3271 static struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, 3167 3272 int *quote) 3168 3273 { ··· 3303 3192 3304 3193 rcu_read_unlock(); 3305 3194 3306 - if (conn) { 3307 - int cnt, q; 3308 - 3309 - switch (conn->type) { 3310 - case ACL_LINK: 3311 - cnt = hdev->acl_cnt; 3312 - break; 3313 - case SCO_LINK: 3314 - case ESCO_LINK: 3315 - cnt = hdev->sco_cnt; 3316 - break; 3317 - case LE_LINK: 3318 - cnt = hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt; 3319 - break; 3320 - default: 3321 - cnt = 0; 3322 - bt_dev_err(hdev, "unknown link type %d", conn->type); 3323 - } 3324 - 3325 - q = cnt / num; 3326 - *quote = q ? q : 1; 3327 - } else 3328 - *quote = 0; 3195 + hci_quote_sent(conn, num, quote); 3329 3196 3330 3197 BT_DBG("conn %p quote %d", conn, *quote); 3331 3198 return conn; ··· 3337 3248 struct hci_chan *chan = NULL; 3338 3249 unsigned int num = 0, min = ~0, cur_prio = 0; 3339 3250 struct hci_conn *conn; 3340 - int cnt, q, conn_num = 0; 3251 + int conn_num = 0; 3341 3252 3342 3253 BT_DBG("%s", hdev->name); 3343 3254 ··· 3387 3298 if (!chan) 3388 3299 return NULL; 3389 3300 3390 - switch (chan->conn->type) { 3391 - case ACL_LINK: 3392 - cnt = hdev->acl_cnt; 3393 - break; 3394 - case AMP_LINK: 3395 - cnt = hdev->block_cnt; 3396 - break; 3397 - case SCO_LINK: 3398 - case ESCO_LINK: 3399 - cnt = hdev->sco_cnt; 3400 - break; 3401 - case LE_LINK: 3402 - cnt = hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt; 3403 - break; 3404 - default: 3405 - cnt = 0; 3406 - bt_dev_err(hdev, "unknown link type %d", chan->conn->type); 3407 - } 3301 + hci_quote_sent(chan->conn, num, quote); 3408 3302 3409 - q = cnt / num; 3410 - *quote = q ? q : 1; 3411 3303 BT_DBG("chan %p quote %d", chan, *quote); 3412 3304 return chan; 3413 3305 } ··· 3677 3607 hci_prio_recalculate(hdev, LE_LINK); 3678 3608 } 3679 3609 3610 + /* Schedule CIS */ 3611 + static void hci_sched_iso(struct hci_dev *hdev) 3612 + { 3613 + struct hci_conn *conn; 3614 + struct sk_buff *skb; 3615 + int quote, *cnt; 3616 + 3617 + BT_DBG("%s", hdev->name); 3618 + 3619 + if (!hci_conn_num(hdev, ISO_LINK)) 3620 + return; 3621 + 3622 + cnt = hdev->iso_pkts ? &hdev->iso_cnt : 3623 + hdev->le_pkts ? &hdev->le_cnt : &hdev->acl_cnt; 3624 + while (*cnt && (conn = hci_low_sent(hdev, ISO_LINK, &quote))) { 3625 + while (quote-- && (skb = skb_dequeue(&conn->data_q))) { 3626 + BT_DBG("skb %p len %d", skb, skb->len); 3627 + hci_send_frame(hdev, skb); 3628 + 3629 + conn->sent++; 3630 + if (conn->sent == ~0) 3631 + conn->sent = 0; 3632 + (*cnt)--; 3633 + } 3634 + } 3635 + } 3636 + 3680 3637 static void hci_tx_work(struct work_struct *work) 3681 3638 { 3682 3639 struct hci_dev *hdev = container_of(work, struct hci_dev, tx_work); 3683 3640 struct sk_buff *skb; 3684 3641 3685 - BT_DBG("%s acl %d sco %d le %d", hdev->name, hdev->acl_cnt, 3686 - hdev->sco_cnt, hdev->le_cnt); 3642 + BT_DBG("%s acl %d sco %d le %d iso %d", hdev->name, hdev->acl_cnt, 3643 + hdev->sco_cnt, hdev->le_cnt, hdev->iso_cnt); 3687 3644 3688 3645 if (!hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { 3689 3646 /* Schedule queues and send stuff to HCI driver */ 3690 3647 hci_sched_sco(hdev); 3691 3648 hci_sched_esco(hdev); 3649 + hci_sched_iso(hdev); 3692 3650 hci_sched_acl(hdev); 3693 3651 hci_sched_le(hdev); 3694 3652 } ··· 3796 3698 handle); 3797 3699 } 3798 3700 3701 + kfree_skb(skb); 3702 + } 3703 + 3704 + static void hci_isodata_packet(struct hci_dev *hdev, struct sk_buff *skb) 3705 + { 3706 + struct hci_iso_hdr *hdr; 3707 + struct hci_conn *conn; 3708 + __u16 handle, flags; 3709 + 3710 + hdr = skb_pull_data(skb, sizeof(*hdr)); 3711 + if (!hdr) { 3712 + bt_dev_err(hdev, "ISO packet too small"); 3713 + goto drop; 3714 + } 3715 + 3716 + handle = __le16_to_cpu(hdr->handle); 3717 + flags = hci_flags(handle); 3718 + handle = hci_handle(handle); 3719 + 3720 + bt_dev_dbg(hdev, "len %d handle 0x%4.4x flags 0x%4.4x", skb->len, 3721 + handle, flags); 3722 + 3723 + hci_dev_lock(hdev); 3724 + conn = hci_conn_hash_lookup_handle(hdev, handle); 3725 + hci_dev_unlock(hdev); 3726 + 3727 + /* TODO: Send to upper protocol */ 3728 + if (!conn) { 3729 + bt_dev_err(hdev, "ISO packet for unknown connection handle %d", 3730 + handle); 3731 + } 3732 + 3733 + drop: 3799 3734 kfree_skb(skb); 3800 3735 } 3801 3736 ··· 3991 3860 case HCI_SCODATA_PKT: 3992 3861 BT_DBG("%s SCO data packet", hdev->name); 3993 3862 hci_scodata_packet(hdev, skb); 3863 + break; 3864 + 3865 + case HCI_ISODATA_PKT: 3866 + BT_DBG("%s ISO data packet", hdev->name); 3867 + hci_isodata_packet(hdev, skb); 3994 3868 break; 3995 3869 3996 3870 default:

+304 -3

net/bluetooth/hci_event.c

··· 3775 3775 } 3776 3776 } 3777 3777 3778 + static u8 hci_cc_le_read_buffer_size_v2(struct hci_dev *hdev, void *data, 3779 + struct sk_buff *skb) 3780 + { 3781 + struct hci_rp_le_read_buffer_size_v2 *rp = data; 3782 + 3783 + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); 3784 + 3785 + if (rp->status) 3786 + return rp->status; 3787 + 3788 + hdev->le_mtu = __le16_to_cpu(rp->acl_mtu); 3789 + hdev->le_pkts = rp->acl_max_pkt; 3790 + hdev->iso_mtu = __le16_to_cpu(rp->iso_mtu); 3791 + hdev->iso_pkts = rp->iso_max_pkt; 3792 + 3793 + hdev->le_cnt = hdev->le_pkts; 3794 + hdev->iso_cnt = hdev->iso_pkts; 3795 + 3796 + BT_DBG("%s acl mtu %d:%d iso mtu %d:%d", hdev->name, hdev->acl_mtu, 3797 + hdev->acl_pkts, hdev->iso_mtu, hdev->iso_pkts); 3798 + 3799 + return rp->status; 3800 + } 3801 + 3802 + static u8 hci_cc_le_set_cig_params(struct hci_dev *hdev, void *data, 3803 + struct sk_buff *skb) 3804 + { 3805 + struct hci_rp_le_set_cig_params *rp = data; 3806 + struct hci_conn *conn; 3807 + int i = 0; 3808 + 3809 + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); 3810 + 3811 + hci_dev_lock(hdev); 3812 + 3813 + if (rp->status) { 3814 + while ((conn = hci_conn_hash_lookup_cig(hdev, rp->cig_id))) { 3815 + conn->state = BT_CLOSED; 3816 + hci_connect_cfm(conn, rp->status); 3817 + hci_conn_del(conn); 3818 + } 3819 + goto unlock; 3820 + } 3821 + 3822 + rcu_read_lock(); 3823 + 3824 + list_for_each_entry_rcu(conn, &hdev->conn_hash.list, list) { 3825 + if (conn->type != ISO_LINK || conn->iso_qos.cig != rp->cig_id || 3826 + conn->state == BT_CONNECTED) 3827 + continue; 3828 + 3829 + conn->handle = __le16_to_cpu(rp->handle[i++]); 3830 + 3831 + bt_dev_dbg(hdev, "%p handle 0x%4.4x link %p", conn, 3832 + conn->handle, conn->link); 3833 + 3834 + /* Create CIS if LE is already connected */ 3835 + if (conn->link && conn->link->state == BT_CONNECTED) 3836 + hci_le_create_cis(conn->link); 3837 + 3838 + if (i == rp->num_handles) 3839 + break; 3840 + } 3841 + 3842 + rcu_read_unlock(); 3843 + 3844 + unlock: 3845 + hci_dev_unlock(hdev); 3846 + 3847 + return rp->status; 3848 + } 3849 + 3850 + static u8 hci_cc_le_setup_iso_path(struct hci_dev *hdev, void *data, 3851 + struct sk_buff *skb) 3852 + { 3853 + struct hci_rp_le_setup_iso_path *rp = data; 3854 + struct hci_cp_le_setup_iso_path *cp; 3855 + struct hci_conn *conn; 3856 + 3857 + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); 3858 + 3859 + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SETUP_ISO_PATH); 3860 + if (!cp) 3861 + return rp->status; 3862 + 3863 + hci_dev_lock(hdev); 3864 + 3865 + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); 3866 + if (!conn) 3867 + goto unlock; 3868 + 3869 + if (rp->status) { 3870 + hci_connect_cfm(conn, rp->status); 3871 + hci_conn_del(conn); 3872 + goto unlock; 3873 + } 3874 + 3875 + switch (cp->direction) { 3876 + /* Input (Host to Controller) */ 3877 + case 0x00: 3878 + /* Only confirm connection if output only */ 3879 + if (conn->iso_qos.out.sdu && !conn->iso_qos.in.sdu) 3880 + hci_connect_cfm(conn, rp->status); 3881 + break; 3882 + /* Output (Controller to Host) */ 3883 + case 0x01: 3884 + /* Confirm connection since conn->iso_qos is always configured 3885 + * last. 3886 + */ 3887 + hci_connect_cfm(conn, rp->status); 3888 + break; 3889 + } 3890 + 3891 + unlock: 3892 + hci_dev_unlock(hdev); 3893 + return rp->status; 3894 + } 3895 + 3778 3896 #define HCI_CC_VL(_op, _func, _min, _max) \ 3779 3897 { \ 3780 3898 .op = _op, \ ··· 4068 3950 HCI_CC_STATUS(HCI_OP_LE_CLEAR_ADV_SETS, hci_cc_le_clear_adv_sets), 4069 3951 HCI_CC(HCI_OP_LE_READ_TRANSMIT_POWER, hci_cc_le_read_transmit_power, 4070 3952 sizeof(struct hci_rp_le_read_transmit_power)), 4071 - HCI_CC_STATUS(HCI_OP_LE_SET_PRIVACY_MODE, hci_cc_le_set_privacy_mode) 3953 + HCI_CC_STATUS(HCI_OP_LE_SET_PRIVACY_MODE, hci_cc_le_set_privacy_mode), 3954 + HCI_CC(HCI_OP_LE_READ_BUFFER_SIZE_V2, hci_cc_le_read_buffer_size_v2, 3955 + sizeof(struct hci_rp_le_read_buffer_size_v2)), 3956 + HCI_CC_VL(HCI_OP_LE_SET_CIG_PARAMS, hci_cc_le_set_cig_params, 3957 + sizeof(struct hci_rp_le_set_cig_params), HCI_MAX_EVENT_SIZE), 3958 + HCI_CC(HCI_OP_LE_SETUP_ISO_PATH, hci_cc_le_setup_iso_path, 3959 + sizeof(struct hci_rp_le_setup_iso_path)), 4072 3960 }; 4073 3961 4074 3962 static u8 hci_cc_func(struct hci_dev *hdev, const struct hci_cc *cc, ··· 4137 4013 queue_work(hdev->workqueue, &hdev->cmd_work); 4138 4014 } 4139 4015 4016 + static void hci_cs_le_create_cis(struct hci_dev *hdev, u8 status) 4017 + { 4018 + struct hci_cp_le_create_cis *cp; 4019 + int i; 4020 + 4021 + bt_dev_dbg(hdev, "status 0x%2.2x", status); 4022 + 4023 + if (!status) 4024 + return; 4025 + 4026 + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_CREATE_CIS); 4027 + if (!cp) 4028 + return; 4029 + 4030 + hci_dev_lock(hdev); 4031 + 4032 + /* Remove connection if command failed */ 4033 + for (i = 0; cp->num_cis; cp->num_cis--, i++) { 4034 + struct hci_conn *conn; 4035 + u16 handle; 4036 + 4037 + handle = __le16_to_cpu(cp->cis[i].cis_handle); 4038 + 4039 + conn = hci_conn_hash_lookup_handle(hdev, handle); 4040 + if (conn) { 4041 + conn->state = BT_CLOSED; 4042 + hci_connect_cfm(conn, status); 4043 + hci_conn_del(conn); 4044 + } 4045 + } 4046 + 4047 + hci_dev_unlock(hdev); 4048 + } 4049 + 4140 4050 #define HCI_CS(_op, _func) \ 4141 4051 { \ 4142 4052 .op = _op, \ ··· 4200 4042 HCI_CS(HCI_OP_LE_CREATE_CONN, hci_cs_le_create_conn), 4201 4043 HCI_CS(HCI_OP_LE_READ_REMOTE_FEATURES, hci_cs_le_read_remote_features), 4202 4044 HCI_CS(HCI_OP_LE_START_ENC, hci_cs_le_start_enc), 4203 - HCI_CS(HCI_OP_LE_EXT_CREATE_CONN, hci_cs_le_ext_create_conn) 4045 + HCI_CS(HCI_OP_LE_EXT_CREATE_CONN, hci_cs_le_ext_create_conn), 4046 + HCI_CS(HCI_OP_LE_CREATE_CIS, hci_cs_le_create_cis), 4204 4047 }; 4205 4048 4206 4049 static void hci_cmd_status_evt(struct hci_dev *hdev, void *data, ··· 4335 4176 hdev->sco_cnt += count; 4336 4177 if (hdev->sco_cnt > hdev->sco_pkts) 4337 4178 hdev->sco_cnt = hdev->sco_pkts; 4179 + break; 4180 + 4181 + case ISO_LINK: 4182 + if (hdev->iso_pkts) { 4183 + hdev->iso_cnt += count; 4184 + if (hdev->iso_cnt > hdev->iso_pkts) 4185 + hdev->iso_cnt = hdev->iso_pkts; 4186 + } else if (hdev->le_pkts) { 4187 + hdev->le_cnt += count; 4188 + if (hdev->le_cnt > hdev->le_pkts) 4189 + hdev->le_cnt = hdev->le_pkts; 4190 + } else { 4191 + hdev->acl_cnt += count; 4192 + if (hdev->acl_cnt > hdev->acl_pkts) 4193 + hdev->acl_cnt = hdev->acl_pkts; 4194 + } 4338 4195 break; 4339 4196 4340 4197 default: ··· 6655 6480 hci_dev_unlock(hdev); 6656 6481 } 6657 6482 6483 + static void hci_le_cis_estabilished_evt(struct hci_dev *hdev, void *data, 6484 + struct sk_buff *skb) 6485 + { 6486 + struct hci_evt_le_cis_established *ev = data; 6487 + struct hci_conn *conn; 6488 + u16 handle = __le16_to_cpu(ev->handle); 6489 + 6490 + bt_dev_dbg(hdev, "status 0x%2.2x", ev->status); 6491 + 6492 + hci_dev_lock(hdev); 6493 + 6494 + conn = hci_conn_hash_lookup_handle(hdev, handle); 6495 + if (!conn) { 6496 + bt_dev_err(hdev, 6497 + "Unable to find connection with handle 0x%4.4x", 6498 + handle); 6499 + goto unlock; 6500 + } 6501 + 6502 + if (conn->role == HCI_ROLE_SLAVE) { 6503 + __le32 interval; 6504 + 6505 + memset(&interval, 0, sizeof(interval)); 6506 + 6507 + memcpy(&interval, ev->c_latency, sizeof(ev->c_latency)); 6508 + conn->iso_qos.in.interval = le32_to_cpu(interval); 6509 + memcpy(&interval, ev->p_latency, sizeof(ev->p_latency)); 6510 + conn->iso_qos.out.interval = le32_to_cpu(interval); 6511 + conn->iso_qos.in.latency = le16_to_cpu(ev->interval); 6512 + conn->iso_qos.out.latency = le16_to_cpu(ev->interval); 6513 + conn->iso_qos.in.sdu = le16_to_cpu(ev->c_mtu); 6514 + conn->iso_qos.out.sdu = le16_to_cpu(ev->p_mtu); 6515 + conn->iso_qos.in.phy = ev->c_phy; 6516 + conn->iso_qos.out.phy = ev->p_phy; 6517 + } 6518 + 6519 + if (!ev->status) { 6520 + conn->state = BT_CONNECTED; 6521 + hci_debugfs_create_conn(conn); 6522 + hci_conn_add_sysfs(conn); 6523 + hci_iso_setup_path(conn); 6524 + goto unlock; 6525 + } 6526 + 6527 + hci_connect_cfm(conn, ev->status); 6528 + hci_conn_del(conn); 6529 + 6530 + unlock: 6531 + hci_dev_unlock(hdev); 6532 + } 6533 + 6534 + static void hci_le_reject_cis(struct hci_dev *hdev, __le16 handle) 6535 + { 6536 + struct hci_cp_le_reject_cis cp; 6537 + 6538 + memset(&cp, 0, sizeof(cp)); 6539 + cp.handle = handle; 6540 + cp.reason = HCI_ERROR_REJ_BAD_ADDR; 6541 + hci_send_cmd(hdev, HCI_OP_LE_REJECT_CIS, sizeof(cp), &cp); 6542 + } 6543 + 6544 + static void hci_le_accept_cis(struct hci_dev *hdev, __le16 handle) 6545 + { 6546 + struct hci_cp_le_accept_cis cp; 6547 + 6548 + memset(&cp, 0, sizeof(cp)); 6549 + cp.handle = handle; 6550 + hci_send_cmd(hdev, HCI_OP_LE_ACCEPT_CIS, sizeof(cp), &cp); 6551 + } 6552 + 6553 + static void hci_le_cis_req_evt(struct hci_dev *hdev, void *data, 6554 + struct sk_buff *skb) 6555 + { 6556 + struct hci_evt_le_cis_req *ev = data; 6557 + u16 acl_handle, cis_handle; 6558 + struct hci_conn *acl, *cis; 6559 + int mask; 6560 + __u8 flags = 0; 6561 + 6562 + acl_handle = __le16_to_cpu(ev->acl_handle); 6563 + cis_handle = __le16_to_cpu(ev->cis_handle); 6564 + 6565 + bt_dev_dbg(hdev, "acl 0x%4.4x handle 0x%4.4x cig 0x%2.2x cis 0x%2.2x", 6566 + acl_handle, cis_handle, ev->cig_id, ev->cis_id); 6567 + 6568 + hci_dev_lock(hdev); 6569 + 6570 + acl = hci_conn_hash_lookup_handle(hdev, acl_handle); 6571 + if (!acl) 6572 + goto unlock; 6573 + 6574 + mask = hci_proto_connect_ind(hdev, &acl->dst, ISO_LINK, &flags); 6575 + if (!(mask & HCI_LM_ACCEPT)) { 6576 + hci_le_reject_cis(hdev, ev->cis_handle); 6577 + goto unlock; 6578 + } 6579 + 6580 + cis = hci_conn_hash_lookup_handle(hdev, cis_handle); 6581 + if (!cis) { 6582 + cis = hci_conn_add(hdev, ISO_LINK, &acl->dst, HCI_ROLE_SLAVE); 6583 + if (!cis) { 6584 + hci_le_reject_cis(hdev, ev->cis_handle); 6585 + goto unlock; 6586 + } 6587 + cis->handle = cis_handle; 6588 + } 6589 + 6590 + cis->iso_qos.cig = ev->cig_id; 6591 + cis->iso_qos.cis = ev->cis_id; 6592 + 6593 + if (!(flags & HCI_PROTO_DEFER)) { 6594 + hci_le_accept_cis(hdev, ev->cis_handle); 6595 + } else { 6596 + cis->state = BT_CONNECT2; 6597 + hci_connect_cfm(cis, 0); 6598 + } 6599 + 6600 + unlock: 6601 + hci_dev_unlock(hdev); 6602 + } 6603 + 6658 6604 #define HCI_LE_EV_VL(_op, _func, _min_len, _max_len) \ 6659 6605 [_op] = { \ 6660 6606 .func = _func, \ ··· 6839 6543 /* [0x12 = HCI_EV_LE_EXT_ADV_SET_TERM] */ 6840 6544 HCI_LE_EV(HCI_EV_LE_EXT_ADV_SET_TERM, hci_le_ext_adv_term_evt, 6841 6545 sizeof(struct hci_evt_le_ext_adv_set_term)), 6546 + /* [0x19 = HCI_EVT_LE_CIS_ESTABLISHED] */ 6547 + HCI_LE_EV(HCI_EVT_LE_CIS_ESTABLISHED, hci_le_cis_estabilished_evt, 6548 + sizeof(struct hci_evt_le_cis_established)), 6549 + /* [0x1a = HCI_EVT_LE_CIS_REQ] */ 6550 + HCI_LE_EV(HCI_EVT_LE_CIS_REQ, hci_le_cis_req_evt, 6551 + sizeof(struct hci_evt_le_cis_req)), 6842 6552 }; 6843 6553 6844 6554 static void hci_le_meta_evt(struct hci_dev *hdev, void *data, ··· 6883 6581 if (skb->len > subev->max_len) 6884 6582 bt_dev_warn(hdev, "unexpected subevent 0x%2.2x length: %u > %u", 6885 6583 ev->subevent, skb->len, subev->max_len); 6886 - 6887 6584 data = hci_le_ev_skb_pull(hdev, skb, ev->subevent, subev->min_len); 6888 6585 if (!data) 6889 6586 return;

+48 -1

net/bluetooth/hci_sync.c

··· 2800 2800 /* Read Buffer Size (ACL mtu, max pkt, etc.) */ 2801 2801 static int hci_read_buffer_size_sync(struct hci_dev *hdev) 2802 2802 { 2803 + /* Use Read LE Buffer Size V2 if supported */ 2804 + if (hdev->commands[41] & 0x20) 2805 + return __hci_cmd_sync_status(hdev, 2806 + HCI_OP_LE_READ_BUFFER_SIZE_V2, 2807 + 0, NULL, HCI_CMD_TIMEOUT); 2808 + 2803 2809 return __hci_cmd_sync_status(hdev, HCI_OP_READ_BUFFER_SIZE, 2804 2810 0, NULL, HCI_CMD_TIMEOUT); 2805 2811 } ··· 3057 3051 if (hdev->dev_type == HCI_AMP) 3058 3052 return hci_init_stage_sync(hdev, amp_init2); 3059 3053 3054 + err = hci_init_stage_sync(hdev, hci_init2); 3055 + if (err) 3056 + return err; 3057 + 3060 3058 if (lmp_bredr_capable(hdev)) { 3061 3059 err = hci_init_stage_sync(hdev, br_init2); 3062 3060 if (err) ··· 3078 3068 hci_dev_set_flag(hdev, HCI_LE_ENABLED); 3079 3069 } 3080 3070 3081 - return hci_init_stage_sync(hdev, hci_init2); 3071 + return 0; 3082 3072 } 3083 3073 3084 3074 static int hci_set_event_mask_sync(struct hci_dev *hdev) ··· 3399 3389 if (ext_adv_capable(hdev)) 3400 3390 events[2] |= 0x02; /* LE Advertising Set Terminated */ 3401 3391 3392 + if (cis_capable(hdev)) { 3393 + events[3] |= 0x01; /* LE CIS Established */ 3394 + if (cis_peripheral_capable(hdev)) 3395 + events[3] |= 0x02; /* LE CIS Request */ 3396 + } 3397 + 3402 3398 return __hci_cmd_sync_status(hdev, HCI_OP_LE_SET_EVENT_MASK, 3403 3399 sizeof(events), events, HCI_CMD_TIMEOUT); 3404 3400 } ··· 3545 3529 sizeof(cp), &cp, HCI_CMD_TIMEOUT); 3546 3530 } 3547 3531 3532 + /* LE Set Host Feature */ 3533 + static int hci_le_set_host_feature_sync(struct hci_dev *hdev) 3534 + { 3535 + struct hci_cp_le_set_host_feature cp; 3536 + 3537 + if (!iso_capable(hdev)) 3538 + return 0; 3539 + 3540 + memset(&cp, 0, sizeof(cp)); 3541 + 3542 + /* Isochronous Channels (Host Support) */ 3543 + cp.bit_number = 32; 3544 + cp.bit_value = 1; 3545 + 3546 + return __hci_cmd_sync_status(hdev, HCI_OP_LE_SET_HOST_FEATURE, 3547 + sizeof(cp), &cp, HCI_CMD_TIMEOUT); 3548 + } 3549 + 3548 3550 /* LE Controller init stage 3 command sequence */ 3549 3551 static const struct hci_init_stage le_init3[] = { 3550 3552 /* HCI_OP_LE_SET_EVENT_MASK */ ··· 3589 3555 HCI_INIT(hci_le_read_num_support_adv_sets_sync), 3590 3556 /* HCI_OP_WRITE_LE_HOST_SUPPORTED */ 3591 3557 HCI_INIT(hci_set_le_support_sync), 3558 + /* HCI_OP_LE_SET_HOST_FEATURE */ 3559 + HCI_INIT(hci_le_set_host_feature_sync), 3592 3560 {} 3593 3561 }; 3594 3562 ··· 5472 5436 /* Re-enable advertising after the connection attempt is finished. */ 5473 5437 hci_resume_advertising_sync(hdev); 5474 5438 return err; 5439 + } 5440 + 5441 + int hci_le_remove_cig_sync(struct hci_dev *hdev, u8 handle) 5442 + { 5443 + struct hci_cp_le_remove_cig cp; 5444 + 5445 + memset(&cp, 0, sizeof(cp)); 5446 + cp.cig_id = handle; 5447 + 5448 + return __hci_cmd_sync_status(hdev, HCI_OP_LE_REMOVE_CIG, sizeof(cp), 5449 + &cp, HCI_CMD_TIMEOUT); 5475 5450 }

Configure Feed

Configure Feed