LXR linux/drivers/bluetooth/h4

   1/* SPDX-License-Identifier: GPL-2.0-or-later */
   2/*
   3 *
   4 *  Generic Bluetooth HCI UART driver
   5 *
   6 *  Copyright (C) 2015-2018  Intel Corporation
   7 */
   8
   9#include <asm/unaligned.h>
  10
  11struct h4_recv_pkt {
  12        u8  type;       /* Packet type */
  13        u8  hlen;       /* Header length */
  14        u8  loff;       /* Data length offset in header */
  15        u8  lsize;      /* Data length field size */
  16        u16 maxlen;     /* Max overall packet length */
  17        int (*recv)(struct hci_dev *hdev, struct sk_buff *skb);
  18};
  19
  20#define H4_RECV_ACL \
  21        .type = HCI_ACLDATA_PKT, \
  22        .hlen = HCI_ACL_HDR_SIZE, \
  23        .loff = 2, \
  24        .lsize = 2, \
  25        .maxlen = HCI_MAX_FRAME_SIZE \
  26
  27#define H4_RECV_SCO \
  28        .type = HCI_SCODATA_PKT, \
  29        .hlen = HCI_SCO_HDR_SIZE, \
  30        .loff = 2, \
  31        .lsize = 1, \
  32        .maxlen = HCI_MAX_SCO_SIZE
  33
  34#define H4_RECV_EVENT \
  35        .type = HCI_EVENT_PKT, \
  36        .hlen = HCI_EVENT_HDR_SIZE, \
  37        .loff = 1, \
  38        .lsize = 1, \
  39        .maxlen = HCI_MAX_EVENT_SIZE
  40
  41static inline struct sk_buff *h4_recv_buf(struct hci_dev *hdev,
  42                                          struct sk_buff *skb,
  43                                          const unsigned char *buffer,
  44                                          int count,
  45                                          const struct h4_recv_pkt *pkts,
  46                                          int pkts_count)
  47{
  48        /* Check for error from previous call */
  49        if (IS_ERR(skb))
  50                skb = NULL;
  51
  52        while (count) {
  53                int i, len;
  54
  55                if (!skb) {
  56                        for (i = 0; i < pkts_count; i++) {
  57                                if (buffer[0] != (&pkts[i])->type)
  58                                        continue;
  59
  60                                skb = bt_skb_alloc((&pkts[i])->maxlen,
  61                                                   GFP_ATOMIC);
  62                                if (!skb)
  63                                        return ERR_PTR(-ENOMEM);
  64
  65                                hci_skb_pkt_type(skb) = (&pkts[i])->type;
  66                                hci_skb_expect(skb) = (&pkts[i])->hlen;
  67                                break;
  68                        }
  69
  70                        /* Check for invalid packet type */
  71                        if (!skb)
  72                                return ERR_PTR(-EILSEQ);
  73
  74                        count -= 1;
  75                        buffer += 1;
  76                }
  77
  78                len = min_t(uint, hci_skb_expect(skb) - skb->len, count);
  79                skb_put_data(skb, buffer, len);
  80
  81                count -= len;
  82                buffer += len;
  83
  84                /* Check for partial packet */
  85                if (skb->len < hci_skb_expect(skb))
  86                        continue;
  87
  88                for (i = 0; i < pkts_count; i++) {
  89                        if (hci_skb_pkt_type(skb) == (&pkts[i])->type)
  90                                break;
  91                }
  92
  93                if (i >= pkts_count) {
  94                        kfree_skb(skb);
  95                        return ERR_PTR(-EILSEQ);
  96                }
  97
  98                if (skb->len == (&pkts[i])->hlen) {
  99                        u16 dlen;
 100
 101                        switch ((&pkts[i])->lsize) {
 102                        case 0:
 103                                /* No variable data length */
 104                                dlen = 0;
 105                                break;
 106                        case 1:
 107                                /* Single octet variable length */
 108                                dlen = skb->data[(&pkts[i])->loff];
 109                                hci_skb_expect(skb) += dlen;
 110
 111                                if (skb_tailroom(skb) < dlen) {
 112                                        kfree_skb(skb);
 113                                        return ERR_PTR(-EMSGSIZE);
 114                                }
 115                                break;
 116                        case 2:
 117                                /* Double octet variable length */
 118                                dlen = get_unaligned_le16(skb->data +
 119                                                          (&pkts[i])->loff);
 120                                hci_skb_expect(skb) += dlen;
 121
 122                                if (skb_tailroom(skb) < dlen) {
 123                                        kfree_skb(skb);
 124                                        return ERR_PTR(-EMSGSIZE);
 125                                }
 126                                break;
 127                        default:
 128                                /* Unsupported variable length */
 129                                kfree_skb(skb);
 130                                return ERR_PTR(-EILSEQ);
 131                        }
 132
 133                        if (!dlen) {
 134                                /* No more data, complete frame */
 135                                (&pkts[i])->recv(hdev, skb);
 136                                skb = NULL;
 137                        }
 138                } else {
 139                        /* Complete frame */
 140                        (&pkts[i])->recv(hdev, skb);
 141                        skb = NULL;
 142                }
 143        }
 144
 145        return skb;
 146}
 147