l2cap.c

蓝牙协议源代码 bluetooth stack for lwip

/*
 * Copyright (c) 2003 EISLAB, Lulea University of Technology.
 * All rights reserved. 
 * 
 * Redistribution and use in source and binary forms, with or without modification, 
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission. 
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
 * OF SUCH DAMAGE.
 *
 * This file is part of the lwBT Bluetooth stack.
 * 
 * Author: Conny Ohult <[e-mail]>
 *
 */

/*-----------------------------------------------------------------------------------*/
/* l2cap.c
 *
 * Implementation of the logical link control and adaption protocol (L2CAP). Supports 
 * higher level protocol multiplexing, packet segmentation and reassembly, and the 
 * conveying of quality of service information.
 */
/*-----------------------------------------------------------------------------------*/

#include "netif/lwbt/l2cap.h"
#include "netif/lwbt/lwbt_memp.h"
#include "lwbtopts.h"
#include "lwip/debug.h"

/* Next Identifier to be sent */
u8_t sigid_nxt;

/* The L2CAP PCB lists. */
struct l2cap_pcb_listen *l2cap_listen_pcbs;  /* List of all L2CAP PCBs in CLOSED state
						but awaiting an incoming conn req */
struct l2cap_pcb *l2cap_active_pcbs;  /* List of all L2CAP PCBs that are in a
					 state in which they accept or send
					 data */
struct l2cap_pcb *l2cap_tmp_pcb;

/* Temp signal */
struct l2cap_sig *l2cap_tmp_sig;

/* Global variable involved in input processing of l2cap data segements */
struct l2cap_seg *l2cap_insegs;
struct l2cap_seg *l2cap_tmp_inseg;

/* Forward declarations */
static u16_t l2cap_cid_alloc(void);

/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_init():
 * 
 * Initializes the L2CAP layer.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_init(void)
{
  /* Clear globals */
  l2cap_listen_pcbs = NULL;
  l2cap_active_pcbs = NULL;
  l2cap_tmp_pcb = NULL;
  l2cap_tmp_sig = NULL;
  l2cap_insegs = NULL;
  l2cap_tmp_inseg = NULL;

  /* Initialize the signal identifier (0x00 shall never be used) */
  sigid_nxt = 0x00;
}
/*-----------------------------------------------------------------------------------*/
/*
 * l2cap_tmr():
 *
 * Called every 1s and implements the retransmission timer that
 * removes a channel if it has been waiting for a request enough
 * time. It also includes a configuration timer.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_tmr(void)
{
  struct l2cap_sig *sig;
  struct l2cap_pcb *pcb;
  err_t ret; 

  /* Step through all of the active pcbs */
  for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
    /* Step through any unresponded signals */
    for(sig = pcb->unrsp_sigs; sig != NULL; sig = sig->next) {
      /* Check if channel is not reliable */
      if(pcb->cfg.outflushto < 0xFFFF) {
	/* Check if rtx is active. Otherwise ertx is active */
	if(sig->rtx > 0) {
	  /* Adjust rtx timer */
	  --sig->rtx;
	  /* Check if rtx has expired */
	  if(sig->rtx == 0) {
	    if(sig->nrtx == 0) {
	      /* Move pcb to closed state */
	      pcb->state = L2CAP_CLOSED;
	      /* Indicate disconnect to upper layer */
	      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_tmr: Max number of retransmissions (rtx) has expired\n"));
	      L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
	    } else {
	      --sig->nrtx;
	      /* Indicate timeout to upper layer */
	      L2CA_ACTION_TO_IND(pcb,ERR_OK,ret);
	      /* Retransmitt signal w timeout doubled */
	      sig->rtx += sig->rtx;
	      ret = l2cap_rexmit_signal(pcb, sig);
	    }
	  } /* if */
	} else {
	  /* Adjust ertx timer */
	  --sig->ertx;
	  /* Check if ertx has expired */
	  if(sig->ertx == 0) {
	    if(sig->nrtx == 0) {
	      /* Move pcb to closed state */
	      pcb->state = L2CAP_CLOSED;
	      /* Indicate disconnect to upper layer */
	      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_tmr: Max number of retransmissions (ertx) has expired\n"));
	      L2CA_ACTION_DISCONN_IND(pcb,ERR_OK,ret);
	    } else {
	      --sig->nrtx;
	      /* Indicate timeout to upper layer */
	      L2CA_ACTION_TO_IND(pcb,ERR_OK,ret);
	      /* Disable ertx, activate rtx and retransmitt signal */
	      sig->ertx = 0;
	      sig->rtx = L2CAP_RTX;
	      ret = l2cap_rexmit_signal(pcb, sig);
	    }
	  } /* if */
	} /* else */
      } /* if */
    } /* for */
    
    /* Check configuration timer */
    if(pcb->state == L2CAP_CONFIG) {
      /* Check if configuration timer is active */
      if(pcb->cfg.cfgto > 0) {
	--pcb->cfg.cfgto;
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_tmr: Configuration timer = %d\n", pcb->cfg.cfgto));
	/* Check if config timer has expired */
	if(pcb->cfg.cfgto == 0) {
	  /* Connection attempt failed. Disconnect */
	  l2ca_disconnect_req(pcb, NULL);
	  /* Notify the application that the connection attempt failed */
	  if(pcb->cfg.l2capcfg & L2CAP_CFG_IR) {
	    L2CA_ACTION_CONN_CFM(pcb, L2CAP_CONN_CFG_TO, 0x0000, ret);
	  } else {
	    L2CA_ACTION_CONN_IND(pcb, ERR_OK, ret);
	  }
	  pcb->cfg.cfgto = L2CAP_CFG_TO; /* Reset timer */
	}
      }
    }
  } /* for */
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_write():
 * 
 * Output L2CAP data to the lower layers. Segments the packet in to PDUs.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2cap_write(struct bd_addr *bdaddr, struct pbuf *p, u16_t len) 
{
  u8_t pb = L2CAP_ACL_START;
  u16_t maxsize;
  u16_t outsize;
  err_t ret = ERR_OK;
  struct pbuf *q;
  u16_t i = 0;

  /*u16_t i;
  struct pbuf *q;
  for(q = p; q != NULL; q = q->next) {
    for(i = 0; i < q->len; ++i) {
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: 0x%x\n", ((u8_t *)q->payload)[i]));
    }
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: *\n"));
    }
  */

  maxsize = lp_pdu_maxsize();
  q = p;

  while(len && ret == ERR_OK) {
  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: len %d maxsize %d p->len %d\n", len, maxsize, p->len));
    if(len > maxsize) {
      ret = lp_acl_write(bdaddr, q, maxsize, pb);
      len -= maxsize;
      outsize = maxsize;
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: Outsize before %d\n", outsize));
      while(q->len < outsize) {
	outsize -= q->len;
	q = q->next;
      }
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: Outsize after %d\n", outsize));
      if(outsize) {
	pbuf_header(q, -outsize);
	i += outsize;
      }
      pb = L2CAP_ACL_CONT;
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: FRAG\n"));
    } else {
      ret = lp_acl_write(bdaddr, q, len, pb);
      len = 0;
    }
  }
  pbuf_header(q, i);
  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_write: DONE\n"));
  return ret;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * l2cap_process_sig():
 * 
 * Parses the received message handles it.
 */
/*-----------------------------------------------------------------------------------*/
void
l2cap_process_sig(struct pbuf *q, struct l2cap_hdr *l2caphdr, struct bd_addr *bdaddr)
{
  struct l2cap_sig_hdr *sighdr;
  struct l2cap_sig *sig = NULL;
  struct l2cap_pcb *pcb = NULL;
  struct l2cap_pcb_listen *lpcb;
  struct l2cap_cfgopt_hdr *opthdr;
  u16_t result, status, flags, psm, dcid, scid;
  u16_t len; 
  u16_t siglen;
  struct pbuf *p, *r = NULL, *s = NULL, *data;
  err_t ret;
  u8_t i;
  u16_t rspstate = L2CAP_CFG_SUCCESS;

  if(q->len != q->tot_len) {
    LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Fragmented packet received. Reassemble into one buffer\n"));
    if((p = pbuf_alloc(PBUF_RAW, q->tot_len, PBUF_RAM)) != NULL) {
      i = 0;
      for(r = q; r != NULL; r = r->next) {
	memcpy(((u8_t *)p->payload) + i, r->payload, r->len);
	i += r->len;
      }
    } else {
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Could not allocate buffer for fragmented packet\n"));
      return; 
    }
  } else {
    p = q;
  }

  len = l2caphdr->len;
  
  while(len > 0) {
    /* Set up signal header */
    sighdr = p->payload;
    pbuf_header(p, -L2CAP_SIGHDR_LEN);
    
    /* Check if this is a response/reject signal, and if so, find the matching request */
    if(sighdr->code % 2) { /* if odd this is a resp/rej signal */
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Response/reject signal received id = %d code = %d\n", 
			   sighdr->id, sighdr->code));
      for(pcb = l2cap_active_pcbs; pcb != NULL; pcb = pcb->next) {
	for(sig = pcb->unrsp_sigs; sig != NULL; sig = sig->next) {
	  if(sig->sigid == sighdr->id) {
	    break; /* found */
	  } 
	}
	if(sig != NULL) {
	  break;
	}
      }
    } else {
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Request signal received id = %d code = %d\n", 
			   sighdr->id, sighdr->code));
    }

    /* Reject packet if length exceeds MTU */
    if(l2caphdr->len > L2CAP_MTU) {		      
      /* Alloc size of reason in cmd rej + MTU */
      if((data = pbuf_alloc(PBUF_RAW, L2CAP_CMD_REJ_SIZE+2, PBUF_RAM)) != NULL) {
	((u16_t *)data->payload)[0] = L2CAP_MTU_EXCEEDED;
	((u16_t *)data->payload)[1] = L2CAP_MTU;

	l2cap_signal(NULL, L2CAP_CMD_REJ, sighdr->id, bdaddr, data);
      }
      break;
    }
    
    switch(sighdr->code) {
    case L2CAP_CMD_REJ:
      /* Remove signal from unresponded list and deallocate it */
      L2CAP_SIG_RMV(&(pcb->unrsp_sigs), sig);
      pbuf_free(sig->p);
      lwbt_memp_free(MEMP_L2CAP_SIG, sig);
      LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Our command was rejected so we disconnect\n")); 
      l2ca_disconnect_req(pcb, NULL);
      break;
    case L2CAP_CONN_REQ:
      psm = ((u16_t *)p->payload)[0];
      /* Search for a listening pcb */
      for(lpcb = l2cap_listen_pcbs; lpcb != NULL; lpcb = lpcb->next) {
	if(lpcb->psm == psm) {
	  /* Found a listening pcb with the correct PSM */
	  break;
	}
      }
      /* If no matching pcb was found, send a connection rsp neg (PSM) */
      if(lpcb == NULL) {
	/* Alloc size of data in conn rsp signal */
	if((data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM)) != NULL) {
	  ((u16_t *)data->payload)[0] = L2CAP_CONN_REF_PSM;
	  ((u16_t *)data->payload)[1] = 0; /* No further info available */
	  ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
	}
      } else {
	/* Initiate a new active pcb */
	pcb = l2cap_new();
	if(pcb == NULL) {
	  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: could not allocate PCB\n"));
	  /* Send a connection rsp neg (no resources available) and alloc size of data in conn rsp 
	     signal */
	  if((data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM)) != NULL) {
	    ((u16_t *)data->payload)[0] = L2CAP_CONN_REF_RES;
	    ((u16_t *)data->payload)[1] = 0; /* No further info available */
	    ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
	  }
	}
	bd_addr_set(&(pcb->remote_bdaddr),bdaddr);
	  
	pcb->scid = l2cap_cid_alloc();
	pcb->dcid = ((u16_t *)p->payload)[1];
	pcb->psm = psm;
	pcb->callback_arg = lpcb->callback_arg;
	pcb->l2ca_connect_ind = lpcb->l2ca_connect_ind;
	    
	pcb->state = L2CAP_CONFIG;
	L2CAP_REG(&l2cap_active_pcbs, pcb);
	
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: A connection request was received. Send a response\n"));
	data = pbuf_alloc(PBUF_RAW, L2CAP_CONN_RSP_SIZE, PBUF_RAM);
	if(data == NULL) {
	  LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_connect_rsp: Could not allocate memory for pbuf\n"));
	  break;
	}
	((u16_t *)data->payload)[0] = pcb->scid;
	((u16_t *)data->payload)[1] = pcb->dcid;
	((u16_t *)data->payload)[2] = L2CAP_CONN_SUCCESS;
	((u16_t *)data->payload)[3] = 0x0000; /* No further information available */
	
	/* Send the response */
	ret = l2cap_signal(pcb, L2CAP_CONN_RSP, sighdr->id, &(pcb->remote_bdaddr), data);
      }
      break;
    case L2CAP_CONN_RSP:
      if(pcb == NULL) {
	/* A response without a matching request is silently discarded */
	break;
      }
      LWIP_ASSERT("l2cap_process_sig: conn rsp, active pcb->state == W4_L2CAP_CONNECT_RSP\n",
		  pcb->state == W4_L2CAP_CONNECT_RSP);
      result = ((u16_t *)p->payload)[2];
      status = ((u16_t *)p->payload)[3];
      switch(result) {
      case L2CAP_CONN_SUCCESS:
	LWIP_DEBUGF(L2CAP_DEBUG, ("l2cap_process_sig: Conn_rsp_sucess, status %d\n", status));
	  
	LWIP_ASSERT("l2cap_process_sig: conn rsp success, pcb->scid == ((u16_t *)p->payload)[1]\n",
		    pcb->scid == ((u16_t *)p->payload)[1]);

	/* Set destination connection id */