diff --git a/net/qrtr/qrtr.c b/net/qrtr/qrtr.c index a9a8c7d5a4a98..c7a5d861906bb 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 @@ -245,14 +248,11 @@ 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) @@ -261,7 +261,7 @@ static struct sk_buff *qrtr_alloc_resume_tx(u32 src_node, hdr = (struct qrtr_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,6 +269,22 @@ 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); + 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 = (__le32 *)skb_put(skb, pkt_len); memset(buf, 0, pkt_len); buf[0] = cpu_to_le32(QRTR_TYPE_RESUME_TX); @@ -278,6 +294,45 @@ 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 = (__le32 *)skb_put(skb, pkt_len); + memset(buf, 0, 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 = (__le32 *)skb_put(skb, pkt_len); + memset(buf, 0, 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 +424,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 +469,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 +530,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 +578,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; }