蓝牙L2CAP剖析（二）

一,部分代码没有完成，只是模拟一个面向连接的从建立到断开的过程，另外，L2CAP的重点状态机和拆包重组没有模拟

bt_l2cap.h

[cpp]  view plain  copy

1. /* 
2. * This file is part of the L2CAP protocal. 
3. * Data  :20160510 
4. * Author: zhongjun 
5. * 
6. */  
7.   
8. #ifndef BT_L2CAP_H_H  
9. #define BT_L2CAP_H_H  
10.   
11. #include "bt_cfg.h"  
12.   
13. #ifdef DEBUG_BT_L2CAP  
14. #define DEBUG(x) {printf x;}  
15. #define BT_L2CAP_DEBUG(x) DEBUG(x)  
16. #else  
17. #define BT_L2CAP_DEBUG(x)   
18. #endif  
19. /* L2CAP CHANNEL */  
20. #define L2CAP_SIG_CH 0x0001  
21. #define L2CAP_CONLESS_CH 0x0002  
22.   
23. /* L2CAP defaults */  
24. #define L2CAP_DEFAULT_MTU   672  
25. #define L2CAP_DEFAULT_FLUSH_TO  0xFFFF  
26.   
27. /* UPPER LAYER PSM */  
28. #define SDP_PSM 0x0001  
29. /* L2CAP command codes */  
30. #define L2CAP_COMMAND_REJ   0x01  
31. #define L2CAP_CONN_REQ      0x02  
32. #define L2CAP_CONN_RSP      0x03  
33. #define L2CAP_CONF_REQ      0x04  
34. #define L2CAP_CONF_RSP      0x05  
35. #define L2CAP_DISCONN_REQ   0x06  
36. #define L2CAP_DISCONN_RSP   0x07  
37. #define L2CAP_ECHO_REQ      0x08  
38. #define L2CAP_ECHO_RSP      0x09  
39. #define L2CAP_INFO_REQ      0x0a  
40. #define L2CAP_INFO_RSP      0x0b  
41.   
42. /* connect result */  
43. #define L2CAP_CR_SUCCESS    0x0000  
44. #define L2CAP_CR_PEND       0x0001  
45. #define L2CAP_CR_NOSUP_PSM  0x0002  
46. #define L2CAP_CR_SEC_BLOCK  0x0003  
47. #define L2CAP_CR_NO_RSR     0x0004  
48. /* config result */  
49. #define L2CAP_CONF_SUCCESS  0x0000  
50. #define L2CAP_CONF_UNACCEPT 0x0001  
51. #define L2CAP_CONF_REJECT   0x0002  
52. #define L2CAP_CONF_UNKNOWN  0x0003  
53.   
54. /* config option */  
55. #define L2CAP_CONF_MTU      0x01  
56. #define L2CAP_CONF_FLUSH_TO 0x02  
57. #define L2CAP_CONF_QOS      0x03  
58. #define L2CAP_CONF_RFC      0x04  
59. #define L2CAP_CONF_RFC_MODE 0x04  
60.   
61. #pragma pack(1)  
62. /* L2CAP structures hdr*/  
63. typedef struct {  
64.     uint16_t    len;  
65.     uint16_t    cid;  
66. }L2CAP_PDU_HDR_Format;  
67.   
68. typedef struct {  
69.     L2CAP_PDU_HDR_Format sig_hdr;  
70.     uint8_t     code;  
71.     uint8_t     ident;  
72.     uint16_t    len;  
73. }L2CAP_SIG_HDR_Format;  
74.   
75. /* L2CAP structures detail*/  
76. typedef struct {  
77.     L2CAP_SIG_HDR_Format com_hdr;  
78.     uint16_t    psm;  
79.     uint16_t    scid;  
80. }L2CAP_Con_Req;  
81.   
82. typedef struct {  
83.     L2CAP_SIG_HDR_Format com_hdr;  
84.     uint16_t    dst_cid;  
85.     uint16_t    src_cid;  
86.     uint16_t    result;  
87.     uint16_t    status;  
88. }L2CAP_Con_Rsp;  
89.   
90. typedef struct {  
91.     L2CAP_SIG_HDR_Format com_hdr;  
92.     uint16_t    dst_cid;  
93.     uint16_t    flags;  
94.     uint8_t     data[0];  
95. }L2CAP_Conf_Req;  
96.   
97. typedef struct {  
98.     L2CAP_SIG_HDR_Format com_hdr;  
99.     uint16_t    src_cid;  
100.     uint16_t    flags;  
101.     uint16_t    result;  
102.     uint8_t     data[0];  
103. }L2CAP_Conf_Rsp;  
104.   
105. typedef struct {  
106.     L2CAP_PDU_HDR_Format com_hdr;  
107.     uint8_t     data[0];  
108. }L2CAP_Upper_Layer_data_Format;  
109.   
110. typedef struct {  
111.     L2CAP_SIG_HDR_Format com_hdr;  
112.     uint16_t    dcid;  
113.     uint16_t    scid;  
114. }L2CAP_Discon_req;  
115.   
116. typedef struct {  
117.     L2CAP_SIG_HDR_Format com_hdr;  
118.     uint16_t    dcid;  
119.     uint16_t    scid;  
120. }L2CAP_Discon_rsp;  
121. #pragma pack()  
122.   
123. /* API */  
124. /* send pdu */  
125. int L2CAP_Send_SIG_Con_Req(uint8_t ident,uint16_t psm,uint16_t scid);  
126. int L2CAP_Send_SIG_Conf_Req(uint8_t ident,uint16_t dcid,uint16_t flags,uint8_t *conf_opt,uint8_t opt_len);  
127. int L2CAP_Send_Upper_Layer_data(uint16_t cid,uint8_t *data,uint16_t length);  
128. int L2CAP_Send_SIG_Discon_Req(uint8_t ident,uint16_t dcid,uint16_t scid);  
129. int L2CAP_Send_PDU(uint8_t *PDU,uint32_t length);  
130.   
131. /* reve pdu */  
132. int L2CAP_Parse_PDU(uint8_t *PDU,uint32_t length);  
133. int L2CAP_Parse_SIG_Con_Rsp_PDU(uint8_t *PDU,uint32_t length);  
134. int L2CAP_Parse_SIG_Conf_Rsp_PDU(uint8_t *PDU,uint32_t length);  
135. int L2CAP_Parse_SIG_Discon_Rsp_PDU(uint8_t *PDU,uint32_t length);  
136. #endif  

bt_l2cap.c

[cpp]  view plain  copy

1. #include "bt_l2cap.h"  
2.   
3. int L2CAP_Send_SIG_Con_Req(uint8_t ident,uint16_t psm,uint16_t scid)  
4. {  
5.     L2CAP_Con_Req PDU;  
6.     PDU.com_hdr.sig_hdr.cid = L2CAP_SIG_CH;  
7.     PDU.com_hdr.sig_hdr.len = sizeof(L2CAP_Con_Req) - sizeof(L2CAP_PDU_HDR_Format);  
8.   
9.     PDU.com_hdr.code = L2CAP_CONN_REQ;  
10.     PDU.com_hdr.ident = ident;  
11.     PDU.com_hdr.len = sizeof(L2CAP_Con_Req) - sizeof(L2CAP_SIG_HDR_Format);  
12.   
13.     PDU.psm = psm;  
14.     PDU.scid = scid;  
15.     L2CAP_Send_PDU((uint8_t *)&PDU,sizeof(L2CAP_Con_Req));  
16. }  
17. int L2CAP_Send_SIG_Conf_Req(uint8_t ident,uint16_t dcid,uint16_t flags,uint8_t *conf_opt,uint8_t opt_len)  
18. {  
19.     L2CAP_Conf_Req *PDU = (L2CAP_Conf_Req *)malloc(sizeof(L2CAP_Conf_Req) + opt_len);  
20.     (PDU->com_hdr).sig_hdr.cid = L2CAP_SIG_CH;  
21.     (PDU->com_hdr).sig_hdr.len = sizeof(L2CAP_Conf_Req) - sizeof(L2CAP_PDU_HDR_Format) + opt_len;  
22.       
23.     (PDU->com_hdr).code = L2CAP_CONF_REQ;  
24.     (PDU->com_hdr).ident = ident;  
25.     (PDU->com_hdr).len = sizeof(L2CAP_Conf_Req) - sizeof(L2CAP_SIG_HDR_Format) + opt_len;  
26.       
27.     PDU->dst_cid = dcid;  
28.     PDU->flags = flags;  
29.     memcpy(PDU->data,conf_opt,opt_len);  
30.   
31.     L2CAP_Send_PDU(PDU,sizeof(L2CAP_Conf_Req) + opt_len);  
32. }  
33.   
34. int L2CAP_Send_Upper_Layer_data(uint16_t cid,uint8_t *data,uint16_t length)  
35. {  
36.     L2CAP_Upper_Layer_data_Format *PDU = (L2CAP_Upper_Layer_data_Format *)malloc(sizeof(L2CAP_Upper_Layer_data_Format) + length);  
37.     (PDU->com_hdr).cid = cid;  
38.     (PDU->com_hdr).len = sizeof(L2CAP_Upper_Layer_data_Format) + length;  
39.   
40.     memcpy(PDU->data,data,length);     
41.     L2CAP_Send_PDU(PDU,sizeof(L2CAP_Upper_Layer_data_Format) + length);  
42. }  
43.   
44. int L2CAP_Send_SIG_Discon_Req(uint8_t ident,uint16_t dcid,uint16_t scid)  
45. {  
46.     L2CAP_Discon_req PDU;  
47.     PDU.com_hdr.sig_hdr.cid = L2CAP_SIG_CH;  
48.     PDU.com_hdr.sig_hdr.len = sizeof(L2CAP_Discon_req) - sizeof(L2CAP_PDU_HDR_Format);  
49.       
50.     PDU.com_hdr.code = L2CAP_DISCONN_REQ;  
51.     PDU.com_hdr.ident = ident;  
52.     PDU.com_hdr.len = sizeof(L2CAP_Con_Req) - sizeof(L2CAP_SIG_HDR_Format);  
53.       
54.     PDU.dcid = dcid;  
55.     PDU.scid = scid;  
56.     L2CAP_Send_PDU((uint8_t *)&PDU,sizeof(L2CAP_Con_Req));  
57. }  
58. int L2CAP_Send_PDU(uint8_t *PDU,uint32_t length)  
59. {  
60.     int index = 0;  
61.     BT_L2CAP_DEBUG((" >> Send L2CAP REQ PDU:"));  
62.     for(index = 0; index < length; index++)  
63.     {  
64.         BT_L2CAP_DEBUG(("0x%02x ",PDU[index]));  
65.     }  
66.     BT_L2CAP_DEBUG(("\n"));  
67. }  
68.   
69.   
70. int L2CAP_Parse_PDU(uint8_t *PDU,uint32_t length)  
71. {  
72.     L2CAP_PDU_HDR_Format *TMP_PDU = (L2CAP_PDU_HDR_Format *)PDU;  
73.     if((TMP_PDU->cid) == L2CAP_SIG_CH)  
74.     {  
75.         uint8_t tmp_code;  
76.         L2CAP_SIG_HDR_Format *pdu = (L2CAP_SIG_HDR_Format *)PDU;  
77.         BT_L2CAP_DEBUG(("SIG PDU\n"));        
78.         tmp_code = pdu->code;  
79.         switch(tmp_code)  
80.         {  
81.             case L2CAP_CONN_RSP:  
82.             {  
83.                 BT_L2CAP_DEBUG(("L2CAP_CONN_RSP\n"));  
84.                 L2CAP_Parse_SIG_Con_Rsp_PDU(PDU,length);  
85.                 break;  
86.             }  
87.             case L2CAP_CONF_RSP:  
88.             {  
89.                 BT_L2CAP_DEBUG(("L2CAP_CONF_RSP\n"));  
90.                 L2CAP_Parse_SIG_Conf_Rsp_PDU(PDU,length);  
91.                 break;  
92.             }  
93.             case L2CAP_DISCONN_RSP:  
94.             {  
95.                 BT_L2CAP_DEBUG(("L2CAP_DISCONN_RSP\n"));  
96.                 L2CAP_Parse_SIG_Discon_Rsp_PDU(PDU,length);  
97.                 break;  
98.             }  
99.             default:  
100.             {  
101.                 break;  
102.             }  
103.         }  
104.           
105.     }  
106.     else  
107.     {  
108.         //TODO  
109.     }  
110. }  
111.   
112. int L2CAP_Parse_SIG_Con_Rsp_PDU(uint8_t *PDU,uint32_t length)  
113. {  
114.     L2CAP_Con_Rsp *tmp_pdu = (L2CAP_Con_Rsp *)PDU;  
115.     if(tmp_pdu->result == L2CAP_CR_SUCCESS)  
116.     {  
117.         BT_L2CAP_DEBUG(("connect successful\n"));  
118.     }  
119.     else if(tmp_pdu->result == L2CAP_CR_PEND)  
120.     {  
121.         BT_L2CAP_DEBUG(("connect pending\n"));  
122.     }  
123. }  
124.   
125. int L2CAP_Parse_SIG_Conf_Rsp_PDU(uint8_t *PDU,uint32_t length)  
126. {  
127.     L2CAP_Conf_Rsp *tmp_pdu = (L2CAP_Conf_Rsp *)PDU;  
128.     if(tmp_pdu->result == L2CAP_CONF_SUCCESS)  
129.     {  
130.         BT_L2CAP_DEBUG(("config successful\n"));  
131.     }  
132.     else if(tmp_pdu->result == L2CAP_CONF_UNACCEPT)  
133.     {  
134.         BT_L2CAP_DEBUG(("config unaccept\n"));  
135.     }  
136. }  
137.   
138. int L2CAP_Parse_SIG_Discon_Rsp_PDU(uint8_t *PDU,uint32_t length)  
139. {  
140.       
141. }  

bt_cfg.h

[cpp]  view plain  copy

1. #ifndef BT_LMP_CFG_H  
2. #define BT_LMP_CFG_H  
3.   
4. #include <stdlib.h>  
5. #include <stdio.h>  
6. #include <string.h>  
7.   
8. #define DEBUG_BT_L2CAP  
9.   
10. typedef unsigned char uint8_t;  
11. typedef unsigned short uint16_t;  
12. typedef unsigned int uint32_t;  
13.   
14.   
15.   
16. typedef  char int8_t;  
17. typedef  short int16_t;  
18. typedef  int int32_t;  
19.   
20. #undef NULL   
21. #if defined(__cplusplus)  
22. #define NULL 0  
23. #else  
24. #define NULL ((void *)0)  
25. #endif  
26.   
27. #endif  

main.c

[cpp]  view plain  copy

1. #include <stdio.h>  
2. #include "bt_l2cap.h"  
3.   
4. int main()  
5. {  
6.     uint8_t con_rsp_pdu[] = {0x0C,0x00,0x01,0x00,0x03,0x79,0x08,0x00,0x06,0x08,0x40,0x00,0x00,0x00,0x00,0x00};   
7.     uint8_t conf_option[] = {0x01,0x02,0xF4,0x01,0x02,0x02,0xFF,0xFF};  
8.     uint8_t conf_rsp_pdu[] = {0x0E,0x00,0x01,0x00,0x05,0x7A,0x0A,0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0xF4,0x01};  
9.     uint8_t upper_layer_data[] = {0x1,0x1,0x1,0x1,0x1,0x1};  
10.     L2CAP_Send_SIG_Con_Req(0x79,SDP_PSM,0x40);  
11.     L2CAP_Parse_PDU(con_rsp_pdu,sizeof(con_rsp_pdu));  
12.   
13.     L2CAP_Send_SIG_Conf_Req(0x79,0x0806,0x0,conf_option,sizeof(conf_option));  
14.     L2CAP_Parse_PDU(conf_rsp_pdu,sizeof(conf_rsp_pdu));  
15.   
16.     L2CAP_Send_Upper_Layer_data(0x40,upper_layer_data,sizeof(upper_layer_data));  
17.   
18.     L2CAP_Send_SIG_Discon_Req(0x79,0x40,0x0806);  
19. }