1/*
2 * Copyright 2007 Oliver Ruiz Dorantes, oliver.ruiz.dorantes_at_gmail.com
3 * All rights reserved. Distributed under the terms of the MIT License.
4 *
5 */
6
7/*-
8 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
9 * All rights reserved.
10 *
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
13 * are met:
14 * 1. Redistributions of source code must retain the above copyright
15 *    notice, this list of conditions and the following disclaimer.
16 * 2. Redistributions in binary form must reproduce the above copyright
17 *    notice, this list of conditions and the following disclaimer in the
18 *    documentation and/or other materials provided with the distribution.
19*/
20
21
22#include <NetBufferUtilities.h>
23
24#include "l2cap_command.h"
25
26/*
27 * Note: All L2CAP implementations are required to support minimal signaling
28 *       MTU of 48 bytes. In order to simplify things we will send one command
29 *       per one L2CAP packet. Given evrything above we can assume that one
30 *       signaling packet will fit into single mbuf.
31 */
32
33
34/* Private types */
35struct _cmd_rej {
36	l2cap_cmd_hdr_t	 	hdr;
37	l2cap_cmd_rej_cp 	param;
38	l2cap_cmd_rej_data_t	data;
39} __attribute__ ((packed)) ;
40
41struct _con_req {
42	l2cap_cmd_hdr_t	 hdr;
43	l2cap_con_req_cp param;
44} __attribute__ ((packed));
45
46struct _con_rsp {
47	l2cap_cmd_hdr_t	 hdr;
48	l2cap_con_rsp_cp param;
49} __attribute__ ((packed));
50
51struct _cfg_req {
52	l2cap_cmd_hdr_t	 hdr;
53	l2cap_cfg_req_cp param;
54} __attribute__ ((packed));
55
56struct _cfg_rsp {
57	l2cap_cmd_hdr_t	 hdr;
58	l2cap_cfg_rsp_cp param;
59} __attribute__ ((packed));
60
61struct _discon_req {
62	l2cap_cmd_hdr_t	 hdr;
63	l2cap_discon_req_cp	 param;
64} __attribute__ ((packed));
65
66struct _discon_rsp {
67	l2cap_cmd_hdr_t	 hdr;
68	l2cap_discon_rsp_cp	 param;
69} __attribute__ ((packed));
70
71struct _info_req {
72	l2cap_cmd_hdr_t	 hdr;
73	l2cap_info_req_cp	 param;
74} __attribute__ ((packed));
75
76struct _info_rsp {
77	l2cap_cmd_hdr_t	 hdr;
78	l2cap_info_rsp_cp	 param;
79	l2cap_info_rsp_data_t data;
80} __attribute__ ((packed));
81
82// Configuration options
83struct _cfg_opt_flow {
84	l2cap_cfg_opt_t	 hdr;
85	l2cap_flow_t		 val;
86} __attribute__ ((packed));
87
88
89struct _cfg_opt_flush {
90	l2cap_cfg_opt_t	 hdr;
91	uint16		 val;
92} __attribute__ ((packed));
93
94struct _cfg_opt_mtu {
95	l2cap_cfg_opt_t	 hdr;
96	uint16		 val;
97} __attribute__ ((packed));
98
99
100
101
102/* L2CAP_CommandRej */
103net_buffer*
104l2cap_cmd_rej(uint8 _ident, uint16 _reason, uint16 _mtu, uint16 _scid, uint16 _dcid)
105{
106
107	net_buffer* _m = gBufferModule->create(sizeof(struct _cmd_rej));
108	if ((_m) == NULL)
109		return NULL;
110
111	NetBufferPrepend<struct _cmd_rej> bufferHeader(_m);
112	status_t status = bufferHeader.Status();
113	if (status < B_OK) {
114		// free the buffer
115		return NULL;
116	}
117
118	bufferHeader->hdr.code = L2CAP_CMD_REJ;
119	bufferHeader->hdr.ident = (_ident);
120	bufferHeader->hdr.length = sizeof(bufferHeader->param);
121
122	bufferHeader->param.reason = htole16((_reason));
123
124	if ((_reason) == L2CAP_REJ_MTU_EXCEEDED) {
125		bufferHeader->data.mtu.mtu = htole16((_mtu));
126		bufferHeader->hdr.length += sizeof(bufferHeader->data.mtu);
127	} else if ((_reason) == L2CAP_REJ_INVALID_CID) {
128		bufferHeader->data.cid.scid = htole16((_scid));
129		bufferHeader->data.cid.dcid = htole16((_dcid));
130		bufferHeader->hdr.length += sizeof(bufferHeader->data.cid);
131	}
132
133	bufferHeader->hdr.length = htole16(bufferHeader->hdr.length);
134
135	bufferHeader.Sync();
136
137	return _m;
138}
139
140
141/* L2CAP_ConnectReq */
142net_buffer*
143l2cap_con_req(uint8 _ident, uint16 _psm, uint16 _scid)
144{
145
146	net_buffer* _m = gBufferModule->create(sizeof(struct _con_req));
147	if ((_m) == NULL)
148		return NULL;
149
150	NetBufferPrepend<struct _con_req> bufferHeader(_m);
151	status_t status = bufferHeader.Status();
152	if (status < B_OK) {
153		/* TODO free the buffer */
154		return NULL;
155	}
156
157	bufferHeader->hdr.code = L2CAP_CON_REQ;
158	bufferHeader->hdr.ident = (_ident);
159	bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param));
160
161	bufferHeader->param.psm = htole16((_psm));
162	bufferHeader->param.scid = htole16((_scid));
163
164	bufferHeader.Sync();
165
166	return _m;
167}
168
169
170/* L2CAP_ConnectRsp */
171net_buffer*
172l2cap_con_rsp(uint8 _ident, uint16 _dcid, uint16 _scid, uint16 _result, uint16 _status)
173{
174
175	net_buffer* _m = gBufferModule->create(sizeof(struct _con_rsp));
176	if ((_m) == NULL)
177		return NULL;
178
179	NetBufferPrepend<struct _con_rsp> bufferHeader(_m);
180	status_t status = bufferHeader.Status();
181	if (status < B_OK) {
182		/* TODO free the buffer */
183		return NULL;
184	}
185
186   	bufferHeader->hdr.code = L2CAP_CON_RSP;
187	bufferHeader->hdr.ident = (_ident);
188	bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param));
189
190	bufferHeader->param.dcid = htole16((_dcid));
191	bufferHeader->param.scid = htole16((_scid));
192	bufferHeader->param.result = htole16((_result));
193	bufferHeader->param.status = htole16((_status)); /* reason */
194
195	bufferHeader.Sync();
196
197	return _m;
198}
199
200
201/* L2CAP_ConfigReq */
202net_buffer*
203l2cap_cfg_req(uint8 _ident, uint16 _dcid, uint16 _flags, net_buffer* _data)
204{
205
206	net_buffer* _m = gBufferModule->create(sizeof(struct _cfg_req));
207	if ((_m) == NULL){
208		/* TODO free the _data buffer? */
209		return NULL;
210	}
211
212	NetBufferPrepend<struct _cfg_req> bufferHeader(_m);
213	status_t status = bufferHeader.Status();
214	if (status < B_OK) {
215		/* TODO free the buffer */
216		return NULL;
217	}
218
219	bufferHeader->hdr.code = L2CAP_CFG_REQ;
220	bufferHeader->hdr.ident = (_ident);
221	bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param));
222
223	bufferHeader->param.dcid = htole16((_dcid));
224	bufferHeader->param.flags = htole16((_flags));
225
226	bufferHeader.Sync();
227
228	/* Add the given data */
229	// TODO: given data can be freed... merge does it?
230	if (_data != NULL)
231		gBufferModule->merge(_m, _data, true);
232
233	return _m;
234}
235
236
237/* L2CAP_ConfigRsp */
238net_buffer*
239l2cap_cfg_rsp(uint8 _ident, uint16 _scid, uint16 _flags, uint16 _result, net_buffer* _data)
240{
241
242	net_buffer* _m = gBufferModule->create(sizeof(struct _cfg_rsp));
243	if ((_m) == NULL){
244		/* TODO free the _data buffer */
245		return NULL;
246	}
247
248	NetBufferPrepend<struct _cfg_rsp> bufferHeader(_m);
249	status_t status = bufferHeader.Status();
250	if (status < B_OK) {
251		/* TODO free the buffer */
252		return NULL;
253	}
254
255	bufferHeader->hdr.code = L2CAP_CFG_RSP;
256	bufferHeader->hdr.ident = (_ident);
257	bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param));
258
259	bufferHeader->param.scid = htole16((_scid));
260	bufferHeader->param.flags = htole16((_flags));
261	bufferHeader->param.result = htole16((_result));
262
263	bufferHeader.Sync();
264
265	if (_data != NULL)
266		gBufferModule->merge(_m, _data, true);
267
268	return _m;
269
270}
271
272
273/* L2CAP_DisconnectReq */
274net_buffer*
275l2cap_discon_req(uint8 _ident, uint16 _dcid, uint16 _scid)
276{
277
278	net_buffer* _m = gBufferModule->create(sizeof(struct _discon_req));
279	if ((_m) == NULL){
280		return NULL;
281	}
282
283	NetBufferPrepend<struct _discon_req> bufferHeader(_m);
284	status_t status = bufferHeader.Status();
285	if (status < B_OK) {
286		/* TODO free the buffer */
287		return NULL;
288	}
289
290	bufferHeader->hdr.code = L2CAP_DISCON_REQ;
291	bufferHeader->hdr.ident = (_ident);
292	bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param));
293
294	bufferHeader->param.dcid = htole16((_dcid));
295	bufferHeader->param.scid = htole16((_scid));
296
297	bufferHeader.Sync();
298
299	return _m;
300}
301
302
303/* L2CA_DisconnectRsp */
304net_buffer*
305l2cap_discon_rsp(uint8 _ident, uint16 _dcid, uint16 _scid)
306{
307
308	net_buffer* _m = gBufferModule->create(sizeof(struct _discon_rsp));
309	if ((_m) == NULL){
310		return NULL;
311	}
312
313	NetBufferPrepend<struct _discon_rsp> bufferHeader(_m);
314	status_t status = bufferHeader.Status();
315	if (status < B_OK) {
316		/* TODO free the buffer */
317		return NULL;
318	}
319
320	bufferHeader->hdr.code = L2CAP_DISCON_RSP;
321	bufferHeader->hdr.ident = (_ident);
322	bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param));
323
324	bufferHeader->param.dcid = htole16((_dcid));
325	bufferHeader->param.scid = htole16((_scid));
326
327	bufferHeader.Sync();
328
329	return _m;
330}
331
332
333/* L2CAP_EchoReq */
334net_buffer*
335l2cap_echo_req(uint8 _ident, void* _data, size_t _size)
336{
337	net_buffer* _m = gBufferModule->create(sizeof(l2cap_cmd_hdr_t));
338	if ((_m) == NULL){
339		/* TODO free the _data buffer */
340		return NULL;
341	}
342
343
344
345	if ((_data) != NULL) {
346	   	gBufferModule->append(_m, _data, _size);
347	}
348
349	return _m;
350}
351
352
353/* L2CAP_InfoReq */
354net_buffer*
355l2cap_info_req(uint8 _ident, uint16 _type)
356{
357
358	net_buffer* _m = gBufferModule->create(sizeof(struct _info_req));
359	if ((_m) == NULL){
360		return NULL;
361	}
362
363	NetBufferPrepend<struct _info_req> bufferHeader(_m);
364	status_t status = bufferHeader.Status();
365	if (status < B_OK) {
366		/* TODO free the buffer */
367		return NULL;
368	}
369
370	bufferHeader->hdr.code = L2CAP_INFO_REQ;
371	bufferHeader->hdr.ident = (_ident);
372	bufferHeader->hdr.length = htole16(sizeof(bufferHeader->param));
373
374	bufferHeader->param.type = htole16((_type));
375
376	bufferHeader.Sync();
377
378	return _m;
379}
380
381
382/* L2CAP_InfoRsp */
383net_buffer*
384l2cap_info_rsp(uint8 _ident, uint16 _type, uint16 _result, uint16 _mtu)
385{
386
387	net_buffer* _m = gBufferModule->create(sizeof(struct _info_rsp));
388	if ((_m) == NULL){
389		return NULL;
390	}
391
392	NetBufferPrepend<struct _info_rsp> bufferHeader(_m);
393	status_t status = bufferHeader.Status();
394	if (status < B_OK) {
395		/* TODO free the buffer */
396		return NULL;
397	}
398
399	bufferHeader->hdr.code = L2CAP_INFO_REQ;
400	bufferHeader->hdr.ident = (_ident);
401	bufferHeader->hdr.length = sizeof(bufferHeader->param);
402
403	bufferHeader->param.type = htole16((_type));
404	bufferHeader->param.result = htole16((_result));
405
406	if ((_result) == L2CAP_SUCCESS) {
407		switch ((_type)) {
408		case L2CAP_CONNLESS_MTU:
409			bufferHeader->data.mtu.mtu = htole16((_mtu));
410			bufferHeader->hdr.length += sizeof((bufferHeader->data.mtu.mtu));
411			break;
412		}
413	}
414
415	bufferHeader->hdr.length = htole16(bufferHeader->hdr.length);
416
417	bufferHeader.Sync();
418
419	return _m;
420
421}
422
423#if 0
424#pragma mark -
425#endif
426
427
428/* Build configuration options  */
429net_buffer*
430l2cap_build_cfg_options(uint16* _mtu, uint16* _flush_timo, l2cap_flow_t* _flow)
431{
432	size_t requestedSize = 0;
433
434	if (_mtu != NULL)
435		requestedSize+=sizeof(*_mtu);
436
437
438	if (_flush_timo != NULL)
439		requestedSize+=sizeof(*_flush_timo);
440
441	if (_flow != NULL)
442		requestedSize+=sizeof(*_flow);
443
444	net_buffer* _m = gBufferModule->create(sizeof(requestedSize));
445
446	if (_m == NULL)
447		return NULL;
448
449	if (_mtu != NULL) {
450		NetBufferPrepend<struct _cfg_opt_mtu> bufferHeader(_m);
451		status_t status = bufferHeader.Status();
452		if (status < B_OK) {
453			/* TODO free the buffer ?? */
454			return NULL;
455		}
456
457		bufferHeader->hdr.type = L2CAP_OPT_MTU;
458		bufferHeader->hdr.length = sizeof(bufferHeader->val);
459		bufferHeader->val = htole16(*(uint16 *)(_mtu));
460
461		bufferHeader.Sync();
462	}
463
464	if (_flush_timo != NULL) {
465
466		NetBufferPrepend<struct _cfg_opt_flush> bufferHeader(_m);
467		status_t status = bufferHeader.Status();
468		if (status < B_OK) {
469			/* TODO free the buffer ?? */
470			return NULL;
471		}
472
473		bufferHeader->hdr.type = L2CAP_OPT_FLUSH_TIMO;
474		bufferHeader->hdr.length = sizeof(bufferHeader->val);
475		bufferHeader->val = htole16(*(int16 *)(_flush_timo));
476
477		bufferHeader.Sync();
478	}
479
480	if (_flow != NULL) {
481
482		NetBufferPrepend<struct _cfg_opt_flow> bufferHeader(_m);
483		status_t status = bufferHeader.Status();
484		if (status < B_OK) {
485			/* TODO free the buffer ?? */
486			return NULL;
487		}
488
489		bufferHeader->hdr.type = L2CAP_OPT_QOS;
490		bufferHeader->hdr.length = sizeof(bufferHeader->val);
491		bufferHeader->val.flags = _flow->flags;
492		bufferHeader->val.service_type = _flow->service_type;
493		bufferHeader->val.token_rate = htole32(_flow->token_rate);
494		bufferHeader->val.token_bucket_size = htole32(_flow->token_bucket_size);
495		bufferHeader->val.peak_bandwidth = htole32(_flow->peak_bandwidth);
496		bufferHeader->val.latency = htole32(_flow->latency);
497		bufferHeader->val.delay_variation = htole32(_flow->delay_variation);
498
499		bufferHeader.Sync();
500	}
501
502	return _m;
503}
504