diff -u hci_bcsp.h.orig hci_bcsp.h --- hci_bcsp.h.orig Wed Oct 27 23:08:04 2004 +++ hci_bcsp.h Thu Oct 28 16:12:00 2004 @@ -42,6 +42,7 @@ struct sk_buff_head unrel; /* Unreliable packets queue */ unsigned long rx_count; + unsigned char *rx_ptr; struct sk_buff *rx_skb; u8 rxseq_txack; /* rxseq == txack. */ u8 rxack; /* Last packet sent by us that the peer ack'ed */ diff -u hci_bcsp.c.orig hci_bcsp.c --- hci_bcsp.c.orig Mon Oct 25 23:22:58 2004 +++ hci_bcsp.c Thu Oct 28 16:13:54 2004 @@ -53,6 +53,9 @@ #include #include + +#include + #include "hci_uart.h" #include "hci_bcsp.h" @@ -401,53 +404,46 @@ } } -static inline void bcsp_unslip_one_byte(struct bcsp_struct *bcsp, unsigned char byte) +static int __fastram bcsp_unslip(struct bcsp_struct *bcsp, unsigned char **p_pdata, int *p_count) { - const u8 c0 = 0xc0, db = 0xdb; - - switch (bcsp->rx_esc_state) { - case BCSP_ESCSTATE_NOESC: - switch (byte) { - case 0xdb: - bcsp->rx_esc_state = BCSP_ESCSTATE_ESC; - break; - default: - memcpy(skb_put(bcsp->rx_skb, 1), &byte, 1); - if ((bcsp->rx_skb-> data[0] & 0x40) != 0 && - bcsp->rx_state != BCSP_W4_CRC) - bcsp_crc_update(&bcsp->message_crc, byte); - bcsp->rx_count--; - } - break; - - case BCSP_ESCSTATE_ESC: - switch (byte) { - case 0xdc: - memcpy(skb_put(bcsp->rx_skb, 1), &c0, 1); - if ((bcsp->rx_skb-> data[0] & 0x40) != 0 && - bcsp->rx_state != BCSP_W4_CRC) - bcsp_crc_update(&bcsp-> message_crc, 0xc0); - bcsp->rx_esc_state = BCSP_ESCSTATE_NOESC; - bcsp->rx_count--; - break; - - case 0xdd: - memcpy(skb_put(bcsp->rx_skb, 1), &db, 1); - if ((bcsp->rx_skb-> data[0] & 0x40) != 0 && - bcsp->rx_state != BCSP_W4_CRC) - bcsp_crc_update(&bcsp-> message_crc, 0xdb); - bcsp->rx_esc_state = BCSP_ESCSTATE_NOESC; - bcsp->rx_count--; - break; - - default: - BT_ERR ("Invalid byte %02x after esc byte", byte); - kfree_skb(bcsp->rx_skb); - bcsp->rx_skb = NULL; - bcsp->rx_state = BCSP_W4_PKT_DELIMITER; - bcsp->rx_count = 0; - } - } + unsigned char *pdata=*p_pdata; + int count=*p_count; + unsigned char c; + int ret=0; + + while(bcsp->rx_count && count){ + c=*pdata++; count--; + if (c==0xc0) {ret=-2; goto unslip_ret;} //unexpected end of slip pkt. + + switch(bcsp->rx_esc_state){ + case BCSP_ESCSTATE_NOESC: + if (c==0xdb){ + bcsp->rx_esc_state = BCSP_ESCSTATE_ESC; + } + else{ + *bcsp->rx_ptr++ = c; bcsp->rx_count--; + } + break; + case BCSP_ESCSTATE_ESC: + switch(c){ + case 0xdd: + *bcsp->rx_ptr++ = 0xdb; + break; + case 0xdc: + *bcsp->rx_ptr++ = 0xc0; + break; + default: + ret=-1; goto unslip_ret; // Illegal char after ESC + } + bcsp->rx_count--; + bcsp->rx_esc_state = BCSP_ESCSTATE_NOESC; + break; + } + } // while +unslip_ret: + *p_pdata = pdata; + *p_count = count; + return ret; } static inline void bcsp_complete_rx_pkt(struct hci_uart *hu) @@ -507,27 +503,32 @@ } /* Recv data */ -static int bcsp_recv(struct hci_uart *hu, void *data, int count) +static int __fastram bcsp_recv(struct hci_uart *hu, void *data, int count) { struct bcsp_struct *bcsp = hu->priv; - register unsigned char *ptr; - + unsigned char *ptr; BT_DBG("hu %p count %d rx_state %ld rx_count %ld", hu, count, bcsp->rx_state, bcsp->rx_count); - ptr = data; while (count) { - if (bcsp->rx_count) { - if (*ptr == 0xc0) { - BT_ERR("Short BCSP packet"); - kfree_skb(bcsp->rx_skb); - bcsp->rx_state = BCSP_W4_PKT_START; - bcsp->rx_count = 0; - } else - bcsp_unslip_one_byte(bcsp, *ptr); - - ptr++; count--; - continue; + // dest buf: bcsp->rx_ptr, rx_count + if (bcsp->rx_count){ + switch (bcsp_unslip(bcsp,&ptr,&count)){ + case -2: + BT_ERR("Short BCSP packet"); + kfree_skb(bcsp->rx_skb); + bcsp->rx_state = BCSP_W4_PKT_START; + bcsp->rx_count = 0; + break; + case -1: + BT_ERR("Illegal SLIP char"); + kfree_skb(bcsp->rx_skb); + bcsp->rx_skb=NULL; + bcsp->rx_state = BCSP_W4_PKT_DELIMITER; + bcsp->rx_count = 0; + break; + } + continue; } switch (bcsp->rx_state) { @@ -553,12 +554,14 @@ bcsp->rx_state = BCSP_W4_DATA; bcsp->rx_count = (bcsp->rx_skb->data[1] >> 4) + (bcsp->rx_skb->data[2] << 4); /* May be 0 */ + bcsp->rx_ptr=skb_put(bcsp->rx_skb,bcsp->rx_count); continue; case BCSP_W4_DATA: if (bcsp->rx_skb->data[0] & 0x40) { /* pkt with crc */ bcsp->rx_state = BCSP_W4_CRC; bcsp->rx_count = 2; + bcsp->rx_ptr=skb_put(bcsp->rx_skb,2); } else bcsp_complete_rx_pkt(hu); continue; @@ -609,7 +612,6 @@ /* Do not increment ptr or decrement count * Allocate packet. Max len of a BCSP pkt= * 0xFFF (payload) +4 (header) +2 (crc) */ - bcsp->rx_skb = bluez_skb_alloc(0x1005, GFP_ATOMIC); if (!bcsp->rx_skb) { BT_ERR("Can't allocate mem for new packet"); @@ -617,6 +619,8 @@ bcsp->rx_count = 0; return 0; } + bcsp->rx_ptr=skb_put(bcsp->rx_skb,bcsp->rx_count); + bcsp->rx_skb->dev = (void *) &hu->hdev; break; }