diff options
Diffstat (limited to 'net/phonet')
| -rw-r--r-- | net/phonet/Kconfig | 11 | ||||
| -rw-r--r-- | net/phonet/af_phonet.c | 17 | ||||
| -rw-r--r-- | net/phonet/datagram.c | 13 | ||||
| -rw-r--r-- | net/phonet/pep.c | 477 | ||||
| -rw-r--r-- | net/phonet/pn_dev.c | 5 | ||||
| -rw-r--r-- | net/phonet/socket.c | 190 |
6 files changed, 707 insertions, 6 deletions
diff --git a/net/phonet/Kconfig b/net/phonet/Kconfig index 6ec7d55..901956a 100644 --- a/net/phonet/Kconfig +++ b/net/phonet/Kconfig @@ -14,3 +14,14 @@ config PHONET To compile this driver as a module, choose M here: the module will be called phonet. If unsure, say N. + +config PHONET_PIPECTRLR + bool "Phonet Pipe Controller" + depends on PHONET + default N + help + The Pipe Controller implementation in Phonet stack to support Pipe + data with Nokia Slim modems like WG2.5 used on ST-Ericsson U8500 + platform. + + If unsure, say N. diff --git a/net/phonet/af_phonet.c b/net/phonet/af_phonet.c index 73aee7f..fd95beb 100644 --- a/net/phonet/af_phonet.c +++ b/net/phonet/af_phonet.c @@ -251,6 +251,16 @@ int pn_skb_send(struct sock *sk, struct sk_buff *skb, else if (phonet_address_lookup(net, daddr) == 0) { dev = phonet_device_get(net); skb->pkt_type = PACKET_LOOPBACK; + } else if (pn_sockaddr_get_object(target) == 0) { + /* Resource routing (small race until phonet_rcv()) */ + struct sock *sk = pn_find_sock_by_res(net, + target->spn_resource); + if (sk) { + sock_put(sk); + dev = phonet_device_get(net); + skb->pkt_type = PACKET_LOOPBACK; + } else + dev = phonet_route_output(net, daddr); } else dev = phonet_route_output(net, daddr); @@ -383,6 +393,13 @@ static int phonet_rcv(struct sk_buff *skb, struct net_device *dev, goto out; } + /* resource routing */ + if (pn_sockaddr_get_object(&sa) == 0) { + struct sock *sk = pn_find_sock_by_res(net, sa.spn_resource); + if (sk) + return sk_receive_skb(sk, skb, 0); + } + /* check if we are the destination */ if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) { /* Phonet packet input */ diff --git a/net/phonet/datagram.c b/net/phonet/datagram.c index 1bd38db..2f03238 100644 --- a/net/phonet/datagram.c +++ b/net/phonet/datagram.c @@ -52,6 +52,19 @@ static int pn_ioctl(struct sock *sk, int cmd, unsigned long arg) answ = skb ? skb->len : 0; release_sock(sk); return put_user(answ, (int __user *)arg); + + case SIOCPNADDRESOURCE: + case SIOCPNDELRESOURCE: { + u32 res; + if (get_user(res, (u32 __user *)arg)) + return -EFAULT; + if (res >= 256) + return -EINVAL; + if (cmd == SIOCPNADDRESOURCE) + return pn_sock_bind_res(sk, res); + else + return pn_sock_unbind_res(sk, res); + } } return -ENOIOCTLCMD; diff --git a/net/phonet/pep.c b/net/phonet/pep.c index 1500302..aa3d870 100644 --- a/net/phonet/pep.c +++ b/net/phonet/pep.c @@ -88,6 +88,15 @@ static int pep_reply(struct sock *sk, struct sk_buff *oskb, const struct pnpipehdr *oph = pnp_hdr(oskb); struct pnpipehdr *ph; struct sk_buff *skb; +#ifdef CONFIG_PHONET_PIPECTRLR + const struct phonethdr *hdr = pn_hdr(oskb); + struct sockaddr_pn spn = { + .spn_family = AF_PHONET, + .spn_resource = 0xD9, + .spn_dev = hdr->pn_sdev, + .spn_obj = hdr->pn_sobj, + }; +#endif skb = alloc_skb(MAX_PNPIPE_HEADER + len, priority); if (!skb) @@ -105,10 +114,271 @@ static int pep_reply(struct sock *sk, struct sk_buff *oskb, ph->pipe_handle = oph->pipe_handle; ph->error_code = code; +#ifdef CONFIG_PHONET_PIPECTRLR + return pn_skb_send(sk, skb, &spn); +#else return pn_skb_send(sk, skb, &pipe_srv); +#endif } #define PAD 0x00 + +#ifdef CONFIG_PHONET_PIPECTRLR +static u8 pipe_negotiate_fc(u8 *host_fc, u8 *remote_fc, int len) +{ + int i, j; + u8 base_fc, final_fc; + + for (i = 0; i < len; i++) { + base_fc = host_fc[i]; + for (j = 0; j < len; j++) { + if (remote_fc[j] == base_fc) { + final_fc = base_fc; + goto done; + } + } + } + return -EINVAL; + +done: + return final_fc; + +} + +static int pipe_get_flow_info(struct sock *sk, struct sk_buff *skb, + u8 *pref_rx_fc, u8 *req_tx_fc) +{ + struct pnpipehdr *hdr; + u8 n_sb; + + if (!pskb_may_pull(skb, sizeof(*hdr) + 4)) + return -EINVAL; + + hdr = pnp_hdr(skb); + n_sb = hdr->data[4]; + + __skb_pull(skb, sizeof(*hdr) + 4); + while (n_sb > 0) { + u8 type, buf[3], len = sizeof(buf); + u8 *data = pep_get_sb(skb, &type, &len, buf); + + if (data == NULL) + return -EINVAL; + + switch (type) { + case PN_PIPE_SB_REQUIRED_FC_TX: + if (len < 3 || (data[2] | data[3] | data[4]) > 3) + break; + req_tx_fc[0] = data[2]; + req_tx_fc[1] = data[3]; + req_tx_fc[2] = data[4]; + break; + + case PN_PIPE_SB_PREFERRED_FC_RX: + if (len < 3 || (data[2] | data[3] | data[4]) > 3) + break; + pref_rx_fc[0] = data[2]; + pref_rx_fc[1] = data[3]; + pref_rx_fc[2] = data[4]; + break; + + } + n_sb--; + } + return 0; +} + +static int pipe_handler_send_req(struct sock *sk, u16 dobj, u8 utid, + u8 msg_id, u8 p_handle, gfp_t priority) +{ + int len; + struct pnpipehdr *ph; + struct sk_buff *skb; + struct sockaddr_pn spn = { + .spn_family = AF_PHONET, + .spn_resource = 0xD9, + .spn_dev = pn_dev(dobj), + .spn_obj = pn_obj(dobj), + }; + + static const u8 data[4] = { + PAD, PAD, PAD, PAD, + }; + + switch (msg_id) { + case PNS_PEP_CONNECT_REQ: + len = sizeof(data); + break; + + case PNS_PEP_DISCONNECT_REQ: + case PNS_PEP_ENABLE_REQ: + case PNS_PEP_DISABLE_REQ: + len = 0; + break; + + default: + return -EINVAL; + } + + skb = alloc_skb(MAX_PNPIPE_HEADER + len, priority); + if (!skb) + return -ENOMEM; + skb_set_owner_w(skb, sk); + + skb_reserve(skb, MAX_PNPIPE_HEADER); + if (len) { + __skb_put(skb, len); + skb_copy_to_linear_data(skb, data, len); + } + __skb_push(skb, sizeof(*ph)); + skb_reset_transport_header(skb); + ph = pnp_hdr(skb); + ph->utid = utid; + ph->message_id = msg_id; + ph->pipe_handle = p_handle; + ph->error_code = PN_PIPE_NO_ERROR; + + return pn_skb_send(sk, skb, &spn); +} + +static int pipe_handler_send_created_ind(struct sock *sk, u16 dobj, + u8 utid, u8 p_handle, u8 msg_id, u8 tx_fc, u8 rx_fc) +{ + int err_code; + struct pnpipehdr *ph; + struct sk_buff *skb; + struct sockaddr_pn spn = { + .spn_family = AF_PHONET, + .spn_resource = 0xD9, + .spn_dev = pn_dev(dobj), + .spn_obj = pn_obj(dobj), + }; + + static u8 data[4] = { + 0x03, 0x04, + }; + data[2] = tx_fc; + data[3] = rx_fc; + + /* + * actually, below is number of sub-blocks and not error code. + * Pipe_created_ind message format does not have any + * error code field. However, the Phonet stack will always send + * an error code as part of pnpipehdr. So, use that err_code to + * specify the number of sub-blocks. + */ + err_code = 0x01; + + skb = alloc_skb(MAX_PNPIPE_HEADER + sizeof(data), GFP_ATOMIC); + if (!skb) + return -ENOMEM; + skb_set_owner_w(skb, sk); + + skb_reserve(skb, MAX_PNPIPE_HEADER); + __skb_put(skb, sizeof(data)); + skb_copy_to_linear_data(skb, data, sizeof(data)); + __skb_push(skb, sizeof(*ph)); + skb_reset_transport_header(skb); + ph = pnp_hdr(skb); + ph->utid = utid; + ph->message_id = msg_id; + ph->pipe_handle = p_handle; + ph->error_code = err_code; + + return pn_skb_send(sk, skb, &spn); +} + +static int pipe_handler_send_ind(struct sock *sk, u16 dobj, u8 utid, + u8 p_handle, u8 msg_id) +{ + int err_code; + struct pnpipehdr *ph; + struct sk_buff *skb; + struct sockaddr_pn spn = { + .spn_family = AF_PHONET, + .spn_resource = 0xD9, + .spn_dev = pn_dev(dobj), + .spn_obj = pn_obj(dobj), + }; + + /* + * actually, below is a filler. + * Pipe_enabled/disabled_ind message format does not have any + * error code field. However, the Phonet stack will always send + * an error code as part of pnpipehdr. So, use that err_code to + * specify the filler value. + */ + err_code = 0x0; + + skb = alloc_skb(MAX_PNPIPE_HEADER, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + skb_set_owner_w(skb, sk); + + skb_reserve(skb, MAX_PNPIPE_HEADER); + __skb_push(skb, sizeof(*ph)); + skb_reset_transport_header(skb); + ph = pnp_hdr(skb); + ph->utid = utid; + ph->message_id = msg_id; + ph->pipe_handle = p_handle; + ph->error_code = err_code; + + return pn_skb_send(sk, skb, &spn); +} + +static int pipe_handler_enable_pipe(struct sock *sk, int cmd) +{ + int ret; + struct pep_sock *pn = pep_sk(sk); + + switch (cmd) { + case PNPIPE_ENABLE: + ret = pipe_handler_send_req(sk, pn->pn_sk.sobject, + PNS_PIPE_ENABLE_UTID, PNS_PEP_ENABLE_REQ, + pn->pipe_handle, GFP_ATOMIC); + break; + + case PNPIPE_DISABLE: + ret = pipe_handler_send_req(sk, pn->pn_sk.sobject, + PNS_PIPE_DISABLE_UTID, PNS_PEP_DISABLE_REQ, + pn->pipe_handle, GFP_ATOMIC); + break; + + default: + ret = -EINVAL; + } + + return ret; +} + +static int pipe_handler_create_pipe(struct sock *sk, int pipe_handle, int cmd) +{ + int ret; + struct pep_sock *pn = pep_sk(sk); + + switch (cmd) { + case PNPIPE_CREATE: + ret = pipe_handler_send_req(sk, pn->pn_sk.sobject, + PNS_PEP_CONNECT_UTID, PNS_PEP_CONNECT_REQ, + pipe_handle, GFP_ATOMIC); + break; + + case PNPIPE_DESTROY: + ret = pipe_handler_send_req(sk, pn->remote_pep, + PNS_PEP_DISCONNECT_UTID, + PNS_PEP_DISCONNECT_REQ, + pn->pipe_handle, GFP_ATOMIC); + break; + + default: + ret = -EINVAL; + } + + return ret; +} +#endif + static int pep_accept_conn(struct sock *sk, struct sk_buff *skb) { static const u8 data[20] = { @@ -173,6 +443,14 @@ static int pipe_snd_status(struct sock *sk, u8 type, u8 status, gfp_t priority) struct pep_sock *pn = pep_sk(sk); struct pnpipehdr *ph; struct sk_buff *skb; +#ifdef CONFIG_PHONET_PIPECTRLR + struct sockaddr_pn spn = { + .spn_family = AF_PHONET, + .spn_resource = 0xD9, + .spn_dev = pn_dev(pn->remote_pep), + .spn_obj = pn_obj(pn->remote_pep), + }; +#endif skb = alloc_skb(MAX_PNPIPE_HEADER + 4, priority); if (!skb) @@ -192,7 +470,11 @@ static int pipe_snd_status(struct sock *sk, u8 type, u8 status, gfp_t priority) ph->data[3] = PAD; ph->data[4] = status; +#ifdef CONFIG_PHONET_PIPECTRLR + return pn_skb_send(sk, skb, &spn); +#else return pn_skb_send(sk, skb, &pipe_srv); +#endif } /* Send our RX flow control information to the sender. @@ -309,6 +591,12 @@ static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) struct pnpipehdr *hdr = pnp_hdr(skb); struct sk_buff_head *queue; int err = 0; +#ifdef CONFIG_PHONET_PIPECTRLR + struct phonethdr *ph = pn_hdr(skb); + static u8 host_pref_rx_fc[3], host_req_tx_fc[3]; + u8 remote_pref_rx_fc[3], remote_req_tx_fc[3]; + u8 negotiated_rx_fc, negotiated_tx_fc; +#endif BUG_ON(sk->sk_state == TCP_CLOSE_WAIT); @@ -317,6 +605,40 @@ static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) pep_reject_conn(sk, skb, PN_PIPE_ERR_PEP_IN_USE); break; +#ifdef CONFIG_PHONET_PIPECTRLR + case PNS_PEP_CONNECT_RESP: + if ((ph->pn_sdev == pn_dev(pn->remote_pep)) && + (ph->pn_sobj == pn_obj(pn->remote_pep))) { + pipe_get_flow_info(sk, skb, remote_pref_rx_fc, + remote_req_tx_fc); + + negotiated_tx_fc = pipe_negotiate_fc(remote_req_tx_fc, + host_pref_rx_fc, + sizeof(host_pref_rx_fc)); + negotiated_rx_fc = pipe_negotiate_fc(host_req_tx_fc, + remote_pref_rx_fc, + sizeof(host_pref_rx_fc)); + + pn->pipe_state = PIPE_DISABLED; + pipe_handler_send_created_ind(sk, pn->remote_pep, + PNS_PIPE_CREATED_IND_UTID, + pn->pipe_handle, PNS_PIPE_CREATED_IND, + negotiated_tx_fc, negotiated_rx_fc); + pipe_handler_send_created_ind(sk, pn->pn_sk.sobject, + PNS_PIPE_CREATED_IND_UTID, + pn->pipe_handle, PNS_PIPE_CREATED_IND, + negotiated_tx_fc, negotiated_rx_fc); + } else { + pipe_handler_send_req(sk, pn->remote_pep, + PNS_PEP_CONNECT_UTID, + PNS_PEP_CONNECT_REQ, pn->pipe_handle, + GFP_ATOMIC); + pipe_get_flow_info(sk, skb, host_pref_rx_fc, + host_req_tx_fc); + } + break; +#endif + case PNS_PEP_DISCONNECT_REQ: pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC); sk->sk_state = TCP_CLOSE_WAIT; @@ -324,11 +646,41 @@ static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) sk->sk_state_change(sk); break; +#ifdef CONFIG_PHONET_PIPECTRLR + case PNS_PEP_DISCONNECT_RESP: + pn->pipe_state = PIPE_IDLE; + pipe_handler_send_req(sk, pn->pn_sk.sobject, + PNS_PEP_DISCONNECT_UTID, + PNS_PEP_DISCONNECT_REQ, pn->pipe_handle, + GFP_KERNEL); + break; +#endif + case PNS_PEP_ENABLE_REQ: /* Wait for PNS_PIPE_(ENABLED|REDIRECTED)_IND */ pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC); break; +#ifdef CONFIG_PHONET_PIPECTRLR + case PNS_PEP_ENABLE_RESP: + if ((ph->pn_sdev == pn_dev(pn->remote_pep)) && + (ph->pn_sobj == pn_obj(pn->remote_pep))) { + pn->pipe_state = PIPE_ENABLED; + pipe_handler_send_ind(sk, pn->remote_pep, + PNS_PIPE_ENABLED_IND_UTID, + pn->pipe_handle, PNS_PIPE_ENABLED_IND); + pipe_handler_send_ind(sk, pn->pn_sk.sobject, + PNS_PIPE_ENABLED_IND_UTID, + pn->pipe_handle, PNS_PIPE_ENABLED_IND); + } else + pipe_handler_send_req(sk, pn->remote_pep, + PNS_PIPE_ENABLE_UTID, + PNS_PEP_ENABLE_REQ, pn->pipe_handle, + GFP_KERNEL); + + break; +#endif + case PNS_PEP_RESET_REQ: switch (hdr->state_after_reset) { case PN_PIPE_DISABLE: @@ -347,6 +699,27 @@ static int pipe_do_rcv(struct sock *sk, struct sk_buff *skb) pep_reply(sk, skb, PN_PIPE_NO_ERROR, NULL, 0, GFP_ATOMIC); break; +#ifdef CONFIG_PHONET_PIPECTRLR + case PNS_PEP_DISABLE_RESP: + if ((ph->pn_sdev == pn_dev(pn->remote_pep)) && + (ph->pn_sobj == pn_obj(pn->remote_pep))) { + pn->pipe_state = PIPE_DISABLED; + pipe_handler_send_ind(sk, pn->remote_pep, + PNS_PIPE_DISABLED_IND_UTID, + pn->pipe_handle, + PNS_PIPE_DISABLED_IND); + pipe_handler_send_ind(sk, pn->pn_sk.sobject, + PNS_PIPE_DISABLED_IND_UTID, + pn->pipe_handle, + PNS_PIPE_DISABLED_IND); + } else + pipe_handler_send_req(sk, pn->remote_pep, + PNS_PIPE_DISABLE_UTID, + PNS_PEP_DISABLE_REQ, pn->pipe_handle, + GFP_KERNEL); + break; +#endif + case PNS_PEP_CTRL_REQ: if (skb_queue_len(&pn->ctrlreq_queue) >= PNPIPE_CTRLREQ_MAX) { atomic_inc(&sk->sk_drops); @@ -520,6 +893,9 @@ static int pep_connreq_rcv(struct sock *sk, struct sk_buff *skb) newpn->rx_fc = newpn->tx_fc = PN_LEGACY_FLOW_CONTROL; newpn->init_enable = enabled; newpn->aligned = aligned; +#ifdef CONFIG_PHONET_PIPECTRLR + newpn->remote_pep = pn->remote_pep; +#endif BUG_ON(!skb_queue_empty(&newsk->sk_receive_queue)); skb_queue_head(&newsk->sk_receive_queue, skb); @@ -621,6 +997,28 @@ drop: return err; } +static int pipe_do_remove(struct sock *sk) +{ + struct pep_sock *pn = pep_sk(sk); + struct pnpipehdr *ph; + struct sk_buff *skb; + + skb = alloc_skb(MAX_PNPIPE_HEADER, GFP_KERNEL); + if (!skb) + return -ENOMEM; + + skb_reserve(skb, MAX_PNPIPE_HEADER); + __skb_push(skb, sizeof(*ph)); + skb_reset_transport_header(skb); + ph = pnp_hdr(skb); + ph->utid = 0; + ph->message_id = PNS_PIPE_REMOVE_REQ; + ph->pipe_handle = pn->pipe_handle; + ph->data[0] = PAD; + + return pn_skb_send(sk, skb, &pipe_srv); +} + /* associated socket ceases to exist */ static void pep_sock_close(struct sock *sk, long timeout) { @@ -639,7 +1037,10 @@ static void pep_sock_close(struct sock *sk, long timeout) sk_for_each_safe(sknode, p, n, &pn->ackq) sk_del_node_init(sknode); sk->sk_state = TCP_CLOSE; - } + } else if ((1 << sk->sk_state) & (TCPF_SYN_RECV|TCPF_ESTABLISHED)) + /* Forcefully remove dangling Phonet pipe */ + pipe_do_remove(sk); + ifindex = pn->ifindex; pn->ifindex = 0; release_sock(sk); @@ -757,6 +1158,10 @@ static int pep_setsockopt(struct sock *sk, int level, int optname, { struct pep_sock *pn = pep_sk(sk); int val = 0, err = 0; +#ifdef CONFIG_PHONET_PIPECTRLR + int remote_pep; + int pipe_handle; +#endif if (level != SOL_PNPIPE) return -ENOPROTOOPT; @@ -767,6 +1172,48 @@ static int pep_setsockopt(struct sock *sk, int level, int optname, lock_sock(sk); switch (optname) { +#ifdef CONFIG_PHONET_PIPECTRLR + case PNPIPE_CREATE: + if (val) { + if (pn->pipe_state > PIPE_IDLE) { + err = -EFAULT; + break; + } + remote_pep = val & 0xFFFF; + pipe_handle = (val >> 16) & 0xFF; + pn->remote_pep = remote_pep; + err = pipe_handler_create_pipe(sk, pipe_handle, + PNPIPE_CREATE); + break; + } + + case PNPIPE_ENABLE: + if (pn->pipe_state != PIPE_DISABLED) { + err = -EFAULT; + break; + } + err = pipe_handler_enable_pipe(sk, PNPIPE_ENABLE); + break; + + case PNPIPE_DISABLE: + if (pn->pipe_state != PIPE_ENABLED) { + err = -EFAULT; + break; + } + + err = pipe_handler_enable_pipe(sk, PNPIPE_DISABLE); + break; + + case PNPIPE_DESTROY: + if (pn->pipe_state < PIPE_DISABLED) { + err = -EFAULT; + break; + } + + err = pipe_handler_create_pipe(sk, 0x0, PNPIPE_DESTROY); + break; +#endif + case PNPIPE_ENCAP: if (val && val != PNPIPE_ENCAP_IP) { err = -EINVAL; @@ -816,6 +1263,13 @@ static int pep_getsockopt(struct sock *sk, int level, int optname, case PNPIPE_ENCAP: val = pn->ifindex ? PNPIPE_ENCAP_IP : PNPIPE_ENCAP_NONE; break; + +#ifdef CONFIG_PHONET_PIPECTRLR + case PNPIPE_INQ: + val = pn->pipe_state; + break; +#endif + case PNPIPE_IFINDEX: val = pn->ifindex; break; @@ -835,6 +1289,15 @@ static int pipe_skb_send(struct sock *sk, struct sk_buff *skb) { struct pep_sock *pn = pep_sk(sk); struct pnpipehdr *ph; + int err; +#ifdef CONFIG_PHONET_PIPECTRLR + struct sockaddr_pn spn = { + .spn_family = AF_PHONET, + .spn_resource = 0xD9, + .spn_dev = pn_dev(pn->remote_pep), + .spn_obj = pn_obj(pn->remote_pep), + }; +#endif if (pn_flow_safe(pn->tx_fc) && !atomic_add_unless(&pn->tx_credits, -1, 0)) { @@ -852,8 +1315,16 @@ static int pipe_skb_send(struct sock *sk, struct sk_buff *skb) } else ph->message_id = PNS_PIPE_DATA; ph->pipe_handle = pn->pipe_handle; +#ifdef CONFIG_PHONET_PIPECTRLR + err = pn_skb_send(sk, skb, &spn); +#else + err = pn_skb_send(sk, skb, &pipe_srv); +#endif + + if (err && pn_flow_safe(pn->tx_fc)) + atomic_inc(&pn->tx_credits); + return err; - return pn_skb_send(sk, skb, &pipe_srv); } static int pep_sendmsg(struct kiocb *iocb, struct sock *sk, @@ -873,7 +1344,7 @@ static int pep_sendmsg(struct kiocb *iocb, struct sock *sk, skb = sock_alloc_send_skb(sk, MAX_PNPIPE_HEADER + len, flags & MSG_DONTWAIT, &err); if (!skb) - return -ENOBUFS; + return err; skb_reserve(skb, MAX_PHONET_HEADER + 3); err = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len); diff --git a/net/phonet/pn_dev.c b/net/phonet/pn_dev.c index b18e48f..947038d 100644 --- a/net/phonet/pn_dev.c +++ b/net/phonet/pn_dev.c @@ -292,8 +292,7 @@ static void phonet_route_autodel(struct net_device *dev) if (bitmap_empty(deleted, 64)) return; /* short-circuit RCU */ synchronize_rcu(); - for (i = find_first_bit(deleted, 64); i < 64; - i = find_next_bit(deleted, 64, i + 1)) { + for_each_set_bit(i, deleted, 64) { rtm_phonet_notify(RTM_DELROUTE, dev, i); dev_put(dev); } @@ -374,6 +373,7 @@ int __init phonet_device_init(void) if (err) return err; + proc_net_fops_create(&init_net, "pnresource", 0, &pn_res_seq_fops); register_netdevice_notifier(&phonet_device_notifier); err = phonet_netlink_register(); if (err) @@ -386,6 +386,7 @@ void phonet_device_exit(void) rtnl_unregister_all(PF_PHONET); unregister_netdevice_notifier(&phonet_device_notifier); unregister_pernet_device(&phonet_net_ops); + proc_net_remove(&init_net, "pnresource"); } int phonet_route_add(struct net_device *dev, u8 daddr) diff --git a/net/phonet/socket.c b/net/phonet/socket.c index 6e9848bf..aca8fba 100644 --- a/net/phonet/socket.c +++ b/net/phonet/socket.c @@ -158,6 +158,7 @@ void pn_sock_unhash(struct sock *sk) spin_lock_bh(&pnsocks.lock); sk_del_node_init(sk); spin_unlock_bh(&pnsocks.lock); + pn_sock_unbind_all_res(sk); } EXPORT_SYMBOL(pn_sock_unhash); @@ -281,7 +282,9 @@ static unsigned int pn_socket_poll(struct file *file, struct socket *sock, if (!mask && sk->sk_state == TCP_CLOSE_WAIT) return POLLHUP; - if (sk->sk_state == TCP_ESTABLISHED && atomic_read(&pn->tx_credits)) + if (sk->sk_state == TCP_ESTABLISHED && + atomic_read(&sk->sk_wmem_alloc) < sk->sk_sndbuf && + atomic_read(&pn->tx_credits)) mask |= POLLOUT | POLLWRNORM | POLLWRBAND; return mask; @@ -563,3 +566,188 @@ const struct file_operations pn_sock_seq_fops = { .release = seq_release_net, }; #endif + +static struct { + struct sock *sk[256]; +} pnres; + +/* + * Find and hold socket based on resource. + */ +struct sock *pn_find_sock_by_res(struct net *net, u8 res) +{ + struct sock *sk; + + if (!net_eq(net, &init_net)) + return NULL; + + rcu_read_lock(); + sk = rcu_dereference(pnres.sk[res]); + if (sk) + sock_hold(sk); + rcu_read_unlock(); + return sk; +} + +static DEFINE_MUTEX(resource_mutex); + +int pn_sock_bind_res(struct sock *sk, u8 res) +{ + int ret = -EADDRINUSE; + + if (!net_eq(sock_net(sk), &init_net)) + return -ENOIOCTLCMD; + if (!capable(CAP_SYS_ADMIN)) + return -EPERM; + if (pn_socket_autobind(sk->sk_socket)) + return -EAGAIN; + + mutex_lock(&resource_mutex); + if (pnres.sk[res] == NULL) { + sock_hold(sk); + rcu_assign_pointer(pnres.sk[res], sk); + ret = 0; + } + mutex_unlock(&resource_mutex); + return ret; +} + +int pn_sock_unbind_res(struct sock *sk, u8 res) +{ + int ret = -ENOENT; + + if (!capable(CAP_SYS_ADMIN)) + return -EPERM; + + mutex_lock(&resource_mutex); + if (pnres.sk[res] == sk) { + rcu_assign_pointer(pnres.sk[res], NULL); + ret = 0; + } + mutex_unlock(&resource_mutex); + + if (ret == 0) { + synchronize_rcu(); + sock_put(sk); + } + return ret; +} + +void pn_sock_unbind_all_res(struct sock *sk) +{ + unsigned res, match = 0; + + mutex_lock(&resource_mutex); + for (res = 0; res < 256; res++) { + if (pnres.sk[res] == sk) { + rcu_assign_pointer(pnres.sk[res], NULL); + match++; + } + } + mutex_unlock(&resource_mutex); + + if (match == 0) + return; + synchronize_rcu(); + while (match > 0) { + sock_put(sk); + match--; + } +} + +#ifdef CONFIG_PROC_FS +static struct sock **pn_res_get_idx(struct seq_file *seq, loff_t pos) +{ + struct net *net = seq_file_net(seq); + unsigned i; + + if (!net_eq(net, &init_net)) + return NULL; + + for (i = 0; i < 256; i++) { + if (pnres.sk[i] == NULL) + continue; + if (!pos) + return pnres.sk + i; + pos--; + } + return NULL; +} + +static struct sock **pn_res_get_next(struct seq_file *seq, struct sock **sk) +{ + struct net *net = seq_file_net(seq); + unsigned i; + + BUG_ON(!net_eq(net, &init_net)); + + for (i = (sk - pnres.sk) + 1; i < 256; i++) + if (pnres.sk[i]) + return pnres.sk + i; + return NULL; +} + +static void *pn_res_seq_start(struct seq_file *seq, loff_t *pos) + __acquires(resource_mutex) +{ + mutex_lock(&resource_mutex); + return *pos ? pn_res_get_idx(seq, *pos - 1) : SEQ_START_TOKEN; +} + +static void *pn_res_seq_next(struct seq_file *seq, void *v, loff_t *pos) +{ + struct sock **sk; + + if (v == SEQ_START_TOKEN) + sk = pn_res_get_idx(seq, 0); + else + sk = pn_res_get_next(seq, v); + (*pos)++; + return sk; +} + +static void pn_res_seq_stop(struct seq_file *seq, void *v) + __releases(resource_mutex) +{ + mutex_unlock(&resource_mutex); +} + +static int pn_res_seq_show(struct seq_file *seq, void *v) +{ + int len; + + if (v == SEQ_START_TOKEN) + seq_printf(seq, "%s%n", "rs uid inode", &len); + else { + struct sock **psk = v; + struct sock *sk = *psk; + + seq_printf(seq, "%02X %5d %lu%n", + (int) (psk - pnres.sk), sock_i_uid(sk), + sock_i_ino(sk), &len); + } + seq_printf(seq, "%*s\n", 63 - len, ""); + return 0; +} + +static const struct seq_operations pn_res_seq_ops = { + .start = pn_res_seq_start, + .next = pn_res_seq_next, + .stop = pn_res_seq_stop, + .show = pn_res_seq_show, +}; + +static int pn_res_open(struct inode *inode, struct file *file) +{ + return seq_open_net(inode, file, &pn_res_seq_ops, + sizeof(struct seq_net_private)); +} + +const struct file_operations pn_res_seq_fops = { + .owner = THIS_MODULE, + .open = pn_res_open, + .read = seq_read, + .llseek = seq_lseek, + .release = seq_release_net, +}; +#endif |
