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 skb_queue_tail(queue, skb); 466 if (!sock_flag(sk, SOCK_DEAD)) 467 sk->sk_data_ready(sk); 468 return NET_RX_SUCCESS; 469} 470 471/* Destroy connected sock. */ 472static void pipe_destruct(struct sock *sk) 473{ 474 struct pep_sock *pn = pep_sk(sk); 475 476 skb_queue_purge(&sk->sk_receive_queue); 477 skb_queue_purge(&pn->ctrlreq_queue); 478} 479 480static u8 pipe_negotiate_fc(const u8 *fcs, unsigned int n) 481{ 482 unsigned int i; 483 u8 final_fc = PN_NO_FLOW_CONTROL; 484 485 for (i = 0; i < n; i++) { 486 u8 fc = fcs[i]; 487 488 if (fc > final_fc && fc < PN_MAX_FLOW_CONTROL) 489 final_fc = fc; 490 } 491 return final_fc; 492} 493 494static int pep_connresp_rcv(struct sock *sk, struct sk_buff *skb) 495{ 496 struct pep_sock *pn = pep_sk(sk); 497 struct pnpipehdr *hdr; 498 u8 n_sb; 499 500 if (!pskb_pull(skb, sizeof(*hdr) + 4)) 501 return -EINVAL; 502 503 hdr = pnp_hdr(skb); 504 if (hdr->error_code != PN_PIPE_NO_ERROR) 505 return -ECONNREFUSED; 506 507 /* Parse sub-blocks */ 508 n_sb = hdr->data[4]; 509 while (n_sb > 0) { 510 u8 type, buf[6], len = sizeof(buf); 511 const u8 *data = pep_get_sb(skb, &type, &len, buf); 512 513 if (data == NULL) 514 return -EINVAL; 515 516 switch (type) { 517 case PN_PIPE_SB_REQUIRED_FC_TX: 518 if (len < 2 || len < data[0]) 519 break; 520 pn->tx_fc = pipe_negotiate_fc(data + 2, len - 2); 521 break; 522 523 case PN_PIPE_SB_PREFERRED_FC_RX: 524 if (len < 2 || len < data[0]) 525 break; 526 pn->rx_fc = pipe_negotiate_fc(data + 2, len - 2); 527 break; 528 529 } 530 n_sb--; 531 } 532 533 return pipe_handler_send_created_ind(sk); 534} 535 536static int pep_enableresp_rcv(struct sock *sk, struct sk_buff *skb) 537{ 538 struct pnpipehdr *hdr = pnp_hdr(skb); 539 540 if (hdr->error_code != PN_PIPE_NO_ERROR) 541 return -ECONNREFUSED; 542 543 return pep_indicate(sk, PNS_PIPE_ENABLED_IND, 0 /* sub-blocks */, 544 NULL, 0, GFP_ATOMIC); 545 546} 547 548static void pipe_start_flow_control(struct sock *sk) 549{ 550 struct pep_sock *pn = pep_sk(sk); 551 552 if (!pn_flow_safe(pn->tx_fc)) { 553 atomic_set(&pn->tx_credits, 1); 554 sk->sk_write_space(sk); 555 } 556 pipe_grant_credits(sk, GFP_ATOMIC); 557} 558 559/* Queue an skb to an actively connected sock. 560 * Socket lock must be held. */ 561static int pipe_handler_do_rcv(struct sock *sk, struct sk_buff *skb) 562{ 563 struct pep_sock *pn = pep_sk(sk); 564 struct pnpipehdr *hdr = pnp_hdr(skb); 565 int err = NET_RX_SUCCESS; 566 567 switch (hdr->message_id) { 568 case PNS_PIPE_ALIGNED_DATA: 569 __skb_pull(skb, 1); 570 /* fall through */ 571 case PNS_PIPE_DATA: 572 __skb_pull(skb, 3); /* Pipe data header */ 573 if (!pn_flow_safe(pn->rx_fc)) { 574 err = sock_queue_rcv_skb(sk, skb); 575 if (!err) 576 return NET_RX_SUCCESS; 577 err = NET_RX_DROP; 578 break; 579 } 580 581 if (pn->rx_credits == 0) { 582 atomic_inc(&sk->sk_drops); 583 err = NET_RX_DROP; 584 break; 585 } 586 pn->rx_credits--; 587 skb->dev = NULL; 588 skb_set_owner_r(skb, sk); 589 skb_queue_tail(&sk->sk_receive_queue, skb); 590 if (!sock_flag(sk, SOCK_DEAD)) 591 sk->sk_data_ready(sk); 592 return NET_RX_SUCCESS; 593 594 case PNS_PEP_CONNECT_RESP: 595 if (sk->sk_state != TCP_SYN_SENT) 596 break; 597 if (!sock_flag(sk, SOCK_DEAD)) 598 sk->sk_state_change(sk); 599 if (pep_connresp_rcv(sk, skb)) { 600 sk->sk_state = TCP_CLOSE_WAIT; 601 break; 602 } 603 if (pn->init_enable == PN_PIPE_DISABLE) 604 sk->sk_state = TCP_SYN_RECV; 605 else { 606 sk->sk_state = TCP_ESTABLISHED; 607 pipe_start_flow_control(sk); 608 } 609 break; 610 611 case PNS_PEP_ENABLE_RESP: 612 if (sk->sk_state != TCP_SYN_SENT) 613 break; 614 615 if (pep_enableresp_rcv(sk, skb)) { 616 sk->sk_state = TCP_CLOSE_WAIT; 617 break; 618 } 619 620 sk->sk_state = TCP_ESTABLISHED; 621 pipe_start_flow_control(sk); 622 break; 623 624 case PNS_PEP_DISCONNECT_RESP: 625 /* sock should already be dead, nothing to do */ 626 break; 627 628 case PNS_PEP_STATUS_IND: 629 pipe_rcv_status(sk, skb); 630 break; 631 } 632 kfree_skb(skb); 633 return err; 634} 635 636/* Listening sock must be locked */ 637static struct sock *pep_find_pipe(const struct hlist_head *hlist, 638 const struct sockaddr_pn *dst, 639 u8 pipe_handle) 640{ 641 struct sock *sknode; 642 u16 dobj = pn_sockaddr_get_object(dst); 643 644 sk_for_each(sknode, hlist) { 645 struct pep_sock *pnnode = pep_sk(sknode); 646 647 /* Ports match, but addresses might not: */ 648 if (pnnode->pn_sk.sobject != dobj) 649 continue; 650 if (pnnode->pipe_handle != pipe_handle) 651 continue; 652 if (sknode->sk_state == TCP_CLOSE_WAIT) 653 continue; 654 655 sock_hold(sknode); 656 return sknode; 657 } 658 return NULL; 659} 660 661/* 662 * Deliver an skb to a listening sock. 663 * Socket lock must be held. 664 * We then queue the skb to the right connected sock (if any). 665 */ 666static int pep_do_rcv(struct sock *sk, struct sk_buff *skb) 667{ 668 struct pep_sock *pn = pep_sk(sk); 669 struct sock *sknode; 670 struct pnpipehdr *hdr; 671 struct sockaddr_pn dst; 672 u8 pipe_handle; 673 674 if (!pskb_may_pull(skb, sizeof(*hdr))) 675 goto drop; 676 677 hdr = pnp_hdr(skb); 678 pipe_handle = hdr->pipe_handle; 679 if (pipe_handle == PN_PIPE_INVALID_HANDLE) 680 goto drop; 681 682 pn_skb_get_dst_sockaddr(skb, &dst); 683 684 /* Look for an existing pipe handle */ 685 sknode = pep_find_pipe(&pn->hlist, &dst, pipe_handle); 686 if (sknode) 687 return sk_receive_skb(sknode, skb, 1); 688 689 switch (hdr->message_id) { 690 case PNS_PEP_CONNECT_REQ: 691 if (sk->sk_state != TCP_LISTEN || sk_acceptq_is_full(sk)) { 692 pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE, 693 GFP_ATOMIC); 694 break; 695 } 696 skb_queue_head(&sk->sk_receive_queue, skb); 697 sk_acceptq_added(sk); 698 if (!sock_flag(sk, SOCK_DEAD)) 699 sk->sk_data_ready(sk); 700 return NET_RX_SUCCESS; 701 702 case PNS_PEP_DISCONNECT_REQ: 703 pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC); 704 break; 705 706 case PNS_PEP_CTRL_REQ: 707 pep_ctrlreq_error(sk, skb, PN_PIPE_INVALID_HANDLE, GFP_ATOMIC); 708 break; 709 710 case PNS_PEP_RESET_REQ: 711 case PNS_PEP_ENABLE_REQ: 712 case PNS_PEP_DISABLE_REQ: 713 /* invalid handle is not even allowed here! */ 714 break; 715 716 default: 717 if ((1 << sk->sk_state) 718 & ~(TCPF_CLOSE|TCPF_LISTEN|TCPF_CLOSE_WAIT)) 719 /* actively connected socket */ 720 return pipe_handler_do_rcv(sk, skb); 721 } 722drop: 723 kfree_skb(skb); 724 return NET_RX_SUCCESS; 725} 726 727static int pipe_do_remove(struct sock *sk) 728{ 729 struct pep_sock *pn = pep_sk(sk); 730 struct pnpipehdr *ph; 731 struct sk_buff *skb; 732 733 skb = pep_alloc_skb(sk, NULL, 0, GFP_KERNEL); 734 if (!skb) 735 return -ENOMEM; 736 737 ph = pnp_hdr(skb); 738 ph->utid = 0; 739 ph->message_id = PNS_PIPE_REMOVE_REQ; 740 ph->pipe_handle = pn->pipe_handle; 741 ph->data[0] = PAD; 742 return pn_skb_send(sk, skb, NULL); 743} 744 745/* associated socket ceases to exist */ 746static void pep_sock_close(struct sock *sk, long timeout) 747{ 748 struct pep_sock *pn = pep_sk(sk); 749 int ifindex = 0; 750 751 sock_hold(sk); /* keep a reference after sk_common_release() */ 752 sk_common_release(sk); 753 754 lock_sock(sk); 755 if ((1 << sk->sk_state) & (TCPF_SYN_RECV|TCPF_ESTABLISHED)) { 756 if (sk->sk_backlog_rcv == pipe_do_rcv) 757 /* Forcefully remove dangling Phonet pipe */ 758 pipe_do_remove(sk); 759 else 760 pipe_handler_request(sk, PNS_PEP_DISCONNECT_REQ, PAD, 761 NULL, 0); 762 } 763 sk->sk_state = TCP_CLOSE; 764 765 ifindex = pn->ifindex; 766 pn->ifindex = 0; 767 release_sock(sk); 768 769 if (ifindex) 770 gprs_detach(sk); 771 sock_put(sk); 772} 773 774static struct sock *pep_sock_accept(struct sock *sk, int flags, int *errp) 775{ 776 struct pep_sock *pn = pep_sk(sk), *newpn; 777 struct sock *newsk = NULL; 778 struct sk_buff *skb; 779 struct pnpipehdr *hdr; 780 struct sockaddr_pn dst, src; 781 int err; 782 u16 peer_type; 783 u8 pipe_handle, enabled, n_sb; 784 u8 aligned = 0; 785 786 skb = skb_recv_datagram(sk, 0, flags & O_NONBLOCK, errp); 787 if (!skb) 788 return NULL; 789 790 lock_sock(sk); 791 if (sk->sk_state != TCP_LISTEN) { 792 err = -EINVAL; 793 goto drop; 794 } 795 sk_acceptq_removed(sk); 796 797 err = -EPROTO; 798 if (!pskb_may_pull(skb, sizeof(*hdr) + 4)) 799 goto drop; 800 801 hdr = pnp_hdr(skb); 802 pipe_handle = hdr->pipe_handle; 803 switch (hdr->state_after_connect) { 804 case PN_PIPE_DISABLE: 805 enabled = 0; 806 break; 807 case PN_PIPE_ENABLE: 808 enabled = 1; 809 break; 810 default: 811 pep_reject_conn(sk, skb, PN_PIPE_ERR_INVALID_PARAM, 812 GFP_KERNEL); 813 goto drop; 814 } 815 peer_type = hdr->other_pep_type << 8; 816 817 /* Parse sub-blocks (options) */ 818 n_sb = hdr->data[4]; 819 while (n_sb > 0) { 820 u8 type, buf[1], len = sizeof(buf); 821 const u8 *data = pep_get_sb(skb, &type, &len, buf); 822 823 if (data == NULL) 824 goto drop; 825 switch (type) { 826 case PN_PIPE_SB_CONNECT_REQ_PEP_SUB_TYPE: 827 if (len < 1) 828 goto drop; 829 peer_type = (peer_type & 0xff00) | data[0]; 830 break; 831 case PN_PIPE_SB_ALIGNED_DATA: 832 aligned = data[0] != 0; 833 break; 834 } 835 n_sb--; 836 } 837 838 /* Check for duplicate pipe handle */ 839 newsk = pep_find_pipe(&pn->hlist, &dst, pipe_handle); 840 if (unlikely(newsk)) { 841 __sock_put(newsk); 842 newsk = NULL; 843 pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE, GFP_KERNEL); 844 goto drop; 845 } 846 847 /* Create a new to-be-accepted sock */ 848 newsk = sk_alloc(sock_net(sk), PF_PHONET, GFP_KERNEL, sk->sk_prot); 849 if (!newsk) { 850 pep_reject_conn(sk, skb, PN_PIPE_ERR_OVERLOAD, GFP_KERNEL); 851 err = -ENOBUFS; 852 goto drop; 853 } 854 855 sock_init_data(NULL, newsk); 856 newsk->sk_state = TCP_SYN_RECV; 857 newsk->sk_backlog_rcv = pipe_do_rcv; 858 newsk->sk_protocol = sk->sk_protocol; 859 newsk->sk_destruct = pipe_destruct; 860 861 newpn = pep_sk(newsk); 862 pn_skb_get_dst_sockaddr(skb, &dst); 863 pn_skb_get_src_sockaddr(skb, &src); 864 newpn->pn_sk.sobject = pn_sockaddr_get_object(&dst); 865 newpn->pn_sk.dobject = pn_sockaddr_get_object(&src); 866 newpn->pn_sk.resource = pn_sockaddr_get_resource(&dst); 867 sock_hold(sk); 868 newpn->listener = sk; 869 skb_queue_head_init(&newpn->ctrlreq_queue); 870 newpn->pipe_handle = pipe_handle; 871 atomic_set(&newpn->tx_credits, 0); 872 newpn->ifindex = 0; 873 newpn->peer_type = peer_type; 874 newpn->rx_credits = 0; 875 newpn->rx_fc = newpn->tx_fc = PN_LEGACY_FLOW_CONTROL; 876 newpn->init_enable = enabled; 877 newpn->aligned = aligned; 878 879 err = pep_accept_conn(newsk, skb); 880 if (err) { 881 sock_put(newsk); 882 newsk = NULL; 883 goto drop; 884 } 885 sk_add_node(newsk, &pn->hlist); 886drop: 887 release_sock(sk); 888 kfree_skb(skb); 889 *errp = err; 890 return newsk; 891} 892 893static int pep_sock_connect(struct sock *sk, struct sockaddr *addr, int len) 894{ 895 struct pep_sock *pn = pep_sk(sk); 896 int err; 897 u8 data[4] = { 0 /* sub-blocks */, PAD, PAD, PAD }; 898 899 if (pn->pipe_handle == PN_PIPE_INVALID_HANDLE) 900 pn->pipe_handle = 1; /* anything but INVALID_HANDLE */ 901 902 err = pipe_handler_request(sk, PNS_PEP_CONNECT_REQ, 903 pn->init_enable, data, 4); 904 if (err) { 905 pn->pipe_handle = PN_PIPE_INVALID_HANDLE; 906 return err; 907 } 908 909 sk->sk_state = TCP_SYN_SENT; 910 911 return 0; 912} 913 914static int pep_sock_enable(struct sock *sk, struct sockaddr *addr, int len) 915{ 916 int err; 917 918 err = pipe_handler_request(sk, PNS_PEP_ENABLE_REQ, PAD, 919 NULL, 0); 920 if (err) 921 return err; 922 923 sk->sk_state = TCP_SYN_SENT; 924 925 return 0; 926} 927 928static int pep_ioctl(struct sock *sk, int cmd, unsigned long arg) 929{ 930 struct pep_sock *pn = pep_sk(sk); 931 int answ; 932 int ret = -ENOIOCTLCMD; 933 934 switch (cmd) { 935 case SIOCINQ: 936 if (sk->sk_state == TCP_LISTEN) { 937 ret = -EINVAL; 938 break; 939 } 940 941 lock_sock(sk); 942 if (sock_flag(sk, SOCK_URGINLINE) && 943 !skb_queue_empty(&pn->ctrlreq_queue)) 944 answ = skb_peek(&pn->ctrlreq_queue)->len; 945 else if (!skb_queue_empty(&sk->sk_receive_queue)) 946 answ = skb_peek(&sk->sk_receive_queue)->len; 947 else 948 answ = 0; 949 release_sock(sk); 950 ret = put_user(answ, (int __user *)arg); 951 break; 952 953 case SIOCPNENABLEPIPE: 954 lock_sock(sk); 955 if (sk->sk_state == TCP_SYN_SENT) 956 ret = -EBUSY; 957 else if (sk->sk_state == TCP_ESTABLISHED) 958 ret = -EISCONN; 959 else 960 ret = pep_sock_enable(sk, NULL, 0); 961 release_sock(sk); 962 break; 963 } 964 965 return ret; 966} 967 968static int pep_init(struct sock *sk) 969{ 970 struct pep_sock *pn = pep_sk(sk); 971 972 sk->sk_destruct = pipe_destruct; 973 INIT_HLIST_HEAD(&pn->hlist); 974 pn->listener = NULL; 975 skb_queue_head_init(&pn->ctrlreq_queue); 976 atomic_set(&pn->tx_credits, 0); 977 pn->ifindex = 0; 978 pn->peer_type = 0; 979 pn->pipe_handle = PN_PIPE_INVALID_HANDLE; 980 pn->rx_credits = 0; 981 pn->rx_fc = pn->tx_fc = PN_LEGACY_FLOW_CONTROL; 982 pn->init_enable = 1; 983 pn->aligned = 0; 984 return 0; 985} 986 987static int pep_setsockopt(struct sock *sk, int level, int optname, 988 char __user *optval, unsigned int optlen) 989{ 990 struct pep_sock *pn = pep_sk(sk); 991 int val = 0, err = 0; 992 993 if (level != SOL_PNPIPE) 994 return -ENOPROTOOPT; 995 if (optlen >= sizeof(int)) { 996 if (get_user(val, (int __user *) optval)) 997 return -EFAULT; 998 } 999 1000 lock_sock(sk); 1001 switch (optname) { 1002 case PNPIPE_ENCAP: 1003 if (val && val != PNPIPE_ENCAP_IP) { 1004 err = -EINVAL; 1005 break; 1006 } 1007 if (!pn->ifindex == !val) 1008 break; /* Nothing to do! */ 1009 if (!capable(CAP_NET_ADMIN)) { 1010 err = -EPERM; 1011 break; 1012 } 1013 if (val) { 1014 release_sock(sk); 1015 err = gprs_attach(sk); 1016 if (err > 0) { 1017 pn->ifindex = err; 1018 err = 0; 1019 } 1020 } else { 1021 pn->ifindex = 0; 1022 release_sock(sk); 1023 gprs_detach(sk); 1024 err = 0; 1025 } 1026 goto out_norel; 1027 1028 case PNPIPE_HANDLE: 1029 if ((sk->sk_state == TCP_CLOSE) && 1030 (val >= 0) && (val < PN_PIPE_INVALID_HANDLE)) 1031 pn->pipe_handle = val; 1032 else 1033 err = -EINVAL; 1034 break; 1035 1036 case PNPIPE_INITSTATE: 1037 pn->init_enable = !!val; 1038 break; 1039 1040 default: 1041 err = -ENOPROTOOPT; 1042 } 1043 release_sock(sk); 1044 1045out_norel: 1046 return err; 1047} 1048 1049static int pep_getsockopt(struct sock *sk, int level, int optname, 1050 char __user *optval, int __user *optlen) 1051{ 1052 struct pep_sock *pn = pep_sk(sk); 1053 int len, val; 1054 1055 if (level != SOL_PNPIPE) 1056 return -ENOPROTOOPT; 1057 if (get_user(len, optlen)) 1058 return -EFAULT; 1059 1060 switch (optname) { 1061 case PNPIPE_ENCAP: 1062 val = pn->ifindex ? PNPIPE_ENCAP_IP : PNPIPE_ENCAP_NONE; 1063 break; 1064 1065 case PNPIPE_IFINDEX: 1066 val = pn->ifindex; 1067 break; 1068 1069 case PNPIPE_HANDLE: 1070 val = pn->pipe_handle; 1071 if (val == PN_PIPE_INVALID_HANDLE) 1072 return -EINVAL; 1073 break; 1074 1075 case PNPIPE_INITSTATE: 1076 val = pn->init_enable; 1077 break; 1078 1079 default: 1080 return -ENOPROTOOPT; 1081 } 1082 1083 len = min_t(unsigned int, sizeof(int), len); 1084 if (put_user(len, optlen)) 1085 return -EFAULT; 1086 if (put_user(val, (int __user *) optval)) 1087 return -EFAULT; 1088 return 0; 1089} 1090 1091static int pipe_skb_send(struct sock *sk, struct sk_buff *skb) 1092{ 1093 struct pep_sock *pn = pep_sk(sk); 1094 struct pnpipehdr *ph; 1095 int err; 1096 1097 if (pn_flow_safe(pn->tx_fc) && 1098 !atomic_add_unless(&pn->tx_credits, -1, 0)) { 1099 kfree_skb(skb); 1100 return -ENOBUFS; 1101 } 1102 1103 skb_push(skb, 3 + pn->aligned); 1104 skb_reset_transport_header(skb); 1105 ph = pnp_hdr(skb); 1106 ph->utid = 0; 1107 if (pn->aligned) { 1108 ph->message_id = PNS_PIPE_ALIGNED_DATA; 1109 ph->data[0] = 0; /* padding */ 1110 } else 1111 ph->message_id = PNS_PIPE_DATA; 1112 ph->pipe_handle = pn->pipe_handle; 1113 err = pn_skb_send(sk, skb, NULL); 1114 1115 if (err && pn_flow_safe(pn->tx_fc)) 1116 atomic_inc(&pn->tx_credits); 1117 return err; 1118 1119} 1120 1121static int pep_sendmsg(struct kiocb *iocb, struct sock *sk, 1122 struct msghdr *msg, size_t len) 1123{ 1124 struct pep_sock *pn = pep_sk(sk); 1125 struct sk_buff *skb; 1126 long timeo; 1127 int flags = msg->msg_flags; 1128 int err, done; 1129 1130 if (len > USHRT_MAX) 1131 return -EMSGSIZE; 1132 1133 if ((msg->msg_flags & ~(MSG_DONTWAIT|MSG_EOR|MSG_NOSIGNAL| 1134 MSG_CMSG_COMPAT)) || 1135 !(msg->msg_flags & MSG_EOR)) 1136 return -EOPNOTSUPP; 1137 1138 skb = sock_alloc_send_skb(sk, MAX_PNPIPE_HEADER + len, 1139 flags & MSG_DONTWAIT, &err); 1140 if (!skb) 1141 return err; 1142 1143 skb_reserve(skb, MAX_PHONET_HEADER + 3 + pn->aligned); 1144 err = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len); 1145 if (err < 0) 1146 goto outfree; 1147 1148 lock_sock(sk); 1149 timeo = sock_sndtimeo(sk, flags & MSG_DONTWAIT); 1150 if ((1 << sk->sk_state) & (TCPF_LISTEN|TCPF_CLOSE)) { 1151 err = -ENOTCONN; 1152 goto out; 1153 } 1154 if (sk->sk_state != TCP_ESTABLISHED) { 1155 /* Wait until the pipe gets to enabled state */ 1156disabled: 1157 err = sk_stream_wait_connect(sk, &timeo); 1158 if (err) 1159 goto out; 1160 1161 if (sk->sk_state == TCP_CLOSE_WAIT) { 1162 err = -ECONNRESET; 1163 goto out; 1164 } 1165 } 1166 BUG_ON(sk->sk_state != TCP_ESTABLISHED); 1167 1168 /* Wait until flow control allows TX */ 1169 done = atomic_read(&pn->tx_credits); 1170 while (!done) { 1171 DEFINE_WAIT(wait); 1172 1173 if (!timeo) { 1174 err = -EAGAIN; 1175 goto out; 1176 } 1177 if (signal_pending(current)) { 1178 err = sock_intr_errno(timeo); 1179 goto out; 1180 } 1181 1182 prepare_to_wait(sk_sleep(sk), &wait, 1183 TASK_INTERRUPTIBLE); 1184 done = sk_wait_event(sk, &timeo, atomic_read(&pn->tx_credits)); 1185 finish_wait(sk_sleep(sk), &wait); 1186 1187 if (sk->sk_state != TCP_ESTABLISHED) 1188 goto disabled; 1189 } 1190 1191 err = pipe_skb_send(sk, skb); 1192 if (err >= 0) 1193 err = len; /* success! */ 1194 skb = NULL; 1195out: 1196 release_sock(sk); 1197outfree: 1198 kfree_skb(skb); 1199 return err; 1200} 1201 1202int pep_writeable(struct sock *sk) 1203{ 1204 struct pep_sock *pn = pep_sk(sk); 1205 1206 return atomic_read(&pn->tx_credits); 1207} 1208 1209int pep_write(struct sock *sk, struct sk_buff *skb) 1210{ 1211 struct sk_buff *rskb, *fs; 1212 int flen = 0; 1213 1214 if (pep_sk(sk)->aligned) 1215 return pipe_skb_send(sk, skb); 1216 1217 rskb = alloc_skb(MAX_PNPIPE_HEADER, GFP_ATOMIC); 1218 if (!rskb) { 1219 kfree_skb(skb); 1220 return -ENOMEM; 1221 } 1222 skb_shinfo(rskb)->frag_list = skb; 1223 rskb->len += skb->len; 1224 rskb->data_len += rskb->len; 1225 rskb->truesize += rskb->len; 1226 1227 /* Avoid nested fragments */ 1228 skb_walk_frags(skb, fs) 1229 flen += fs->len; 1230 skb->next = skb_shinfo(skb)->frag_list; 1231 skb_frag_list_init(skb); 1232 skb->len -= flen; 1233 skb->data_len -= flen; 1234 skb->truesize -= flen; 1235 1236 skb_reserve(rskb, MAX_PHONET_HEADER + 3); 1237 return pipe_skb_send(sk, rskb); 1238} 1239 1240struct sk_buff *pep_read(struct sock *sk) 1241{ 1242 struct sk_buff *skb = skb_dequeue(&sk->sk_receive_queue); 1243 1244 if (sk->sk_state == TCP_ESTABLISHED) 1245 pipe_grant_credits(sk, GFP_ATOMIC); 1246 return skb; 1247} 1248 1249static int pep_recvmsg(struct kiocb *iocb, struct sock *sk, 1250 struct msghdr *msg, size_t len, int noblock, 1251 int flags, int *addr_len) 1252{ 1253 struct sk_buff *skb; 1254 int err; 1255 1256 if (flags & ~(MSG_OOB|MSG_PEEK|MSG_TRUNC|MSG_DONTWAIT|MSG_WAITALL| 1257 MSG_NOSIGNAL|MSG_CMSG_COMPAT)) 1258 return -EOPNOTSUPP; 1259 1260 if (unlikely(1 << sk->sk_state & (TCPF_LISTEN | TCPF_CLOSE))) 1261 return -ENOTCONN; 1262 1263 if ((flags & MSG_OOB) || sock_flag(sk, SOCK_URGINLINE)) { 1264 /* Dequeue and acknowledge control request */ 1265 struct pep_sock *pn = pep_sk(sk); 1266 1267 if (flags & MSG_PEEK) 1268 return -EOPNOTSUPP; 1269 skb = skb_dequeue(&pn->ctrlreq_queue); 1270 if (skb) { 1271 pep_ctrlreq_error(sk, skb, PN_PIPE_NO_ERROR, 1272 GFP_KERNEL); 1273 msg->msg_flags |= MSG_OOB; 1274 goto copy; 1275 } 1276 if (flags & MSG_OOB) 1277 return -EINVAL; 1278 } 1279 1280 skb = skb_recv_datagram(sk, flags, noblock, &err); 1281 lock_sock(sk); 1282 if (skb == NULL) { 1283 if (err == -ENOTCONN && sk->sk_state == TCP_CLOSE_WAIT) 1284 err = -ECONNRESET; 1285 release_sock(sk); 1286 return err; 1287 } 1288 1289 if (sk->sk_state == TCP_ESTABLISHED) 1290 pipe_grant_credits(sk, GFP_KERNEL); 1291 release_sock(sk); 1292copy: 1293 msg->msg_flags |= MSG_EOR; 1294 if (skb->len > len) 1295 msg->msg_flags |= MSG_TRUNC; 1296 else 1297 len = skb->len; 1298 1299 err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, len); 1300 if (!err) 1301 err = (flags & MSG_TRUNC) ? skb->len : len; 1302 1303 skb_free_datagram(sk, skb); 1304 return err; 1305} 1306 1307static void pep_sock_unhash(struct sock *sk) 1308{ 1309 struct pep_sock *pn = pep_sk(sk); 1310 struct sock *skparent = NULL; 1311 1312 lock_sock(sk); 1313 1314 if (pn->listener != NULL) { 1315 skparent = pn->listener; 1316 pn->listener = NULL; 1317 release_sock(sk); 1318 1319 pn = pep_sk(skparent); 1320 lock_sock(skparent); 1321 sk_del_node_init(sk); 1322 sk = skparent; 1323 } 1324 1325 /* Unhash a listening sock only when it is closed 1326 * and all of its active connected pipes are closed. */ 1327 if (hlist_empty(&pn->hlist)) 1328 pn_sock_unhash(&pn->pn_sk.sk); 1329 release_sock(sk); 1330 1331 if (skparent) 1332 sock_put(skparent); 1333} 1334 1335static struct proto pep_proto = { 1336 .close = pep_sock_close, 1337 .accept = pep_sock_accept, 1338 .connect = pep_sock_connect, 1339 .ioctl = pep_ioctl, 1340 .init = pep_init, 1341 .setsockopt = pep_setsockopt, 1342 .getsockopt = pep_getsockopt, 1343 .sendmsg = pep_sendmsg, 1344 .recvmsg = pep_recvmsg, 1345 .backlog_rcv = pep_do_rcv, 1346 .hash = pn_sock_hash, 1347 .unhash = pep_sock_unhash, 1348 .get_port = pn_sock_get_port, 1349 .obj_size = sizeof(struct pep_sock), 1350 .owner = THIS_MODULE, 1351 .name = "PNPIPE", 1352}; 1353 1354static struct phonet_protocol pep_pn_proto = { 1355 .ops = &phonet_stream_ops, 1356 .prot = &pep_proto, 1357 .sock_type = SOCK_SEQPACKET, 1358}; 1359 1360static int __init pep_register(void) 1361{ 1362 return phonet_proto_register(PN_PROTO_PIPE, &pep_pn_proto); 1363} 1364 1365static void __exit pep_unregister(void) 1366{ 1367 phonet_proto_unregister(PN_PROTO_PIPE, &pep_pn_proto); 1368} 1369 1370module_init(pep_register); 1371module_exit(pep_unregister); 1372MODULE_AUTHOR("Remi Denis-Courmont, Nokia"); 1373MODULE_DESCRIPTION("Phonet pipe protocol"); 1374MODULE_LICENSE("GPL"); 1375MODULE_ALIAS_NET_PF_PROTO(PF_PHONET, PN_PROTO_PIPE); 1376