Bluetooth: Add configuration support for ERTM and Streaming mode (f2fcfcd6) · Commits · 戴 / test

include/net/bluetooth/l2cap.h

+21 −7

Original line number	Diff line number	Diff line
		@@ -27,8 +27,9 @@

		/* L2CAP defaults */
		#define L2CAP_DEFAULT_MTU 672
		#define L2CAP_DEFAULT_MIN_MTU 48
		#define L2CAP_DEFAULT_FLUSH_TO 0xffff
		#define L2CAP_DEFAULT_RX_WINDOW 1
		#define L2CAP_DEFAULT_TX_WINDOW 1
		#define L2CAP_DEFAULT_MAX_RECEIVE 1
		#define L2CAP_DEFAULT_RETRANS_TO 300 /* 300 milliseconds */
		#define L2CAP_DEFAULT_MONITOR_TO 1000 /* 1 second */
		@@ -272,6 +273,9 @@ struct l2cap_pinfo {
		__u16 omtu;
		__u16 flush_to;
		__u8 mode;
		__u8 num_conf_req;
		__u8 num_conf_rsp;

		__u8 fcs;
		__u8 sec_level;
		__u8 role_switch;
		@@ -280,10 +284,15 @@ struct l2cap_pinfo {
		__u8 conf_req[64];
		__u8 conf_len;
		__u8 conf_state;
		__u8 conf_retry;

		__u8 ident;

		__u8 remote_tx_win;
		__u8 remote_max_tx;
		__u16 retrans_timeout;
		__u16 monitor_timeout;
		__u16 max_pdu_size;

		__le16 sport;

		struct l2cap_conn *conn;
		@@ -294,9 +303,14 @@ struct l2cap_pinfo {
		#define L2CAP_CONF_REQ_SENT 0x01
		#define L2CAP_CONF_INPUT_DONE 0x02
		#define L2CAP_CONF_OUTPUT_DONE 0x04
		#define L2CAP_CONF_CONNECT_PEND 0x80
		#define L2CAP_CONF_MTU_DONE 0x08
		#define L2CAP_CONF_MODE_DONE 0x10
		#define L2CAP_CONF_CONNECT_PEND 0x20
		#define L2CAP_CONF_STATE2_DEVICE 0x80

		#define L2CAP_CONF_MAX_CONF_REQ 2
		#define L2CAP_CONF_MAX_CONF_RSP 2

		#define L2CAP_CONF_MAX_RETRIES 2

		void l2cap_load(void);

net/bluetooth/l2cap.c

+234 −28

Original line number	Diff line number	Diff line
		@@ -966,6 +966,7 @@ static int l2cap_sock_connect(struct socket sock, struct sockaddr addr, int al
		case L2CAP_MODE_BASIC:
		break;
		case L2CAP_MODE_ERTM:
		case L2CAP_MODE_STREAMING:
		if (enable_ertm)
		break;
		/* fall through */
		@@ -1029,6 +1030,7 @@ static int l2cap_sock_listen(struct socket *sock, int backlog)
		case L2CAP_MODE_BASIC:
		break;
		case L2CAP_MODE_ERTM:
		case L2CAP_MODE_STREAMING:
		if (enable_ertm)
		break;
		/* fall through */
		@@ -1739,15 +1741,65 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
		*ptr += L2CAP_CONF_OPT_SIZE + len;
		}

		static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
		{
		u32 local_feat_mask = l2cap_feat_mask;
		if (enable_ertm)
		local_feat_mask \|= L2CAP_FEAT_ERTM;

		switch (mode) {
		case L2CAP_MODE_ERTM:
		return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
		case L2CAP_MODE_STREAMING:
		return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
		default:
		return 0x00;
		}
		}

		static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
		{
		switch (mode) {
		case L2CAP_MODE_STREAMING:
		case L2CAP_MODE_ERTM:
		if (l2cap_mode_supported(mode, remote_feat_mask))
		return mode;
		/* fall through */
		default:
		return L2CAP_MODE_BASIC;
		}
		}

		static int l2cap_build_conf_req(struct sock sk, void data)
		{
		struct l2cap_pinfo *pi = l2cap_pi(sk);
		struct l2cap_conf_req *req = data;
		struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
		struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_ERTM };
		void *ptr = req->data;

		BT_DBG("sk %p", sk);

		if (pi->num_conf_req \|\| pi->num_conf_rsp)
		goto done;

		switch (pi->mode) {
		case L2CAP_MODE_STREAMING:
		case L2CAP_MODE_ERTM:
		pi->conf_state \|= L2CAP_CONF_STATE2_DEVICE;
		if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask)) {
		struct l2cap_disconn_req req;
		req.dcid = cpu_to_le16(pi->dcid);
		req.scid = cpu_to_le16(pi->scid);
		l2cap_send_cmd(pi->conn, l2cap_get_ident(pi->conn),
		L2CAP_DISCONN_REQ, sizeof(req), &req);
		}
		break;
		default:
		pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
		break;
		}

		done:
		switch (pi->mode) {
		case L2CAP_MODE_BASIC:
		if (pi->imtu != L2CAP_DEFAULT_MTU)
		@@ -1756,10 +1808,22 @@ static int l2cap_build_conf_req(struct sock sk, void data)

		case L2CAP_MODE_ERTM:
		rfc.mode = L2CAP_MODE_ERTM;
		rfc.txwin_size = L2CAP_DEFAULT_RX_WINDOW;
		rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
		rfc.max_transmit = L2CAP_DEFAULT_MAX_RECEIVE;
		rfc.retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
		rfc.monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
		rfc.retrans_timeout = 0;
		rfc.monitor_timeout = 0;
		rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_RX_APDU);

		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
		sizeof(rfc), (unsigned long) &rfc);
		break;

		case L2CAP_MODE_STREAMING:
		rfc.mode = L2CAP_MODE_STREAMING;
		rfc.txwin_size = 0;
		rfc.max_transmit = 0;
		rfc.retrans_timeout = 0;
		rfc.monitor_timeout = 0;
		rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_RX_APDU);

		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
		@@ -1825,30 +1889,83 @@ static int l2cap_parse_conf_req(struct sock sk, void data)
		}
		}

		if (pi->num_conf_rsp \|\| pi->num_conf_req)
		goto done;

		switch (pi->mode) {
		case L2CAP_MODE_STREAMING:
		case L2CAP_MODE_ERTM:
		pi->conf_state \|= L2CAP_CONF_STATE2_DEVICE;
		if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
		return -ECONNREFUSED;
		break;
		default:
		pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
		break;
		}

		done:
		if (pi->mode != rfc.mode) {
		result = L2CAP_CONF_UNACCEPT;
		rfc.mode = pi->mode;

		if (pi->num_conf_rsp == 1)
		return -ECONNREFUSED;

		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
		sizeof(rfc), (unsigned long) &rfc);
		}


		if (result == L2CAP_CONF_SUCCESS) {
		/* Configure output options and let the other side know
		* which ones we don't like. */

		if (rfc.mode == L2CAP_MODE_BASIC) {
		if (mtu < pi->omtu)
		if (mtu < L2CAP_DEFAULT_MIN_MTU)
		result = L2CAP_CONF_UNACCEPT;
		else {
		pi->omtu = mtu;
		pi->conf_state \|= L2CAP_CONF_OUTPUT_DONE;
		pi->conf_state \|= L2CAP_CONF_MTU_DONE;
		}

		l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
		} else {

		switch (rfc.mode) {
		case L2CAP_MODE_BASIC:
		pi->fcs = L2CAP_FCS_NONE;
		pi->conf_state \|= L2CAP_CONF_MODE_DONE;
		break;

		case L2CAP_MODE_ERTM:
		pi->remote_tx_win = rfc.txwin_size;
		pi->remote_max_tx = rfc.max_transmit;
		pi->max_pdu_size = rfc.max_pdu_size;

		rfc.retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
		rfc.monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;

		pi->conf_state \|= L2CAP_CONF_MODE_DONE;
		break;

		case L2CAP_MODE_STREAMING:
		pi->remote_tx_win = rfc.txwin_size;
		pi->max_pdu_size = rfc.max_pdu_size;

		pi->conf_state \|= L2CAP_CONF_MODE_DONE;
		break;

		default:
		result = L2CAP_CONF_UNACCEPT;

		memset(&rfc, 0, sizeof(rfc));
		rfc.mode = L2CAP_MODE_BASIC;
		rfc.mode = pi->mode;
		}

		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
		sizeof(rfc), (unsigned long) &rfc);
		}
		}

		if (result == L2CAP_CONF_SUCCESS)
		pi->conf_state \|= L2CAP_CONF_OUTPUT_DONE;
		}
		rsp->scid = cpu_to_le16(pi->dcid);
		rsp->result = cpu_to_le16(result);
		rsp->flags = cpu_to_le16(0x0000);
		@@ -1856,6 +1973,73 @@ static int l2cap_parse_conf_req(struct sock sk, void data)
		return ptr - data;
		}

		static int l2cap_parse_conf_rsp(struct sock sk, void rsp, int len, void data, u16 result)
		{
		struct l2cap_pinfo *pi = l2cap_pi(sk);
		struct l2cap_conf_req *req = data;
		void *ptr = req->data;
		int type, olen;
		unsigned long val;
		struct l2cap_conf_rfc rfc;

		BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);

		while (len >= L2CAP_CONF_OPT_SIZE) {
		len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);

		switch (type) {
		case L2CAP_CONF_MTU:
		if (val < L2CAP_DEFAULT_MIN_MTU) {
		*result = L2CAP_CONF_UNACCEPT;
		pi->omtu = L2CAP_DEFAULT_MIN_MTU;
		} else
		pi->omtu = val;
		l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
		break;

		case L2CAP_CONF_FLUSH_TO:
		pi->flush_to = val;
		l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
		2, pi->flush_to);
		break;

		case L2CAP_CONF_RFC:
		if (olen == sizeof(rfc))
		memcpy(&rfc, (void *)val, olen);

		if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
		rfc.mode != pi->mode)
		return -ECONNREFUSED;

		pi->mode = rfc.mode;
		pi->fcs = 0;

		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
		sizeof(rfc), (unsigned long) &rfc);
		break;
		}
		}

		if (*result == L2CAP_CONF_SUCCESS) {
		switch (rfc.mode) {
		case L2CAP_MODE_ERTM:
		pi->remote_tx_win = rfc.txwin_size;
		pi->retrans_timeout = rfc.retrans_timeout;
		pi->monitor_timeout = rfc.monitor_timeout;
		pi->max_pdu_size = le16_to_cpu(rfc.max_pdu_size);
		break;
		case L2CAP_MODE_STREAMING:
		pi->max_pdu_size = le16_to_cpu(rfc.max_pdu_size);
		break;
		}
		}

		req->dcid = cpu_to_le16(pi->dcid);
		req->flags = cpu_to_le16(0x0000);

		return ptr - data;
		}

		static int l2cap_build_conf_rsp(struct sock sk, void data, u16 result, u16 flags)
		{
		struct l2cap_conf_rsp *rsp = data;
		@@ -2042,6 +2226,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd

		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
		l2cap_build_conf_req(sk, req), req);
		l2cap_pi(sk)->num_conf_req++;
		break;

		case L2CAP_CR_PEND:
		@@ -2100,10 +2285,17 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr

		/* Complete config. */
		len = l2cap_parse_conf_req(sk, rsp);
		if (len < 0)
		if (len < 0) {
		struct l2cap_disconn_req req;
		req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
		req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
		l2cap_send_cmd(conn, l2cap_get_ident(conn),
		L2CAP_DISCONN_REQ, sizeof(req), &req);
		goto unlock;
		}

		l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
		l2cap_pi(sk)->num_conf_rsp++;

		/* Reset config buffer. */
		l2cap_pi(sk)->conf_len = 0;
		@@ -2121,6 +2313,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
		u8 buf[64];
		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
		l2cap_build_conf_req(sk, buf), buf);
		l2cap_pi(sk)->num_conf_req++;
		}

		unlock:
		@@ -2150,16 +2343,29 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
		break;

		case L2CAP_CONF_UNACCEPT:
		if (++l2cap_pi(sk)->conf_retry < L2CAP_CONF_MAX_RETRIES) {
		char req[128];
		/* It does not make sense to adjust L2CAP parameters
		* that are currently defined in the spec. We simply
		* resend config request that we sent earlier. It is
		* stupid, but it helps qualification testing which
		* expects at least some response from us. */
		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
		l2cap_build_conf_req(sk, req), req);
		if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
		int len = cmd->len - sizeof(*rsp);
		char req[64];

		/* throw out any old stored conf requests */
		result = L2CAP_CONF_SUCCESS;
		len = l2cap_parse_conf_rsp(sk, rsp->data,
		len, req, &result);
		if (len < 0) {
		struct l2cap_disconn_req req;
		req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
		req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
		l2cap_send_cmd(conn, l2cap_get_ident(conn),
		L2CAP_DISCONN_REQ, sizeof(req), &req);
		goto done;
		}

		l2cap_send_cmd(conn, l2cap_get_ident(conn),
		L2CAP_CONF_REQ, len, req);
		l2cap_pi(sk)->num_conf_req++;
		if (result != L2CAP_CONF_SUCCESS)
		goto done;
		break;
		}

		default:

Admin message