NFC: LLCP raw socket support - kernel/git/torvalds/linux.git (original) (raw)
This adds support for socket of type SOCK_RAW to LLCP. sk_buff are copied and sent to raw sockets with a 2 bytes extra header: The first byte header contains the nfc adapter index. The second one contains flags: - 0x01 - Direction (0=RX, 1=TX) - 0x02-0x80 - Reserved A raw socket has to be explicitly bound to a nfc adapter. This is achieved by specifying the adapter index to be bound to in the dev_idx field of the sockaddr_nfc_llcp struct passed to bind(). Signed-off-by: Thierry Escande thierry.escande@linux.intel.com Signed-off-by: Samuel Ortiz sameo@linux.intel.com
@@ -183,4 +183,15 @@ struct sockaddr_nfc_llcp {
#define NFC_HEADER_SIZE 1
+/**
+ * Pseudo-header info for raw socket packets
+ * First byte is the adapter index
+ * Second byte contains flags
+ * - 0x01 - Direction (0=RX, 1=TX)
+ * - 0x02-0x80 - Reserved
+ **/
+#define NFC_LLCP_RAW_HEADER_SIZE 2
+#define NFC_LLCP_DIRECTION_RX 0x00
+#define NFC_LLCP_DIRECTION_TX 0x01
#endif /*__LINUX_NFC_H */
@@ -312,6 +312,8 @@ int nfc_llcp_send_symm(struct nfc_dev *dev)
skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
+ nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX);
return nfc_data_exchange(dev, local->target_idx, skb,
nfc_llcp_recv, local);
}
@@ -558,6 +558,46 @@ static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
sock->recv_ack_n = (sock->recv_n - 1) % 16;
}
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+ struct sk_buff *skb, u8 direction)
+{
+ struct hlist_node *node;
+ struct sk_buff *skb_copy = NULL, *nskb;
+ struct sock *sk;
+ u8 *data;
+ read_lock(&local->raw_sockets.lock);
+ sk_for_each(sk, node, &local->raw_sockets.head) {
+ if (sk->sk_state != LLCP_BOUND)
+ continue;
+ if (skb_copy == NULL) {
+ skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
+ GFP_ATOMIC);
+ if (skb_copy == NULL)
+ continue;
+ data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
+ data[0] = local->dev ? local->dev->idx : 0xFF;
+ data[1] = direction;
+ }
+ nskb = skb_clone(skb_copy, GFP_ATOMIC);
+ if (!nskb)
+ continue;
+ if (sock_queue_rcv_skb(sk, nskb))
+ kfree_skb(nskb);
+ }
+ read_unlock(&local->raw_sockets.lock);
+ kfree_skb(skb_copy);
+}
static void nfc_llcp_tx_work(struct work_struct *work)
{
struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
@@ -578,6 +618,9 @@ static void nfc_llcp_tx_work(struct work_struct *work)
DUMP_PREFIX_OFFSET, 16, 1,
skb->data, skb->len, true);
+ nfc_llcp_send_to_raw_sock(local, skb,
+ NFC_LLCP_DIRECTION_TX);
ret = nfc_data_exchange(local->dev, local->target_idx,
skb, nfc_llcp_recv, local);
@@ -1022,6 +1065,8 @@ static void nfc_llcp_rx_work(struct work_struct *work)
print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
16, 1, skb->data, skb->len, true);
+ nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
switch (ptype) {
case LLCP_PDU_SYMM:
pr_debug("SYMM\n");
@@ -1158,6 +1203,7 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
rwlock_init(&local->sockets.lock);
rwlock_init(&local->connecting_sockets.lock);
+ rwlock_init(&local->raw_sockets.lock);
nfc_llcp_build_gb(local);
@@ -86,6 +86,7 @@ struct nfc_llcp_local {
/* sockets array */
struct llcp_sock_list sockets;
struct llcp_sock_list connecting_sockets;
+ struct llcp_sock_list raw_sockets;
};
struct nfc_llcp_sock {
@@ -184,6 +185,8 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local);
void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap);
int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock);
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+ struct sk_buff *skb, u8 direction);
/* Sock API */
struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp);
@@ -142,6 +142,60 @@ error:
return ret;
}
+static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
+ int alen)
+{
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ struct nfc_llcp_local *local;
+ struct nfc_dev *dev;
+ struct sockaddr_nfc_llcp llcp_addr;
+ int len, ret = 0;
+ if (!addr || addr->sa_family != AF_NFC)
+ return -EINVAL;
+ pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+ memset(&llcp_addr, 0, sizeof(llcp_addr));
+ len = min_t(unsigned int, sizeof(llcp_addr), alen);
+ memcpy(&llcp_addr, addr, len);
+ lock_sock(sk);
+ if (sk->sk_state != LLCP_CLOSED) {
+ ret = -EBADFD;
+ goto error;
+ }
+ dev = nfc_get_device(llcp_addr.dev_idx);
+ if (dev == NULL) {
+ ret = -ENODEV;
+ goto error;
+ }
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ ret = -ENODEV;
+ goto put_dev;
+ }
+ llcp_sock->dev = dev;
+ llcp_sock->local = nfc_llcp_local_get(local);
+ llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+ nfc_llcp_sock_link(&local->raw_sockets, sk);
+ sk->sk_state = LLCP_BOUND;
+put_dev:
+ nfc_put_device(dev);
+error:
+ release_sock(sk);
+ return ret;
+}
static int llcp_sock_listen(struct socket *sock, int backlog)
{
struct sock *sk = sock->sk;
@@ -418,7 +472,10 @@ static int llcp_sock_release(struct socket *sock)
release_sock(sk);
- nfc_llcp_sock_unlink(&local->sockets, sk);
+ if (sock->type == SOCK_RAW)
+ nfc_llcp_sock_unlink(&local->raw_sockets, sk);
+ else
+ nfc_llcp_sock_unlink(&local->sockets, sk);
out:
sock_orphan(sk);
@@ -614,7 +671,7 @@ static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
if (!(flags & MSG_PEEK)) {
/* SOCK_STREAM: re-queue skb if it contains unreceived data */
- if (sk->sk_type == SOCK_STREAM) {
+ if (sk->sk_type == SOCK_STREAM || sk->sk_type == SOCK_RAW) {
skb_pull(skb, copied);
if (skb->len) {
skb_queue_head(&sk->sk_receive_queue, skb);
@@ -655,6 +712,26 @@ static const struct proto_ops llcp_sock_ops = {
.mmap = sock_no_mmap,
};
+static const struct proto_ops llcp_rawsock_ops = {
+ .family = PF_NFC,
+ .owner = THIS_MODULE,
+ .bind = llcp_raw_sock_bind,
+ .connect = sock_no_connect,
+ .release = llcp_sock_release,
+ .socketpair = sock_no_socketpair,
+ .accept = sock_no_accept,
+ .getname = llcp_sock_getname,
+ .poll = llcp_sock_poll,
+ .ioctl = sock_no_ioctl,
+ .listen = sock_no_listen,
+ .shutdown = sock_no_shutdown,
+ .setsockopt = sock_no_setsockopt,
+ .getsockopt = sock_no_getsockopt,
+ .sendmsg = sock_no_sendmsg,
+ .recvmsg = llcp_sock_recvmsg,
+ .mmap = sock_no_mmap,
+};
static void llcp_sock_destruct(struct sock *sk)
{
struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
@@ -732,10 +809,15 @@ static int llcp_sock_create(struct net *net, struct socket *sock,
pr_debug("%p\n", sock);
- if (sock->type != SOCK_STREAM && sock->type != SOCK_DGRAM)
+ if (sock->type != SOCK_STREAM &&
+ sock->type != SOCK_DGRAM &&
+ sock->type != SOCK_RAW)
return -ESOCKTNOSUPPORT;
- sock->ops = &llcp_sock_ops;
+ if (sock->type == SOCK_RAW)
+ sock->ops = &llcp_rawsock_ops;
+ else
+ sock->ops = &llcp_sock_ops;
sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);
if (sk == NULL)