Bluetooth: Handle update connection parameters

When connected, the peripheral/slave can trigger changing connection parameters.
Handle such request on central/master and if needed update controller settings
regarding the relevant connection parameters.

Change-Id: If3c1d91752f8d3875918bd8bfe923330790f530e
Signed-off-by: Arkadiusz Lichwa <arkadiusz.lichwa@tieto.com>

include/bluetooth/hci.h

@@ -271,6 +271,17 @@ struct bt_hci_cp_le_create_conn {
 	uint16_t     max_ce_len;
 } __packed;
 
+#define BT_HCI_OP_LE_CONN_UPDATE		BT_OP(BT_OGF_LE, 0x0013)
+struct hci_cp_le_conn_update {
+	uint16_t handle;
+	uint16_t conn_interval_min;
+	uint16_t conn_interval_max;
+	uint16_t conn_latency;
+	uint16_t supervision_timeout;
+	uint16_t min_ce_len;
+	uint16_t max_ce_len;
+} __packed;
+
 #define BT_HCI_OP_LE_ENCRYPT			BT_OP(BT_OGF_LE, 0x0017)
 struct bt_hci_cp_le_encrypt {
 	uint8_t  key[16];

net/bluetooth/hci_core.c

@@ -1363,6 +1363,28 @@ int bt_stop_scanning(void)
 	return 0;
 }
 
+int bt_hci_le_conn_update(uint16_t handle, uint16_t min, uint16_t max,
+			  uint16_t latency, uint16_t timeout)
+{
+	struct hci_cp_le_conn_update *conn_update;
+	struct bt_buf *buf;
+
+	buf = bt_hci_cmd_create(BT_HCI_OP_LE_CONN_UPDATE, sizeof(*conn_update));
+	if (!buf) {
+		return -ENOBUFS;
+	}
+
+	conn_update = bt_buf_add(buf, sizeof(*conn_update));
+	memset(conn_update, 0, sizeof(*conn_update));
+	conn_update->handle = sys_cpu_to_le16(handle);
+	conn_update->conn_interval_min = sys_cpu_to_le16(min);
+	conn_update->conn_interval_max = sys_cpu_to_le16(max);
+	conn_update->conn_latency = sys_cpu_to_le16(latency);
+	conn_update->supervision_timeout = sys_cpu_to_le16(timeout);
+
+	return bt_hci_cmd_send(BT_HCI_OP_LE_CONN_UPDATE, buf);
+}
+
 static int hci_le_create_conn(const bt_addr_le_t *addr)
 {
 	struct bt_buf *buf;

net/bluetooth/hci_core.h

@@ -155,3 +155,7 @@ int bt_hci_cmd_send_sync(uint16_t opcode, struct bt_buf *buf,
 const char *bt_addr_str(const bt_addr_t *addr);
 const char *bt_addr_le_str(const bt_addr_le_t *addr);
 #endif
+
+int bt_hci_le_conn_update(uint16_t handle, uint16_t min, uint16_t max,
+			  uint16_t latency, uint16_t timeout);
+

net/bluetooth/l2cap.c

@@ -57,6 +57,9 @@
 #define LE_CONN_LATENCY		0x0000
 #define LE_CONN_TIMEOUT		0x002a
 
+#define BT_L2CAP_CONN_PARAM_ACCEPTED 0
+#define BT_L2CAP_CONN_PARAM_REJECTED 1
+
 static struct bt_l2cap_chan *channels;
 
 static uint8_t get_ident(struct bt_conn *conn)
@@ -166,6 +169,77 @@ static void le_conn_param_rsp(struct bt_conn *conn, struct bt_buf *buf)
 	BT_DBG("LE conn param rsp result %u\n", sys_le16_to_cpu(rsp->result));
 }
 
+static uint16_t le_validate_conn_params(uint16_t min, uint16_t max, uint16_t latency,
+				   uint16_t timeout)
+{
+	uint16_t max_latency;
+
+	if (min > max || min < 6 || max > 3200) {
+		return BT_L2CAP_CONN_PARAM_REJECTED;
+	}
+
+	if (timeout < 10 || timeout > 3200) {
+		return BT_L2CAP_CONN_PARAM_REJECTED;
+	}
+
+	/* calculation based on BT spec 4.2 [Vol3, PartA, 4.20]
+	 * max_latency = ((timeout * 4)/(max * 1.25 * 2)) - 1;
+	 */
+	max_latency = (timeout * 4 / max) - 1;
+	if (latency > 499 || latency > max_latency) {
+		return BT_L2CAP_CONN_PARAM_REJECTED;
+	}
+
+	return BT_L2CAP_CONN_PARAM_ACCEPTED;
+}
+
+static void le_conn_param_update_req(struct bt_conn *conn, uint8_t ident,
+				     struct bt_buf *buf)
+{
+	uint16_t min, max, latency, timeout, result;
+	struct bt_l2cap_sig_hdr *hdr;
+	struct bt_l2cap_conn_param_rsp *rsp;
+	struct bt_l2cap_conn_param_req *req = (void *)buf->data;
+
+	if (buf->len < sizeof(*req)) {
+		BT_ERR("Too small LE conn update param req\n");
+		return;
+	}
+
+	if (conn->role != BT_HCI_ROLE_MASTER) {
+		return;
+	}
+
+	min = sys_le16_to_cpu(req->min_interval);
+	max = sys_le16_to_cpu(req->max_interval);
+	latency = sys_le16_to_cpu(req->latency);
+	timeout = sys_le16_to_cpu(req->timeout);
+
+	BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x timeout: 0x%4.4x",
+	       min, max, latency, timeout);
+
+	buf = bt_l2cap_create_pdu(conn);
+	if (!buf) {
+		return;
+	}
+
+	result = le_validate_conn_params(min, max, latency, timeout);
+
+	hdr = bt_buf_add(buf, sizeof(*hdr));
+	hdr->code = BT_L2CAP_CONN_PARAM_RSP;
+	hdr->ident = ident;
+	hdr->len = sys_cpu_to_le16(sizeof(*rsp));
+
+	rsp = bt_buf_add(buf, sizeof(*rsp));
+	memset(rsp, 0, sizeof(*rsp));
+	rsp->result = sys_cpu_to_le16(result);
+	bt_l2cap_send(conn, BT_L2CAP_CID_LE_SIG, buf);
+
+	if (result == BT_L2CAP_CONN_PARAM_ACCEPTED) {
+		bt_hci_le_conn_update(conn->handle, min, max, latency, timeout);
+	}
+}
+
 static void le_sig(struct bt_conn *conn, struct bt_buf *buf)
 {
 	struct bt_l2cap_sig_hdr *hdr = (void *)buf->data;
@@ -196,6 +270,9 @@ static void le_sig(struct bt_conn *conn, struct bt_buf *buf)
 	case BT_L2CAP_CONN_PARAM_RSP:
 		le_conn_param_rsp(conn, buf);
 		break;
+	case BT_L2CAP_CONN_PARAM_REQ:
+		le_conn_param_update_req(conn, hdr->ident, buf);
+		break;
 	default:
 		BT_WARN("Unknown L2CAP PDU code 0x%02x\n", hdr->code);
 		rej_not_understood(conn, hdr->ident);