static __u8 *nci_extract_rf_params_nfca_passive_poll(struct nci_dev *ndev,
struct nci_rf_intf_activated_ntf *ntf, __u8 *data)
{
struct rf_tech_specific_params_nfca_poll *nfca_poll;
nfca_poll = &ntf->rf_tech_specific_params.nfca_poll;
nfca_poll->sens_res = __le16_to_cpu(*((__u16 *)data));
data += 2;
nfca_poll->nfcid1_len = *data++;
nfc_dbg("sens_res 0x%x, nfcid1_len %d",
nfca_poll->sens_res,
nfca_poll->nfcid1_len);
memcpy(nfca_poll->nfcid1, data, nfca_poll->nfcid1_len);
data += nfca_poll->nfcid1_len;
nfca_poll->sel_res_len = *data++;
if (nfca_poll->sel_res_len != 0)
nfca_poll->sel_res = *data++;
nfc_dbg("sel_res_len %d, sel_res 0x%x",
nfca_poll->sel_res_len,
nfca_poll->sel_res);
return data;
}
static void nci_target_found(struct nci_dev *ndev,
struct nci_rf_intf_activated_ntf *ntf)
{
struct nfc_target nfc_tgt;
if (ntf->rf_protocol == NCI_RF_PROTOCOL_T2T) /* T2T MifareUL */
nfc_tgt.supported_protocols = NFC_PROTO_MIFARE_MASK;
else if (ntf->rf_protocol == NCI_RF_PROTOCOL_ISO_DEP) /* 4A */
nfc_tgt.supported_protocols = NFC_PROTO_ISO14443_MASK;
else
nfc_tgt.supported_protocols = 0;
nfc_tgt.sens_res = ntf->rf_tech_specific_params.nfca_poll.sens_res;
nfc_tgt.sel_res = ntf->rf_tech_specific_params.nfca_poll.sel_res;
if (!(nfc_tgt.supported_protocols & ndev->poll_prots)) {
nfc_dbg("the target found does not have the desired protocol");
return;
}
nfc_dbg("new target found, supported_protocols 0x%x",
nfc_tgt.supported_protocols);
ndev->target_available_prots = nfc_tgt.supported_protocols;
nfc_targets_found(ndev->nfc_dev, &nfc_tgt, 1);
}
static int nci_start_poll(struct nfc_dev *nfc_dev, __u32 protocols)
{
struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
int rc;
nfc_dbg("entry");
if (test_bit(NCI_DISCOVERY, &ndev->flags)) {
nfc_err("unable to start poll, since poll is already active");
return -EBUSY;
}
if (test_bit(NCI_POLL_ACTIVE, &ndev->flags)) {
nfc_dbg("target already active, first deactivate...");
rc = nci_request(ndev, nci_rf_deactivate_req, 0,
msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
if (rc)
return -EBUSY;
}
rc = nci_request(ndev, nci_rf_discover_req, protocols,
msecs_to_jiffies(NCI_RF_DISC_TIMEOUT));
if (!rc)
ndev->poll_prots = protocols;
return rc;
}
static void nci_core_conn_credits_ntf_packet(struct nci_dev *ndev,
struct sk_buff *skb)
{
struct nci_core_conn_credit_ntf *ntf = (void *) skb->data;
int i;
nfc_dbg("entry, num_entries %d", ntf->num_entries);
if (ntf->num_entries > NCI_MAX_NUM_CONN)
ntf->num_entries = NCI_MAX_NUM_CONN;
/* update the credits */
for (i = 0; i < ntf->num_entries; i++) {
nfc_dbg("entry[%d]: conn_id %d, credits %d", i,
ntf->conn_entries[i].conn_id,
ntf->conn_entries[i].credits);
if (ntf->conn_entries[i].conn_id == NCI_STATIC_RF_CONN_ID) {
/* found static rf connection */
atomic_add(ntf->conn_entries[i].credits,
&ndev->credits_cnt);
}
}
/* trigger the next tx */
if (!skb_queue_empty(&ndev->tx_q))
queue_work(ndev->tx_wq, &ndev->tx_work);
}
static void nci_cmd_work(struct work_struct *work)
{
struct nci_dev *ndev = container_of(work, struct nci_dev, cmd_work);
struct sk_buff *skb;
nfc_dbg("entry, cmd_cnt %d", atomic_read(&ndev->cmd_cnt));
/* Send queued command */
if (atomic_read(&ndev->cmd_cnt)) {
skb = skb_dequeue(&ndev->cmd_q);
if (!skb)
return;
atomic_dec(&ndev->cmd_cnt);
nfc_dbg("NCI TX: MT=cmd, PBF=%d, GID=0x%x, OID=0x%x, plen=%d",
nci_pbf(skb->data),
nci_opcode_gid(nci_opcode(skb->data)),
nci_opcode_oid(nci_opcode(skb->data)),
nci_plen(skb->data));
nci_send_frame(skb);
mod_timer(&ndev->cmd_timer,
jiffies + msecs_to_jiffies(NCI_CMD_TIMEOUT));
}
}
static void nci_core_reset_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
{
struct nci_core_reset_rsp *rsp = (void *) skb->data;
nfc_dbg("entry, status 0x%x", rsp->status);
if (rsp->status == NCI_STATUS_OK)
ndev->nci_ver = rsp->nci_ver;
nfc_dbg("nci_ver 0x%x", ndev->nci_ver);
nci_req_complete(ndev, rsp->status);
}
void nci_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb)
{
__u16 ntf_opcode = nci_opcode(skb->data);
nfc_dbg("NCI RX: MT=ntf, PBF=%d, GID=0x%x, OID=0x%x, plen=%d",
nci_pbf(skb->data),
nci_opcode_gid(ntf_opcode),
nci_opcode_oid(ntf_opcode),
nci_plen(skb->data));
/* strip the nci control header */
skb_pull(skb, NCI_CTRL_HDR_SIZE);
switch (ntf_opcode) {
case NCI_OP_CORE_CONN_CREDITS_NTF:
nci_core_conn_credits_ntf_packet(ndev, skb);
break;
case NCI_OP_RF_INTF_ACTIVATED_NTF:
nci_rf_intf_activated_ntf_packet(ndev, skb);
break;
case NCI_OP_RF_DEACTIVATE_NTF:
nci_rf_deactivate_ntf_packet(ndev, skb);
break;
default:
nfc_err("unknown ntf opcode 0x%x", ntf_opcode);
break;
}
kfree_skb(skb);
}
static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev,
struct sk_buff *skb)
{
struct nci_rf_deactivate_ntf *ntf = (void *) skb->data;
nfc_dbg("entry, type 0x%x, reason 0x%x", ntf->type, ntf->reason);
clear_bit(NCI_POLL_ACTIVE, &ndev->flags);
ndev->target_active_prot = 0;
/* drop tx data queue */
skb_queue_purge(&ndev->tx_q);
/* drop partial rx data packet */
if (ndev->rx_data_reassembly) {
kfree_skb(ndev->rx_data_reassembly);
ndev->rx_data_reassembly = 0;
}
/* set the available credits to initial value */
atomic_set(&ndev->credits_cnt, ndev->initial_num_credits);
/* complete the data exchange transaction, if exists */
if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
nci_data_exchange_complete(ndev, NULL, -EIO);
}
/* Send NCI data */
int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb)
{
int rc = 0;
nfc_dbg("entry, conn_id 0x%x, plen %d", conn_id, skb->len);
/* check if the packet need to be fragmented */
if (skb->len <= ndev->max_data_pkt_payload_size) {
/* no need to fragment packet */
nci_push_data_hdr(ndev, conn_id, skb, NCI_PBF_LAST);
skb_queue_tail(&ndev->tx_q, skb);
} else {
/* fragment packet and queue the fragments */
rc = nci_queue_tx_data_frags(ndev, conn_id, skb);
if (rc) {
nfc_err("failed to fragment tx data packet");
goto free_exit;
}
}
queue_work(ndev->tx_wq, &ndev->tx_work);
goto exit;
free_exit:
kfree_skb(skb);
exit:
return rc;
}
static void rawsock_tx_work(struct work_struct *work)
{
struct sock *sk = to_rawsock_sk(work);
struct nfc_dev *dev = nfc_rawsock(sk)->dev;
u32 target_idx = nfc_rawsock(sk)->target_idx;
struct sk_buff *skb;
int rc;
nfc_dbg("sk=%p target_idx=%u", sk, target_idx);
if (sk->sk_shutdown & SEND_SHUTDOWN) {
rawsock_write_queue_purge(sk);
return;
}
skb = skb_dequeue(&sk->sk_write_queue);
sock_hold(sk);
rc = nfc_data_exchange(dev, target_idx, skb,
rawsock_data_exchange_complete, sk);
if (rc) {
rawsock_report_error(sk, rc);
sock_put(sk);
}
}
/**
* nci_free_device - deallocate nci device
*
* @ndev: The nci device to deallocate
*/
void nci_free_device(struct nci_dev *ndev)
{
nfc_dbg("entry");
nfc_free_device(ndev->nfc_dev);
kfree(ndev);
}
static int rawsock_create(struct net *net, struct socket *sock,
const struct nfc_protocol *nfc_proto)
{
struct sock *sk;
nfc_dbg("sock=%p", sock);
if (sock->type != SOCK_SEQPACKET)
return -ESOCKTNOSUPPORT;
sock->ops = &rawsock_ops;
sk = sk_alloc(net, PF_NFC, GFP_KERNEL, nfc_proto->proto);
if (!sk)
return -ENOMEM;
sock_init_data(sock, sk);
sk->sk_protocol = nfc_proto->id;
sk->sk_destruct = rawsock_destruct;
sock->state = SS_UNCONNECTED;
INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
nfc_rawsock(sk)->tx_work_scheduled = false;
return 0;
}
static int rawsock_recvmsg(struct kiocb *iocb, struct socket *sock,
struct msghdr *msg, size_t len, int flags)
{
int noblock = flags & MSG_DONTWAIT;
struct sock *sk = sock->sk;
struct sk_buff *skb;
int copied;
int rc;
nfc_dbg("sock=%p sk=%p len=%zu flags=%d", sock, sk, len, flags);
skb = skb_recv_datagram(sk, flags, noblock, &rc);
if (!skb)
return rc;
msg->msg_namelen = 0;
copied = skb->len;
if (len < copied) {
msg->msg_flags |= MSG_TRUNC;
copied = len;
}
rc = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
skb_free_datagram(sk, skb);
return rc ? : copied;
}
static int nci_activate_target(struct nfc_dev *nfc_dev, __u32 target_idx,
__u32 protocol)
{
struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
nfc_dbg("entry, target_idx %d, protocol 0x%x", target_idx, protocol);
if (!test_bit(NCI_POLL_ACTIVE, &ndev->flags)) {
nfc_err("there is no available target to activate");
return -EINVAL;
}
if (ndev->target_active_prot) {
nfc_err("there is already an active target");
return -EBUSY;
}
if (!(ndev->target_available_prots & (1 << protocol))) {
nfc_err("target does not support the requested protocol 0x%x",
protocol);
return -EINVAL;
}
ndev->target_active_prot = protocol;
ndev->target_available_prots = 0;
return 0;
}
/* Send NCI command */
int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload)
{
struct nci_ctrl_hdr *hdr;
struct sk_buff *skb;
nfc_dbg("entry, opcode 0x%x, plen %d", opcode, plen);
skb = nci_skb_alloc(ndev, (NCI_CTRL_HDR_SIZE + plen), GFP_KERNEL);
if (!skb) {
nfc_err("no memory for command");
return -ENOMEM;
}
hdr = (struct nci_ctrl_hdr *) skb_put(skb, NCI_CTRL_HDR_SIZE);
hdr->gid = nci_opcode_gid(opcode);
hdr->oid = nci_opcode_oid(opcode);
hdr->plen = plen;
nci_mt_set((__u8 *)hdr, NCI_MT_CMD_PKT);
nci_pbf_set((__u8 *)hdr, NCI_PBF_LAST);
if (plen)
memcpy(skb_put(skb, plen), payload, plen);
skb->dev = (void *) ndev;
skb_queue_tail(&ndev->cmd_q, skb);
queue_work(ndev->cmd_wq, &ndev->cmd_work);
return 0;
}
static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
int err)
{
struct sock *sk = (struct sock *) context;
BUG_ON(in_irq());
nfc_dbg("sk=%p err=%d", sk, err);
if (err)
goto error;
err = rawsock_add_header(skb);
if (err)
goto error;
err = sock_queue_rcv_skb(sk, skb);
if (err)
goto error;
spin_lock_bh(&sk->sk_write_queue.lock);
if (!skb_queue_empty(&sk->sk_write_queue))
schedule_work(&nfc_rawsock(sk)->tx_work);
else
nfc_rawsock(sk)->tx_work_scheduled = false;
spin_unlock_bh(&sk->sk_write_queue.lock);
sock_put(sk);
return;
error:
rawsock_report_error(sk, err);
sock_put(sk);
}
static int nci_dev_down(struct nfc_dev *nfc_dev)
{
struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
nfc_dbg("entry");
return nci_close_device(ndev);
}
static int nci_dev_up(struct nfc_dev *nfc_dev)
{
struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
nfc_dbg("entry");
return nci_open_device(ndev);
}
/* NCI command timer function */
static void nci_cmd_timer(unsigned long arg)
{
struct nci_dev *ndev = (void *) arg;
nfc_dbg("entry");
atomic_set(&ndev->cmd_cnt, 1);
queue_work(ndev->cmd_wq, &ndev->cmd_work);
}
static void nci_rf_disc_map_rsp_packet(struct nci_dev *ndev,
struct sk_buff *skb)
{
__u8 status = skb->data[0];
nfc_dbg("entry, status 0x%x", status);
nci_req_complete(ndev, status);
}
static void rawsock_write_queue_purge(struct sock *sk)
{
nfc_dbg("sk=%p", sk);
spin_lock_bh(&sk->sk_write_queue.lock);
__skb_queue_purge(&sk->sk_write_queue);
nfc_rawsock(sk)->tx_work_scheduled = false;
spin_unlock_bh(&sk->sk_write_queue.lock);
}
static void rawsock_report_error(struct sock *sk, int err)
{
nfc_dbg("sk=%p err=%d", sk, err);
sk->sk_shutdown = SHUTDOWN_MASK;
sk->sk_err = -err;
sk->sk_error_report(sk);
rawsock_write_queue_purge(sk);
}
static void nci_rf_disc_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
{
__u8 status = skb->data[0];
nfc_dbg("entry, status 0x%x", status);
if (status == NCI_STATUS_OK)
set_bit(NCI_DISCOVERY, &ndev->flags);
nci_req_complete(ndev, status);
}
static int rawsock_release(struct socket *sock)
{
struct sock *sk = sock->sk;
nfc_dbg("sock=%p", sock);
sock_orphan(sk);
sock_put(sk);
return 0;
}
static void nci_rf_deactivate_rsp_packet(struct nci_dev *ndev,
struct sk_buff *skb)
{
__u8 status = skb->data[0];
nfc_dbg("entry, status 0x%x", status);
clear_bit(NCI_DISCOVERY, &ndev->flags);
nci_req_complete(ndev, status);
}
static void nci_core_conn_create_rsp_packet(struct nci_dev *ndev,
struct sk_buff *skb)
{
struct nci_core_conn_create_rsp *rsp = (void *) skb->data;
nfc_dbg("entry, status 0x%x", rsp->status);
if (rsp->status != NCI_STATUS_OK)
return;
ndev->max_pkt_payload_size = rsp->max_pkt_payload_size;
ndev->initial_num_credits = rsp->initial_num_credits;
ndev->conn_id = rsp->conn_id;
atomic_set(&ndev->credits_cnt, ndev->initial_num_credits);
nfc_dbg("max_pkt_payload_size %d", ndev->max_pkt_payload_size);
nfc_dbg("initial_num_credits %d", ndev->initial_num_credits);
nfc_dbg("conn_id %d", ndev->conn_id);
}
/**
* nci_unregister_device - unregister a nci device in the nfc subsystem
*
* @dev: The nci device to unregister
*/
void nci_unregister_device(struct nci_dev *ndev)
{
nfc_dbg("entry");
nci_close_device(ndev);
destroy_workqueue(ndev->cmd_wq);
destroy_workqueue(ndev->rx_wq);
destroy_workqueue(ndev->tx_wq);
nfc_unregister_device(ndev->nfc_dev);
}
void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
{
__u16 rsp_opcode = nci_opcode(skb->data);
/* we got a rsp, stop the cmd timer */
del_timer(&ndev->cmd_timer);
nfc_dbg("NCI RX: MT=rsp, PBF=%d, GID=0x%x, OID=0x%x, plen=%d",
nci_pbf(skb->data),
nci_opcode_gid(rsp_opcode),
nci_opcode_oid(rsp_opcode),
nci_plen(skb->data));
/* strip the nci control header */
skb_pull(skb, NCI_CTRL_HDR_SIZE);
switch (rsp_opcode) {
case NCI_OP_CORE_RESET_RSP:
nci_core_reset_rsp_packet(ndev, skb);
break;
case NCI_OP_CORE_INIT_RSP:
nci_core_init_rsp_packet(ndev, skb);
break;
case NCI_OP_CORE_CONN_CREATE_RSP:
nci_core_conn_create_rsp_packet(ndev, skb);
break;
case NCI_OP_RF_DISCOVER_MAP_RSP:
nci_rf_disc_map_rsp_packet(ndev, skb);
break;
case NCI_OP_RF_DISCOVER_RSP:
nci_rf_disc_rsp_packet(ndev, skb);
break;
case NCI_OP_RF_DEACTIVATE_RSP:
nci_rf_deactivate_rsp_packet(ndev, skb);
break;
default:
nfc_err("unknown rsp opcode 0x%x", rsp_opcode);
break;
}
kfree_skb(skb);
/* trigger the next cmd */
atomic_set(&ndev->cmd_cnt, 1);
if (!skb_queue_empty(&ndev->cmd_q))
queue_work(ndev->cmd_wq, &ndev->cmd_work);
}
/* Rx Data packet */
void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
{
__u8 pbf = nci_pbf(skb->data);
nfc_dbg("entry, len %d", skb->len);
nfc_dbg("NCI RX: MT=data, PBF=%d, conn_id=%d, plen=%d",
nci_pbf(skb->data),
nci_conn_id(skb->data),
nci_plen(skb->data));
/* strip the nci data header */
skb_pull(skb, NCI_DATA_HDR_SIZE);
if (ndev->target_active_prot == NFC_PROTO_MIFARE) {
/* frame I/F => remove the status byte */
nfc_dbg("NFC_PROTO_MIFARE => remove the status byte");
skb_trim(skb, (skb->len - 1));
}
nci_add_rx_data_frag(ndev, skb, pbf);
}
static void nci_tx_work(struct work_struct *work)
{
struct nci_dev *ndev = container_of(work, struct nci_dev, tx_work);
struct sk_buff *skb;
nfc_dbg("entry, credits_cnt %d", atomic_read(&ndev->credits_cnt));
/* Send queued tx data */
while (atomic_read(&ndev->credits_cnt)) {
skb = skb_dequeue(&ndev->tx_q);
if (!skb)
return;
atomic_dec(&ndev->credits_cnt);
nfc_dbg("NCI TX: MT=data, PBF=%d, conn_id=%d, plen=%d",
nci_pbf(skb->data),
nci_conn_id(skb->data),
nci_plen(skb->data));
nci_send_frame(skb);
}
}
