linux/net/nfc/rawsock.c
<<
>>
Prefs
   1/*
   2 * Copyright (C) 2011 Instituto Nokia de Tecnologia
   3 *
   4 * Authors:
   5 *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
   6 *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
   7 *
   8 * This program is free software; you can redistribute it and/or modify
   9 * it under the terms of the GNU General Public License as published by
  10 * the Free Software Foundation; either version 2 of the License, or
  11 * (at your option) any later version.
  12 *
  13 * This program is distributed in the hope that it will be useful,
  14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16 * GNU General Public License for more details.
  17 *
  18 * You should have received a copy of the GNU General Public License
  19 * along with this program; if not, see <http://www.gnu.org/licenses/>.
  20 */
  21
  22#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
  23
  24#include <net/tcp_states.h>
  25#include <linux/nfc.h>
  26#include <linux/export.h>
  27
  28#include "nfc.h"
  29
  30static struct nfc_sock_list raw_sk_list = {
  31        .lock = __RW_LOCK_UNLOCKED(raw_sk_list.lock)
  32};
  33
  34static void nfc_sock_link(struct nfc_sock_list *l, struct sock *sk)
  35{
  36        write_lock(&l->lock);
  37        sk_add_node(sk, &l->head);
  38        write_unlock(&l->lock);
  39}
  40
  41static void nfc_sock_unlink(struct nfc_sock_list *l, struct sock *sk)
  42{
  43        write_lock(&l->lock);
  44        sk_del_node_init(sk);
  45        write_unlock(&l->lock);
  46}
  47
  48static void rawsock_write_queue_purge(struct sock *sk)
  49{
  50        pr_debug("sk=%p\n", sk);
  51
  52        spin_lock_bh(&sk->sk_write_queue.lock);
  53        __skb_queue_purge(&sk->sk_write_queue);
  54        nfc_rawsock(sk)->tx_work_scheduled = false;
  55        spin_unlock_bh(&sk->sk_write_queue.lock);
  56}
  57
  58static void rawsock_report_error(struct sock *sk, int err)
  59{
  60        pr_debug("sk=%p err=%d\n", sk, err);
  61
  62        sk->sk_shutdown = SHUTDOWN_MASK;
  63        sk->sk_err = -err;
  64        sk->sk_error_report(sk);
  65
  66        rawsock_write_queue_purge(sk);
  67}
  68
  69static int rawsock_release(struct socket *sock)
  70{
  71        struct sock *sk = sock->sk;
  72
  73        pr_debug("sock=%p sk=%p\n", sock, sk);
  74
  75        if (!sk)
  76                return 0;
  77
  78        if (sock->type == SOCK_RAW)
  79                nfc_sock_unlink(&raw_sk_list, sk);
  80
  81        sock_orphan(sk);
  82        sock_put(sk);
  83
  84        return 0;
  85}
  86
  87static int rawsock_connect(struct socket *sock, struct sockaddr *_addr,
  88                           int len, int flags)
  89{
  90        struct sock *sk = sock->sk;
  91        struct sockaddr_nfc *addr = (struct sockaddr_nfc *)_addr;
  92        struct nfc_dev *dev;
  93        int rc = 0;
  94
  95        pr_debug("sock=%p sk=%p flags=%d\n", sock, sk, flags);
  96
  97        if (!addr || len < sizeof(struct sockaddr_nfc) ||
  98            addr->sa_family != AF_NFC)
  99                return -EINVAL;
 100
 101        pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n",
 102                 addr->dev_idx, addr->target_idx, addr->nfc_protocol);
 103
 104        lock_sock(sk);
 105
 106        if (sock->state == SS_CONNECTED) {
 107                rc = -EISCONN;
 108                goto error;
 109        }
 110
 111        dev = nfc_get_device(addr->dev_idx);
 112        if (!dev) {
 113                rc = -ENODEV;
 114                goto error;
 115        }
 116
 117        if (addr->target_idx > dev->target_next_idx - 1 ||
 118            addr->target_idx < dev->target_next_idx - dev->n_targets) {
 119                rc = -EINVAL;
 120                goto error;
 121        }
 122
 123        rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol);
 124        if (rc)
 125                goto put_dev;
 126
 127        nfc_rawsock(sk)->dev = dev;
 128        nfc_rawsock(sk)->target_idx = addr->target_idx;
 129        sock->state = SS_CONNECTED;
 130        sk->sk_state = TCP_ESTABLISHED;
 131        sk->sk_state_change(sk);
 132
 133        release_sock(sk);
 134        return 0;
 135
 136put_dev:
 137        nfc_put_device(dev);
 138error:
 139        release_sock(sk);
 140        return rc;
 141}
 142
 143static int rawsock_add_header(struct sk_buff *skb)
 144{
 145        *skb_push(skb, NFC_HEADER_SIZE) = 0;
 146
 147        return 0;
 148}
 149
 150static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
 151                                           int err)
 152{
 153        struct sock *sk = (struct sock *) context;
 154
 155        BUG_ON(in_irq());
 156
 157        pr_debug("sk=%p err=%d\n", sk, err);
 158
 159        if (err)
 160                goto error;
 161
 162        err = rawsock_add_header(skb);
 163        if (err)
 164                goto error_skb;
 165
 166        err = sock_queue_rcv_skb(sk, skb);
 167        if (err)
 168                goto error_skb;
 169
 170        spin_lock_bh(&sk->sk_write_queue.lock);
 171        if (!skb_queue_empty(&sk->sk_write_queue))
 172                schedule_work(&nfc_rawsock(sk)->tx_work);
 173        else
 174                nfc_rawsock(sk)->tx_work_scheduled = false;
 175        spin_unlock_bh(&sk->sk_write_queue.lock);
 176
 177        sock_put(sk);
 178        return;
 179
 180error_skb:
 181        kfree_skb(skb);
 182
 183error:
 184        rawsock_report_error(sk, err);
 185        sock_put(sk);
 186}
 187
 188static void rawsock_tx_work(struct work_struct *work)
 189{
 190        struct sock *sk = to_rawsock_sk(work);
 191        struct nfc_dev *dev = nfc_rawsock(sk)->dev;
 192        u32 target_idx = nfc_rawsock(sk)->target_idx;
 193        struct sk_buff *skb;
 194        int rc;
 195
 196        pr_debug("sk=%p target_idx=%u\n", sk, target_idx);
 197
 198        if (sk->sk_shutdown & SEND_SHUTDOWN) {
 199                rawsock_write_queue_purge(sk);
 200                return;
 201        }
 202
 203        skb = skb_dequeue(&sk->sk_write_queue);
 204
 205        sock_hold(sk);
 206        rc = nfc_data_exchange(dev, target_idx, skb,
 207                               rawsock_data_exchange_complete, sk);
 208        if (rc) {
 209                rawsock_report_error(sk, rc);
 210                sock_put(sk);
 211        }
 212}
 213
 214static int rawsock_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
 215{
 216        struct sock *sk = sock->sk;
 217        struct nfc_dev *dev = nfc_rawsock(sk)->dev;
 218        struct sk_buff *skb;
 219        int rc;
 220
 221        pr_debug("sock=%p sk=%p len=%zu\n", sock, sk, len);
 222
 223        if (msg->msg_namelen)
 224                return -EOPNOTSUPP;
 225
 226        if (sock->state != SS_CONNECTED)
 227                return -ENOTCONN;
 228
 229        skb = nfc_alloc_send_skb(dev, sk, msg->msg_flags, len, &rc);
 230        if (skb == NULL)
 231                return rc;
 232
 233        rc = memcpy_from_msg(skb_put(skb, len), msg, len);
 234        if (rc < 0) {
 235                kfree_skb(skb);
 236                return rc;
 237        }
 238
 239        spin_lock_bh(&sk->sk_write_queue.lock);
 240        __skb_queue_tail(&sk->sk_write_queue, skb);
 241        if (!nfc_rawsock(sk)->tx_work_scheduled) {
 242                schedule_work(&nfc_rawsock(sk)->tx_work);
 243                nfc_rawsock(sk)->tx_work_scheduled = true;
 244        }
 245        spin_unlock_bh(&sk->sk_write_queue.lock);
 246
 247        return len;
 248}
 249
 250static int rawsock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
 251                           int flags)
 252{
 253        int noblock = flags & MSG_DONTWAIT;
 254        struct sock *sk = sock->sk;
 255        struct sk_buff *skb;
 256        int copied;
 257        int rc;
 258
 259        pr_debug("sock=%p sk=%p len=%zu flags=%d\n", sock, sk, len, flags);
 260
 261        skb = skb_recv_datagram(sk, flags, noblock, &rc);
 262        if (!skb)
 263                return rc;
 264
 265        copied = skb->len;
 266        if (len < copied) {
 267                msg->msg_flags |= MSG_TRUNC;
 268                copied = len;
 269        }
 270
 271        rc = skb_copy_datagram_msg(skb, 0, msg, copied);
 272
 273        skb_free_datagram(sk, skb);
 274
 275        return rc ? : copied;
 276}
 277
 278static const struct proto_ops rawsock_ops = {
 279        .family         = PF_NFC,
 280        .owner          = THIS_MODULE,
 281        .release        = rawsock_release,
 282        .bind           = sock_no_bind,
 283        .connect        = rawsock_connect,
 284        .socketpair     = sock_no_socketpair,
 285        .accept         = sock_no_accept,
 286        .getname        = sock_no_getname,
 287        .poll           = datagram_poll,
 288        .ioctl          = sock_no_ioctl,
 289        .listen         = sock_no_listen,
 290        .shutdown       = sock_no_shutdown,
 291        .setsockopt     = sock_no_setsockopt,
 292        .getsockopt     = sock_no_getsockopt,
 293        .sendmsg        = rawsock_sendmsg,
 294        .recvmsg        = rawsock_recvmsg,
 295        .mmap           = sock_no_mmap,
 296};
 297
 298static const struct proto_ops rawsock_raw_ops = {
 299        .family         = PF_NFC,
 300        .owner          = THIS_MODULE,
 301        .release        = rawsock_release,
 302        .bind           = sock_no_bind,
 303        .connect        = sock_no_connect,
 304        .socketpair     = sock_no_socketpair,
 305        .accept         = sock_no_accept,
 306        .getname        = sock_no_getname,
 307        .poll           = datagram_poll,
 308        .ioctl          = sock_no_ioctl,
 309        .listen         = sock_no_listen,
 310        .shutdown       = sock_no_shutdown,
 311        .setsockopt     = sock_no_setsockopt,
 312        .getsockopt     = sock_no_getsockopt,
 313        .sendmsg        = sock_no_sendmsg,
 314        .recvmsg        = rawsock_recvmsg,
 315        .mmap           = sock_no_mmap,
 316};
 317
 318static void rawsock_destruct(struct sock *sk)
 319{
 320        pr_debug("sk=%p\n", sk);
 321
 322        if (sk->sk_state == TCP_ESTABLISHED) {
 323                nfc_deactivate_target(nfc_rawsock(sk)->dev,
 324                                      nfc_rawsock(sk)->target_idx);
 325                nfc_put_device(nfc_rawsock(sk)->dev);
 326        }
 327
 328        skb_queue_purge(&sk->sk_receive_queue);
 329
 330        if (!sock_flag(sk, SOCK_DEAD)) {
 331                pr_err("Freeing alive NFC raw socket %p\n", sk);
 332                return;
 333        }
 334}
 335
 336static int rawsock_create(struct net *net, struct socket *sock,
 337                          const struct nfc_protocol *nfc_proto, int kern)
 338{
 339        struct sock *sk;
 340
 341        pr_debug("sock=%p\n", sock);
 342
 343        if ((sock->type != SOCK_SEQPACKET) && (sock->type != SOCK_RAW))
 344                return -ESOCKTNOSUPPORT;
 345
 346        if (sock->type == SOCK_RAW)
 347                sock->ops = &rawsock_raw_ops;
 348        else
 349                sock->ops = &rawsock_ops;
 350
 351        sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto, kern);
 352        if (!sk)
 353                return -ENOMEM;
 354
 355        sock_init_data(sock, sk);
 356        sk->sk_protocol = nfc_proto->id;
 357        sk->sk_destruct = rawsock_destruct;
 358        sock->state = SS_UNCONNECTED;
 359        if (sock->type == SOCK_RAW)
 360                nfc_sock_link(&raw_sk_list, sk);
 361        else {
 362                INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
 363                nfc_rawsock(sk)->tx_work_scheduled = false;
 364        }
 365
 366        return 0;
 367}
 368
 369void nfc_send_to_raw_sock(struct nfc_dev *dev, struct sk_buff *skb,
 370                          u8 payload_type, u8 direction)
 371{
 372        struct sk_buff *skb_copy = NULL, *nskb;
 373        struct sock *sk;
 374        u8 *data;
 375
 376        read_lock(&raw_sk_list.lock);
 377
 378        sk_for_each(sk, &raw_sk_list.head) {
 379                if (!skb_copy) {
 380                        skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
 381                                                      GFP_ATOMIC, true);
 382                        if (!skb_copy)
 383                                continue;
 384
 385                        data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
 386
 387                        data[0] = dev ? dev->idx : 0xFF;
 388                        data[1] = direction & 0x01;
 389                        data[1] |= (payload_type << 1);
 390                }
 391
 392                nskb = skb_clone(skb_copy, GFP_ATOMIC);
 393                if (!nskb)
 394                        continue;
 395
 396                if (sock_queue_rcv_skb(sk, nskb))
 397                        kfree_skb(nskb);
 398        }
 399
 400        read_unlock(&raw_sk_list.lock);
 401
 402        kfree_skb(skb_copy);
 403}
 404EXPORT_SYMBOL(nfc_send_to_raw_sock);
 405
 406static struct proto rawsock_proto = {
 407        .name     = "NFC_RAW",
 408        .owner    = THIS_MODULE,
 409        .obj_size = sizeof(struct nfc_rawsock),
 410};
 411
 412static const struct nfc_protocol rawsock_nfc_proto = {
 413        .id       = NFC_SOCKPROTO_RAW,
 414        .proto    = &rawsock_proto,
 415        .owner    = THIS_MODULE,
 416        .create   = rawsock_create
 417};
 418
 419int __init rawsock_init(void)
 420{
 421        int rc;
 422
 423        rc = nfc_proto_register(&rawsock_nfc_proto);
 424
 425        return rc;
 426}
 427
 428void rawsock_exit(void)
 429{
 430        nfc_proto_unregister(&rawsock_nfc_proto);
 431}
 432