蓝牙L2CAP剖析（二）

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

bt_l2cap.h

 /*
 * This file is part of the L2CAP protocal.
 * Data  :20160510
 * Author: zhongjun
 *
 */

 #ifndef BT_L2CAP_H_H
 #define BT_L2CAP_H_H

 #include "bt_cfg.h"

 #ifdef DEBUG_BT_L2CAP
 #define DEBUG(x) {printf x;}
 #define BT_L2CAP_DEBUG(x) DEBUG(x)
 #else
 #define BT_L2CAP_DEBUG(x)
 #endif
 /* L2CAP CHANNEL */
 #define L2CAP_SIG_CH 0x0001
 #define L2CAP_CONLESS_CH 0x0002

 /* L2CAP defaults */
 #define L2CAP_DEFAULT_MTU   672
 #define L2CAP_DEFAULT_FLUSH_TO  0xFFFF

 /* UPPER LAYER PSM */
 #define SDP_PSM 0x0001
 /* L2CAP command codes */
 #define L2CAP_COMMAND_REJ   0x01
 #define L2CAP_CONN_REQ      0x02
 #define L2CAP_CONN_RSP      0x03
 #define L2CAP_CONF_REQ      0x04
 #define L2CAP_CONF_RSP      0x05
 #define L2CAP_DISCONN_REQ   0x06
 #define L2CAP_DISCONN_RSP   0x07
 #define L2CAP_ECHO_REQ      0x08
 #define L2CAP_ECHO_RSP      0x09
 #define L2CAP_INFO_REQ      0x0a
 #define L2CAP_INFO_RSP      0x0b

 /* connect result */
 #define L2CAP_CR_SUCCESS    0x0000
 #define L2CAP_CR_PEND       0x0001
 #define L2CAP_CR_NOSUP_PSM  0x0002
 #define L2CAP_CR_SEC_BLOCK  0x0003
 #define L2CAP_CR_NO_RSR     0x0004
 /* config result */
 #define L2CAP_CONF_SUCCESS  0x0000
 #define L2CAP_CONF_UNACCEPT 0x0001
 #define L2CAP_CONF_REJECT   0x0002
 #define L2CAP_CONF_UNKNOWN  0x0003

 /* config option */
 #define L2CAP_CONF_MTU      0x01
 #define L2CAP_CONF_FLUSH_TO 0x02
 #define L2CAP_CONF_QOS      0x03
 #define L2CAP_CONF_RFC      0x04
 #define L2CAP_CONF_RFC_MODE 0x04

 #pragma pack(1)
 /* L2CAP structures hdr*/
 typedef struct {
     uint16_t    len;
     uint16_t    cid;
 }L2CAP_PDU_HDR_Format;

 typedef struct {
     L2CAP_PDU_HDR_Format sig_hdr;
     uint8_t     code;
     uint8_t     ident;
     uint16_t    len;
 }L2CAP_SIG_HDR_Format;

 /* L2CAP structures detail*/
 typedef struct {
     L2CAP_SIG_HDR_Format com_hdr;
     uint16_t    psm;
     uint16_t    scid;
 }L2CAP_Con_Req;

 typedef struct {
     L2CAP_SIG_HDR_Format com_hdr;
     uint16_t    dst_cid;
     uint16_t    src_cid;
     uint16_t    result;
     uint16_t    status;
 }L2CAP_Con_Rsp;

 typedef struct {
     L2CAP_SIG_HDR_Format com_hdr;
     uint16_t    dst_cid;
     uint16_t    flags;
     uint8_t     data[0];
 }L2CAP_Conf_Req;

 typedef struct {
     L2CAP_SIG_HDR_Format com_hdr;
     uint16_t    src_cid;
     uint16_t    flags;
     uint16_t    result;
     uint8_t     data[0];
 }L2CAP_Conf_Rsp;

 typedef struct {
     L2CAP_PDU_HDR_Format com_hdr;
     uint8_t     data[0];
 }L2CAP_Upper_Layer_data_Format;

 typedef struct {
     L2CAP_SIG_HDR_Format com_hdr;
     uint16_t    dcid;
     uint16_t    scid;
 }L2CAP_Discon_req;

 typedef struct {
     L2CAP_SIG_HDR_Format com_hdr;
     uint16_t    dcid;
     uint16_t    scid;
 }L2CAP_Discon_rsp;
 #pragma pack()

 /* API */
 /* send pdu */
 int L2CAP_Send_SIG_Con_Req(uint8_t ident,uint16_t psm,uint16_t scid);
 int L2CAP_Send_SIG_Conf_Req(uint8_t ident,uint16_t dcid,uint16_t flags,uint8_t *conf_opt,uint8_t opt_len);
 int L2CAP_Send_Upper_Layer_data(uint16_t cid,uint8_t *data,uint16_t length);
 int L2CAP_Send_SIG_Discon_Req(uint8_t ident,uint16_t dcid,uint16_t scid);
 int L2CAP_Send_PDU(uint8_t *PDU,uint32_t length);

 /* reve pdu */
 int L2CAP_Parse_PDU(uint8_t *PDU,uint32_t length);
 int L2CAP_Parse_SIG_Con_Rsp_PDU(uint8_t *PDU,uint32_t length);
 int L2CAP_Parse_SIG_Conf_Rsp_PDU(uint8_t *PDU,uint32_t length);
 int L2CAP_Parse_SIG_Discon_Rsp_PDU(uint8_t *PDU,uint32_t length);
 #endif

bt_l2cap.c

 #include "bt_l2cap.h"

 int L2CAP_Send_SIG_Con_Req(uint8_t ident,uint16_t psm,uint16_t scid)
 {
     L2CAP_Con_Req PDU;
     PDU.com_hdr.sig_hdr.cid = L2CAP_SIG_CH;
     PDU.com_hdr.sig_hdr.len = sizeof(L2CAP_Con_Req) - sizeof(L2CAP_PDU_HDR_Format);

     PDU.com_hdr.code = L2CAP_CONN_REQ;
     PDU.com_hdr.ident = ident;
     PDU.com_hdr.len = sizeof(L2CAP_Con_Req) - sizeof(L2CAP_SIG_HDR_Format);

     PDU.psm = psm;
     PDU.scid = scid;
     L2CAP_Send_PDU((uint8_t *)&PDU,sizeof(L2CAP_Con_Req));
 }
 int L2CAP_Send_SIG_Conf_Req(uint8_t ident,uint16_t dcid,uint16_t flags,uint8_t *conf_opt,uint8_t opt_len)
 {
     L2CAP_Conf_Req *PDU = (L2CAP_Conf_Req *)malloc(sizeof(L2CAP_Conf_Req) + opt_len);
     (PDU->com_hdr).sig_hdr.cid = L2CAP_SIG_CH;
     (PDU->com_hdr).sig_hdr.len = sizeof(L2CAP_Conf_Req) - sizeof(L2CAP_PDU_HDR_Format) + opt_len;

     (PDU->com_hdr).code = L2CAP_CONF_REQ;
     (PDU->com_hdr).ident = ident;
     (PDU->com_hdr).len = sizeof(L2CAP_Conf_Req) - sizeof(L2CAP_SIG_HDR_Format) + opt_len;

     PDU->dst_cid = dcid;
     PDU->flags = flags;
     memcpy(PDU->data,conf_opt,opt_len);

     L2CAP_Send_PDU(PDU,sizeof(L2CAP_Conf_Req) + opt_len);
 }

 int L2CAP_Send_Upper_Layer_data(uint16_t cid,uint8_t *data,uint16_t length)
 {
     L2CAP_Upper_Layer_data_Format *PDU = (L2CAP_Upper_Layer_data_Format *)malloc(sizeof(L2CAP_Upper_Layer_data_Format) + length);
     (PDU->com_hdr).cid = cid;
     (PDU->com_hdr).len = sizeof(L2CAP_Upper_Layer_data_Format) + length;

     memcpy(PDU->data,data,length);
     L2CAP_Send_PDU(PDU,sizeof(L2CAP_Upper_Layer_data_Format) + length);
 }

 int L2CAP_Send_SIG_Discon_Req(uint8_t ident,uint16_t dcid,uint16_t scid)
 {
     L2CAP_Discon_req PDU;
     PDU.com_hdr.sig_hdr.cid = L2CAP_SIG_CH;
     PDU.com_hdr.sig_hdr.len = sizeof(L2CAP_Discon_req) - sizeof(L2CAP_PDU_HDR_Format);

     PDU.com_hdr.code = L2CAP_DISCONN_REQ;
     PDU.com_hdr.ident = ident;
     PDU.com_hdr.len = sizeof(L2CAP_Con_Req) - sizeof(L2CAP_SIG_HDR_Format);

     PDU.dcid = dcid;
     PDU.scid = scid;
     L2CAP_Send_PDU((uint8_t *)&PDU,sizeof(L2CAP_Con_Req));
 }
 int L2CAP_Send_PDU(uint8_t *PDU,uint32_t length)
 {
     int index = 0;
     BT_L2CAP_DEBUG((" >> Send L2CAP REQ PDU:"));
     for(index = 0; index < length; index++)
     {
         BT_L2CAP_DEBUG(("0x%02x ",PDU[index]));
     }
     BT_L2CAP_DEBUG(("\n"));
 }


 int L2CAP_Parse_PDU(uint8_t *PDU,uint32_t length)
 {
     L2CAP_PDU_HDR_Format *TMP_PDU = (L2CAP_PDU_HDR_Format *)PDU;
     if((TMP_PDU->cid) == L2CAP_SIG_CH)
     {
         uint8_t tmp_code;
         L2CAP_SIG_HDR_Format *pdu = (L2CAP_SIG_HDR_Format *)PDU;
         BT_L2CAP_DEBUG(("SIG PDU\n"));
         tmp_code = pdu->code;
         switch(tmp_code)
         {
             case L2CAP_CONN_RSP:
             {
                 BT_L2CAP_DEBUG(("L2CAP_CONN_RSP\n"));
                 L2CAP_Parse_SIG_Con_Rsp_PDU(PDU,length);
                 break;
             }
             case L2CAP_CONF_RSP:
             {
                 BT_L2CAP_DEBUG(("L2CAP_CONF_RSP\n"));
                 L2CAP_Parse_SIG_Conf_Rsp_PDU(PDU,length);
                 break;
             }
             case L2CAP_DISCONN_RSP:
             {
                 BT_L2CAP_DEBUG(("L2CAP_DISCONN_RSP\n"));
                 L2CAP_Parse_SIG_Discon_Rsp_PDU(PDU,length);
                 break;
             }
             default:
             {
                 break;
             }
         }

     }
     else
     {
         //TODO
     }
 }

 int L2CAP_Parse_SIG_Con_Rsp_PDU(uint8_t *PDU,uint32_t length)
 {
     L2CAP_Con_Rsp *tmp_pdu = (L2CAP_Con_Rsp *)PDU;
     if(tmp_pdu->result == L2CAP_CR_SUCCESS)
     {
         BT_L2CAP_DEBUG(("connect successful\n"));
     }
     else if(tmp_pdu->result == L2CAP_CR_PEND)
     {
         BT_L2CAP_DEBUG(("connect pending\n"));
     }
 }

 int L2CAP_Parse_SIG_Conf_Rsp_PDU(uint8_t *PDU,uint32_t length)
 {
     L2CAP_Conf_Rsp *tmp_pdu = (L2CAP_Conf_Rsp *)PDU;
     if(tmp_pdu->result == L2CAP_CONF_SUCCESS)
     {
         BT_L2CAP_DEBUG(("config successful\n"));
     }
     else if(tmp_pdu->result == L2CAP_CONF_UNACCEPT)
     {
         BT_L2CAP_DEBUG(("config unaccept\n"));
     }
 }

 int L2CAP_Parse_SIG_Discon_Rsp_PDU(uint8_t *PDU,uint32_t length)
 {

 }

bt_cfg.h

 #ifndef BT_LMP_CFG_H
 #define BT_LMP_CFG_H

 #include
 #include
 #include

 #define DEBUG_BT_L2CAP

 typedef unsigned char uint8_t;
 typedef unsigned short uint16_t;
 typedef unsigned int uint32_t;



 typedef  char int8_t;
 typedef  short int16_t;
 typedef  int int32_t;

 #undef NULL
 #if defined(__cplusplus)
 #define NULL 0
 #else
 #define NULL ((void *)0)
 #endif

 #endif

main.c

 #include
 #include "bt_l2cap.h"

 int main()
 {
     uint8_t con_rsp_pdu[] = {0x0C,0x00,0x01,0x00,0x03,0x79,0x08,0x00,0x06,0x08,0x40,0x00,0x00,0x00,0x00,0x00};
     uint8_t conf_option[] = {0x01,0x02,0xF4,0x01,0x02,0x02,0xFF,0xFF};
     uint8_t conf_rsp_pdu[] = {0x0E,0x00,0x01,0x00,0x05,0x7A,0x0A,0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0xF4,0x01};
     uint8_t upper_layer_data[] = {0x1,0x1,0x1,0x1,0x1,0x1};
     L2CAP_Send_SIG_Con_Req(0x79,SDP_PSM,0x40);
     L2CAP_Parse_PDU(con_rsp_pdu,sizeof(con_rsp_pdu));

     L2CAP_Send_SIG_Conf_Req(0x79,0x0806,0x0,conf_option,sizeof(conf_option));
     L2CAP_Parse_PDU(conf_rsp_pdu,sizeof(conf_rsp_pdu));

     L2CAP_Send_Upper_Layer_data(0x40,upper_layer_data,sizeof(upper_layer_data));

     L2CAP_Send_SIG_Discon_Req(0x79,0x40,0x0806);
 }