hollalinux
/
linux-sg2042
mirror of https://github.com/sophgo/linux-riscv.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
linux-sg2042/net/nfc/digital_dep.c

1543 lines
32 KiB
C
Raw Blame History

/*
* NFC Digital Protocol stack
* Copyright (c) 2013, Intel Corporation.
*
* This program is free software; you can redistribute it and/or modify it
* under the terms and conditions of the GNU General Public License,
* version 2, as published by the Free Software Foundation.
*
* This program is distributed in the hope it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
*/
#define pr_fmt(fmt) "digital: %s: " fmt, __func__
#include "digital.h"
#define DIGITAL_NFC_DEP_N_RETRY_NACK 2
#define DIGITAL_NFC_DEP_N_RETRY_ATN 2
#define DIGITAL_NFC_DEP_FRAME_DIR_OUT 0xD4
#define DIGITAL_NFC_DEP_FRAME_DIR_IN 0xD5
#define DIGITAL_NFC_DEP_NFCA_SOD_SB 0xF0
#define DIGITAL_CMD_ATR_REQ 0x00
#define DIGITAL_CMD_ATR_RES 0x01
#define DIGITAL_CMD_PSL_REQ 0x04
#define DIGITAL_CMD_PSL_RES 0x05
#define DIGITAL_CMD_DEP_REQ 0x06
#define DIGITAL_CMD_DEP_RES 0x07
#define DIGITAL_ATR_REQ_MIN_SIZE 16
#define DIGITAL_ATR_REQ_MAX_SIZE 64
#define DIGITAL_DID_MAX 14
#define DIGITAL_PAYLOAD_SIZE_MAX 254
#define DIGITAL_PAYLOAD_BITS_TO_PP(s) (((s) & 0x3) << 4)
#define DIGITAL_PAYLOAD_PP_TO_BITS(s) (((s) >> 4) & 0x3)
#define DIGITAL_PAYLOAD_BITS_TO_FSL(s) ((s) & 0x3)
#define DIGITAL_PAYLOAD_FSL_TO_BITS(s) ((s) & 0x3)
#define DIGITAL_GB_BIT 0x02
#define DIGITAL_NFC_DEP_REQ_RES_HEADROOM 2 /* SoD: [SB (NFC-A)] + LEN */
#define DIGITAL_NFC_DEP_REQ_RES_TAILROOM 2 /* EoD: 2-byte CRC */
#define DIGITAL_NFC_DEP_PFB_TYPE(pfb) ((pfb) & 0xE0)
#define DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT 0x10
#define DIGITAL_NFC_DEP_PFB_MI_BIT 0x10
#define DIGITAL_NFC_DEP_PFB_NACK_BIT 0x10
#define DIGITAL_NFC_DEP_PFB_DID_BIT 0x04
#define DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb) \
((pfb) & DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT)
#define DIGITAL_NFC_DEP_MI_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_MI_BIT)
#define DIGITAL_NFC_DEP_NACK_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_NACK_BIT)
#define DIGITAL_NFC_DEP_NAD_BIT_SET(pfb) ((pfb) & 0x08)
#define DIGITAL_NFC_DEP_DID_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_DID_BIT)
#define DIGITAL_NFC_DEP_PFB_PNI(pfb) ((pfb) & 0x03)
#define DIGITAL_NFC_DEP_PFB_I_PDU 0x00
#define DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU 0x40
#define DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU 0x80
struct digital_atr_req {
u8 dir;
u8 cmd;
u8 nfcid3[10];
u8 did;
u8 bs;
u8 br;
u8 pp;
u8 gb[0];
} __packed;
struct digital_atr_res {
u8 dir;
u8 cmd;
u8 nfcid3[10];
u8 did;
u8 bs;
u8 br;
u8 to;
u8 pp;
u8 gb[0];
} __packed;
struct digital_psl_req {
u8 dir;
u8 cmd;
u8 did;
u8 brs;
u8 fsl;
} __packed;
struct digital_psl_res {
u8 dir;
u8 cmd;
u8 did;
} __packed;
struct digital_dep_req_res {
u8 dir;
u8 cmd;
u8 pfb;
} __packed;
static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp);
static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp);
static const u8 digital_payload_bits_map[4] = {
[0] = 64,
[1] = 128,
[2] = 192,
[3] = 254
};
static u8 digital_payload_bits_to_size(u8 payload_bits)
{
if (payload_bits >= ARRAY_SIZE(digital_payload_bits_map))
return 0;
return digital_payload_bits_map[payload_bits];
}
static u8 digital_payload_size_to_bits(u8 payload_size)
{
int i;
for (i = 0; i < ARRAY_SIZE(digital_payload_bits_map); i++)
if (digital_payload_bits_map[i] == payload_size)
return i;
return 0xff;
}
static void digital_skb_push_dep_sod(struct nfc_digital_dev *ddev,
struct sk_buff *skb)
{
skb_push(skb, sizeof(u8));
skb->data[0] = skb->len;
if (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)
*skb_push(skb, sizeof(u8)) = DIGITAL_NFC_DEP_NFCA_SOD_SB;
}
static int digital_skb_pull_dep_sod(struct nfc_digital_dev *ddev,
struct sk_buff *skb)
{
u8 size;
if (skb->len < 2)
return -EIO;
if (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)
skb_pull(skb, sizeof(u8));
size = skb->data[0];
if (size != skb->len)
return -EIO;
skb_pull(skb, sizeof(u8));
return 0;
}
static struct sk_buff *
digital_send_dep_data_prep(struct nfc_digital_dev *ddev, struct sk_buff *skb,
struct digital_dep_req_res *dep_req_res,
struct digital_data_exch *data_exch)
{
struct sk_buff *new_skb;
if (skb->len > ddev->remote_payload_max) {
dep_req_res->pfb |= DIGITAL_NFC_DEP_PFB_MI_BIT;
new_skb = digital_skb_alloc(ddev, ddev->remote_payload_max);
if (!new_skb) {
kfree_skb(ddev->chaining_skb);
ddev->chaining_skb = NULL;
return ERR_PTR(-ENOMEM);
}
skb_reserve(new_skb, ddev->tx_headroom + NFC_HEADER_SIZE +
DIGITAL_NFC_DEP_REQ_RES_HEADROOM);
memcpy(skb_put(new_skb, ddev->remote_payload_max), skb->data,
ddev->remote_payload_max);
skb_pull(skb, ddev->remote_payload_max);
ddev->chaining_skb = skb;
ddev->data_exch = data_exch;
} else {
ddev->chaining_skb = NULL;
new_skb = skb;
}
return new_skb;
}
static struct sk_buff *
digital_recv_dep_data_gather(struct nfc_digital_dev *ddev, u8 pfb,
struct sk_buff *resp,
int (*send_ack)(struct nfc_digital_dev *ddev,
struct digital_data_exch
*data_exch),
struct digital_data_exch *data_exch)
{
struct sk_buff *new_skb;
int rc;
if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb) && (!ddev->chaining_skb)) {
ddev->chaining_skb =
nfc_alloc_recv_skb(8 * ddev->local_payload_max,
GFP_KERNEL);
if (!ddev->chaining_skb) {
rc = -ENOMEM;
goto error;
}
}
if (ddev->chaining_skb) {
if (resp->len > skb_tailroom(ddev->chaining_skb)) {
new_skb = skb_copy_expand(ddev->chaining_skb,
skb_headroom(
ddev->chaining_skb),
8 * ddev->local_payload_max,
GFP_KERNEL);
if (!new_skb) {
rc = -ENOMEM;
goto error;
}
kfree_skb(ddev->chaining_skb);
ddev->chaining_skb = new_skb;
}
memcpy(skb_put(ddev->chaining_skb, resp->len), resp->data,
resp->len);
kfree_skb(resp);
resp = NULL;
if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb)) {
rc = send_ack(ddev, data_exch);
if (rc)
goto error;
return NULL;
}
resp = ddev->chaining_skb;
ddev->chaining_skb = NULL;
}
return resp;
error:
kfree_skb(resp);
kfree_skb(ddev->chaining_skb);
ddev->chaining_skb = NULL;
return ERR_PTR(rc);
}
static void digital_in_recv_psl_res(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp)
{
struct nfc_target *target = arg;
struct digital_psl_res *psl_res;
int rc;
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto exit;
}
rc = ddev->skb_check_crc(resp);
if (rc) {
PROTOCOL_ERR("14.4.1.6");
goto exit;
}
rc = digital_skb_pull_dep_sod(ddev, resp);
if (rc) {
PROTOCOL_ERR("14.4.1.2");
goto exit;
}
psl_res = (struct digital_psl_res *)resp->data;
if ((resp->len != sizeof(*psl_res)) ||
(psl_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN) ||
(psl_res->cmd != DIGITAL_CMD_PSL_RES)) {
rc = -EIO;
goto exit;
}
rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
NFC_DIGITAL_RF_TECH_424F);
if (rc)
goto exit;
rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
NFC_DIGITAL_FRAMING_NFCF_NFC_DEP);
if (rc)
goto exit;
if (!DIGITAL_DRV_CAPS_IN_CRC(ddev) &&
(ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)) {
ddev->skb_add_crc = digital_skb_add_crc_f;
ddev->skb_check_crc = digital_skb_check_crc_f;
}
ddev->curr_rf_tech = NFC_DIGITAL_RF_TECH_424F;
nfc_dep_link_is_up(ddev->nfc_dev, target->idx, NFC_COMM_ACTIVE,
NFC_RF_INITIATOR);
ddev->curr_nfc_dep_pni = 0;
exit:
dev_kfree_skb(resp);
if (rc)
ddev->curr_protocol = 0;
}
static int digital_in_send_psl_req(struct nfc_digital_dev *ddev,
struct nfc_target *target)
{
struct sk_buff *skb;
struct digital_psl_req *psl_req;
int rc;
u8 payload_size, payload_bits;
skb = digital_skb_alloc(ddev, sizeof(*psl_req));
if (!skb)
return -ENOMEM;
skb_put(skb, sizeof(*psl_req));
psl_req = (struct digital_psl_req *)skb->data;
psl_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
psl_req->cmd = DIGITAL_CMD_PSL_REQ;
psl_req->did = 0;
psl_req->brs = (0x2 << 3) | 0x2; /* 424F both directions */
payload_size = min(ddev->local_payload_max, ddev->remote_payload_max);
payload_bits = digital_payload_size_to_bits(payload_size);
psl_req->fsl = DIGITAL_PAYLOAD_BITS_TO_FSL(payload_bits);
ddev->local_payload_max = payload_size;
ddev->remote_payload_max = payload_size;
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
rc = digital_in_send_cmd(ddev, skb, 500, digital_in_recv_psl_res,
target);
if (rc)
kfree_skb(skb);
return rc;
}
static void digital_in_recv_atr_res(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp)
{
struct nfc_target *target = arg;
struct digital_atr_res *atr_res;
u8 gb_len, payload_bits;
int rc;
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto exit;
}
rc = ddev->skb_check_crc(resp);
if (rc) {
PROTOCOL_ERR("14.4.1.6");
goto exit;
}
rc = digital_skb_pull_dep_sod(ddev, resp);
if (rc) {
PROTOCOL_ERR("14.4.1.2");
goto exit;
}
if (resp->len < sizeof(struct digital_atr_res)) {
rc = -EIO;
goto exit;
}
gb_len = resp->len - sizeof(struct digital_atr_res);
atr_res = (struct digital_atr_res *)resp->data;
payload_bits = DIGITAL_PAYLOAD_PP_TO_BITS(atr_res->pp);
ddev->remote_payload_max = digital_payload_bits_to_size(payload_bits);
if (!ddev->remote_payload_max) {
rc = -EINVAL;
goto exit;
}
rc = nfc_set_remote_general_bytes(ddev->nfc_dev, atr_res->gb, gb_len);
if (rc)
goto exit;
if ((ddev->protocols & NFC_PROTO_FELICA_MASK) &&
(ddev->curr_rf_tech != NFC_DIGITAL_RF_TECH_424F)) {
rc = digital_in_send_psl_req(ddev, target);
if (!rc)
goto exit;
}
rc = nfc_dep_link_is_up(ddev->nfc_dev, target->idx, NFC_COMM_ACTIVE,
NFC_RF_INITIATOR);
ddev->curr_nfc_dep_pni = 0;
exit:
dev_kfree_skb(resp);
if (rc)
ddev->curr_protocol = 0;
}
int digital_in_send_atr_req(struct nfc_digital_dev *ddev,
struct nfc_target *target, __u8 comm_mode, __u8 *gb,
size_t gb_len)
{
struct sk_buff *skb;
struct digital_atr_req *atr_req;
uint size;
int rc;
u8 payload_bits;
size = DIGITAL_ATR_REQ_MIN_SIZE + gb_len;
if (size > DIGITAL_ATR_REQ_MAX_SIZE) {
PROTOCOL_ERR("14.6.1.1");
return -EINVAL;
}
skb = digital_skb_alloc(ddev, size);
if (!skb)
return -ENOMEM;
skb_put(skb, sizeof(struct digital_atr_req));
atr_req = (struct digital_atr_req *)skb->data;
memset(atr_req, 0, sizeof(struct digital_atr_req));
atr_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
atr_req->cmd = DIGITAL_CMD_ATR_REQ;
if (target->nfcid2_len)
memcpy(atr_req->nfcid3, target->nfcid2, NFC_NFCID2_MAXSIZE);
else
get_random_bytes(atr_req->nfcid3, NFC_NFCID3_MAXSIZE);
atr_req->did = 0;
atr_req->bs = 0;
atr_req->br = 0;
ddev->local_payload_max = DIGITAL_PAYLOAD_SIZE_MAX;
payload_bits = digital_payload_size_to_bits(ddev->local_payload_max);
atr_req->pp = DIGITAL_PAYLOAD_BITS_TO_PP(payload_bits);
if (gb_len) {
atr_req->pp |= DIGITAL_GB_BIT;
memcpy(skb_put(skb, gb_len), gb, gb_len);
}
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
rc = digital_in_send_cmd(ddev, skb, 500, digital_in_recv_atr_res,
target);
if (rc)
kfree_skb(skb);
return rc;
}
static int digital_in_send_ack(struct nfc_digital_dev *ddev,
struct digital_data_exch *data_exch)
{
struct digital_dep_req_res *dep_req;
struct sk_buff *skb;
int rc;
skb = digital_skb_alloc(ddev, 1);
if (!skb)
return -ENOMEM;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_req = (struct digital_dep_req_res *)skb->data;
dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
dep_req->cmd = DIGITAL_CMD_DEP_REQ;
dep_req->pfb = DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU |
ddev->curr_nfc_dep_pni;
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
ddev->saved_skb = skb_get(skb);
ddev->saved_skb_len = skb->len;
rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
data_exch);
if (rc) {
kfree_skb(skb);
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
}
return rc;
}
static int digital_in_send_nack(struct nfc_digital_dev *ddev,
struct digital_data_exch *data_exch)
{
struct digital_dep_req_res *dep_req;
struct sk_buff *skb;
int rc;
skb = digital_skb_alloc(ddev, 1);
if (!skb)
return -ENOMEM;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_req = (struct digital_dep_req_res *)skb->data;
dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
dep_req->cmd = DIGITAL_CMD_DEP_REQ;
dep_req->pfb = DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU |
DIGITAL_NFC_DEP_PFB_NACK_BIT | ddev->curr_nfc_dep_pni;
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
data_exch);
if (rc)
kfree_skb(skb);
return rc;
}
static int digital_in_send_atn(struct nfc_digital_dev *ddev,
struct digital_data_exch *data_exch)
{
struct digital_dep_req_res *dep_req;
struct sk_buff *skb;
int rc;
skb = digital_skb_alloc(ddev, 1);
if (!skb)
return -ENOMEM;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_req = (struct digital_dep_req_res *)skb->data;
dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
dep_req->cmd = DIGITAL_CMD_DEP_REQ;
dep_req->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU;
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
data_exch);
if (rc)
kfree_skb(skb);
return rc;
}
static int digital_in_send_rtox(struct nfc_digital_dev *ddev,
struct digital_data_exch *data_exch, u8 rtox)
{
struct digital_dep_req_res *dep_req;
struct sk_buff *skb;
int rc;
skb = digital_skb_alloc(ddev, 1);
if (!skb)
return -ENOMEM;
*skb_put(skb, 1) = rtox;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_req = (struct digital_dep_req_res *)skb->data;
dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
dep_req->cmd = DIGITAL_CMD_DEP_REQ;
dep_req->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU |
DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT;
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
ddev->saved_skb = skb_get(skb);
ddev->saved_skb_len = skb->len;
rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
data_exch);
if (rc) {
kfree_skb(skb);
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
}
return rc;
}
static int digital_in_send_saved_skb(struct nfc_digital_dev *ddev,
struct digital_data_exch *data_exch)
{
skb_get(ddev->saved_skb);
skb_push(ddev->saved_skb, ddev->saved_skb_len);
return digital_in_send_cmd(ddev, ddev->saved_skb, 1500,
digital_in_recv_dep_res, data_exch);
}
static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp)
{
struct digital_data_exch *data_exch = arg;
struct digital_dep_req_res *dep_res;
u8 pfb;
uint size;
int rc;
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
if (((rc != -ETIMEDOUT) || ddev->nack_count) &&
(ddev->nack_count++ < DIGITAL_NFC_DEP_N_RETRY_NACK)) {
ddev->atn_count = 0;
rc = digital_in_send_nack(ddev, data_exch);
if (rc)
goto error;
return;
} else if ((rc == -ETIMEDOUT) &&
(ddev->atn_count++ < DIGITAL_NFC_DEP_N_RETRY_ATN)) {
ddev->nack_count = 0;
rc = digital_in_send_atn(ddev, data_exch);
if (rc)
goto error;
return;
}
goto exit;
}
rc = digital_skb_pull_dep_sod(ddev, resp);
if (rc) {
PROTOCOL_ERR("14.4.1.2");
goto exit;
}
rc = ddev->skb_check_crc(resp);
if (rc) {
if ((resp->len >= 4) &&
(ddev->nack_count++ < DIGITAL_NFC_DEP_N_RETRY_NACK)) {
ddev->atn_count = 0;
rc = digital_in_send_nack(ddev, data_exch);
if (rc)
goto error;
kfree_skb(resp);
return;
}
PROTOCOL_ERR("14.4.1.6");
goto error;
}
ddev->atn_count = 0;
ddev->nack_count = 0;
if (resp->len > ddev->local_payload_max) {
rc = -EMSGSIZE;
goto exit;
}
size = sizeof(struct digital_dep_req_res);
dep_res = (struct digital_dep_req_res *)resp->data;
if (resp->len < size || dep_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN ||
dep_res->cmd != DIGITAL_CMD_DEP_RES) {
rc = -EIO;
goto error;
}
pfb = dep_res->pfb;
if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb)) {
PROTOCOL_ERR("14.8.2.1");
rc = -EIO;
goto error;
}
if (DIGITAL_NFC_DEP_NAD_BIT_SET(pfb)) {
rc = -EIO;
goto exit;
}
if (size > resp->len) {
rc = -EIO;
goto error;
}
skb_pull(resp, size);
switch (DIGITAL_NFC_DEP_PFB_TYPE(pfb)) {
case DIGITAL_NFC_DEP_PFB_I_PDU:
if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) {
PROTOCOL_ERR("14.12.3.3");
rc = -EIO;
goto error;
}
ddev->curr_nfc_dep_pni =
DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
resp = digital_recv_dep_data_gather(ddev, pfb, resp,
digital_in_send_ack,
data_exch);
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto error;
}
/* If resp is NULL then we're still chaining so return and
* wait for the next part of the PDU. Else, the PDU is
* complete so pass it up.
*/
if (!resp)
return;
rc = 0;
break;
case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) {
PROTOCOL_ERR("14.12.3.3");
rc = -EIO;
goto exit;
}
ddev->curr_nfc_dep_pni =
DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
if (ddev->chaining_skb && !DIGITAL_NFC_DEP_NACK_BIT_SET(pfb)) {
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
rc = digital_in_send_dep_req(ddev, NULL,
ddev->chaining_skb,
ddev->data_exch);
if (rc)
goto error;
return;
}
pr_err("Received a ACK/NACK PDU\n");
rc = -EINVAL;
goto exit;
case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
if (!DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) { /* ATN */
rc = digital_in_send_saved_skb(ddev, data_exch);
if (rc) {
kfree_skb(ddev->saved_skb);
goto error;
}
return;
}
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
rc = digital_in_send_rtox(ddev, data_exch, resp->data[0]);
if (rc)
goto error;
kfree_skb(resp);
return;
}
exit:
data_exch->cb(data_exch->cb_context, resp, rc);
error:
kfree(data_exch);
kfree_skb(ddev->chaining_skb);
ddev->chaining_skb = NULL;
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
if (rc)
kfree_skb(resp);
}
int digital_in_send_dep_req(struct nfc_digital_dev *ddev,
struct nfc_target *target, struct sk_buff *skb,
struct digital_data_exch *data_exch)
{
struct digital_dep_req_res *dep_req;
struct sk_buff *chaining_skb, *tmp_skb;
int rc;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_req = (struct digital_dep_req_res *)skb->data;
dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
dep_req->cmd = DIGITAL_CMD_DEP_REQ;
dep_req->pfb = ddev->curr_nfc_dep_pni;
ddev->atn_count = 0;
ddev->nack_count = 0;
chaining_skb = ddev->chaining_skb;
tmp_skb = digital_send_dep_data_prep(ddev, skb, dep_req, data_exch);
if (IS_ERR(tmp_skb))
return PTR_ERR(tmp_skb);
digital_skb_push_dep_sod(ddev, tmp_skb);
ddev->skb_add_crc(tmp_skb);
ddev->saved_skb = skb_get(tmp_skb);
ddev->saved_skb_len = tmp_skb->len;
rc = digital_in_send_cmd(ddev, tmp_skb, 1500, digital_in_recv_dep_res,
data_exch);
if (rc) {
if (tmp_skb != skb)
kfree_skb(tmp_skb);
kfree_skb(chaining_skb);
ddev->chaining_skb = NULL;
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
}
return rc;
}
static void digital_tg_set_rf_tech(struct nfc_digital_dev *ddev, u8 rf_tech)
{
ddev->curr_rf_tech = rf_tech;
ddev->skb_add_crc = digital_skb_add_crc_none;
ddev->skb_check_crc = digital_skb_check_crc_none;
if (DIGITAL_DRV_CAPS_TG_CRC(ddev))
return;
switch (ddev->curr_rf_tech) {
case NFC_DIGITAL_RF_TECH_106A:
ddev->skb_add_crc = digital_skb_add_crc_a;
ddev->skb_check_crc = digital_skb_check_crc_a;
break;
case NFC_DIGITAL_RF_TECH_212F:
case NFC_DIGITAL_RF_TECH_424F:
ddev->skb_add_crc = digital_skb_add_crc_f;
ddev->skb_check_crc = digital_skb_check_crc_f;
break;
default:
break;
}
}
static int digital_tg_send_ack(struct nfc_digital_dev *ddev,
struct digital_data_exch *data_exch)
{
struct digital_dep_req_res *dep_res;
struct sk_buff *skb;
int rc;
skb = digital_skb_alloc(ddev, 1);
if (!skb)
return -ENOMEM;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_res = (struct digital_dep_req_res *)skb->data;
dep_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
dep_res->cmd = DIGITAL_CMD_DEP_RES;
dep_res->pfb = DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU |
ddev->curr_nfc_dep_pni;
if (ddev->did) {
dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT;
memcpy(skb_put(skb, sizeof(ddev->did)), &ddev->did,
sizeof(ddev->did));
}
ddev->curr_nfc_dep_pni =
DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
ddev->saved_skb = skb_get(skb);
ddev->saved_skb_len = skb->len;
rc = digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req,
data_exch);
if (rc) {
kfree_skb(skb);
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
}
return rc;
}
static int digital_tg_send_atn(struct nfc_digital_dev *ddev)
{
struct digital_dep_req_res *dep_res;
struct sk_buff *skb;
int rc;
skb = digital_skb_alloc(ddev, 1);
if (!skb)
return -ENOMEM;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_res = (struct digital_dep_req_res *)skb->data;
dep_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
dep_res->cmd = DIGITAL_CMD_DEP_RES;
dep_res->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU;
if (ddev->did) {
dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT;
memcpy(skb_put(skb, sizeof(ddev->did)), &ddev->did,
sizeof(ddev->did));
}
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
rc = digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req,
NULL);
if (rc)
kfree_skb(skb);
return rc;
}
static int digital_tg_send_saved_skb(struct nfc_digital_dev *ddev)
{
skb_get(ddev->saved_skb);
skb_push(ddev->saved_skb, ddev->saved_skb_len);
return digital_tg_send_cmd(ddev, ddev->saved_skb, 1500,
digital_tg_recv_dep_req, NULL);
}
static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp)
{
int rc;
struct digital_dep_req_res *dep_req;
u8 pfb;
size_t size;
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto exit;
}
rc = ddev->skb_check_crc(resp);
if (rc) {
PROTOCOL_ERR("14.4.1.6");
goto exit;
}
rc = digital_skb_pull_dep_sod(ddev, resp);
if (rc) {
PROTOCOL_ERR("14.4.1.2");
goto exit;
}
if (resp->len > ddev->local_payload_max) {
rc = -EMSGSIZE;
goto exit;
}
size = sizeof(struct digital_dep_req_res);
dep_req = (struct digital_dep_req_res *)resp->data;
if (resp->len < size || dep_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
dep_req->cmd != DIGITAL_CMD_DEP_REQ) {
rc = -EIO;
goto exit;
}
pfb = dep_req->pfb;
if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb)) {
if (ddev->did && (ddev->did == resp->data[3])) {
size++;
} else {
rc = -EIO;
goto exit;
}
} else if (ddev->did) {
rc = -EIO;
goto exit;
}
if (DIGITAL_NFC_DEP_NAD_BIT_SET(pfb)) {
rc = -EIO;
goto exit;
}
if (size > resp->len) {
rc = -EIO;
goto exit;
}
skb_pull(resp, size);
switch (DIGITAL_NFC_DEP_PFB_TYPE(pfb)) {
case DIGITAL_NFC_DEP_PFB_I_PDU:
pr_debug("DIGITAL_NFC_DEP_PFB_I_PDU\n");
if ((ddev->atn_count && (DIGITAL_NFC_DEP_PFB_PNI(pfb - 1) !=
ddev->curr_nfc_dep_pni)) ||
(DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni)) {
PROTOCOL_ERR("14.12.3.4");
rc = -EIO;
goto exit;
}
if (ddev->atn_count) {
ddev->atn_count = 0;
rc = digital_tg_send_saved_skb(ddev);
if (rc)
goto exit;
return;
}
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
resp = digital_recv_dep_data_gather(ddev, pfb, resp,
digital_tg_send_ack, NULL);
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto exit;
}
/* If resp is NULL then we're still chaining so return and
* wait for the next part of the PDU. Else, the PDU is
* complete so pass it up.
*/
if (!resp)
return;
rc = 0;
break;
case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
if (!DIGITAL_NFC_DEP_NACK_BIT_SET(pfb)) { /* ACK */
if ((ddev->atn_count &&
(DIGITAL_NFC_DEP_PFB_PNI(pfb - 1) !=
ddev->curr_nfc_dep_pni)) ||
(DIGITAL_NFC_DEP_PFB_PNI(pfb) !=
ddev->curr_nfc_dep_pni) ||
!ddev->chaining_skb || !ddev->saved_skb) {
rc = -EIO;
goto exit;
}
if (ddev->atn_count) {
ddev->atn_count = 0;
rc = digital_tg_send_saved_skb(ddev);
if (rc)
goto exit;
return;
}
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
rc = digital_tg_send_dep_res(ddev, ddev->chaining_skb);
if (rc)
goto exit;
} else { /* NACK */
if ((DIGITAL_NFC_DEP_PFB_PNI(pfb + 1) !=
ddev->curr_nfc_dep_pni) ||
!ddev->saved_skb) {
rc = -EIO;
goto exit;
}
ddev->atn_count = 0;
rc = digital_tg_send_saved_skb(ddev);
if (rc) {
kfree_skb(ddev->saved_skb);
goto exit;
}
}
return;
case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
if (DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) {
rc = -EINVAL;
goto exit;
}
rc = digital_tg_send_atn(ddev);
if (rc)
goto exit;
ddev->atn_count++;
kfree_skb(resp);
return;
}
rc = nfc_tm_data_received(ddev->nfc_dev, resp);
exit:
kfree_skb(ddev->chaining_skb);
ddev->chaining_skb = NULL;
ddev->atn_count = 0;
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
if (rc)
kfree_skb(resp);
}
int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb)
{
struct digital_dep_req_res *dep_res;
struct sk_buff *chaining_skb, *tmp_skb;
int rc;
skb_push(skb, sizeof(struct digital_dep_req_res));
dep_res = (struct digital_dep_req_res *)skb->data;
dep_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
dep_res->cmd = DIGITAL_CMD_DEP_RES;
dep_res->pfb = ddev->curr_nfc_dep_pni;
if (ddev->did) {
dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT;
memcpy(skb_put(skb, sizeof(ddev->did)), &ddev->did,
sizeof(ddev->did));
}
ddev->curr_nfc_dep_pni =
DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
chaining_skb = ddev->chaining_skb;
tmp_skb = digital_send_dep_data_prep(ddev, skb, dep_res, NULL);
if (IS_ERR(tmp_skb))
return PTR_ERR(tmp_skb);
digital_skb_push_dep_sod(ddev, tmp_skb);
ddev->skb_add_crc(tmp_skb);
ddev->saved_skb = skb_get(tmp_skb);
ddev->saved_skb_len = tmp_skb->len;
rc = digital_tg_send_cmd(ddev, tmp_skb, 1500, digital_tg_recv_dep_req,
NULL);
if (rc) {
if (tmp_skb != skb)
kfree_skb(tmp_skb);
kfree_skb(chaining_skb);
ddev->chaining_skb = NULL;
kfree_skb(ddev->saved_skb);
ddev->saved_skb = NULL;
}
return rc;
}
static void digital_tg_send_psl_res_complete(struct nfc_digital_dev *ddev,
void *arg, struct sk_buff *resp)
{
u8 rf_tech = (unsigned long)arg;
if (IS_ERR(resp))
return;
digital_tg_set_rf_tech(ddev, rf_tech);
digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
digital_tg_listen(ddev, 1500, digital_tg_recv_dep_req, NULL);
dev_kfree_skb(resp);
}
static int digital_tg_send_psl_res(struct nfc_digital_dev *ddev, u8 did,
u8 rf_tech)
{
struct digital_psl_res *psl_res;
struct sk_buff *skb;
int rc;
skb = digital_skb_alloc(ddev, sizeof(struct digital_psl_res));
if (!skb)
return -ENOMEM;
skb_put(skb, sizeof(struct digital_psl_res));
psl_res = (struct digital_psl_res *)skb->data;
psl_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
psl_res->cmd = DIGITAL_CMD_PSL_RES;
psl_res->did = did;
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
ddev->curr_nfc_dep_pni = 0;
rc = digital_tg_send_cmd(ddev, skb, 0, digital_tg_send_psl_res_complete,
(void *)(unsigned long)rf_tech);
if (rc)
kfree_skb(skb);
return rc;
}
static void digital_tg_recv_psl_req(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp)
{
int rc;
struct digital_psl_req *psl_req;
u8 rf_tech;
u8 dsi, payload_size, payload_bits;
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto exit;
}
rc = ddev->skb_check_crc(resp);
if (rc) {
PROTOCOL_ERR("14.4.1.6");
goto exit;
}
rc = digital_skb_pull_dep_sod(ddev, resp);
if (rc) {
PROTOCOL_ERR("14.4.1.2");
goto exit;
}
psl_req = (struct digital_psl_req *)resp->data;
if (resp->len != sizeof(struct digital_psl_req) ||
psl_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
psl_req->cmd != DIGITAL_CMD_PSL_REQ) {
rc = -EIO;
goto exit;
}
dsi = (psl_req->brs >> 3) & 0x07;
switch (dsi) {
case 0:
rf_tech = NFC_DIGITAL_RF_TECH_106A;
break;
case 1:
rf_tech = NFC_DIGITAL_RF_TECH_212F;
break;
case 2:
rf_tech = NFC_DIGITAL_RF_TECH_424F;
break;
default:
pr_err("Unsupported dsi value %d\n", dsi);
goto exit;
}
payload_bits = DIGITAL_PAYLOAD_FSL_TO_BITS(psl_req->fsl);
payload_size = digital_payload_bits_to_size(payload_bits);
if (!payload_size || (payload_size > min(ddev->local_payload_max,
ddev->remote_payload_max))) {
rc = -EINVAL;
goto exit;
}
ddev->local_payload_max = payload_size;
ddev->remote_payload_max = payload_size;
rc = digital_tg_send_psl_res(ddev, psl_req->did, rf_tech);
exit:
kfree_skb(resp);
}
static void digital_tg_send_atr_res_complete(struct nfc_digital_dev *ddev,
void *arg, struct sk_buff *resp)
{
int offset;
if (IS_ERR(resp)) {
digital_poll_next_tech(ddev);
return;
}
offset = 2;
if (resp->data[0] == DIGITAL_NFC_DEP_NFCA_SOD_SB)
offset++;
ddev->atn_count = 0;
if (resp->data[offset] == DIGITAL_CMD_PSL_REQ)
digital_tg_recv_psl_req(ddev, arg, resp);
else
digital_tg_recv_dep_req(ddev, arg, resp);
}
static int digital_tg_send_atr_res(struct nfc_digital_dev *ddev,
struct digital_atr_req *atr_req)
{
struct digital_atr_res *atr_res;
struct sk_buff *skb;
u8 *gb, payload_bits;
size_t gb_len;
int rc;
gb = nfc_get_local_general_bytes(ddev->nfc_dev, &gb_len);
if (!gb)
gb_len = 0;
skb = digital_skb_alloc(ddev, sizeof(struct digital_atr_res) + gb_len);
if (!skb)
return -ENOMEM;
skb_put(skb, sizeof(struct digital_atr_res));
atr_res = (struct digital_atr_res *)skb->data;
memset(atr_res, 0, sizeof(struct digital_atr_res));
atr_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
atr_res->cmd = DIGITAL_CMD_ATR_RES;
memcpy(atr_res->nfcid3, atr_req->nfcid3, sizeof(atr_req->nfcid3));
atr_res->to = 8;
ddev->local_payload_max = DIGITAL_PAYLOAD_SIZE_MAX;
payload_bits = digital_payload_size_to_bits(ddev->local_payload_max);
atr_res->pp = DIGITAL_PAYLOAD_BITS_TO_PP(payload_bits);
if (gb_len) {
skb_put(skb, gb_len);
atr_res->pp |= DIGITAL_GB_BIT;
memcpy(atr_res->gb, gb, gb_len);
}
digital_skb_push_dep_sod(ddev, skb);
ddev->skb_add_crc(skb);
ddev->curr_nfc_dep_pni = 0;
rc = digital_tg_send_cmd(ddev, skb, 999,
digital_tg_send_atr_res_complete, NULL);
if (rc)
kfree_skb(skb);
return rc;
}
void digital_tg_recv_atr_req(struct nfc_digital_dev *ddev, void *arg,
struct sk_buff *resp)
{
int rc;
struct digital_atr_req *atr_req;
size_t gb_len, min_size;
u8 poll_tech_count, payload_bits;
if (IS_ERR(resp)) {
rc = PTR_ERR(resp);
resp = NULL;
goto exit;
}
if (!resp->len) {
rc = -EIO;
goto exit;
}
if (resp->data[0] == DIGITAL_NFC_DEP_NFCA_SOD_SB) {
min_size = DIGITAL_ATR_REQ_MIN_SIZE + 2;
digital_tg_set_rf_tech(ddev, NFC_DIGITAL_RF_TECH_106A);
} else {
min_size = DIGITAL_ATR_REQ_MIN_SIZE + 1;
digital_tg_set_rf_tech(ddev, NFC_DIGITAL_RF_TECH_212F);
}
if (resp->len < min_size) {
rc = -EIO;
goto exit;
}
ddev->curr_protocol = NFC_PROTO_NFC_DEP_MASK;
rc = ddev->skb_check_crc(resp);
if (rc) {
PROTOCOL_ERR("14.4.1.6");
goto exit;
}
rc = digital_skb_pull_dep_sod(ddev, resp);
if (rc) {
PROTOCOL_ERR("14.4.1.2");
goto exit;
}
atr_req = (struct digital_atr_req *)resp->data;
if (atr_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
atr_req->cmd != DIGITAL_CMD_ATR_REQ ||
atr_req->did > DIGITAL_DID_MAX) {
rc = -EINVAL;
goto exit;
}
payload_bits = DIGITAL_PAYLOAD_PP_TO_BITS(atr_req->pp);
ddev->remote_payload_max = digital_payload_bits_to_size(payload_bits);
if (!ddev->remote_payload_max) {
rc = -EINVAL;
goto exit;
}
ddev->did = atr_req->did;
rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED);
if (rc)
goto exit;
rc = digital_tg_send_atr_res(ddev, atr_req);
if (rc)
goto exit;
gb_len = resp->len - sizeof(struct digital_atr_req);
poll_tech_count = ddev->poll_tech_count;
ddev->poll_tech_count = 0;
rc = nfc_tm_activated(ddev->nfc_dev, NFC_PROTO_NFC_DEP_MASK,
NFC_COMM_PASSIVE, atr_req->gb, gb_len);
if (rc) {
ddev->poll_tech_count = poll_tech_count;
goto exit;
}
rc = 0;
exit:
if (rc)
digital_poll_next_tech(ddev);
dev_kfree_skb(resp);
}
Reference in New Issue View Git Blame Copy Permalink