From 2faef1bba1a18473b7c9a59900d4bf4476ce1c20 Mon Sep 17 00:00:00 2001
From: Claudio Takahasi <claudio.takahasi@openbossa.org>
Date: Fri, 11 Feb 2011 19:28:55 -0200
Subject: [PATCH] --- yaml --- r: 237402 b: refs/heads/master c:
 de73115a7d67e1b81dbde2285a7657f3e3867703 h: refs/heads/master v: v3

---
 [refs]                              |  2 +-
 trunk/include/net/bluetooth/l2cap.h | 15 ++++++++
 trunk/net/bluetooth/l2cap_core.c    | 59 ++++++++++++++++++++++++++++-
 3 files changed, 74 insertions(+), 2 deletions(-)

diff --git a/[refs] b/[refs]
index a64ee1136ff2..affe87e8c5bb 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 3300d9a930a79508032e3e03ac2bde3a22dd048d
+refs/heads/master: de73115a7d67e1b81dbde2285a7657f3e3867703
diff --git a/trunk/include/net/bluetooth/l2cap.h b/trunk/include/net/bluetooth/l2cap.h
index 06f245dcf6b2..4f4bff1eaed6 100644
--- a/trunk/include/net/bluetooth/l2cap.h
+++ b/trunk/include/net/bluetooth/l2cap.h
@@ -261,6 +261,21 @@ struct l2cap_info_rsp {
 #define L2CAP_IR_SUCCESS    0x0000
 #define L2CAP_IR_NOTSUPP    0x0001
 
+struct l2cap_conn_param_update_req {
+	__le16      min;
+	__le16      max;
+	__le16      latency;
+	__le16      to_multiplier;
+} __packed;
+
+struct l2cap_conn_param_update_rsp {
+	__le16      result;
+} __packed;
+
+/* Connection Parameters result */
+#define L2CAP_CONN_PARAM_ACCEPTED	0x0000
+#define L2CAP_CONN_PARAM_REJECTED	0x0001
+
 /* ----- L2CAP connections ----- */
 struct l2cap_chan_list {
 	struct sock	*head;
diff --git a/trunk/net/bluetooth/l2cap_core.c b/trunk/net/bluetooth/l2cap_core.c
index ce781a43f1d5..e0e7b82cff02 100644
--- a/trunk/net/bluetooth/l2cap_core.c
+++ b/trunk/net/bluetooth/l2cap_core.c
@@ -2501,6 +2501,63 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
 	return 0;
 }
 
+static int inline l2cap_check_conn_param(u16 min, u16 max, u16 latency,
+							u16 to_multiplier)
+{
+	u16 max_latency;
+
+	if (min > max || min < 6 || max > 3200)
+		return -EINVAL;
+
+	if (to_multiplier < 10 || to_multiplier > 3200)
+		return -EINVAL;
+
+	if (max >= to_multiplier * 8)
+		return -EINVAL;
+
+	max_latency = (to_multiplier * 8 / max) - 1;
+	if (latency > 499 || latency > max_latency)
+		return -EINVAL;
+
+	return 0;
+}
+
+static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
+					struct l2cap_cmd_hdr *cmd, u8 *data)
+{
+	struct hci_conn *hcon = conn->hcon;
+	struct l2cap_conn_param_update_req *req;
+	struct l2cap_conn_param_update_rsp rsp;
+	u16 min, max, latency, to_multiplier, cmd_len;
+
+	if (!(hcon->link_mode & HCI_LM_MASTER))
+		return -EINVAL;
+
+	cmd_len = __le16_to_cpu(cmd->len);
+	if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
+		return -EPROTO;
+
+	req = (struct l2cap_conn_param_update_req *) data;
+	min 		= __le16_to_cpu(req->min);
+	max 		= __le16_to_cpu(req->max);
+	latency		= __le16_to_cpu(req->latency);
+	to_multiplier	= __le16_to_cpu(req->to_multiplier);
+
+	BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
+						min, max, latency, to_multiplier);
+
+	memset(&rsp, 0, sizeof(rsp));
+	if (l2cap_check_conn_param(min, max, latency, to_multiplier))
+		rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
+	else
+		rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
+
+	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
+							sizeof(rsp), &rsp);
+
+	return 0;
+}
+
 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
 			struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
 {
@@ -2567,7 +2624,7 @@ static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
 		return 0;
 
 	case L2CAP_CONN_PARAM_UPDATE_REQ:
-		return -EINVAL;
+		return l2cap_conn_param_update_req(conn, cmd, data);
 
 	case L2CAP_CONN_PARAM_UPDATE_RSP:
 		return 0;