From 0b1e6fb2315eb3dc1be164c87d9589fcd06e6e79 Mon Sep 17 00:00:00 2001
From: Jaganath Kanakkassery <jaganathx.kanakkassery@intel.com>
Date: Thu, 15 Dec 2016 20:59:36 +0530
Subject: [PATCH] Bluetooth: RFCOMM: Respond to RLS command

If remote sends RLS command then it should be responded with the
value received.

> ACL Data RX: Handle 256 flags 0x02 dlen 12
      Channel: 64 len 8 [PSM 3 mode 0] {chan 0}
      RFCOMM: Unnumbered Info with Header Check (UIH) (0xef)
         Address: 0x03 cr 1 dlci 0x00
         Control: 0xef poll/final 0
         Length: 4
         FCS: 0x70
         MCC Message type: Remote Line Status CMD (0x14)
           Length: 2
           dlci 10 error: 5

< ACL Data TX: Handle 256 flags 0x00 dlen 12
      Channel: 64 len 8 [PSM 3 mode 0] {chan 0}
      RFCOMM: Unnumbered Info with Header Check (UIH) (0xef)
         Address: 0x01 cr 0 dlci 0x00
         Control: 0xef poll/final 0
         Length: 4
         FCS: 0xaa
         MCC Message type: Remote Line Status RSP (0x14)
           Length: 2
           dlci 10 error: 5

Change-Id: Ib70e02aede2088bca748d1eb68bee9b1bc47a65d
Signed-off-by: Jaganath Kanakkassery <jaganathx.kanakkassery@intel.com>
---
 subsys/bluetooth/host/rfcomm.c          | 46 +++++++++++++++++++++++++
 subsys/bluetooth/host/rfcomm_internal.h |  6 ++++
 2 files changed, 52 insertions(+)

diff --git a/subsys/bluetooth/host/rfcomm.c b/subsys/bluetooth/host/rfcomm.c
index 5795a658b26..594cfb13271 100644
--- a/subsys/bluetooth/host/rfcomm.c
+++ b/subsys/bluetooth/host/rfcomm.c
@@ -580,6 +580,26 @@ static int rfcomm_send_msc(struct bt_rfcomm_dlc *dlc, uint8_t cr)
 	return bt_l2cap_chan_send(&dlc->session->br_chan.chan, buf);
 }
 
+static int rfcomm_send_rls(struct bt_rfcomm_dlc *dlc, uint8_t cr,
+			   uint8_t line_status)
+{
+	struct bt_rfcomm_rls *rls;
+	struct net_buf *buf;
+	uint8_t fcs;
+
+	buf = rfcomm_make_uih_msg(dlc, cr, BT_RFCOMM_RLS, sizeof(*rls));
+
+	rls = net_buf_add(buf, sizeof(*rls));
+	/* cr bit should be always 1 in RLS */
+	rls->dlci = BT_RFCOMM_SET_ADDR(dlc->dlci, 1);
+	rls->line_status = line_status;
+
+	fcs = rfcomm_calc_fcs(BT_RFCOMM_FCS_LEN_UIH, buf->data);
+	net_buf_add_u8(buf, fcs);
+
+	return bt_l2cap_chan_send(&dlc->session->br_chan.chan, buf);
+}
+
 static void rfcomm_dlc_connected(struct bt_rfcomm_dlc *dlc)
 {
 	dlc->state = BT_RFCOMM_STATE_CONNECTED;
@@ -893,6 +913,29 @@ static void rfcomm_handle_msc(struct bt_rfcomm_session *session,
 	}
 }
 
+static void rfcomm_handle_rls(struct bt_rfcomm_session *session,
+			      struct net_buf *buf, uint8_t cr)
+{
+	struct bt_rfcomm_rls *rls = (void *)buf->data;
+	uint8_t dlci = BT_RFCOMM_GET_DLCI(rls->dlci);
+	struct bt_rfcomm_dlc *dlc;
+
+	BT_DBG("dlci %d", dlci);
+
+	if (!cr) {
+		/* Ignore if its a response */
+		return;
+	}
+
+	dlc = rfcomm_dlcs_lookup_dlci(session->dlcs, dlci);
+	if (!dlc) {
+		return;
+	}
+
+	/* As per the ETSI same line status has to returned in the response */
+	rfcomm_send_rls(dlc, BT_RFCOMM_MSG_RESP_CR, rls->line_status);
+}
+
 static void rfcomm_handle_pn(struct bt_rfcomm_session *session,
 			     struct net_buf *buf, uint8_t cr)
 {
@@ -989,6 +1032,9 @@ static void rfcomm_handle_msg(struct bt_rfcomm_session *session,
 	case BT_RFCOMM_MSC:
 		rfcomm_handle_msc(session, buf, cr);
 		break;
+	case BT_RFCOMM_RLS:
+		rfcomm_handle_rls(session, buf, cr);
+		break;
 	default:
 		BT_WARN("Unknown/Unsupported RFCOMM Msg type 0x%02x", msg_type);
 		break;
diff --git a/subsys/bluetooth/host/rfcomm_internal.h b/subsys/bluetooth/host/rfcomm_internal.h
index 9733f65f12b..5f44946cd77 100644
--- a/subsys/bluetooth/host/rfcomm_internal.h
+++ b/subsys/bluetooth/host/rfcomm_internal.h
@@ -77,6 +77,12 @@ struct bt_rfcomm_msc {
 #define BT_RFCOMM_DISC  0x43
 #define BT_RFCOMM_DM    0x0f
 
+#define BT_RFCOMM_RLS   0x14
+struct bt_rfcomm_rls {
+	uint8_t  dlci;
+	uint8_t  line_status;
+} __packed;
+
 /* DV = 1 IC = 0 RTR = 1 RTC = 1 FC = 0 EXT = 0 */
 #define BT_RFCOMM_DEFAULT_V24_SIG 0x8d