LXR linux/drivers/bluetooth/h4

   1/*
   2 *
   3 *  Generic Bluetooth HCI UART driver
   4 *
   5 *  Copyright (C) 2015-2018  Intel Corporation
   6 *
   7 *
   8 *  This program is free software; you can redistribute it and/or modify
   9 *  it under the terms of the GNU General Public License as published by
  10 *  the Free Software Foundation; either version 2 of the License, or
  11 *  (at your option) any later version.
  12 *
  13 *  This program is distributed in the hope that it will be useful,
  14 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16 *  GNU General Public License for more details.
  17 *
  18 *  You should have received a copy of the GNU General Public License
  19 *  along with this program; if not, write to the Free Software
  20 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  21 *
  22 */
  23
  24#include <asm/unaligned.h>
  25
  26struct h4_recv_pkt {
  27        u8  type;       /* Packet type */
  28        u8  hlen;       /* Header length */
  29        u8  loff;       /* Data length offset in header */
  30        u8  lsize;      /* Data length field size */
  31        u16 maxlen;     /* Max overall packet length */
  32        int (*recv)(struct hci_dev *hdev, struct sk_buff *skb);
  33};
  34
  35#define H4_RECV_ACL \
  36        .type = HCI_ACLDATA_PKT, \
  37        .hlen = HCI_ACL_HDR_SIZE, \
  38        .loff = 2, \
  39        .lsize = 2, \
  40        .maxlen = HCI_MAX_FRAME_SIZE \
  41
  42#define H4_RECV_SCO \
  43        .type = HCI_SCODATA_PKT, \
  44        .hlen = HCI_SCO_HDR_SIZE, \
  45        .loff = 2, \
  46        .lsize = 1, \
  47        .maxlen = HCI_MAX_SCO_SIZE
  48
  49#define H4_RECV_EVENT \
  50        .type = HCI_EVENT_PKT, \
  51        .hlen = HCI_EVENT_HDR_SIZE, \
  52        .loff = 1, \
  53        .lsize = 1, \
  54        .maxlen = HCI_MAX_EVENT_SIZE
  55
  56static inline struct sk_buff *h4_recv_buf(struct hci_dev *hdev,
  57                                          struct sk_buff *skb,
  58                                          const unsigned char *buffer,
  59                                          int count,
  60                                          const struct h4_recv_pkt *pkts,
  61                                          int pkts_count)
  62{
  63        while (count) {
  64                int i, len;
  65
  66                if (!count)
  67                        break;
  68
  69                if (!skb) {
  70                        for (i = 0; i < pkts_count; i++) {
  71                                if (buffer[0] != (&pkts[i])->type)
  72                                        continue;
  73
  74                                skb = bt_skb_alloc((&pkts[i])->maxlen,
  75                                                   GFP_ATOMIC);
  76                                if (!skb)
  77                                        return ERR_PTR(-ENOMEM);
  78
  79                                hci_skb_pkt_type(skb) = (&pkts[i])->type;
  80                                hci_skb_expect(skb) = (&pkts[i])->hlen;
  81                                break;
  82                        }
  83
  84                        /* Check for invalid packet type */
  85                        if (!skb)
  86                                return ERR_PTR(-EILSEQ);
  87
  88                        count -= 1;
  89                        buffer += 1;
  90                }
  91
  92                len = min_t(uint, hci_skb_expect(skb) - skb->len, count);
  93                skb_put_data(skb, buffer, len);
  94
  95                count -= len;
  96                buffer += len;
  97
  98                /* Check for partial packet */
  99                if (skb->len < hci_skb_expect(skb))
 100                        continue;
 101
 102                for (i = 0; i < pkts_count; i++) {
 103                        if (hci_skb_pkt_type(skb) == (&pkts[i])->type)
 104                                break;
 105                }
 106
 107                if (i >= pkts_count) {
 108                        kfree_skb(skb);
 109                        return ERR_PTR(-EILSEQ);
 110                }
 111
 112                if (skb->len == (&pkts[i])->hlen) {
 113                        u16 dlen;
 114
 115                        switch ((&pkts[i])->lsize) {
 116                        case 0:
 117                                /* No variable data length */
 118                                dlen = 0;
 119                                break;
 120                        case 1:
 121                                /* Single octet variable length */
 122                                dlen = skb->data[(&pkts[i])->loff];
 123                                hci_skb_expect(skb) += dlen;
 124
 125                                if (skb_tailroom(skb) < dlen) {
 126                                        kfree_skb(skb);
 127                                        return ERR_PTR(-EMSGSIZE);
 128                                }
 129                                break;
 130                        case 2:
 131                                /* Double octet variable length */
 132                                dlen = get_unaligned_le16(skb->data +
 133                                                          (&pkts[i])->loff);
 134                                hci_skb_expect(skb) += dlen;
 135
 136                                if (skb_tailroom(skb) < dlen) {
 137                                        kfree_skb(skb);
 138                                        return ERR_PTR(-EMSGSIZE);
 139                                }
 140                                break;
 141                        default:
 142                                /* Unsupported variable length */
 143                                kfree_skb(skb);
 144                                return ERR_PTR(-EILSEQ);
 145                        }
 146
 147                        if (!dlen) {
 148                                /* No more data, complete frame */
 149                                (&pkts[i])->recv(hdev, skb);
 150                                skb = NULL;
 151                        }
 152                } else {
 153                        /* Complete frame */
 154                        (&pkts[i])->recv(hdev, skb);
 155                        skb = NULL;
 156                }
 157        }
 158
 159        return skb;
 160}
 161