From 028edd59baf6d74462ade1775ad93243452f85d3 Mon Sep 17 00:00:00 2001
From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>
Date: Mon, 4 May 2015 17:22:10 +0300
Subject: [PATCH] Bluetooth: Add code to handle BT_ATT_OP_MTU_REQ

Add code to respond to BT_ATT_OP_MTU_REQ with BT_ATT_OP_MTU_RSP
selecting the MTU based on the amount of room bt_buf has:

< ACL Data RX: Handle 39 flags 0x02 dlen 7
      ATT: Exchange MTU Response (0x03) len 2
        Server RX MTU: 65

Change-Id: I47b8179fcee0d6adff5761abc37cfcb1ed636d65
Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>
---
 net/bluetooth/att.c | 49 +++++++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/att.h |  3 +++
 2 files changed, 52 insertions(+)

diff --git a/net/bluetooth/att.c b/net/bluetooth/att.c
index be137f6487e..72fe00bfadc 100644
--- a/net/bluetooth/att.c
+++ b/net/bluetooth/att.c
@@ -35,6 +35,7 @@
 #include <string.h>
 #include <errno.h>
 #include <misc/byteorder.h>
+#include <misc/util.h>
 
 #include <bluetooth/hci.h>
 #include <bluetooth/bluetooth.h>
@@ -63,6 +64,51 @@ static void send_err_rsp(struct bt_conn *conn, uint8_t req, uint16_t handle,
 	bt_conn_send(conn, buf);
 }
 
+static void att_mtu_req(struct bt_conn *conn, struct bt_buf *data)
+{
+	struct bt_att_exchange_mtu_req *req;
+	struct bt_att_exchange_mtu_rsp *rsp;
+	struct bt_buf *buf;
+	uint16_t mtu;
+
+	if (data->len != sizeof(*req)) {
+		send_err_rsp(conn, BT_ATT_OP_MTU_REQ, 0,
+			     BT_ATT_ERR_INVALID_PDU);
+		return;
+	}
+
+	req = (void *)data->data;
+
+	mtu = sys_le16_to_cpu(req->mtu);
+
+	BT_DBG("Client MTU %u\n", mtu);
+
+	if (mtu > BT_ATT_MAX_LE_MTU || mtu < BT_ATT_DEFAULT_LE_MTU) {
+		send_err_rsp(conn, BT_ATT_OP_MTU_REQ, 0,
+			     BT_ATT_ERR_INVALID_PDU);
+		return;
+	}
+
+	buf = bt_att_create_pdu(conn, BT_ATT_OP_MTU_RSP, sizeof(*rsp));
+	if (!buf) {
+		return;
+	}
+
+	/* Select MTU based on the amount of room we have in bt_buf including
+	 * one extra byte for ATT header.
+	 */
+	mtu = min(mtu, bt_buf_tailroom(buf) + 1);
+
+	BT_DBG("Server MTU %u\n", mtu);
+
+	/* TODO: Store the MTU negotiated */
+
+	rsp = (void *)bt_buf_add(buf, sizeof(*rsp));
+	rsp->mtu = sys_cpu_to_le16(mtu);
+
+	bt_conn_send(conn, buf);
+}
+
 void bt_att_recv(struct bt_conn *conn, struct bt_buf *buf)
 {
 	struct bt_att_hdr *hdr = (void *)buf->data;
@@ -77,6 +123,9 @@ void bt_att_recv(struct bt_conn *conn, struct bt_buf *buf)
 	bt_buf_pull(buf, sizeof(*hdr));
 
 	switch (hdr->code) {
+	case BT_ATT_OP_MTU_REQ:
+		att_mtu_req(conn, buf);
+		break;
 	default:
 		BT_DBG("Unhandled ATT code %u\n", hdr->code);
 		send_err_rsp(conn, hdr->code, 0, BT_ATT_ERR_NOT_SUPPORTED);
diff --git a/net/bluetooth/att.h b/net/bluetooth/att.h
index 724cb04b4b8..ca17196c8c6 100644
--- a/net/bluetooth/att.h
+++ b/net/bluetooth/att.h
@@ -30,6 +30,9 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#define BT_ATT_DEFAULT_LE_MTU	23
+#define BT_ATT_MAX_LE_MTU	517
+
 struct bt_att_hdr {
 	uint8_t  code;
 } PACK_STRUCT;