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