linux/net/phonet/pep.c
<<
>>
Prefs
   1/*
   2 * File: pep.c
   3 *
   4 * Phonet pipe protocol end point socket
   5 *
   6 * Copyright (C) 2008 Nokia Corporation.
   7 *
   8 * Author: RĂ©mi Denis-Courmont <remi.denis-courmont@nokia.com>
   9 *
  10 * This program is free software; you can redistribute it and/or
  11 * modify it under the terms of the GNU General Public License
  12 * version 2 as published by the Free Software Foundation.
  13 *
  14 * This program is distributed in the hope that it will be useful, but
  15 * WITHOUT ANY WARRANTY; without even the implied warranty of
  16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 * General Public License for more details.
  18 *
  19 * You should have received a copy of the GNU General Public License
  20 * along with this program; if not, write to the Free Software
  21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
  22 * 02110-1301 USA
  23 */
  24
  25#include <linux/kernel.h>
  26#include <linux/slab.h>
  27#include <linux/socket.h>
  28#include <net/sock.h>
  29#include <net/tcp_states.h>
  30#include <asm/ioctls.h>
  31
  32#include <linux/phonet.h>
  33#include <net/phonet/phonet.h>
  34#include <net/phonet/pep.h>
  35#include <net/phonet/gprs.h>
  36
  37/* sk_state values:
  38 * TCP_CLOSE            sock not in use yet
  39 * TCP_CLOSE_WAIT       disconnected pipe
  40 * TCP_LISTEN           listening pipe endpoint
  41 * TCP_SYN_RECV         connected pipe in disabled state
  42 * TCP_ESTABLISHED      connected pipe in enabled state
  43 *
  44 * pep_sock locking:
  45 *  - sk_state, ackq, hlist: sock lock needed
  46 *  - listener: read only
  47 *  - pipe_handle: read only
  48 */
  49
  50#define CREDITS_MAX     10
  51#define CREDITS_THR     7
  52
  53static const struct sockaddr_pn pipe_srv = {
  54        .spn_family = AF_PHONET,
  55        .spn_resource = 0xD9, /* pipe service */
  56};
  57
  58#define pep_sb_size(s) (((s) + 5) & ~3) /* 2-bytes head, 32-bits aligned */
  59
  60/* Get the next TLV sub-block. */
  61static unsigned char *pep_get_sb(struct sk_buff *skb, u8 *ptype, u8 *plen,
  62                                        void *buf)
  63{
  64        void *data = NULL;
  65        struct {
  66                u8 sb_type;
  67                u8 sb_len;
  68        } *ph, h;
  69        int buflen = *plen;
  70
  71        ph = skb_header_pointer(skb, 0, 2, &h);
  72        if (ph == NULL || ph->sb_len < 2 || !pskb_may_pull(skb, ph->sb_len))
  73                return NULL;
  74        ph->sb_len -= 2;
  75        *ptype = ph->sb_type;
  76        *plen = ph->sb_len;
  77
  78        if (buflen > ph->sb_len)
  79                buflen = ph->sb_len;
  80        data = skb_header_pointer(skb, 2, buflen, buf);
  81        __skb_pull(skb, 2 + ph->sb_len);
  82        return data;
  83}
  84
  85static int pep_reply(struct sock *sk, struct sk_buff *oskb,
  86                        u8 code, const void *data, int len, gfp_t priority)
  87{
  88        const struct pnpipehdr *oph = pnp_hdr(oskb);
  89        struct pnpipehdr *ph;
  90        struct sk_buff *skb;
  91
  92        skb = alloc_skb(MAX_PNPIPE_HEADER + len, priority);
  93        if (!skb)
  94                return -ENOMEM;
  95        skb_set_owner_w(skb, sk);
  96
  97        skb_reserve(skb, MAX_PNPIPE_HEADER);
  98        __skb_put(skb, len);
  99        skb_copy_to_linear_data(skb, data, len);
 100        __skb_push(skb, sizeof(*ph));
 101        skb_reset_transport_header(skb);
 102        ph = pnp_hdr(skb);
 103        ph->utid = oph->utid;
 104        ph->message_id = oph->message_id + 1; /* REQ -> RESP */
 105        ph->pipe_handle = oph->pipe_handle;
 106        ph->error_code = code;
 107
 108        return pn_skb_send(sk, skb, &pipe_srv);
 109}
 110
 111#define PAD 0x00
 112
 113#ifdef CONFIG_PHONET_PIPECTRLR
 114static u8 pipe_negotiate_fc(u8 *host_fc, u8 *remote_fc, int len)
 115{
 116        int i, j;
 117        u8 base_fc, final_fc;
 118
 119        for (i = 0; i < len; i++) {
 120                base_fc = host_fc[i];
 121                for (j = 0; j < len; j++) {
 122                        if (remote_fc[j] == base_fc) {
 123                                final_fc = base_fc;
 124                                goto done;
 125                        }
 126                }
 127        }
 128        return -EINVAL;
 129
 130done:
 131        return final_fc;
 132
 133}
 134
 135static int pipe_get_flow_info(struct sock *sk, struct sk_buff *skb,
 136                u8 *pref_rx_fc, u8 *req_tx_fc)
 137{
 138        struct pnpipehdr *hdr;
 139        u8 n_sb;
 140
 141        if (!pskb_may_pull(skb, sizeof(*hdr) + 4))
 142                return -EINVAL;
 143
 144        hdr = pnp_hdr(skb);
 145        n_sb = hdr->data[4];
 146
 147        __skb_pull(skb, sizeof(*hdr) + 4);
 148        while (n_sb > 0) {
 149                u8 type, buf[3], len = sizeof(buf);
 150                u8 *data = pep_get_sb(skb, &type, &len, buf);
 151
 152                if (data == NULL)
 153                        return -EINVAL;
 154
 155                switch (type) {
 156                case PN_PIPE_SB_REQUIRED_FC_TX:
 157                        if (len < 3 || (data[2] | data[3] | data[4]) > 3)
 158                                break;
 159                        req_tx_fc[0] = data[2];
 160                        req_tx_fc[1] = data[3];
 161                        req_tx_fc[2] = data[4];
 162                        break;
 163
 164                case PN_PIPE_SB_PREFERRED_FC_RX:
 165                        if (len < 3 || (data[2] | data[3] | data[4]) > 3)
 166                                break;
 167                        pref_rx_fc[0] = data[2];
 168                        pref_rx_fc[1] = data[3];
 169                        pref_rx_fc[2] = data[4];
 170                        break;
 171
 172                }
 173                n_sb--;
 174        }
 175        return 0;
 176}
 177
 178static int pipe_handler_send_req(struct sock *sk, u8 utid,
 179                u8 msg_id, gfp_t priority)
 180{
 181        int len;
 182        struct pnpipehdr *ph;
 183        struct sk_buff *skb;
 184        struct pep_sock *pn = pep_sk(sk);
 185
 186        static const u8 data[4] = {
 187                PAD, PAD, PAD, PAD,
 188        };
 189
 190        switch (msg_id) {
 191        case PNS_PEP_CONNECT_REQ:
 192                len = sizeof(data);
 193                break;
 194
 195        case PNS_PEP_DISCONNECT_REQ:
 196        case PNS_PEP_ENABLE_REQ:
 197        case PNS_PEP_DISABLE_REQ:
 198                len = 0;
 199                break;
 200
 201        default:
 202                return -EINVAL;
 203        }
 204
 205        skb = alloc_skb(MAX_PNPIPE_HEADER + len, priority);
 206        if (!skb)
 207                return -ENOMEM;
 208        skb_set_owner_w(skb, sk);
 209
 210        skb_reserve(skb, MAX_PNPIPE_HEADER);
 211        if (len) {
 212                __skb_put(skb, len);
 213                skb_copy_to_linear_data(skb, data, len);
 214        }
 215        __skb_push(skb, sizeof(*ph));
 216        skb_reset_transport_header(skb);
 217        ph = pnp_hdr(skb);
 218        ph->utid = utid;
 219        ph->message_id = msg_id;
 220        ph->pipe_handle = pn->pipe_handle;
 221        ph->error_code = PN_PIPE_NO_ERROR;
 222
 223        return pn_skb_send(sk, skb, &pn->remote_pep);
 224}
 225
 226static int pipe_handler_send_created_ind(struct sock *sk,
 227                u8 utid, u8 msg_id)
 228{
 229        int err_code;
 230        struct pnpipehdr *ph;
 231        struct sk_buff *skb;
 232
 233        struct pep_sock *pn = pep_sk(sk);
 234        static u8 data[4] = {
 235                0x03, 0x04,
 236        };
 237        data[2] = pn->tx_fc;
 238        data[3] = pn->rx_fc;
 239
 240        /*
 241         * actually, below is number of sub-blocks and not error code.
 242         * Pipe_created_ind message format does not have any
 243         * error code field. However, the Phonet stack will always send
 244         * an error code as part of pnpipehdr. So, use that err_code to
 245         * specify the number of sub-blocks.
 246         */
 247        err_code = 0x01;
 248
 249        skb = alloc_skb(MAX_PNPIPE_HEADER + sizeof(data), GFP_ATOMIC);
 250        if (!skb)
 251                return -ENOMEM;
 252        skb_set_owner_w(skb, sk);
 253
 254        skb_reserve(skb, MAX_PNPIPE_HEADER);
 255        __skb_put(skb, sizeof(data));
 256        skb_copy_to_linear_data(skb, data, sizeof(data));
 257        __skb_push(skb, sizeof(*ph));
 258        skb_reset_transport_header(skb);
 259        ph = pnp_hdr(skb);
 260        ph->utid = utid;
 261        ph->message_id = msg_id;
 262        ph->pipe_handle = pn->pipe_handle;
 263        ph->error_code = err_code;
 264
 265        return pn_skb_send(sk, skb, &pn->remote_pep);
 266}
 267
 268static int pipe_handler_send_ind(struct sock *sk, u8 utid, u8 msg_id)
 269{
 270        int err_code;
 271        struct pnpipehdr *ph;
 272        struct sk_buff *skb;
 273        struct pep_sock *pn = pep_sk(sk);
 274
 275        /*
 276         * actually, below is a filler.
 277         * Pipe_enabled/disabled_ind message format does not have any
 278         * error code field. However, the Phonet stack will always send
 279         * an error code as part of pnpipehdr. So, use that err_code to
 280         * specify the filler value.
 281         */
 282        err_code = 0x0;
 283
 284        skb = alloc_skb(MAX_PNPIPE_HEADER, GFP_ATOMIC);
 285        if (!skb)
 286                return -ENOMEM;
 287        skb_set_owner_w(skb, sk);
 288
 289        skb_reserve(skb, MAX_PNPIPE_HEADER);
 290        __skb_push(skb, sizeof(*ph));
 291        skb_reset_transport_header(skb);
 292        ph = pnp_hdr(skb);
 293        ph->utid = utid;
 294        ph->message_id = msg_id;
 295        ph->pipe_handle = pn->pipe_handle;
 296        ph->error_code = err_code;
 297
 298        return pn_skb_send(sk, skb, &pn->remote_pep);
 299}
 300
 301static int pipe_handler_enable_pipe(struct sock *sk, int enable)
 302{
 303        int utid, req;
 304
 305        if (enable) {
 306                utid = PNS_PIPE_ENABLE_UTID;
 307                req = PNS_PEP_ENABLE_REQ;
 308        } else {
 309                utid = PNS_PIPE_DISABLE_UTID;
 310                req = PNS_PEP_DISABLE_REQ;
 311        }
 312        return pipe_handler_send_req(sk, utid, req, GFP_ATOMIC);
 313}
 314#endif
 315
 316static int pep_accept_conn(struct sock *sk, struct sk_buff *skb)
 317{
 318        static const u8 data[20] = {
 319                PAD, PAD, PAD, 2 /* sub-blocks */,
 320                PN_PIPE_SB_REQUIRED_FC_TX, pep_sb_size(5), 3, PAD,
 321                        PN_MULTI_CREDIT_FLOW_CONTROL,
 322                        PN_ONE_CREDIT_FLOW_CONTROL,
 323                        PN_LEGACY_FLOW_CONTROL,
 324                        PAD,
 325                PN_PIPE_SB_PREFERRED_FC_RX, pep_sb_size(5), 3, PAD,
 326                        PN_MULTI_CREDIT_FLOW_CONTROL,
 327                        PN_ONE_CREDIT_FLOW_CONTROL,
 328                        PN_LEGACY_FLOW_CONTROL,
 329                        PAD,
 330        };
 331
 332        might_sleep();
 333        return pep_reply(sk, skb, PN_PIPE_NO_ERROR, data, sizeof(data),
 334                                GFP_KERNEL);
 335}
 336
 337static int pep_reject_conn(struct sock *sk, struct sk_buff *skb, u8 code)
 338{
 339        static const u8 data[4] = { PAD, PAD, PAD, 0 /* sub-blocks */ };
 340        WARN_ON(code == PN_PIPE_NO_ERROR);
 341        return pep_reply(sk, skb, code, data, sizeof(data), GFP_ATOMIC);
 342}
 343
 344/* Control requests are not sent by the pipe service and have a specific
 345 * message format. */
 346static int pep_ctrlreq_error(struct sock *sk, struct sk_buff *oskb, u8 code,
 347                                gfp_t priority)
 348{
 349        const struct pnpipehdr *oph = pnp_hdr(oskb);
 350        struct sk_buff *skb;
 351        struct pnpipehdr *ph;
 352        struct sockaddr_pn dst;
 353
 354        skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority);
 355        if (!skb)
 356                return -ENOMEM;
 357        skb_set_owner_w(skb, sk);
 358
 359        skb_reserve(skb, MAX_PHONET_HEADER);
 360        ph = (struct pnpipehdr *)skb_put(skb, sizeof(*ph) + 4);
 361
 362        ph->utid = oph->utid;
 363        ph->message_id = PNS_PEP_CTRL_RESP;
 364        ph->pipe_handle = oph->pipe_handle;
 365        ph->data[0] = oph->data[1]; /* CTRL id */
 366        ph->data[1] = oph->data[0]; /* PEP type */
 367        ph->data[2] = code; /* error code, at an usual offset */
 368        ph->data[3] = PAD;
 369        ph->data[4] = PAD;
 370
 371        pn_skb_get_src_sockaddr(oskb, &dst);
 372        return pn_skb_send(sk, skb, &dst);
 373}
 374
 375static int pipe_snd_status(struct sock *sk, u8 type, u8 status, gfp_t priority)
 376{
 377        struct pep_sock *pn = pep_sk(sk);
 378        struct pnpipehdr *ph;
 379        struct sk_buff *skb;
 380
 381        skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority);
 382        if (!skb)
 383                return -ENOMEM;
 384        skb_set_owner_w(skb, sk);
 385
 386        skb_reserve(skb, MAX_PNPIPE_HEADER + 4);
 387        __skb_push(skb, sizeof(*ph) + 4);
 388        skb_reset_transport_header(skb);
 389        ph = pnp_hdr(skb);
 390        ph->utid = 0;
 391        ph->message_id = PNS_PEP_STATUS_IND;
 392        ph->pipe_handle = pn->pipe_handle;
 393        ph->pep_type = PN_PEP_TYPE_COMMON;
 394        ph->data[1] = type;
 395        ph->data[2] = PAD;
 396        ph->data[3] = PAD;
 397        ph->data[4] = status;
 398
 399#ifdef CONFIG_PHONET_PIPECTRLR
 400        return pn_skb_send(sk, skb, &pn->remote_pep);
 401#else
 402        return pn_skb_send(sk, skb, &pipe_srv);
 403#endif
 404}
 405
 406/* Send our RX flow control information to the sender.
 407 * Socket must be locked. */
 408static void pipe_grant_credits(struct sock *sk)
 409{
 410        struct pep_sock *pn = pep_sk(sk);
 411
 412        BUG_ON(sk->sk_state != TCP_ESTABLISHED);
 413
 414        switch (pn->rx_fc) {
 415        case PN_LEGACY_FLOW_CONTROL: /* TODO */
 416                break;
 417        case PN_ONE_CREDIT_FLOW_CONTROL:
 418                pipe_snd_status(sk, PN_PEP_IND_FLOW_CONTROL,
 419                                PEP_IND_READY, GFP_ATOMIC);
 420                pn->rx_credits = 1;
 421                break;
 422        case PN_MULTI_CREDIT_FLOW_CONTROL:
 423                if ((pn->rx_credits + CREDITS_THR) > CREDITS_MAX)
 424                        break;
 425                if (pipe_snd_status(sk, PN_PEP_IND_ID_MCFC_GRANT_CREDITS,
 426                                        CREDITS_MAX - pn->rx_credits,
 427                                        GFP_ATOMIC) == 0)
 428                        pn->rx_credits = CREDITS_MAX;
 429                break;
 430        }
 431}
 432
 433static int pipe_rcv_status(struct sock *sk, struct sk_buff *skb)
 434{
 435        struct pep_sock *pn = pep_sk(sk);
 436        struct pnpipehdr *hdr;
 437        int wake = 0;
 438
 439        if (!pskb_may_pull(skb, sizeof(*hdr) + 4))
 440                return -EINVAL;
 441
 442        hdr = pnp_hdr(skb);
 443        if (hdr->data[0] != PN_PEP_TYPE_COMMON) {
 444                LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP type: %u\n",
 445                                (unsigned)hdr->data[0]);
 446                return -EOPNOTSUPP;
 447        }
 448
 449        switch (hdr->data[1]) {
 450        case PN_PEP_IND_FLOW_CONTROL:
 451                switch (pn->tx_fc) {
 452                case PN_LEGACY_FLOW_CONTROL:
 453                        switch (hdr->data[4]) {
 454                        case PEP_IND_BUSY:
 455                                atomic_set(&pn->tx_credits, 0);
 456                                break;
 457                        case PEP_IND_READY:
 458                                atomic_set(&pn->tx_credits, wake = 1);
 459                                break;
 460                        }
 461                        break;
 462                case PN_ONE_CREDIT_FLOW_CONTROL:
 463                        if (hdr->data[4] == PEP_IND_READY)
 464                                atomic_set(&pn->tx_credits, wake = 1);
 465                        break;
 466                }
 467                break;
 468
 469        case PN_PEP_IND_ID_MCFC_GRANT_CREDITS:
 470                if (pn->tx_fc != PN_MULTI_CREDIT_FLOW_CONTROL)
 471                        break;
 472                atomic_add(wake = hdr->data[4], &pn->tx_credits);
 473                break;
 474
 475        default:
 476                LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP indication: %u\n",
 477                                (unsigned)hdr->data[1]);
 478                return -EOPNOTSUPP;
 479        }
 480        if (wake)
 481                sk->sk_write_space(sk);
 482        return 0;
 483}
 484
 485static int pipe_rcv_created(struct sock *sk, struct sk_buff *skb)
 486{
 487        struct pep_sock *pn = pep_sk(sk);
 488        struct pnpipehdr *hdr = pnp_hdr(skb);
 489        u8 n_sb = hdr->data[0];
 490
 491        pn->rx_fc = pn->tx_fc = PN_LEGACY_FLOW_CONTROL;
 492        __skb_pull(skb, sizeof(*hdr));
 493        while (n_sb > 0) {
 494                u8 type, buf[2], len = sizeof(buf);
 495                u8 *data = pep_get_sb(skb, &type, &len, buf);
 496
 497                if (data == NULL)
 498                        return -EINVAL;
 499                switch (type) {
 500                case PN_PIPE_SB_NEGOTIATED_FC:
 501                        if (len < 2 || (data[0] | data[1]) > 3)
 502                                break;
 503                        pn->tx_fc = data[0] & 3;
 504                        pn->rx_fc = data[1] & 3;
 505                        break;
 506                }
 507                n_sb--;
 508        }
 509        return 0;
 510}
 511
 512/* Queue an skb to a connected sock.
 513 * Socket lock must be held. */
 514static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb)
 515{
 516        struct pep_sock *pn = pep_sk(sk);
 517        struct pnpipehdr *hdr = pnp_hdr(skb);
 518        struct sk_buff_head *queue;
 519        int err = 0;
 520
 521        BUG_ON(sk->sk_state == TCP_CLOSE_WAIT);
 522
 523        switch (hdr->message_id) {
 524        case PNS_PEP_CONNECT_REQ:
 525                pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE);
 526                break;
 527
 528        case PNS_PEP_DISCONNECT_REQ:
 529                pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
 530                sk->sk_state = TCP_CLOSE_WAIT;
 531                if (!sock_flag(sk, SOCK_DEAD))
 532                        sk->sk_state_change(sk);
 533                break;
 534
 535#ifdef CONFIG_PHONET_PIPECTRLR
 536        case PNS_PEP_DISCONNECT_RESP:
 537                pn->pipe_state = PIPE_IDLE;
 538                sk->sk_state = TCP_CLOSE;
 539                break;
 540#endif
 541
 542        case PNS_PEP_ENABLE_REQ:
 543                /* Wait for PNS_PIPE_(ENABLED|REDIRECTED)_IND */
 544                pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
 545                break;
 546
 547#ifdef CONFIG_PHONET_PIPECTRLR
 548        case PNS_PEP_ENABLE_RESP:
 549                pn->pipe_state = PIPE_ENABLED;
 550                pipe_handler_send_ind(sk, PNS_PIPE_ENABLED_IND_UTID,
 551                                PNS_PIPE_ENABLED_IND);
 552
 553                if (!pn_flow_safe(pn->tx_fc)) {
 554                        atomic_set(&pn->tx_credits, 1);
 555                        sk->sk_write_space(sk);
 556                }
 557                if (sk->sk_state == TCP_ESTABLISHED)
 558                        break; /* Nothing to do */
 559                sk->sk_state = TCP_ESTABLISHED;
 560                pipe_grant_credits(sk);
 561                break;
 562#endif
 563
 564        case PNS_PEP_RESET_REQ:
 565                switch (hdr->state_after_reset) {
 566                case PN_PIPE_DISABLE:
 567                        pn->init_enable = 0;
 568                        break;
 569                case PN_PIPE_ENABLE:
 570                        pn->init_enable = 1;
 571                        break;
 572                default: /* not allowed to send an error here!? */
 573                        err = -EINVAL;
 574                        goto out;
 575                }
 576                /* fall through */
 577        case PNS_PEP_DISABLE_REQ:
 578                atomic_set(&pn->tx_credits, 0);
 579                pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
 580                break;
 581
 582#ifdef CONFIG_PHONET_PIPECTRLR
 583        case PNS_PEP_DISABLE_RESP:
 584                pn->pipe_state = PIPE_DISABLED;
 585                atomic_set(&pn->tx_credits, 0);
 586                pipe_handler_send_ind(sk, PNS_PIPE_DISABLED_IND_UTID,
 587                                PNS_PIPE_DISABLED_IND);
 588                sk->sk_state = TCP_SYN_RECV;
 589                pn->rx_credits = 0;
 590                break;
 591#endif
 592
 593        case PNS_PEP_CTRL_REQ:
 594                if (skb_queue_len(&pn->ctrlreq_queue) >= PNPIPE_CTRLREQ_MAX) {
 595                        atomic_inc(&sk->sk_drops);
 596                        break;
 597                }
 598                __skb_pull(skb, 4);
 599                queue = &pn->ctrlreq_queue;
 600                goto queue;
 601
 602        case PNS_PIPE_ALIGNED_DATA:
 603                __skb_pull(skb, 1);
 604                /* fall through */
 605        case PNS_PIPE_DATA:
 606                __skb_pull(skb, 3); /* Pipe data header */
 607                if (!pn_flow_safe(pn->rx_fc)) {
 608                        err = sock_queue_rcv_skb(sk, skb);
 609                        if (!err)
 610                                return 0;
 611                        break;
 612                }
 613
 614                if (pn->rx_credits == 0) {
 615                        atomic_inc(&sk->sk_drops);
 616                        err = -ENOBUFS;
 617                        break;
 618                }
 619                pn->rx_credits--;
 620                queue = &sk->sk_receive_queue;
 621                goto queue;
 622
 623        case PNS_PEP_STATUS_IND:
 624                pipe_rcv_status(sk, skb);
 625                break;
 626
 627        case PNS_PIPE_REDIRECTED_IND:
 628                err = pipe_rcv_created(sk, skb);
 629                break;
 630
 631        case PNS_PIPE_CREATED_IND:
 632                err = pipe_rcv_created(sk, skb);
 633                if (err)
 634                        break;
 635                /* fall through */
 636        case PNS_PIPE_RESET_IND:
 637                if (!pn->init_enable)
 638                        break;
 639                /* fall through */
 640        case PNS_PIPE_ENABLED_IND:
 641                if (!pn_flow_safe(pn->tx_fc)) {
 642                        atomic_set(&pn->tx_credits, 1);
 643                        sk->sk_write_space(sk);
 644                }
 645                if (sk->sk_state == TCP_ESTABLISHED)
 646                        break; /* Nothing to do */
 647                sk->sk_state = TCP_ESTABLISHED;
 648                pipe_grant_credits(sk);
 649                break;
 650
 651        case PNS_PIPE_DISABLED_IND:
 652                sk->sk_state = TCP_SYN_RECV;
 653                pn->rx_credits = 0;
 654                break;
 655
 656        default:
 657                LIMIT_NETDEBUG(KERN_DEBUG"Phonet unknown PEP message: %u\n",
 658                                hdr->message_id);
 659                err = -EINVAL;
 660        }
 661out:
 662        kfree_skb(skb);
 663        return err;
 664
 665queue:
 666        skb->dev = NULL;
 667        skb_set_owner_r(skb, sk);
 668        err = skb->len;
 669        skb_queue_tail(queue, skb);
 670        if (!sock_flag(sk, SOCK_DEAD))
 671                sk->sk_data_ready(sk, err);
 672        return 0;
 673}
 674
 675/* Destroy connected sock. */
 676static void pipe_destruct(struct sock *sk)
 677{
 678        struct pep_sock *pn = pep_sk(sk);
 679
 680        skb_queue_purge(&sk->sk_receive_queue);
 681        skb_queue_purge(&pn->ctrlreq_queue);
 682}
 683
 684#ifdef CONFIG_PHONET_PIPECTRLR
 685static int pep_connresp_rcv(struct sock *sk, struct sk_buff *skb)
 686{
 687        struct pep_sock *pn = pep_sk(sk);
 688        u8 host_pref_rx_fc[3] = {3, 2, 1}, host_req_tx_fc[3] = {3, 2, 1};
 689        u8 remote_pref_rx_fc[3], remote_req_tx_fc[3];
 690        u8 negotiated_rx_fc, negotiated_tx_fc;
 691        int ret;
 692
 693        pipe_get_flow_info(sk, skb, remote_pref_rx_fc,
 694                        remote_req_tx_fc);
 695        negotiated_tx_fc = pipe_negotiate_fc(remote_req_tx_fc,
 696                        host_pref_rx_fc,
 697                        sizeof(host_pref_rx_fc));
 698        negotiated_rx_fc = pipe_negotiate_fc(host_req_tx_fc,
 699                        remote_pref_rx_fc,
 700                        sizeof(host_pref_rx_fc));
 701
 702        pn->pipe_state = PIPE_DISABLED;
 703        sk->sk_state = TCP_SYN_RECV;
 704        sk->sk_backlog_rcv = pipe_do_rcv;
 705        sk->sk_destruct = pipe_destruct;
 706        pn->rx_credits = 0;
 707        pn->rx_fc = negotiated_rx_fc;
 708        pn->tx_fc = negotiated_tx_fc;
 709        sk->sk_state_change(sk);
 710
 711        ret = pipe_handler_send_created_ind(sk,
 712                        PNS_PIPE_CREATED_IND_UTID,
 713                        PNS_PIPE_CREATED_IND
 714                        );
 715
 716        return ret;
 717}
 718#endif
 719
 720static int pep_connreq_rcv(struct sock *sk, struct sk_buff *skb)
 721{
 722        struct sock *newsk;
 723        struct pep_sock *newpn, *pn = pep_sk(sk);
 724        struct pnpipehdr *hdr;
 725        struct sockaddr_pn dst;
 726        u16 peer_type;
 727        u8 pipe_handle, enabled, n_sb;
 728        u8 aligned = 0;
 729
 730        if (!pskb_pull(skb, sizeof(*hdr) + 4))
 731                return -EINVAL;
 732
 733        hdr = pnp_hdr(skb);
 734        pipe_handle = hdr->pipe_handle;
 735        switch (hdr->state_after_connect) {
 736        case PN_PIPE_DISABLE:
 737                enabled = 0;
 738                break;
 739        case PN_PIPE_ENABLE:
 740                enabled = 1;
 741                break;
 742        default:
 743                pep_reject_conn(sk, skb, PN_PIPE_ERR_INVALID_PARAM);
 744                return -EINVAL;
 745        }
 746        peer_type = hdr->other_pep_type << 8;
 747
 748        if (unlikely(sk->sk_state != TCP_LISTEN) || sk_acceptq_is_full(sk)) {
 749                pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE);
 750                return -ENOBUFS;
 751        }
 752
 753        /* Parse sub-blocks (options) */
 754        n_sb = hdr->data[4];
 755        while (n_sb > 0) {
 756                u8 type, buf[1], len = sizeof(buf);
 757                const u8 *data = pep_get_sb(skb, &type, &len, buf);
 758
 759                if (data == NULL)
 760                        return -EINVAL;
 761                switch (type) {
 762                case PN_PIPE_SB_CONNECT_REQ_PEP_SUB_TYPE:
 763                        if (len < 1)
 764                                return -EINVAL;
 765                        peer_type = (peer_type & 0xff00) | data[0];
 766                        break;
 767                case PN_PIPE_SB_ALIGNED_DATA:
 768                        aligned = data[0] != 0;
 769                        break;
 770                }
 771                n_sb--;
 772        }
 773
 774        skb = skb_clone(skb, GFP_ATOMIC);
 775        if (!skb)
 776                return -ENOMEM;
 777
 778        /* Create a new to-be-accepted sock */
 779        newsk = sk_alloc(sock_net(sk), PF_PHONET, GFP_ATOMIC, sk->sk_prot);
 780        if (!newsk) {
 781                kfree_skb(skb);
 782                return -ENOMEM;
 783        }
 784        sock_init_data(NULL, newsk);
 785        newsk->sk_state = TCP_SYN_RECV;
 786        newsk->sk_backlog_rcv = pipe_do_rcv;
 787        newsk->sk_protocol = sk->sk_protocol;
 788        newsk->sk_destruct = pipe_destruct;
 789
 790        newpn = pep_sk(newsk);
 791        pn_skb_get_dst_sockaddr(skb, &dst);
 792        newpn->pn_sk.sobject = pn_sockaddr_get_object(&dst);
 793        newpn->pn_sk.resource = pn->pn_sk.resource;
 794        skb_queue_head_init(&newpn->ctrlreq_queue);
 795        newpn->pipe_handle = pipe_handle;
 796        atomic_set(&newpn->tx_credits, 0);
 797        newpn->peer_type = peer_type;
 798        newpn->rx_credits = 0;
 799        newpn->rx_fc = newpn->tx_fc = PN_LEGACY_FLOW_CONTROL;
 800        newpn->init_enable = enabled;
 801        newpn->aligned = aligned;
 802
 803        BUG_ON(!skb_queue_empty(&newsk->sk_receive_queue));
 804        skb_queue_head(&newsk->sk_receive_queue, skb);
 805        if (!sock_flag(sk, SOCK_DEAD))
 806                sk->sk_data_ready(sk, 0);
 807
 808        sk_acceptq_added(sk);
 809        sk_add_node(newsk, &pn->ackq);
 810        return 0;
 811}
 812
 813/* Listening sock must be locked */
 814static struct sock *pep_find_pipe(const struct hlist_head *hlist,
 815                                        const struct sockaddr_pn *dst,
 816                                        u8 pipe_handle)
 817{
 818        struct hlist_node *node;
 819        struct sock *sknode;
 820        u16 dobj = pn_sockaddr_get_object(dst);
 821
 822        sk_for_each(sknode, node, hlist) {
 823                struct pep_sock *pnnode = pep_sk(sknode);
 824
 825                /* Ports match, but addresses might not: */
 826                if (pnnode->pn_sk.sobject != dobj)
 827                        continue;
 828                if (pnnode->pipe_handle != pipe_handle)
 829                        continue;
 830                if (sknode->sk_state == TCP_CLOSE_WAIT)
 831                        continue;
 832
 833                sock_hold(sknode);
 834                return sknode;
 835        }
 836        return NULL;
 837}
 838
 839/*
 840 * Deliver an skb to a listening sock.
 841 * Socket lock must be held.
 842 * We then queue the skb to the right connected sock (if any).
 843 */
 844static int pep_do_rcv(struct sock *sk, struct sk_buff *skb)
 845{
 846        struct pep_sock *pn = pep_sk(sk);
 847        struct sock *sknode;
 848        struct pnpipehdr *hdr;
 849        struct sockaddr_pn dst;
 850        int err = NET_RX_SUCCESS;
 851        u8 pipe_handle;
 852
 853        if (!pskb_may_pull(skb, sizeof(*hdr)))
 854                goto drop;
 855
 856        hdr = pnp_hdr(skb);
 857        pipe_handle = hdr->pipe_handle;
 858        if (pipe_handle == PN_PIPE_INVALID_HANDLE)
 859                goto drop;
 860
 861        pn_skb_get_dst_sockaddr(skb, &dst);
 862
 863        /* Look for an existing pipe handle */
 864        sknode = pep_find_pipe(&pn->hlist, &dst, pipe_handle);
 865        if (sknode)
 866                return sk_receive_skb(sknode, skb, 1);
 867
 868        /* Look for a pipe handle pending accept */
 869        sknode = pep_find_pipe(&pn->ackq, &dst, pipe_handle);
 870        if (sknode) {
 871                sock_put(sknode);
 872                if (net_ratelimit())
 873                        printk(KERN_WARNING"Phonet unconnected PEP ignored");
 874                err = NET_RX_DROP;
 875                goto drop;
 876        }
 877
 878        switch (hdr->message_id) {
 879        case PNS_PEP_CONNECT_REQ:
 880                err = pep_connreq_rcv(sk, skb);
 881                break;
 882
 883#ifdef CONFIG_PHONET_PIPECTRLR
 884        case PNS_PEP_CONNECT_RESP:
 885                err = pep_connresp_rcv(sk, skb);
 886                break;
 887#endif
 888
 889        case PNS_PEP_DISCONNECT_REQ:
 890                pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC);
 891                break;
 892
 893        case PNS_PEP_CTRL_REQ:
 894                pep_ctrlreq_error(sk, skb, PN_PIPE_INVALID_HANDLE, GFP_ATOMIC);
 895                break;
 896
 897        case PNS_PEP_RESET_REQ:
 898        case PNS_PEP_ENABLE_REQ:
 899        case PNS_PEP_DISABLE_REQ:
 900                /* invalid handle is not even allowed here! */
 901        default:
 902                err = NET_RX_DROP;
 903        }
 904drop:
 905        kfree_skb(skb);
 906        return err;
 907}
 908
 909static int pipe_do_remove(struct sock *sk)
 910{
 911        struct pep_sock *pn = pep_sk(sk);
 912        struct pnpipehdr *ph;
 913        struct sk_buff *skb;
 914
 915        skb = alloc_skb(MAX_PNPIPE_HEADER, GFP_KERNEL);
 916        if (!skb)
 917                return -ENOMEM;
 918
 919        skb_reserve(skb, MAX_PNPIPE_HEADER);
 920        __skb_push(skb, sizeof(*ph));
 921        skb_reset_transport_header(skb);
 922        ph = pnp_hdr(skb);
 923        ph->utid = 0;
 924        ph->message_id = PNS_PIPE_REMOVE_REQ;
 925        ph->pipe_handle = pn->pipe_handle;
 926        ph->data[0] = PAD;
 927
 928        return pn_skb_send(sk, skb, &pipe_srv);
 929}
 930
 931/* associated socket ceases to exist */
 932static void pep_sock_close(struct sock *sk, long timeout)
 933{
 934        struct pep_sock *pn = pep_sk(sk);
 935        int ifindex = 0;
 936
 937        sock_hold(sk); /* keep a reference after sk_common_release() */
 938        sk_common_release(sk);
 939
 940        lock_sock(sk);
 941        if (sk->sk_state == TCP_LISTEN) {
 942                /* Destroy the listen queue */
 943                struct sock *sknode;
 944                struct hlist_node *p, *n;
 945
 946                sk_for_each_safe(sknode, p, n, &pn->ackq)
 947                        sk_del_node_init(sknode);
 948                sk->sk_state = TCP_CLOSE;
 949        } else if ((1 << sk->sk_state) & (TCPF_SYN_RECV|TCPF_ESTABLISHED))
 950                /* Forcefully remove dangling Phonet pipe */
 951                pipe_do_remove(sk);
 952
 953#ifdef CONFIG_PHONET_PIPECTRLR
 954        if (pn->pipe_state != PIPE_IDLE) {
 955                /* send pep disconnect request */
 956                pipe_handler_send_req(sk,
 957                                PNS_PEP_DISCONNECT_UTID, PNS_PEP_DISCONNECT_REQ,
 958                                GFP_KERNEL);
 959
 960                pn->pipe_state = PIPE_IDLE;
 961                sk->sk_state = TCP_CLOSE;
 962        }
 963#endif
 964
 965        ifindex = pn->ifindex;
 966        pn->ifindex = 0;
 967        release_sock(sk);
 968
 969        if (ifindex)
 970                gprs_detach(sk);
 971        sock_put(sk);
 972}
 973
 974static int pep_wait_connreq(struct sock *sk, int noblock)
 975{
 976        struct task_struct *tsk = current;
 977        struct pep_sock *pn = pep_sk(sk);
 978        long timeo = sock_rcvtimeo(sk, noblock);
 979
 980        for (;;) {
 981                DEFINE_WAIT(wait);
 982
 983                if (sk->sk_state != TCP_LISTEN)
 984                        return -EINVAL;
 985                if (!hlist_empty(&pn->ackq))
 986                        break;
 987                if (!timeo)
 988                        return -EWOULDBLOCK;
 989                if (signal_pending(tsk))
 990                        return sock_intr_errno(timeo);
 991
 992                prepare_to_wait_exclusive(sk_sleep(sk), &wait,
 993                                                TASK_INTERRUPTIBLE);
 994                release_sock(sk);
 995                timeo = schedule_timeout(timeo);
 996                lock_sock(sk);
 997                finish_wait(sk_sleep(sk), &wait);
 998        }
 999
1000        return 0;
1001}
1002
1003static struct sock *pep_sock_accept(struct sock *sk, int flags, int *errp)
1004{
1005        struct pep_sock *pn = pep_sk(sk);
1006        struct sock *newsk = NULL;
1007        struct sk_buff *oskb;
1008        int err;
1009
1010        lock_sock(sk);
1011        err = pep_wait_connreq(sk, flags & O_NONBLOCK);
1012        if (err)
1013                goto out;
1014
1015        newsk = __sk_head(&pn->ackq);
1016
1017        oskb = skb_dequeue(&newsk->sk_receive_queue);
1018        err = pep_accept_conn(newsk, oskb);
1019        if (err) {
1020                skb_queue_head(&newsk->sk_receive_queue, oskb);
1021                newsk = NULL;
1022                goto out;
1023        }
1024        kfree_skb(oskb);
1025
1026        sock_hold(sk);
1027        pep_sk(newsk)->listener = sk;
1028
1029        sock_hold(newsk);
1030        sk_del_node_init(newsk);
1031        sk_acceptq_removed(sk);
1032        sk_add_node(newsk, &pn->hlist);
1033        __sock_put(newsk);
1034
1035out:
1036        release_sock(sk);
1037        *errp = err;
1038        return newsk;
1039}
1040
1041#ifdef CONFIG_PHONET_PIPECTRLR
1042static int pep_sock_connect(struct sock *sk, struct sockaddr *addr, int len)
1043{
1044        struct pep_sock *pn = pep_sk(sk);
1045        struct sockaddr_pn *spn =  (struct sockaddr_pn *)addr;
1046
1047        memcpy(&pn->remote_pep, spn, sizeof(struct sockaddr_pn));
1048
1049        return pipe_handler_send_req(sk,
1050                        PNS_PEP_CONNECT_UTID, PNS_PEP_CONNECT_REQ,
1051                        GFP_ATOMIC);
1052}
1053#endif
1054
1055static int pep_ioctl(struct sock *sk, int cmd, unsigned long arg)
1056{
1057        struct pep_sock *pn = pep_sk(sk);
1058        int answ;
1059
1060        switch (cmd) {
1061        case SIOCINQ:
1062                if (sk->sk_state == TCP_LISTEN)
1063                        return -EINVAL;
1064
1065                lock_sock(sk);
1066                if (sock_flag(sk, SOCK_URGINLINE) &&
1067                    !skb_queue_empty(&pn->ctrlreq_queue))
1068                        answ = skb_peek(&pn->ctrlreq_queue)->len;
1069                else if (!skb_queue_empty(&sk->sk_receive_queue))
1070                        answ = skb_peek(&sk->sk_receive_queue)->len;
1071                else
1072                        answ = 0;
1073                release_sock(sk);
1074                return put_user(answ, (int __user *)arg);
1075        }
1076
1077        return -ENOIOCTLCMD;
1078}
1079
1080static int pep_init(struct sock *sk)
1081{
1082        struct pep_sock *pn = pep_sk(sk);
1083
1084        INIT_HLIST_HEAD(&pn->ackq);
1085        INIT_HLIST_HEAD(&pn->hlist);
1086        skb_queue_head_init(&pn->ctrlreq_queue);
1087        pn->pipe_handle = PN_PIPE_INVALID_HANDLE;
1088        return 0;
1089}
1090
1091static int pep_setsockopt(struct sock *sk, int level, int optname,
1092                                char __user *optval, unsigned int optlen)
1093{
1094        struct pep_sock *pn = pep_sk(sk);
1095        int val = 0, err = 0;
1096
1097        if (level != SOL_PNPIPE)
1098                return -ENOPROTOOPT;
1099        if (optlen >= sizeof(int)) {
1100                if (get_user(val, (int __user *) optval))
1101                        return -EFAULT;
1102        }
1103
1104        lock_sock(sk);
1105        switch (optname) {
1106#ifdef CONFIG_PHONET_PIPECTRLR
1107        case PNPIPE_PIPE_HANDLE:
1108                if (val) {
1109                        if (pn->pipe_state > PIPE_IDLE) {
1110                                err = -EFAULT;
1111                                break;
1112                        }
1113                        pn->pipe_handle = val;
1114                        break;
1115                }
1116#endif
1117
1118        case PNPIPE_ENCAP:
1119                if (val && val != PNPIPE_ENCAP_IP) {
1120                        err = -EINVAL;
1121                        break;
1122                }
1123                if (!pn->ifindex == !val)
1124                        break; /* Nothing to do! */
1125                if (!capable(CAP_NET_ADMIN)) {
1126                        err = -EPERM;
1127                        break;
1128                }
1129                if (val) {
1130                        release_sock(sk);
1131                        err = gprs_attach(sk);
1132                        if (err > 0) {
1133                                pn->ifindex = err;
1134                                err = 0;
1135                        }
1136                } else {
1137                        pn->ifindex = 0;
1138                        release_sock(sk);
1139                        gprs_detach(sk);
1140                        err = 0;
1141                }
1142                goto out_norel;
1143
1144#ifdef CONFIG_PHONET_PIPECTRLR
1145        case PNPIPE_ENABLE:
1146                if (pn->pipe_state <= PIPE_IDLE) {
1147                        err = -ENOTCONN;
1148                        break;
1149                }
1150                err = pipe_handler_enable_pipe(sk, val);
1151                break;
1152#endif
1153
1154        default:
1155                err = -ENOPROTOOPT;
1156        }
1157        release_sock(sk);
1158
1159out_norel:
1160        return err;
1161}
1162
1163static int pep_getsockopt(struct sock *sk, int level, int optname,
1164                                char __user *optval, int __user *optlen)
1165{
1166        struct pep_sock *pn = pep_sk(sk);
1167        int len, val;
1168
1169        if (level != SOL_PNPIPE)
1170                return -ENOPROTOOPT;
1171        if (get_user(len, optlen))
1172                return -EFAULT;
1173
1174        switch (optname) {
1175        case PNPIPE_ENCAP:
1176                val = pn->ifindex ? PNPIPE_ENCAP_IP : PNPIPE_ENCAP_NONE;
1177                break;
1178
1179        case PNPIPE_IFINDEX:
1180                val = pn->ifindex;
1181                break;
1182
1183#ifdef CONFIG_PHONET_PIPECTRLR
1184        case PNPIPE_ENABLE:
1185                if (pn->pipe_state <= PIPE_IDLE)
1186                        return -ENOTCONN;
1187                val = pn->pipe_state != PIPE_DISABLED;
1188                break;
1189#endif
1190
1191        default:
1192                return -ENOPROTOOPT;
1193        }
1194
1195        len = min_t(unsigned int, sizeof(int), len);
1196        if (put_user(len, optlen))
1197                return -EFAULT;
1198        if (put_user(val, (int __user *) optval))
1199                return -EFAULT;
1200        return 0;
1201}
1202
1203static int pipe_skb_send(struct sock *sk, struct sk_buff *skb)
1204{
1205        struct pep_sock *pn = pep_sk(sk);
1206        struct pnpipehdr *ph;
1207        int err;
1208
1209        if (pn_flow_safe(pn->tx_fc) &&
1210            !atomic_add_unless(&pn->tx_credits, -1, 0)) {
1211                kfree_skb(skb);
1212                return -ENOBUFS;
1213        }
1214
1215        skb_push(skb, 3 + pn->aligned);
1216        skb_reset_transport_header(skb);
1217        ph = pnp_hdr(skb);
1218        ph->utid = 0;
1219        if (pn->aligned) {
1220                ph->message_id = PNS_PIPE_ALIGNED_DATA;
1221                ph->data[0] = 0; /* padding */
1222        } else
1223                ph->message_id = PNS_PIPE_DATA;
1224        ph->pipe_handle = pn->pipe_handle;
1225#ifdef CONFIG_PHONET_PIPECTRLR
1226        err = pn_skb_send(sk, skb, &pn->remote_pep);
1227#else
1228        err = pn_skb_send(sk, skb, &pipe_srv);
1229#endif
1230
1231        if (err && pn_flow_safe(pn->tx_fc))
1232                atomic_inc(&pn->tx_credits);
1233        return err;
1234
1235}
1236
1237static int pep_sendmsg(struct kiocb *iocb, struct sock *sk,
1238                        struct msghdr *msg, size_t len)
1239{
1240        struct pep_sock *pn = pep_sk(sk);
1241        struct sk_buff *skb;
1242        long timeo;
1243        int flags = msg->msg_flags;
1244        int err, done;
1245
1246        if ((msg->msg_flags & ~(MSG_DONTWAIT|MSG_EOR|MSG_NOSIGNAL|
1247                                MSG_CMSG_COMPAT)) ||
1248                        !(msg->msg_flags & MSG_EOR))
1249                return -EOPNOTSUPP;
1250
1251        skb = sock_alloc_send_skb(sk, MAX_PNPIPE_HEADER + len,
1252                                        flags & MSG_DONTWAIT, &err);
1253        if (!skb)
1254                return err;
1255
1256        skb_reserve(skb, MAX_PHONET_HEADER + 3);
1257        err = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len);
1258        if (err < 0)
1259                goto outfree;
1260
1261        lock_sock(sk);
1262        timeo = sock_sndtimeo(sk, flags & MSG_DONTWAIT);
1263        if ((1 << sk->sk_state) & (TCPF_LISTEN|TCPF_CLOSE)) {
1264                err = -ENOTCONN;
1265                goto out;
1266        }
1267        if (sk->sk_state != TCP_ESTABLISHED) {
1268                /* Wait until the pipe gets to enabled state */
1269disabled:
1270                err = sk_stream_wait_connect(sk, &timeo);
1271                if (err)
1272                        goto out;
1273
1274                if (sk->sk_state == TCP_CLOSE_WAIT) {
1275                        err = -ECONNRESET;
1276                        goto out;
1277                }
1278        }
1279        BUG_ON(sk->sk_state != TCP_ESTABLISHED);
1280
1281        /* Wait until flow control allows TX */
1282        done = atomic_read(&pn->tx_credits);
1283        while (!done) {
1284                DEFINE_WAIT(wait);
1285
1286                if (!timeo) {
1287                        err = -EAGAIN;
1288                        goto out;
1289                }
1290                if (signal_pending(current)) {
1291                        err = sock_intr_errno(timeo);
1292                        goto out;
1293                }
1294
1295                prepare_to_wait(sk_sleep(sk), &wait,
1296                                TASK_INTERRUPTIBLE);
1297                done = sk_wait_event(sk, &timeo, atomic_read(&pn->tx_credits));
1298                finish_wait(sk_sleep(sk), &wait);
1299
1300                if (sk->sk_state != TCP_ESTABLISHED)
1301                        goto disabled;
1302        }
1303
1304        err = pipe_skb_send(sk, skb);
1305        if (err >= 0)
1306                err = len; /* success! */
1307        skb = NULL;
1308out:
1309        release_sock(sk);
1310outfree:
1311        kfree_skb(skb);
1312        return err;
1313}
1314
1315int pep_writeable(struct sock *sk)
1316{
1317        struct pep_sock *pn = pep_sk(sk);
1318
1319        return atomic_read(&pn->tx_credits);
1320}
1321
1322int pep_write(struct sock *sk, struct sk_buff *skb)
1323{
1324        struct sk_buff *rskb, *fs;
1325        int flen = 0;
1326
1327        if (pep_sk(sk)->aligned)
1328                return pipe_skb_send(sk, skb);
1329
1330        rskb = alloc_skb(MAX_PNPIPE_HEADER, GFP_ATOMIC);
1331        if (!rskb) {
1332                kfree_skb(skb);
1333                return -ENOMEM;
1334        }
1335        skb_shinfo(rskb)->frag_list = skb;
1336        rskb->len += skb->len;
1337        rskb->data_len += rskb->len;
1338        rskb->truesize += rskb->len;
1339
1340        /* Avoid nested fragments */
1341        skb_walk_frags(skb, fs)
1342                flen += fs->len;
1343        skb->next = skb_shinfo(skb)->frag_list;
1344        skb_frag_list_init(skb);
1345        skb->len -= flen;
1346        skb->data_len -= flen;
1347        skb->truesize -= flen;
1348
1349        skb_reserve(rskb, MAX_PHONET_HEADER + 3);
1350        return pipe_skb_send(sk, rskb);
1351}
1352
1353struct sk_buff *pep_read(struct sock *sk)
1354{
1355        struct sk_buff *skb = skb_dequeue(&sk->sk_receive_queue);
1356
1357        if (sk->sk_state == TCP_ESTABLISHED)
1358                pipe_grant_credits(sk);
1359        return skb;
1360}
1361
1362static int pep_recvmsg(struct kiocb *iocb, struct sock *sk,
1363                        struct msghdr *msg, size_t len, int noblock,
1364                        int flags, int *addr_len)
1365{
1366        struct sk_buff *skb;
1367        int err;
1368
1369        if (flags & ~(MSG_OOB|MSG_PEEK|MSG_TRUNC|MSG_DONTWAIT|MSG_WAITALL|
1370                        MSG_NOSIGNAL|MSG_CMSG_COMPAT))
1371                return -EOPNOTSUPP;
1372
1373        if (unlikely(1 << sk->sk_state & (TCPF_LISTEN | TCPF_CLOSE)))
1374                return -ENOTCONN;
1375
1376        if ((flags & MSG_OOB) || sock_flag(sk, SOCK_URGINLINE)) {
1377                /* Dequeue and acknowledge control request */
1378                struct pep_sock *pn = pep_sk(sk);
1379
1380                if (flags & MSG_PEEK)
1381                        return -EOPNOTSUPP;
1382                skb = skb_dequeue(&pn->ctrlreq_queue);
1383                if (skb) {
1384                        pep_ctrlreq_error(sk, skb, PN_PIPE_NO_ERROR,
1385                                                GFP_KERNEL);
1386                        msg->msg_flags |= MSG_OOB;
1387                        goto copy;
1388                }
1389                if (flags & MSG_OOB)
1390                        return -EINVAL;
1391        }
1392
1393        skb = skb_recv_datagram(sk, flags, noblock, &err);
1394        lock_sock(sk);
1395        if (skb == NULL) {
1396                if (err == -ENOTCONN && sk->sk_state == TCP_CLOSE_WAIT)
1397                        err = -ECONNRESET;
1398                release_sock(sk);
1399                return err;
1400        }
1401
1402        if (sk->sk_state == TCP_ESTABLISHED)
1403                pipe_grant_credits(sk);
1404        release_sock(sk);
1405copy:
1406        msg->msg_flags |= MSG_EOR;
1407        if (skb->len > len)
1408                msg->msg_flags |= MSG_TRUNC;
1409        else
1410                len = skb->len;
1411
1412        err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, len);
1413        if (!err)
1414                err = (flags & MSG_TRUNC) ? skb->len : len;
1415
1416        skb_free_datagram(sk, skb);
1417        return err;
1418}
1419
1420static void pep_sock_unhash(struct sock *sk)
1421{
1422        struct pep_sock *pn = pep_sk(sk);
1423        struct sock *skparent = NULL;
1424
1425        lock_sock(sk);
1426
1427#ifndef CONFIG_PHONET_PIPECTRLR
1428        if ((1 << sk->sk_state) & ~(TCPF_CLOSE|TCPF_LISTEN)) {
1429                skparent = pn->listener;
1430                release_sock(sk);
1431
1432                pn = pep_sk(skparent);
1433                lock_sock(skparent);
1434                sk_del_node_init(sk);
1435                sk = skparent;
1436        }
1437#endif
1438        /* Unhash a listening sock only when it is closed
1439         * and all of its active connected pipes are closed. */
1440        if (hlist_empty(&pn->hlist))
1441                pn_sock_unhash(&pn->pn_sk.sk);
1442        release_sock(sk);
1443
1444        if (skparent)
1445                sock_put(skparent);
1446}
1447
1448static struct proto pep_proto = {
1449        .close          = pep_sock_close,
1450        .accept         = pep_sock_accept,
1451#ifdef CONFIG_PHONET_PIPECTRLR
1452        .connect        = pep_sock_connect,
1453#endif
1454        .ioctl          = pep_ioctl,
1455        .init           = pep_init,
1456        .setsockopt     = pep_setsockopt,
1457        .getsockopt     = pep_getsockopt,
1458        .sendmsg        = pep_sendmsg,
1459        .recvmsg        = pep_recvmsg,
1460        .backlog_rcv    = pep_do_rcv,
1461        .hash           = pn_sock_hash,
1462        .unhash         = pep_sock_unhash,
1463        .get_port       = pn_sock_get_port,
1464        .obj_size       = sizeof(struct pep_sock),
1465        .owner          = THIS_MODULE,
1466        .name           = "PNPIPE",
1467};
1468
1469static struct phonet_protocol pep_pn_proto = {
1470        .ops            = &phonet_stream_ops,
1471        .prot           = &pep_proto,
1472        .sock_type      = SOCK_SEQPACKET,
1473};
1474
1475static int __init pep_register(void)
1476{
1477        return phonet_proto_register(PN_PROTO_PIPE, &pep_pn_proto);
1478}
1479
1480static void __exit pep_unregister(void)
1481{
1482        phonet_proto_unregister(PN_PROTO_PIPE, &pep_pn_proto);
1483}
1484
1485module_init(pep_register);
1486module_exit(pep_unregister);
1487MODULE_AUTHOR("Remi Denis-Courmont, Nokia");
1488MODULE_DESCRIPTION("Phonet pipe protocol");
1489MODULE_LICENSE("GPL");
1490MODULE_ALIAS_NET_PF_PROTO(PF_PHONET, PN_PROTO_PIPE);
1491