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

data.c (7144B)


      1// SPDX-License-Identifier: GPL-2.0-only
      2/*
      3 *  The NFC Controller Interface is the communication protocol between an
      4 *  NFC Controller (NFCC) and a Device Host (DH).
      5 *
      6 *  Copyright (C) 2011 Texas Instruments, Inc.
      7 *  Copyright (C) 2014 Marvell International Ltd.
      8 *
      9 *  Written by Ilan Elias <ilane@ti.com>
     10 */
     11
     12#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
     13
     14#include <linux/types.h>
     15#include <linux/interrupt.h>
     16#include <linux/wait.h>
     17#include <linux/bitops.h>
     18#include <linux/skbuff.h>
     19
     20#include "../nfc.h"
     21#include <net/nfc/nci.h>
     22#include <net/nfc/nci_core.h>
     23#include <linux/nfc.h>
     24
     25/* Complete data exchange transaction and forward skb to nfc core */
     26void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
     27				__u8 conn_id, int err)
     28{
     29	const struct nci_conn_info *conn_info;
     30	data_exchange_cb_t cb;
     31	void *cb_context;
     32
     33	conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
     34	if (!conn_info) {
     35		kfree_skb(skb);
     36		goto exit;
     37	}
     38
     39	cb = conn_info->data_exchange_cb;
     40	cb_context = conn_info->data_exchange_cb_context;
     41
     42	pr_debug("len %d, err %d\n", skb ? skb->len : 0, err);
     43
     44	/* data exchange is complete, stop the data timer */
     45	del_timer_sync(&ndev->data_timer);
     46	clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
     47
     48	if (cb) {
     49		/* forward skb to nfc core */
     50		cb(cb_context, skb, err);
     51	} else if (skb) {
     52		pr_err("no rx callback, dropping rx data...\n");
     53
     54		/* no waiting callback, free skb */
     55		kfree_skb(skb);
     56	}
     57
     58exit:
     59	clear_bit(NCI_DATA_EXCHANGE, &ndev->flags);
     60}
     61
     62/* ----------------- NCI TX Data ----------------- */
     63
     64static inline void nci_push_data_hdr(struct nci_dev *ndev,
     65				     __u8 conn_id,
     66				     struct sk_buff *skb,
     67				     __u8 pbf)
     68{
     69	struct nci_data_hdr *hdr;
     70	int plen = skb->len;
     71
     72	hdr = skb_push(skb, NCI_DATA_HDR_SIZE);
     73	hdr->conn_id = conn_id;
     74	hdr->rfu = 0;
     75	hdr->plen = plen;
     76
     77	nci_mt_set((__u8 *)hdr, NCI_MT_DATA_PKT);
     78	nci_pbf_set((__u8 *)hdr, pbf);
     79}
     80
     81int nci_conn_max_data_pkt_payload_size(struct nci_dev *ndev, __u8 conn_id)
     82{
     83	const struct nci_conn_info *conn_info;
     84
     85	conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
     86	if (!conn_info)
     87		return -EPROTO;
     88
     89	return conn_info->max_pkt_payload_len;
     90}
     91EXPORT_SYMBOL(nci_conn_max_data_pkt_payload_size);
     92
     93static int nci_queue_tx_data_frags(struct nci_dev *ndev,
     94				   __u8 conn_id,
     95				   struct sk_buff *skb) {
     96	const struct nci_conn_info *conn_info;
     97	int total_len = skb->len;
     98	const unsigned char *data = skb->data;
     99	unsigned long flags;
    100	struct sk_buff_head frags_q;
    101	struct sk_buff *skb_frag;
    102	int frag_len;
    103	int rc = 0;
    104
    105	pr_debug("conn_id 0x%x, total_len %d\n", conn_id, total_len);
    106
    107	conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
    108	if (!conn_info) {
    109		rc = -EPROTO;
    110		goto exit;
    111	}
    112
    113	__skb_queue_head_init(&frags_q);
    114
    115	while (total_len) {
    116		frag_len =
    117			min_t(int, total_len, conn_info->max_pkt_payload_len);
    118
    119		skb_frag = nci_skb_alloc(ndev,
    120					 (NCI_DATA_HDR_SIZE + frag_len),
    121					 GFP_ATOMIC);
    122		if (skb_frag == NULL) {
    123			rc = -ENOMEM;
    124			goto free_exit;
    125		}
    126		skb_reserve(skb_frag, NCI_DATA_HDR_SIZE);
    127
    128		/* first, copy the data */
    129		skb_put_data(skb_frag, data, frag_len);
    130
    131		/* second, set the header */
    132		nci_push_data_hdr(ndev, conn_id, skb_frag,
    133				  ((total_len == frag_len) ?
    134				   (NCI_PBF_LAST) : (NCI_PBF_CONT)));
    135
    136		__skb_queue_tail(&frags_q, skb_frag);
    137
    138		data += frag_len;
    139		total_len -= frag_len;
    140
    141		pr_debug("frag_len %d, remaining total_len %d\n",
    142			 frag_len, total_len);
    143	}
    144
    145	/* queue all fragments atomically */
    146	spin_lock_irqsave(&ndev->tx_q.lock, flags);
    147
    148	while ((skb_frag = __skb_dequeue(&frags_q)) != NULL)
    149		__skb_queue_tail(&ndev->tx_q, skb_frag);
    150
    151	spin_unlock_irqrestore(&ndev->tx_q.lock, flags);
    152
    153	/* free the original skb */
    154	kfree_skb(skb);
    155
    156	goto exit;
    157
    158free_exit:
    159	while ((skb_frag = __skb_dequeue(&frags_q)) != NULL)
    160		kfree_skb(skb_frag);
    161
    162exit:
    163	return rc;
    164}
    165
    166/* Send NCI data */
    167int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb)
    168{
    169	const struct nci_conn_info *conn_info;
    170	int rc = 0;
    171
    172	pr_debug("conn_id 0x%x, plen %d\n", conn_id, skb->len);
    173
    174	conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
    175	if (!conn_info) {
    176		rc = -EPROTO;
    177		goto free_exit;
    178	}
    179
    180	/* check if the packet need to be fragmented */
    181	if (skb->len <= conn_info->max_pkt_payload_len) {
    182		/* no need to fragment packet */
    183		nci_push_data_hdr(ndev, conn_id, skb, NCI_PBF_LAST);
    184
    185		skb_queue_tail(&ndev->tx_q, skb);
    186	} else {
    187		/* fragment packet and queue the fragments */
    188		rc = nci_queue_tx_data_frags(ndev, conn_id, skb);
    189		if (rc) {
    190			pr_err("failed to fragment tx data packet\n");
    191			goto free_exit;
    192		}
    193	}
    194
    195	ndev->cur_conn_id = conn_id;
    196	queue_work(ndev->tx_wq, &ndev->tx_work);
    197
    198	goto exit;
    199
    200free_exit:
    201	kfree_skb(skb);
    202
    203exit:
    204	return rc;
    205}
    206EXPORT_SYMBOL(nci_send_data);
    207
    208/* ----------------- NCI RX Data ----------------- */
    209
    210static void nci_add_rx_data_frag(struct nci_dev *ndev,
    211				 struct sk_buff *skb,
    212				 __u8 pbf, __u8 conn_id, __u8 status)
    213{
    214	int reassembly_len;
    215	int err = 0;
    216
    217	if (status) {
    218		err = status;
    219		goto exit;
    220	}
    221
    222	if (ndev->rx_data_reassembly) {
    223		reassembly_len = ndev->rx_data_reassembly->len;
    224
    225		/* first, make enough room for the already accumulated data */
    226		if (skb_cow_head(skb, reassembly_len)) {
    227			pr_err("error adding room for accumulated rx data\n");
    228
    229			kfree_skb(skb);
    230			skb = NULL;
    231
    232			kfree_skb(ndev->rx_data_reassembly);
    233			ndev->rx_data_reassembly = NULL;
    234
    235			err = -ENOMEM;
    236			goto exit;
    237		}
    238
    239		/* second, combine the two fragments */
    240		memcpy(skb_push(skb, reassembly_len),
    241		       ndev->rx_data_reassembly->data,
    242		       reassembly_len);
    243
    244		/* third, free old reassembly */
    245		kfree_skb(ndev->rx_data_reassembly);
    246		ndev->rx_data_reassembly = NULL;
    247	}
    248
    249	if (pbf == NCI_PBF_CONT) {
    250		/* need to wait for next fragment, store skb and exit */
    251		ndev->rx_data_reassembly = skb;
    252		return;
    253	}
    254
    255exit:
    256	if (ndev->nfc_dev->rf_mode == NFC_RF_TARGET) {
    257		/* Data received in Target mode, forward to nfc core */
    258		err = nfc_tm_data_received(ndev->nfc_dev, skb);
    259		if (err)
    260			pr_err("unable to handle received data\n");
    261	} else {
    262		nci_data_exchange_complete(ndev, skb, conn_id, err);
    263	}
    264}
    265
    266/* Rx Data packet */
    267void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
    268{
    269	__u8 pbf = nci_pbf(skb->data);
    270	__u8 status = 0;
    271	__u8 conn_id = nci_conn_id(skb->data);
    272	const struct nci_conn_info *conn_info;
    273
    274	pr_debug("len %d\n", skb->len);
    275
    276	pr_debug("NCI RX: MT=data, PBF=%d, conn_id=%d, plen=%d\n",
    277		 nci_pbf(skb->data),
    278		 nci_conn_id(skb->data),
    279		 nci_plen(skb->data));
    280
    281	conn_info = nci_get_conn_info_by_conn_id(ndev, nci_conn_id(skb->data));
    282	if (!conn_info)
    283		return;
    284
    285	/* strip the nci data header */
    286	skb_pull(skb, NCI_DATA_HDR_SIZE);
    287
    288	if (ndev->target_active_prot == NFC_PROTO_MIFARE ||
    289	    ndev->target_active_prot == NFC_PROTO_JEWEL ||
    290	    ndev->target_active_prot == NFC_PROTO_FELICA ||
    291	    ndev->target_active_prot == NFC_PROTO_ISO15693) {
    292		/* frame I/F => remove the status byte */
    293		pr_debug("frame I/F => remove the status byte\n");
    294		status = skb->data[skb->len - 1];
    295		skb_trim(skb, (skb->len - 1));
    296	}
    297
    298	nci_add_rx_data_frag(ndev, skb, pbf, conn_id, nci_to_errno(status));
    299}