Bluetooth: Add basic L2CAP support

This patch adds basic L2CAP functions, including support for the LE
signaling channel as well as performing a connection parameter update
in slave (peripheral) role.

Change-Id: Ie1f9751475adcf26576e9587bf6d68d15d6a2569
Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>

net/bluetooth/conn.c

@@ -166,7 +166,7 @@ static void conn_rx_fiber(int arg1, int arg2)
 			break;
 		}
 
-		bt_buf_put(buf);
+		bt_l2cap_recv(conn, buf);
 	}
 
 	BT_DBG("handle %u disconnected - cleaning up\n", conn->handle);
@@ -252,6 +252,8 @@ struct bt_conn *bt_conn_add(struct bt_dev *dev, uint16_t handle)
 	fiber_start(conn->tx_stack, BT_CONN_TX_STACK_SIZE, conn_tx_fiber,
 		    (int)bt_conn_get(conn), 0, 7, 0);
 
+	bt_l2cap_update_conn_param(conn);
+
 	return conn;
 }
 

net/bluetooth/l2cap.c

@@ -0,0 +1,176 @@
+#include <nanokernel.h>
+#include <toolchain.h>
+#include <string.h>
+#include <errno.h>
+#include <misc/byteorder.h>
+
+#include <bluetooth/hci.h>
+#include <bluetooth/bluetooth.h>
+
+#include "hci_core.h"
+#include "conn.h"
+#include "l2cap.h"
+
+#define LE_CONN_MIN_INTERVAL	0x0028
+#define LE_CONN_MAX_INTERVAL	0x0038
+#define LE_CONN_LATENCY		0x0000
+#define LE_CONN_TIMEOUT		0x002a
+
+static uint8_t get_ident(struct bt_conn *conn)
+{
+	conn->l2_ident++;
+
+	/* handle integer overflow (0 is not valid) */
+	if (!conn->l2_ident)
+		conn->l2_ident++;
+
+	return conn->l2_ident;
+}
+
+struct bt_buf *bt_l2cap_create_pdu(struct bt_conn *conn, uint16_t cid,
+				   size_t len)
+{
+	struct bt_l2cap_hdr *hdr;
+	struct bt_buf *buf;
+
+	buf = bt_conn_create_pdu(conn, sizeof(*hdr) + len);
+	if (!buf)
+		return NULL;
+
+	hdr = (void *)bt_buf_add(buf, sizeof(*hdr));
+	hdr->len = sys_cpu_to_le16(len);
+	hdr->cid = sys_cpu_to_le16(cid);
+
+	return buf;
+}
+
+static void rej_not_understood(struct bt_conn *conn, uint8_t ident)
+{
+	struct bt_l2cap_cmd_reject *rej;
+	struct bt_l2cap_sig_hdr *hdr;
+	struct bt_buf *buf;
+
+	buf = bt_l2cap_create_pdu(conn, BT_L2CAP_CID_LE_SIG,
+				  sizeof(*hdr) + sizeof(*rej));
+	if (!buf)
+		return;
+
+	hdr = (void *)bt_buf_add(buf, sizeof(*hdr));
+	hdr->code = BT_L2CAP_CMD_REJECT;
+	hdr->ident = ident;
+	hdr->len = sys_cpu_to_le16(sizeof(*rej));
+
+	rej = (void *)bt_buf_add(buf, sizeof(*rej));
+	rej->reason = sys_cpu_to_le16(BT_L2CAP_REJ_NOT_UNDERSTOOD);
+
+	bt_conn_send(conn, buf);
+}
+
+static void le_conn_param_rsp(struct bt_conn *conn, struct bt_buf *buf)
+{
+	struct bt_l2cap_conn_param_rsp *rsp = (void *)buf->data;
+
+	if (buf->len < sizeof(*rsp)) {
+		BT_ERR("Too small LE conn param rsp\n");
+		return;
+	}
+
+	BT_DBG("LE conn param rsp result %u\n", sys_le16_to_cpu(rsp->result));
+}
+
+static void le_sig(struct bt_conn *conn, struct bt_buf *buf)
+{
+	struct bt_l2cap_sig_hdr *hdr = (void *)buf->data;
+	uint16_t len;
+
+	if (buf->len < sizeof(*hdr)) {
+		BT_ERR("Too small L2CAP LE signaling PDU\n");
+		goto drop;
+	}
+
+	len = sys_le16_to_cpu(hdr->len);
+	bt_buf_pull(buf, sizeof(*hdr));
+
+	BT_DBG("LE signaling code %u ident %u len %u\n", hdr->code,
+	       hdr->ident, len);
+
+	if (buf->len != len) {
+		BT_ERR("L2CAP length mismatch (%u != %u)\n", buf->len, len);
+		goto drop;
+	}
+
+	if (!hdr->ident) {
+		BT_ERR("Invalid ident value in L2CAP PDU\n");
+		goto drop;
+	}
+
+	switch (hdr->code) {
+	case BT_L2CAP_CONN_PARAM_RSP:
+		le_conn_param_rsp(conn, buf);
+		break;
+	default:
+		BT_ERR("Unknown L2CAP PDU code %u\n", hdr->code);
+		rej_not_understood(conn, hdr->ident);
+		break;
+	}
+
+drop:
+	bt_buf_put(buf);
+}
+
+void bt_l2cap_recv(struct bt_conn *conn, struct bt_buf *buf)
+{
+	struct bt_l2cap_hdr *hdr = (void *)buf->data;
+	uint16_t cid;
+
+	if (buf->len < sizeof(*hdr)) {
+		BT_ERR("Too small L2CAP PDU received\n");
+		bt_buf_put(buf);
+		return;
+	}
+
+	cid = sys_le16_to_cpu(hdr->cid);
+	bt_buf_pull(buf, sizeof(*hdr));
+
+	BT_DBG("Packet for CID %u len %u\n", cid, buf->len);
+
+	switch (cid) {
+	case BT_L2CAP_CID_LE_SIG:
+		le_sig(conn, buf);
+		break;
+	default:
+		BT_ERR("Ignoring data for unknown CID %u\n", cid);
+		bt_buf_put(buf);
+		break;
+	}
+}
+
+void bt_l2cap_update_conn_param(struct bt_conn *conn)
+{
+	struct bt_l2cap_sig_hdr *hdr;
+	struct bt_l2cap_conn_param_req *req;
+	struct bt_buf *buf;
+
+	/* Check if we need to update anything */
+	if (conn->le_conn_interval >= LE_CONN_MIN_INTERVAL &&
+	    conn->le_conn_interval <= LE_CONN_MAX_INTERVAL)
+		return;
+
+	buf = bt_l2cap_create_pdu(conn, BT_L2CAP_CID_LE_SIG,
+				  sizeof(*hdr) + sizeof(*req));
+	if (!buf)
+		return;
+
+	hdr = (void *)bt_buf_add(buf, sizeof(*hdr));
+	hdr->code = BT_L2CAP_CONN_PARAM_REQ;
+	hdr->ident = get_ident(conn);
+	hdr->len = sys_cpu_to_le16(sizeof(*req));
+
+	req = (void *)bt_buf_add(buf, sizeof(*req));
+	req->min_interval = sys_cpu_to_le16(LE_CONN_MIN_INTERVAL);
+	req->max_interval = sys_cpu_to_le16(LE_CONN_MAX_INTERVAL);
+	req->latency = sys_cpu_to_le16(LE_CONN_LATENCY);
+	req->timeout = sys_cpu_to_le16(LE_CONN_TIMEOUT);
+
+	bt_conn_send(conn, buf);
+}

net/bluetooth/l2cap.h

@@ -43,3 +43,36 @@ struct bt_l2cap_sig_hdr {
 	uint8_t  ident;
 	uint16_t len;
 } PACK_STRUCT;
+
+#define BT_L2CAP_REJ_NOT_UNDERSTOOD	0x0000
+#define BT_L2CAP_REJ_MTU_EXCEEDED	0x0001
+#define BT_L2CAP_REJ_INVALID_CID	0x0002
+
+#define BT_L2CAP_CMD_REJECT		0x01
+struct bt_l2cap_cmd_reject {
+	uint16_t reason;
+	uint8_t  data[0];
+} PACK_STRUCT;
+
+#define BT_L2CAP_CONN_PARAM_REQ		0x12
+struct bt_l2cap_conn_param_req {
+	uint16_t min_interval;
+	uint16_t max_interval;
+	uint16_t latency;
+	uint16_t timeout;
+} PACK_STRUCT;
+
+#define BT_L2CAP_CONN_PARAM_RSP		0x13
+struct bt_l2cap_conn_param_rsp {
+	uint16_t result;
+} PACK_STRUCT;
+
+/* Prepare an L2CAP PDU to be sent over a connection */
+struct bt_buf *bt_l2cap_create_pdu(struct bt_conn *conn, uint16_t cid,
+				   size_t len);
+
+/* Receive a new L2CAP PDU from a connection */
+void bt_l2cap_recv(struct bt_conn *conn, struct bt_buf *buf);
+
+/* Perform connection parameter update request */
+void bt_l2cap_update_conn_param(struct bt_conn *conn);