cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

h4_recv.h (2984B)


      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}