diff options
Diffstat (limited to 'net/qrtr/qrtr.c')
| -rw-r--r-- | net/qrtr/qrtr.c | 109 |
1 files changed, 99 insertions, 10 deletions
diff --git a/net/qrtr/qrtr.c b/net/qrtr/qrtr.c index a9a8c7d5a4a9..5586609afa27 100644 --- a/net/qrtr/qrtr.c +++ b/net/qrtr/qrtr.c @@ -111,6 +111,9 @@ struct qrtr_node { struct list_head item; }; +static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb); +static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb); + /* Release node resources and free the node. * * Do not call directly, use qrtr_node_release. To be used with @@ -236,7 +239,7 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len) return -ENOMEM; skb_reset_transport_header(skb); - memcpy(skb_put(skb, len), data, len); + skb_put_data(skb, data, len); skb_queue_tail(&node->rx_queue, skb); schedule_work(&node->work); @@ -245,23 +248,20 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len) } EXPORT_SYMBOL_GPL(qrtr_endpoint_post); -/* Allocate and construct a resume-tx packet. */ -static struct sk_buff *qrtr_alloc_resume_tx(u32 src_node, - u32 dst_node, u32 port) +static struct sk_buff *qrtr_alloc_ctrl_packet(u32 type, size_t pkt_len, + u32 src_node, u32 dst_node) { - const int pkt_len = 20; struct qrtr_hdr *hdr; struct sk_buff *skb; - __le32 *buf; skb = alloc_skb(QRTR_HDR_SIZE + pkt_len, GFP_KERNEL); if (!skb) return NULL; skb_reset_transport_header(skb); - hdr = (struct qrtr_hdr *)skb_put(skb, QRTR_HDR_SIZE); + hdr = skb_put(skb, QRTR_HDR_SIZE); hdr->version = cpu_to_le32(QRTR_PROTO_VER); - hdr->type = cpu_to_le32(QRTR_TYPE_RESUME_TX); + hdr->type = cpu_to_le32(type); hdr->src_node_id = cpu_to_le32(src_node); hdr->src_port_id = cpu_to_le32(QRTR_PORT_CTRL); hdr->confirm_rx = cpu_to_le32(0); @@ -269,8 +269,23 @@ static struct sk_buff *qrtr_alloc_resume_tx(u32 src_node, hdr->dst_node_id = cpu_to_le32(dst_node); hdr->dst_port_id = cpu_to_le32(QRTR_PORT_CTRL); - buf = (__le32 *)skb_put(skb, pkt_len); - memset(buf, 0, pkt_len); + return skb; +} + +/* Allocate and construct a resume-tx packet. */ +static struct sk_buff *qrtr_alloc_resume_tx(u32 src_node, + u32 dst_node, u32 port) +{ + const int pkt_len = 20; + struct sk_buff *skb; + __le32 *buf; + + skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_RESUME_TX, pkt_len, + src_node, dst_node); + if (!skb) + return NULL; + + buf = skb_put_zero(skb, pkt_len); buf[0] = cpu_to_le32(QRTR_TYPE_RESUME_TX); buf[1] = cpu_to_le32(src_node); buf[2] = cpu_to_le32(port); @@ -278,6 +293,43 @@ static struct sk_buff *qrtr_alloc_resume_tx(u32 src_node, return skb; } +/* Allocate and construct a BYE message to signal remote termination */ +static struct sk_buff *qrtr_alloc_local_bye(u32 src_node) +{ + const int pkt_len = 20; + struct sk_buff *skb; + __le32 *buf; + + skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_BYE, pkt_len, + src_node, qrtr_local_nid); + if (!skb) + return NULL; + + buf = skb_put_zero(skb, pkt_len); + buf[0] = cpu_to_le32(QRTR_TYPE_BYE); + + return skb; +} + +static struct sk_buff *qrtr_alloc_del_client(struct sockaddr_qrtr *sq) +{ + const int pkt_len = 20; + struct sk_buff *skb; + __le32 *buf; + + skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_DEL_CLIENT, pkt_len, + sq->sq_node, QRTR_NODE_BCAST); + if (!skb) + return NULL; + + buf = skb_put_zero(skb, pkt_len); + buf[0] = cpu_to_le32(QRTR_TYPE_DEL_CLIENT); + buf[1] = cpu_to_le32(sq->sq_node); + buf[2] = cpu_to_le32(sq->sq_port); + + return skb; +} + static struct qrtr_sock *qrtr_port_lookup(int port); static void qrtr_port_put(struct qrtr_sock *ipc); @@ -369,11 +421,17 @@ EXPORT_SYMBOL_GPL(qrtr_endpoint_register); void qrtr_endpoint_unregister(struct qrtr_endpoint *ep) { struct qrtr_node *node = ep->node; + struct sk_buff *skb; mutex_lock(&node->ep_lock); node->ep = NULL; mutex_unlock(&node->ep_lock); + /* Notify the local controller about the event */ + skb = qrtr_alloc_local_bye(node->nid); + if (skb) + qrtr_local_enqueue(NULL, skb); + qrtr_node_release(node); ep->node = NULL; } @@ -408,8 +466,15 @@ static void qrtr_port_put(struct qrtr_sock *ipc) /* Remove port assignment. */ static void qrtr_port_remove(struct qrtr_sock *ipc) { + struct sk_buff *skb; int port = ipc->us.sq_port; + skb = qrtr_alloc_del_client(&ipc->us); + if (skb) { + skb_set_owner_w(skb, &ipc->sk); + qrtr_bcast_enqueue(NULL, skb); + } + if (port == QRTR_PORT_CTRL) port = 0; @@ -462,6 +527,26 @@ static int qrtr_port_assign(struct qrtr_sock *ipc, int *port) return 0; } +/* Reset all non-control ports */ +static void qrtr_reset_ports(void) +{ + struct qrtr_sock *ipc; + int id; + + mutex_lock(&qrtr_port_lock); + idr_for_each_entry(&qrtr_ports, ipc, id) { + /* Don't reset control port */ + if (id == 0) + continue; + + sock_hold(&ipc->sk); + ipc->sk.sk_err = ENETRESET; + wake_up_interruptible(sk_sleep(&ipc->sk)); + sock_put(&ipc->sk); + } + mutex_unlock(&qrtr_port_lock); +} + /* Bind socket to address. * * Socket should be locked upon call. @@ -490,6 +575,10 @@ static int __qrtr_bind(struct socket *sock, sock_reset_flag(sk, SOCK_ZAPPED); + /* Notify all open ports about the new controller */ + if (port == QRTR_PORT_CTRL) + qrtr_reset_ports(); + return 0; } |