diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c index 9912824030f61e78b386c621221eacbbee3539a2..aa7803f16773f91796d95aa13bfa3cfed69fc345 100644 --- a/drivers/nfc/pn533.c +++ b/drivers/nfc/pn533.c @@ -298,14 +298,6 @@ struct pn533_cmd_activate_response { u8 gt[]; } __packed; -/* PN533_CMD_IN_JUMP_FOR_DEP */ -struct pn533_cmd_jump_dep { - u8 active; - u8 baud; - u8 next; - u8 data[]; -} __packed; - struct pn533_cmd_jump_dep_response { u8 status; u8 tg; @@ -1861,53 +1853,46 @@ static void pn533_deactivate_target(struct nfc_dev *nfc_dev, static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg, - u8 *params, int params_len) + struct sk_buff *resp) { - struct pn533_cmd_jump_dep_response *resp; - struct nfc_target nfc_target; + struct pn533_cmd_jump_dep_response *rsp; u8 target_gt_len; int rc; - struct pn533_cmd_jump_dep *cmd = (struct pn533_cmd_jump_dep *)arg; - u8 active = cmd->active; + u8 active = *(u8 *)arg; kfree(arg); - if (params_len == -ENOENT) { - nfc_dev_dbg(&dev->interface->dev, ""); - return 0; - } - - if (params_len < 0) { - nfc_dev_err(&dev->interface->dev, - "Error %d when bringing DEP link up", - params_len); - return 0; - } + if (IS_ERR(resp)) + return PTR_ERR(resp); if (dev->tgt_available_prots && !(dev->tgt_available_prots & (1 << NFC_PROTO_NFC_DEP))) { nfc_dev_err(&dev->interface->dev, "The target does not support DEP"); - return -EINVAL; + rc = -EINVAL; + goto error; } - resp = (struct pn533_cmd_jump_dep_response *) params; - rc = resp->status & PN533_CMD_RET_MASK; + rsp = (struct pn533_cmd_jump_dep_response *)resp->data; + + rc = rsp->status & PN533_CMD_RET_MASK; if (rc != PN533_CMD_RET_SUCCESS) { nfc_dev_err(&dev->interface->dev, "Bringing DEP link up failed %d", rc); - return 0; + goto error; } if (!dev->tgt_available_prots) { + struct nfc_target nfc_target; + nfc_dev_dbg(&dev->interface->dev, "Creating new target"); nfc_target.supported_protocols = NFC_PROTO_NFC_DEP_MASK; nfc_target.nfcid1_len = 10; - memcpy(nfc_target.nfcid1, resp->nfcid3t, nfc_target.nfcid1_len); + memcpy(nfc_target.nfcid1, rsp->nfcid3t, nfc_target.nfcid1_len); rc = nfc_targets_found(dev->nfc_dev, &nfc_target, 1); if (rc) - return 0; + goto error; dev->tgt_available_prots = 0; } @@ -1915,15 +1900,17 @@ static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg, dev->tgt_active_prot = NFC_PROTO_NFC_DEP; /* ATR_RES general bytes are located at offset 17 */ - target_gt_len = PN533_FRAME_CMD_PARAMS_LEN(dev->in_frame) - 17; + target_gt_len = resp->len - 17; rc = nfc_set_remote_general_bytes(dev->nfc_dev, - resp->gt, target_gt_len); + rsp->gt, target_gt_len); if (rc == 0) rc = nfc_dep_link_is_up(dev->nfc_dev, - dev->nfc_dev->targets[0].idx, - !active, NFC_RF_INITIATOR); + dev->nfc_dev->targets[0].idx, + !active, NFC_RF_INITIATOR); - return 0; +error: + dev_kfree_skb(resp); + return rc; } static int pn533_mod_to_baud(struct pn533 *dev) @@ -1945,10 +1932,11 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target, u8 comm_mode, u8* gb, size_t gb_len) { struct pn533 *dev = nfc_get_drvdata(nfc_dev); - struct pn533_cmd_jump_dep *cmd; - u8 cmd_len, *data_ptr; + struct sk_buff *skb; + int rc, baud, skb_len; + u8 *next, *arg; + u8 passive_data[PASSIVE_DATA_LEN] = {0x00, 0xff, 0xff, 0x00, 0x3}; - int rc, baud; nfc_dev_dbg(&dev->interface->dev, "%s", __func__); @@ -1971,43 +1959,48 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target, return baud; } - cmd_len = sizeof(struct pn533_cmd_jump_dep) + gb_len; + skb_len = 3 + gb_len; /* ActPass + BR + Next */ if (comm_mode == NFC_COMM_PASSIVE) - cmd_len += PASSIVE_DATA_LEN; + skb_len += PASSIVE_DATA_LEN; - cmd = kzalloc(cmd_len, GFP_KERNEL); - if (cmd == NULL) + skb = pn533_alloc_skb(skb_len); + if (!skb) return -ENOMEM; - pn533_tx_frame_init(dev->out_frame, PN533_CMD_IN_JUMP_FOR_DEP); + *skb_put(skb, 1) = !comm_mode; /* ActPass */ + *skb_put(skb, 1) = baud; /* Baud rate */ + + next = skb_put(skb, 1); /* Next */ + *next = 0; - cmd->active = !comm_mode; - cmd->next = 0; - cmd->baud = baud; - data_ptr = cmd->data; - if (comm_mode == NFC_COMM_PASSIVE && cmd->baud > 0) { - memcpy(data_ptr, passive_data, PASSIVE_DATA_LEN); - cmd->next |= 1; - data_ptr += PASSIVE_DATA_LEN; + if (comm_mode == NFC_COMM_PASSIVE && baud > 0) { + memcpy(skb_put(skb, PASSIVE_DATA_LEN), passive_data, + PASSIVE_DATA_LEN); + *next |= 1; } if (gb != NULL && gb_len > 0) { - cmd->next |= 4; /* We have some Gi */ - memcpy(data_ptr, gb, gb_len); + memcpy(skb_put(skb, gb_len), gb, gb_len); + *next |= 4; /* We have some Gi */ } else { - cmd->next = 0; + *next = 0; } - memcpy(PN533_FRAME_CMD_PARAMS_PTR(dev->out_frame), cmd, cmd_len); - dev->out_frame->datalen += cmd_len; + arg = kmalloc(sizeof(*arg), GFP_KERNEL); + if (!arg) { + dev_kfree_skb(skb); + return -ENOMEM; + } - pn533_tx_frame_finish(dev->out_frame); + *arg = !comm_mode; - rc = pn533_send_cmd_frame_async(dev, dev->out_frame, dev->in_frame, - PN533_NORMAL_FRAME_MAX_LEN, - pn533_in_dep_link_up_complete, cmd); - if (rc < 0) - kfree(cmd); + rc = pn533_send_cmd_async(dev, PN533_CMD_IN_JUMP_FOR_DEP, skb, + pn533_in_dep_link_up_complete, arg); + + if (rc < 0) { + dev_kfree_skb(skb); + kfree(arg); + } return rc; }