forked from Minki/linux
NFC Digital: Add target NFC-DEP support
This adds support for NFC-DEP target mode for NFC-A and NFC-F technologies. If the driver provides it, the stack uses an automatic mode for technology detection and automatic anti-collision. Otherwise the stack tries to use non-automatic synchronization and listens for SENS_REQ and SENSF_REQ commands. The detection, activation, and data exchange procedures work exactly the same way as in initiator mode, as described in the previous commits, except that the digital stack waits for commands and sends responses back to the peer device. Signed-off-by: Thierry Escande <thierry.escande@linux.intel.com> Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>
This commit is contained in:
Thierry Escande
Samuel Ortiz
parent
7d0911c02f
commit
1c7a4c24fb
@ -56,8 +56,9 @@ struct sk_buff *digital_skb_alloc(struct nfc_digital_dev *ddev,
|
||||
unsigned int len);
|
||||
|
||||
int digital_send_cmd(struct nfc_digital_dev *ddev, u8 cmd_type,
|
||||
struct sk_buff *skb, u16 timeout,
|
||||
nfc_digital_cmd_complete_t cmd_cb, void *cb_context);
|
||||
struct sk_buff *skb, struct digital_tg_mdaa_params *params,
|
||||
u16 timeout, nfc_digital_cmd_complete_t cmd_cb,
|
||||
void *cb_context);
|
||||
|
||||
int digital_in_configure_hw(struct nfc_digital_dev *ddev, int type, int param);
|
||||
static inline int digital_in_send_cmd(struct nfc_digital_dev *ddev,
|
||||
@ -65,8 +66,8 @@ static inline int digital_in_send_cmd(struct nfc_digital_dev *ddev,
|
||||
nfc_digital_cmd_complete_t cmd_cb,
|
||||
void *cb_context)
|
||||
{
|
||||
return digital_send_cmd(ddev, DIGITAL_CMD_IN_SEND, skb, timeout, cmd_cb,
|
||||
cb_context);
|
||||
return digital_send_cmd(ddev, DIGITAL_CMD_IN_SEND, skb, NULL, timeout,
|
||||
cmd_cb, cb_context);
|
||||
}
|
||||
|
||||
void digital_poll_next_tech(struct nfc_digital_dev *ddev);
|
||||
@ -86,6 +87,36 @@ 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);
|
||||
|
||||
int digital_tg_configure_hw(struct nfc_digital_dev *ddev, int type, int param);
|
||||
static inline int digital_tg_send_cmd(struct nfc_digital_dev *ddev,
|
||||
struct sk_buff *skb, u16 timeout,
|
||||
nfc_digital_cmd_complete_t cmd_cb, void *cb_context)
|
||||
{
|
||||
return digital_send_cmd(ddev, DIGITAL_CMD_TG_SEND, skb, NULL, timeout,
|
||||
cmd_cb, cb_context);
|
||||
}
|
||||
|
||||
void digital_tg_recv_sens_req(struct nfc_digital_dev *ddev, void *arg,
|
||||
struct sk_buff *resp);
|
||||
|
||||
void digital_tg_recv_sensf_req(struct nfc_digital_dev *ddev, void *arg,
|
||||
struct sk_buff *resp);
|
||||
|
||||
static inline int digital_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
|
||||
nfc_digital_cmd_complete_t cb, void *arg)
|
||||
{
|
||||
return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN, NULL, NULL,
|
||||
timeout, cb, arg);
|
||||
}
|
||||
|
||||
void digital_tg_recv_atr_req(struct nfc_digital_dev *ddev, void *arg,
|
||||
struct sk_buff *resp);
|
||||
|
||||
int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb);
|
||||
|
||||
int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech);
|
||||
int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech);
|
||||
|
||||
typedef u16 (*crc_func_t)(u16, const u8 *, size_t);
|
||||
|
||||
#define CRC_A_INIT 0x6363
|
||||
|
||||
@ -32,6 +32,7 @@ struct digital_cmd {
|
||||
u16 timeout;
|
||||
struct sk_buff *req;
|
||||
struct sk_buff *resp;
|
||||
struct digital_tg_mdaa_params *mdaa_params;
|
||||
|
||||
nfc_digital_cmd_complete_t cmd_cb;
|
||||
void *cb_context;
|
||||
@ -131,6 +132,7 @@ static void digital_wq_cmd_complete(struct work_struct *work)
|
||||
|
||||
cmd->cmd_cb(ddev, cmd->cb_context, cmd->resp);
|
||||
|
||||
kfree(cmd->mdaa_params);
|
||||
kfree(cmd);
|
||||
|
||||
schedule_work(&ddev->cmd_work);
|
||||
@ -150,6 +152,7 @@ static void digital_wq_cmd(struct work_struct *work)
|
||||
{
|
||||
int rc;
|
||||
struct digital_cmd *cmd;
|
||||
struct digital_tg_mdaa_params *params;
|
||||
struct nfc_digital_dev *ddev = container_of(work,
|
||||
struct nfc_digital_dev,
|
||||
cmd_work);
|
||||
@ -174,6 +177,24 @@ static void digital_wq_cmd(struct work_struct *work)
|
||||
rc = ddev->ops->in_send_cmd(ddev, cmd->req, cmd->timeout,
|
||||
digital_send_cmd_complete, cmd);
|
||||
break;
|
||||
|
||||
case DIGITAL_CMD_TG_SEND:
|
||||
rc = ddev->ops->tg_send_cmd(ddev, cmd->req, cmd->timeout,
|
||||
digital_send_cmd_complete, cmd);
|
||||
break;
|
||||
|
||||
case DIGITAL_CMD_TG_LISTEN:
|
||||
rc = ddev->ops->tg_listen(ddev, cmd->timeout,
|
||||
digital_send_cmd_complete, cmd);
|
||||
break;
|
||||
|
||||
case DIGITAL_CMD_TG_LISTEN_MDAA:
|
||||
params = cmd->mdaa_params;
|
||||
|
||||
rc = ddev->ops->tg_listen_mdaa(ddev, params, cmd->timeout,
|
||||
digital_send_cmd_complete, cmd);
|
||||
break;
|
||||
|
||||
default:
|
||||
PR_ERR("Unknown cmd type %d", cmd->type);
|
||||
return;
|
||||
@ -189,14 +210,16 @@ static void digital_wq_cmd(struct work_struct *work)
|
||||
mutex_unlock(&ddev->cmd_lock);
|
||||
|
||||
kfree_skb(cmd->req);
|
||||
kfree(cmd->mdaa_params);
|
||||
kfree(cmd);
|
||||
|
||||
schedule_work(&ddev->cmd_work);
|
||||
}
|
||||
|
||||
int digital_send_cmd(struct nfc_digital_dev *ddev, u8 cmd_type,
|
||||
struct sk_buff *skb, u16 timeout,
|
||||
nfc_digital_cmd_complete_t cmd_cb, void *cb_context)
|
||||
struct sk_buff *skb, struct digital_tg_mdaa_params *params,
|
||||
u16 timeout, nfc_digital_cmd_complete_t cmd_cb,
|
||||
void *cb_context)
|
||||
{
|
||||
struct digital_cmd *cmd;
|
||||
|
||||
@ -207,6 +230,7 @@ int digital_send_cmd(struct nfc_digital_dev *ddev, u8 cmd_type,
|
||||
cmd->type = cmd_type;
|
||||
cmd->timeout = timeout;
|
||||
cmd->req = skb;
|
||||
cmd->mdaa_params = params;
|
||||
cmd->cmd_cb = cmd_cb;
|
||||
cmd->cb_context = cb_context;
|
||||
INIT_LIST_HEAD(&cmd->queue);
|
||||
@ -231,6 +255,38 @@ int digital_in_configure_hw(struct nfc_digital_dev *ddev, int type, int param)
|
||||
return rc;
|
||||
}
|
||||
|
||||
int digital_tg_configure_hw(struct nfc_digital_dev *ddev, int type, int param)
|
||||
{
|
||||
int rc;
|
||||
|
||||
rc = ddev->ops->tg_configure_hw(ddev, type, param);
|
||||
if (rc)
|
||||
PR_ERR("tg_configure_hw failed: %d", rc);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
static int digital_tg_listen_mdaa(struct nfc_digital_dev *ddev, u8 rf_tech)
|
||||
{
|
||||
struct digital_tg_mdaa_params *params;
|
||||
|
||||
params = kzalloc(sizeof(struct digital_tg_mdaa_params), GFP_KERNEL);
|
||||
if (!params)
|
||||
return -ENOMEM;
|
||||
|
||||
params->sens_res = DIGITAL_SENS_RES_NFC_DEP;
|
||||
get_random_bytes(params->nfcid1, sizeof(params->nfcid1));
|
||||
params->sel_res = DIGITAL_SEL_RES_NFC_DEP;
|
||||
|
||||
params->nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1;
|
||||
params->nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2;
|
||||
get_random_bytes(params->nfcid2 + 2, NFC_NFCID2_MAXSIZE - 2);
|
||||
params->sc = DIGITAL_SENSF_FELICA_SC;
|
||||
|
||||
return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN_MDAA, NULL, params,
|
||||
500, digital_tg_recv_atr_req, NULL);
|
||||
}
|
||||
|
||||
int digital_target_found(struct nfc_digital_dev *ddev,
|
||||
struct nfc_target *target, u8 protocol)
|
||||
{
|
||||
@ -412,6 +468,22 @@ static int digital_start_poll(struct nfc_dev *nfc_dev, __u32 im_protocols,
|
||||
digital_in_send_sensf_req);
|
||||
}
|
||||
|
||||
if (tm_protocols & NFC_PROTO_NFC_DEP_MASK) {
|
||||
if (ddev->ops->tg_listen_mdaa) {
|
||||
digital_add_poll_tech(ddev, 0,
|
||||
digital_tg_listen_mdaa);
|
||||
} else {
|
||||
digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106A,
|
||||
digital_tg_listen_nfca);
|
||||
|
||||
digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_212F,
|
||||
digital_tg_listen_nfcf);
|
||||
|
||||
digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_424F,
|
||||
digital_tg_listen_nfcf);
|
||||
}
|
||||
}
|
||||
|
||||
if (!ddev->poll_tech_count) {
|
||||
PR_ERR("Unsupported protocols: im=0x%x, tm=0x%x",
|
||||
matching_im_protocols, matching_tm_protocols);
|
||||
@ -496,7 +568,9 @@ static void digital_deactivate_target(struct nfc_dev *nfc_dev,
|
||||
|
||||
static int digital_tg_send(struct nfc_dev *dev, struct sk_buff *skb)
|
||||
{
|
||||
return -EOPNOTSUPP;
|
||||
struct nfc_digital_dev *ddev = nfc_get_drvdata(dev);
|
||||
|
||||
return digital_tg_send_dep_res(ddev, skb);
|
||||
}
|
||||
|
||||
static void digital_in_send_complete(struct nfc_digital_dev *ddev, void *arg,
|
||||
@ -654,6 +728,7 @@ void nfc_digital_unregister_device(struct nfc_digital_dev *ddev)
|
||||
|
||||
list_for_each_entry_safe(cmd, n, &ddev->cmd_queue, queue) {
|
||||
list_del(&cmd->queue);
|
||||
kfree(cmd->mdaa_params);
|
||||
kfree(cmd);
|
||||
}
|
||||
}
|
||||
|
||||
@ -379,3 +379,349 @@ int digital_in_send_dep_req(struct nfc_digital_dev *ddev,
|
||||
return digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
|
||||
data_exch);
|
||||
}
|
||||
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
if (DIGITAL_NFC_DEP_DID_BIT_SET(dep_req->pfb))
|
||||
size++;
|
||||
|
||||
if (resp->len < size) {
|
||||
rc = -EIO;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
switch (DIGITAL_NFC_DEP_PFB_TYPE(dep_req->pfb)) {
|
||||
case DIGITAL_NFC_DEP_PFB_I_PDU:
|
||||
PR_DBG("DIGITAL_NFC_DEP_PFB_I_PDU");
|
||||
ddev->curr_nfc_dep_pni = DIGITAL_NFC_DEP_PFB_PNI(dep_req->pfb);
|
||||
break;
|
||||
case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
|
||||
PR_ERR("Received a ACK/NACK PDU");
|
||||
rc = -EINVAL;
|
||||
goto exit;
|
||||
break;
|
||||
case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
|
||||
PR_ERR("Received a SUPERVISOR PDU");
|
||||
rc = -EINVAL;
|
||||
goto exit;
|
||||
break;
|
||||
}
|
||||
|
||||
skb_pull(resp, size);
|
||||
|
||||
rc = nfc_tm_data_received(ddev->nfc_dev, resp);
|
||||
|
||||
exit:
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
digital_skb_push_dep_sod(ddev, skb);
|
||||
|
||||
ddev->skb_add_crc(skb);
|
||||
|
||||
return digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req,
|
||||
NULL);
|
||||
}
|
||||
|
||||
static void digital_tg_send_psl_res_complete(struct nfc_digital_dev *ddev,
|
||||
void *arg, struct sk_buff *resp)
|
||||
{
|
||||
u8 rf_tech = PTR_ERR(arg);
|
||||
|
||||
if (IS_ERR(resp))
|
||||
return;
|
||||
|
||||
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);
|
||||
|
||||
rc = digital_tg_send_cmd(ddev, skb, 0, digital_tg_send_psl_res_complete,
|
||||
ERR_PTR(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;
|
||||
|
||||
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("Unsuported dsi value %d", dsi);
|
||||
goto exit;
|
||||
}
|
||||
|
||||
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++;
|
||||
|
||||
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;
|
||||
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;
|
||||
atr_res->pp = DIGITAL_LR_BITS_PAYLOAD_SIZE_254B;
|
||||
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);
|
||||
|
||||
rc = digital_tg_send_cmd(ddev, skb, 999,
|
||||
digital_tg_send_atr_res_complete, NULL);
|
||||
if (rc) {
|
||||
kfree_skb(skb);
|
||||
return rc;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
ddev->curr_rf_tech = NFC_DIGITAL_RF_TECH_106A;
|
||||
ddev->skb_add_crc = digital_skb_add_crc_a;
|
||||
ddev->skb_check_crc = digital_skb_check_crc_a;
|
||||
} else {
|
||||
min_size = DIGITAL_ATR_REQ_MIN_SIZE + 1;
|
||||
|
||||
ddev->curr_rf_tech = NFC_DIGITAL_RF_TECH_212F;
|
||||
ddev->skb_add_crc = digital_skb_add_crc_f;
|
||||
ddev->skb_check_crc = digital_skb_check_crc_f;
|
||||
}
|
||||
|
||||
if (resp->len < min_size) {
|
||||
rc = -EIO;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
if (DIGITAL_DRV_CAPS_TG_CRC(ddev)) {
|
||||
ddev->skb_add_crc = digital_skb_add_crc_none;
|
||||
ddev->skb_check_crc = digital_skb_check_crc_none;
|
||||
}
|
||||
|
||||
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) {
|
||||
rc = -EINVAL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
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);
|
||||
rc = nfc_tm_activated(ddev->nfc_dev, NFC_PROTO_NFC_DEP_MASK,
|
||||
NFC_COMM_PASSIVE, atr_req->gb, gb_len);
|
||||
if (rc)
|
||||
goto exit;
|
||||
|
||||
ddev->poll_tech_count = 0;
|
||||
|
||||
rc = 0;
|
||||
exit:
|
||||
if (rc)
|
||||
digital_poll_next_tech(ddev);
|
||||
|
||||
dev_kfree_skb(resp);
|
||||
}
|
||||
|
||||
@ -475,3 +475,299 @@ int digital_in_send_sensf_req(struct nfc_digital_dev *ddev, u8 rf_tech)
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
static int digital_tg_send_sel_res(struct nfc_digital_dev *ddev)
|
||||
{
|
||||
struct sk_buff *skb;
|
||||
int rc;
|
||||
|
||||
skb = digital_skb_alloc(ddev, 1);
|
||||
if (!skb)
|
||||
return -ENOMEM;
|
||||
|
||||
*skb_put(skb, 1) = DIGITAL_SEL_RES_NFC_DEP;
|
||||
|
||||
if (!DIGITAL_DRV_CAPS_TG_CRC(ddev))
|
||||
digital_skb_add_crc_a(skb);
|
||||
|
||||
rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_atr_req,
|
||||
NULL);
|
||||
if (rc)
|
||||
kfree_skb(skb);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
static void digital_tg_recv_sel_req(struct nfc_digital_dev *ddev, void *arg,
|
||||
struct sk_buff *resp)
|
||||
{
|
||||
int rc;
|
||||
|
||||
if (IS_ERR(resp)) {
|
||||
rc = PTR_ERR(resp);
|
||||
resp = NULL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
if (!DIGITAL_DRV_CAPS_TG_CRC(ddev)) {
|
||||
rc = digital_skb_check_crc_a(resp);
|
||||
if (rc) {
|
||||
PROTOCOL_ERR("4.4.1.3");
|
||||
goto exit;
|
||||
}
|
||||
}
|
||||
|
||||
/* Silently ignore SEL_REQ content and send a SEL_RES for NFC-DEP */
|
||||
|
||||
rc = digital_tg_send_sel_res(ddev);
|
||||
|
||||
exit:
|
||||
if (rc)
|
||||
digital_poll_next_tech(ddev);
|
||||
|
||||
dev_kfree_skb(resp);
|
||||
}
|
||||
|
||||
static int digital_tg_send_sdd_res(struct nfc_digital_dev *ddev)
|
||||
{
|
||||
struct sk_buff *skb;
|
||||
struct digital_sdd_res *sdd_res;
|
||||
int rc, i;
|
||||
|
||||
skb = digital_skb_alloc(ddev, sizeof(struct digital_sdd_res));
|
||||
if (!skb)
|
||||
return -ENOMEM;
|
||||
|
||||
skb_put(skb, sizeof(struct digital_sdd_res));
|
||||
sdd_res = (struct digital_sdd_res *)skb->data;
|
||||
|
||||
sdd_res->nfcid1[0] = 0x08;
|
||||
get_random_bytes(sdd_res->nfcid1 + 1, 3);
|
||||
|
||||
sdd_res->bcc = 0;
|
||||
for (i = 0; i < 4; i++)
|
||||
sdd_res->bcc ^= sdd_res->nfcid1[i];
|
||||
|
||||
rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_sel_req,
|
||||
NULL);
|
||||
if (rc)
|
||||
kfree_skb(skb);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
static void digital_tg_recv_sdd_req(struct nfc_digital_dev *ddev, void *arg,
|
||||
struct sk_buff *resp)
|
||||
{
|
||||
u8 *sdd_req;
|
||||
int rc;
|
||||
|
||||
if (IS_ERR(resp)) {
|
||||
rc = PTR_ERR(resp);
|
||||
resp = NULL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
sdd_req = resp->data;
|
||||
|
||||
if (resp->len < 2 || sdd_req[0] != DIGITAL_CMD_SEL_REQ_CL1 ||
|
||||
sdd_req[1] != DIGITAL_SDD_REQ_SEL_PAR) {
|
||||
rc = -EINVAL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
rc = digital_tg_send_sdd_res(ddev);
|
||||
|
||||
exit:
|
||||
if (rc)
|
||||
digital_poll_next_tech(ddev);
|
||||
|
||||
dev_kfree_skb(resp);
|
||||
}
|
||||
|
||||
static int digital_tg_send_sens_res(struct nfc_digital_dev *ddev)
|
||||
{
|
||||
struct sk_buff *skb;
|
||||
u8 *sens_res;
|
||||
int rc;
|
||||
|
||||
skb = digital_skb_alloc(ddev, 2);
|
||||
if (!skb)
|
||||
return -ENOMEM;
|
||||
|
||||
sens_res = skb_put(skb, 2);
|
||||
|
||||
sens_res[0] = (DIGITAL_SENS_RES_NFC_DEP >> 8) & 0xFF;
|
||||
sens_res[1] = DIGITAL_SENS_RES_NFC_DEP & 0xFF;
|
||||
|
||||
rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_sdd_req,
|
||||
NULL);
|
||||
if (rc)
|
||||
kfree_skb(skb);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
void digital_tg_recv_sens_req(struct nfc_digital_dev *ddev, void *arg,
|
||||
struct sk_buff *resp)
|
||||
{
|
||||
u8 sens_req;
|
||||
int rc;
|
||||
|
||||
if (IS_ERR(resp)) {
|
||||
rc = PTR_ERR(resp);
|
||||
resp = NULL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
sens_req = resp->data[0];
|
||||
|
||||
if (!resp->len || (sens_req != DIGITAL_CMD_SENS_REQ &&
|
||||
sens_req != DIGITAL_CMD_ALL_REQ)) {
|
||||
rc = -EINVAL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
rc = digital_tg_send_sens_res(ddev);
|
||||
|
||||
exit:
|
||||
if (rc)
|
||||
digital_poll_next_tech(ddev);
|
||||
|
||||
dev_kfree_skb(resp);
|
||||
}
|
||||
|
||||
int digital_tg_send_sensf_res(struct nfc_digital_dev *ddev,
|
||||
struct digital_sensf_req *sensf_req)
|
||||
{
|
||||
struct sk_buff *skb;
|
||||
u8 size;
|
||||
int rc;
|
||||
struct digital_sensf_res *sensf_res;
|
||||
|
||||
size = sizeof(struct digital_sensf_res);
|
||||
|
||||
if (sensf_req->rc != DIGITAL_SENSF_REQ_RC_NONE)
|
||||
size -= sizeof(sensf_res->rd);
|
||||
|
||||
skb = digital_skb_alloc(ddev, size);
|
||||
if (!skb)
|
||||
return -ENOMEM;
|
||||
|
||||
skb_put(skb, size);
|
||||
|
||||
sensf_res = (struct digital_sensf_res *)skb->data;
|
||||
|
||||
memset(sensf_res, 0, size);
|
||||
|
||||
sensf_res->cmd = DIGITAL_CMD_SENSF_RES;
|
||||
sensf_res->nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1;
|
||||
sensf_res->nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2;
|
||||
get_random_bytes(&sensf_res->nfcid2[2], 6);
|
||||
|
||||
switch (sensf_req->rc) {
|
||||
case DIGITAL_SENSF_REQ_RC_SC:
|
||||
sensf_res->rd[0] = sensf_req->sc1;
|
||||
sensf_res->rd[1] = sensf_req->sc2;
|
||||
break;
|
||||
case DIGITAL_SENSF_REQ_RC_AP:
|
||||
sensf_res->rd[0] = DIGITAL_SENSF_RES_RD_AP_B1;
|
||||
sensf_res->rd[1] = DIGITAL_SENSF_RES_RD_AP_B2;
|
||||
break;
|
||||
}
|
||||
|
||||
*skb_push(skb, sizeof(u8)) = size + 1;
|
||||
|
||||
if (!DIGITAL_DRV_CAPS_TG_CRC(ddev))
|
||||
digital_skb_add_crc_f(skb);
|
||||
|
||||
rc = digital_tg_send_cmd(ddev, skb, 300,
|
||||
digital_tg_recv_atr_req, NULL);
|
||||
if (rc)
|
||||
kfree_skb(skb);
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
void digital_tg_recv_sensf_req(struct nfc_digital_dev *ddev, void *arg,
|
||||
struct sk_buff *resp)
|
||||
{
|
||||
struct digital_sensf_req *sensf_req;
|
||||
int rc;
|
||||
|
||||
if (IS_ERR(resp)) {
|
||||
rc = PTR_ERR(resp);
|
||||
resp = NULL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
if (!DIGITAL_DRV_CAPS_TG_CRC(ddev)) {
|
||||
rc = digital_skb_check_crc_f(resp);
|
||||
if (rc) {
|
||||
PROTOCOL_ERR("6.4.1.8");
|
||||
goto exit;
|
||||
}
|
||||
}
|
||||
|
||||
if (resp->len != sizeof(struct digital_sensf_req) + 1) {
|
||||
rc = -EINVAL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
skb_pull(resp, 1);
|
||||
sensf_req = (struct digital_sensf_req *)resp->data;
|
||||
|
||||
if (sensf_req->cmd != DIGITAL_CMD_SENSF_REQ) {
|
||||
rc = -EINVAL;
|
||||
goto exit;
|
||||
}
|
||||
|
||||
rc = digital_tg_send_sensf_res(ddev, sensf_req);
|
||||
|
||||
exit:
|
||||
if (rc)
|
||||
digital_poll_next_tech(ddev);
|
||||
|
||||
dev_kfree_skb(resp);
|
||||
}
|
||||
|
||||
int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech)
|
||||
{
|
||||
int rc;
|
||||
|
||||
rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
|
||||
if (rc)
|
||||
return rc;
|
||||
|
||||
rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
|
||||
NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
|
||||
if (rc)
|
||||
return rc;
|
||||
|
||||
return digital_tg_listen(ddev, 300, digital_tg_recv_sens_req, NULL);
|
||||
}
|
||||
|
||||
int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech)
|
||||
{
|
||||
int rc;
|
||||
u8 *nfcid2;
|
||||
|
||||
rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
|
||||
if (rc)
|
||||
return rc;
|
||||
|
||||
rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
|
||||
NFC_DIGITAL_FRAMING_NFCF_NFC_DEP);
|
||||
if (rc)
|
||||
return rc;
|
||||
|
||||
nfcid2 = kzalloc(NFC_NFCID2_MAXSIZE, GFP_KERNEL);
|
||||
if (!nfcid2)
|
||||
return -ENOMEM;
|
||||
|
||||
nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1;
|
||||
nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2;
|
||||
get_random_bytes(nfcid2 + 2, NFC_NFCID2_MAXSIZE - 2);
|
||||
|
||||
return digital_tg_listen(ddev, 300, digital_tg_recv_sensf_req, nfcid2);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user