蓝牙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   
  5. #include   
  6. #include   
  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   
  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. }  

你可能感兴趣的:(蓝牙)