^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1) // SPDX-License-Identifier: GPL-2.0-only
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 3) * Copyright (c) 2015, Sony Mobile Communications Inc.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 4) * Copyright (c) 2013, The Linux Foundation. All rights reserved.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 5) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 6) #include <linux/module.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 7) #include <linux/netlink.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 8) #include <linux/qrtr.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 9) #include <linux/termios.h> /* For TIOCINQ/OUTQ */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) #include <linux/spinlock.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 11) #include <linux/wait.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 12)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13) #include <net/sock.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 15) #include "qrtr.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 16)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17) #define QRTR_PROTO_VER_1 1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) #define QRTR_PROTO_VER_2 3
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20) /* auto-bind range */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21) #define QRTR_MIN_EPH_SOCKET 0x4000
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22) #define QRTR_MAX_EPH_SOCKET 0x7fff
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25) * struct qrtr_hdr_v1 - (I|R)PCrouter packet header version 1
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) * @version: protocol version
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27) * @type: packet type; one of QRTR_TYPE_*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28) * @src_node_id: source node
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29) * @src_port_id: source port
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) * @confirm_rx: boolean; whether a resume-tx packet should be send in reply
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31) * @size: length of packet, excluding this header
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32) * @dst_node_id: destination node
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) * @dst_port_id: destination port
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) struct qrtr_hdr_v1 {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36) __le32 version;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) __le32 type;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38) __le32 src_node_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39) __le32 src_port_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40) __le32 confirm_rx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41) __le32 size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) __le32 dst_node_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) __le32 dst_port_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44) } __packed;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47) * struct qrtr_hdr_v2 - (I|R)PCrouter packet header later versions
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) * @version: protocol version
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49) * @type: packet type; one of QRTR_TYPE_*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50) * @flags: bitmask of QRTR_FLAGS_*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51) * @optlen: length of optional header data
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52) * @size: length of packet, excluding this header and optlen
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53) * @src_node_id: source node
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54) * @src_port_id: source port
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55) * @dst_node_id: destination node
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56) * @dst_port_id: destination port
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58) struct qrtr_hdr_v2 {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59) u8 version;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60) u8 type;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61) u8 flags;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62) u8 optlen;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63) __le32 size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64) __le16 src_node_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 65) __le16 src_port_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 66) __le16 dst_node_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 67) __le16 dst_port_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 68) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 69)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 70) #define QRTR_FLAGS_CONFIRM_RX BIT(0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 71)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 72) struct qrtr_cb {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 73) u32 src_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 74) u32 src_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 75) u32 dst_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 76) u32 dst_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 77)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 78) u8 type;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 79) u8 confirm_rx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 80) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 81)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 82) #define QRTR_HDR_MAX_SIZE max_t(size_t, sizeof(struct qrtr_hdr_v1), \
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 83) sizeof(struct qrtr_hdr_v2))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 84)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 85) struct qrtr_sock {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 86) /* WARNING: sk must be the first member */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 87) struct sock sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 88) struct sockaddr_qrtr us;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 89) struct sockaddr_qrtr peer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 90) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 91)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 92) static inline struct qrtr_sock *qrtr_sk(struct sock *sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 93) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 94) BUILD_BUG_ON(offsetof(struct qrtr_sock, sk) != 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 95) return container_of(sk, struct qrtr_sock, sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 96) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 97)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 98) static unsigned int qrtr_local_nid = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 99)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100) /* for node ids */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) static RADIX_TREE(qrtr_nodes, GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) static DEFINE_SPINLOCK(qrtr_nodes_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) /* broadcast list */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) static LIST_HEAD(qrtr_all_nodes);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) /* lock for qrtr_all_nodes and node reference */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) static DEFINE_MUTEX(qrtr_node_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) /* local port allocation management */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) static DEFINE_IDR(qrtr_ports);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) static DEFINE_MUTEX(qrtr_port_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113) * struct qrtr_node - endpoint node
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) * @ep_lock: lock for endpoint management and callbacks
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) * @ep: endpoint
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116) * @ref: reference count for node
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) * @nid: node id
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) * @qrtr_tx_flow: tree of qrtr_tx_flow, keyed by node << 32 | port
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) * @qrtr_tx_lock: lock for qrtr_tx_flow inserts
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) * @rx_queue: receive queue
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) * @item: list item for broadcast list
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) struct qrtr_node {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) struct mutex ep_lock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) struct qrtr_endpoint *ep;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126) struct kref ref;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) unsigned int nid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129) struct radix_tree_root qrtr_tx_flow;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) struct mutex qrtr_tx_lock; /* for qrtr_tx_flow */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132) struct sk_buff_head rx_queue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) struct list_head item;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137) * struct qrtr_tx_flow - tx flow control
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) * @resume_tx: waiters for a resume tx from the remote
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) * @pending: number of waiting senders
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) * @tx_failed: indicates that a message with confirm_rx flag was lost
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) struct qrtr_tx_flow {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143) struct wait_queue_head resume_tx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) int pending;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) int tx_failed;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) #define QRTR_TX_FLOW_HIGH 10
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) #define QRTR_TX_FLOW_LOW 5
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) int type, struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153) struct sockaddr_qrtr *to);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) int type, struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) struct sockaddr_qrtr *to);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) static struct qrtr_sock *qrtr_port_lookup(int port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) static void qrtr_port_put(struct qrtr_sock *ipc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) /* Release node resources and free the node.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) * Do not call directly, use qrtr_node_release. To be used with
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) * kref_put_mutex. As such, the node mutex is expected to be locked on call.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) static void __qrtr_node_release(struct kref *kref)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) struct radix_tree_iter iter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) struct qrtr_tx_flow *flow;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) unsigned long flags;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) void __rcu **slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) spin_lock_irqsave(&qrtr_nodes_lock, flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) if (node->nid != QRTR_EP_NID_AUTO)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) radix_tree_delete(&qrtr_nodes, node->nid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) list_del(&node->item);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) mutex_unlock(&qrtr_node_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181) skb_queue_purge(&node->rx_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) /* Free tx flow counters */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) flow = *slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) radix_tree_iter_delete(&node->qrtr_tx_flow, &iter, slot);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) kfree(flow);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189) kfree(node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) /* Increment reference to node. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) static struct qrtr_node *qrtr_node_acquire(struct qrtr_node *node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) if (node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) kref_get(&node->ref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) return node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) /* Decrement reference to node and release as necessary. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201) static void qrtr_node_release(struct qrtr_node *node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) * qrtr_tx_resume() - reset flow control counter
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) * @node: qrtr_node that the QRTR_TYPE_RESUME_TX packet arrived on
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) * @skb: resume_tx packet
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) static void qrtr_tx_resume(struct qrtr_node *node, struct sk_buff *skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215) struct qrtr_ctrl_pkt *pkt = (struct qrtr_ctrl_pkt *)skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216) u64 remote_node = le32_to_cpu(pkt->client.node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) u32 remote_port = le32_to_cpu(pkt->client.port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) struct qrtr_tx_flow *flow;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219) unsigned long key;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) key = remote_node << 32 | remote_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224) flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226) if (flow) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) spin_lock(&flow->resume_tx.lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) flow->pending = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229) spin_unlock(&flow->resume_tx.lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) wake_up_interruptible_all(&flow->resume_tx);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233) consume_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237) * qrtr_tx_wait() - flow control for outgoing packets
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) * @node: qrtr_node that the packet is to be send to
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239) * @dest_node: node id of the destination
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240) * @dest_port: port number of the destination
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241) * @type: type of message
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243) * The flow control scheme is based around the low and high "watermarks". When
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244) * the low watermark is passed the confirm_rx flag is set on the outgoing
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 245) * message, which will trigger the remote to send a control message of the type
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 246) * QRTR_TYPE_RESUME_TX to reset the counter. If the high watermark is hit
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 247) * further transmision should be paused.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 248) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 249) * Return: 1 if confirm_rx should be set, 0 otherwise or errno failure
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 250) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 251) static int qrtr_tx_wait(struct qrtr_node *node, int dest_node, int dest_port,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 252) int type)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 253) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 254) unsigned long key = (u64)dest_node << 32 | dest_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 255) struct qrtr_tx_flow *flow;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 256) int confirm_rx = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 257) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 258)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 259) /* Never set confirm_rx on non-data packets */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 260) if (type != QRTR_TYPE_DATA)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 261) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 262)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 263) mutex_lock(&node->qrtr_tx_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 264) flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 265) if (!flow) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 266) flow = kzalloc(sizeof(*flow), GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 267) if (flow) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 268) init_waitqueue_head(&flow->resume_tx);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 269) if (radix_tree_insert(&node->qrtr_tx_flow, key, flow)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 270) kfree(flow);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 271) flow = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 272) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 273) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 274) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 275) mutex_unlock(&node->qrtr_tx_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 276)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 277) /* Set confirm_rx if we where unable to find and allocate a flow */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 278) if (!flow)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 279) return 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 280)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 281) spin_lock_irq(&flow->resume_tx.lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 282) ret = wait_event_interruptible_locked_irq(flow->resume_tx,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 283) flow->pending < QRTR_TX_FLOW_HIGH ||
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 284) flow->tx_failed ||
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 285) !node->ep);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 286) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 287) confirm_rx = ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 288) } else if (!node->ep) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 289) confirm_rx = -EPIPE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 290) } else if (flow->tx_failed) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 291) flow->tx_failed = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 292) confirm_rx = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 293) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 294) flow->pending++;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 295) confirm_rx = flow->pending == QRTR_TX_FLOW_LOW;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 296) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 297) spin_unlock_irq(&flow->resume_tx.lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 298)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 299) return confirm_rx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 300) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 301)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 302) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 303) * qrtr_tx_flow_failed() - flag that tx of confirm_rx flagged messages failed
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 304) * @node: qrtr_node that the packet is to be send to
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 305) * @dest_node: node id of the destination
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 306) * @dest_port: port number of the destination
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 307) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 308) * Signal that the transmission of a message with confirm_rx flag failed. The
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 309) * flow's "pending" counter will keep incrementing towards QRTR_TX_FLOW_HIGH,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 310) * at which point transmission would stall forever waiting for the resume TX
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 311) * message associated with the dropped confirm_rx message.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 312) * Work around this by marking the flow as having a failed transmission and
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 313) * cause the next transmission attempt to be sent with the confirm_rx.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 314) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 315) static void qrtr_tx_flow_failed(struct qrtr_node *node, int dest_node,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 316) int dest_port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 317) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 318) unsigned long key = (u64)dest_node << 32 | dest_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 319) struct qrtr_tx_flow *flow;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 320)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 321) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 322) flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 323) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 324) if (flow) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 325) spin_lock_irq(&flow->resume_tx.lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 326) flow->tx_failed = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 327) spin_unlock_irq(&flow->resume_tx.lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 328) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 329) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 330)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 331) /* Pass an outgoing packet socket buffer to the endpoint driver. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 332) static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 333) int type, struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 334) struct sockaddr_qrtr *to)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 335) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 336) struct qrtr_hdr_v1 *hdr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 337) size_t len = skb->len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 338) int rc, confirm_rx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 339)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 340) confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 341) if (confirm_rx < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 342) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 343) return confirm_rx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 344) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 345)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 346) hdr = skb_push(skb, sizeof(*hdr));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 347) hdr->version = cpu_to_le32(QRTR_PROTO_VER_1);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 348) hdr->type = cpu_to_le32(type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 349) hdr->src_node_id = cpu_to_le32(from->sq_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 350) hdr->src_port_id = cpu_to_le32(from->sq_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 351) if (to->sq_port == QRTR_PORT_CTRL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 352) hdr->dst_node_id = cpu_to_le32(node->nid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 353) hdr->dst_port_id = cpu_to_le32(QRTR_PORT_CTRL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 354) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 355) hdr->dst_node_id = cpu_to_le32(to->sq_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 356) hdr->dst_port_id = cpu_to_le32(to->sq_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 357) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 358)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 359) hdr->size = cpu_to_le32(len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 360) hdr->confirm_rx = !!confirm_rx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 361)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 362) rc = skb_put_padto(skb, ALIGN(len, 4) + sizeof(*hdr));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 363)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 364) if (!rc) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 365) mutex_lock(&node->ep_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 366) rc = -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 367) if (node->ep)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 368) rc = node->ep->xmit(node->ep, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 369) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 370) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 371) mutex_unlock(&node->ep_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 372) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 373) /* Need to ensure that a subsequent message carries the otherwise lost
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 374) * confirm_rx flag if we dropped this one */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 375) if (rc && confirm_rx)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 376) qrtr_tx_flow_failed(node, to->sq_node, to->sq_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 377)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 378) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 379) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 380)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 381) /* Lookup node by id.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 382) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 383) * callers must release with qrtr_node_release()
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 384) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 385) static struct qrtr_node *qrtr_node_lookup(unsigned int nid)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 386) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 387) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 388) unsigned long flags;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 389)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 390) spin_lock_irqsave(&qrtr_nodes_lock, flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 391) node = radix_tree_lookup(&qrtr_nodes, nid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 392) node = qrtr_node_acquire(node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 393) spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 394)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 395) return node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 396) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 397)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 398) /* Assign node id to node.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 399) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 400) * This is mostly useful for automatic node id assignment, based on
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 401) * the source id in the incoming packet.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 402) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 403) static void qrtr_node_assign(struct qrtr_node *node, unsigned int nid)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 404) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 405) unsigned long flags;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 406)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 407) if (node->nid != QRTR_EP_NID_AUTO || nid == QRTR_EP_NID_AUTO)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 408) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 409)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 410) spin_lock_irqsave(&qrtr_nodes_lock, flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 411) radix_tree_insert(&qrtr_nodes, nid, node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 412) node->nid = nid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 413) spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 414) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 415)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 416) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 417) * qrtr_endpoint_post() - post incoming data
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 418) * @ep: endpoint handle
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 419) * @data: data pointer
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 420) * @len: size of data in bytes
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 421) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 422) * Return: 0 on success; negative error code on failure
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 423) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 424) int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 425) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 426) struct qrtr_node *node = ep->node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 427) const struct qrtr_hdr_v1 *v1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 428) const struct qrtr_hdr_v2 *v2;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 429) struct qrtr_sock *ipc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 430) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 431) struct qrtr_cb *cb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 432) size_t size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 433) unsigned int ver;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 434) size_t hdrlen;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 435)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 436) if (len == 0 || len & 3)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 437) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 438)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 439) skb = __netdev_alloc_skb(NULL, len, GFP_ATOMIC | __GFP_NOWARN);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 440) if (!skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 441) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 442)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 443) cb = (struct qrtr_cb *)skb->cb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 444)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 445) /* Version field in v1 is little endian, so this works for both cases */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 446) ver = *(u8*)data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 447)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 448) switch (ver) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 449) case QRTR_PROTO_VER_1:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 450) if (len < sizeof(*v1))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 451) goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 452) v1 = data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 453) hdrlen = sizeof(*v1);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 454)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 455) cb->type = le32_to_cpu(v1->type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 456) cb->src_node = le32_to_cpu(v1->src_node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 457) cb->src_port = le32_to_cpu(v1->src_port_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 458) cb->confirm_rx = !!v1->confirm_rx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 459) cb->dst_node = le32_to_cpu(v1->dst_node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 460) cb->dst_port = le32_to_cpu(v1->dst_port_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 461)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 462) size = le32_to_cpu(v1->size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 463) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 464) case QRTR_PROTO_VER_2:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 465) if (len < sizeof(*v2))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 466) goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 467) v2 = data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 468) hdrlen = sizeof(*v2) + v2->optlen;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 469)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 470) cb->type = v2->type;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 471) cb->confirm_rx = !!(v2->flags & QRTR_FLAGS_CONFIRM_RX);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 472) cb->src_node = le16_to_cpu(v2->src_node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 473) cb->src_port = le16_to_cpu(v2->src_port_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 474) cb->dst_node = le16_to_cpu(v2->dst_node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 475) cb->dst_port = le16_to_cpu(v2->dst_port_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 476)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 477) if (cb->src_port == (u16)QRTR_PORT_CTRL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 478) cb->src_port = QRTR_PORT_CTRL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 479) if (cb->dst_port == (u16)QRTR_PORT_CTRL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 480) cb->dst_port = QRTR_PORT_CTRL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 481)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 482) size = le32_to_cpu(v2->size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 483) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 484) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 485) pr_err("qrtr: Invalid version %d\n", ver);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 486) goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 487) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 488)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 489) if (!size || len != ALIGN(size, 4) + hdrlen)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 490) goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 491)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 492) if (cb->dst_port != QRTR_PORT_CTRL && cb->type != QRTR_TYPE_DATA &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 493) cb->type != QRTR_TYPE_RESUME_TX)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 494) goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 495)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 496) skb_put_data(skb, data + hdrlen, size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 497)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 498) qrtr_node_assign(node, cb->src_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 499)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 500) if (cb->type == QRTR_TYPE_RESUME_TX) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 501) qrtr_tx_resume(node, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 502) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 503) ipc = qrtr_port_lookup(cb->dst_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 504) if (!ipc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 505) goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 506)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 507) if (sock_queue_rcv_skb(&ipc->sk, skb)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 508) qrtr_port_put(ipc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 509) goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 510) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 511)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 512) qrtr_port_put(ipc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 513) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 514)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 515) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 516)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 517) err:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 518) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 519) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 520)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 521) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 522) EXPORT_SYMBOL_GPL(qrtr_endpoint_post);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 523)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 524) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 525) * qrtr_alloc_ctrl_packet() - allocate control packet skb
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 526) * @pkt: reference to qrtr_ctrl_pkt pointer
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 527) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 528) * Returns newly allocated sk_buff, or NULL on failure
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 529) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 530) * This function allocates a sk_buff large enough to carry a qrtr_ctrl_pkt and
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 531) * on success returns a reference to the control packet in @pkt.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 532) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 533) static struct sk_buff *qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt **pkt)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 534) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 535) const int pkt_len = sizeof(struct qrtr_ctrl_pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 536) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 537)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 538) skb = alloc_skb(QRTR_HDR_MAX_SIZE + pkt_len, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 539) if (!skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 540) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 541)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 542) skb_reserve(skb, QRTR_HDR_MAX_SIZE);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 543) *pkt = skb_put_zero(skb, pkt_len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 544)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 545) return skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 546) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 547)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 548) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 549) * qrtr_endpoint_register() - register a new endpoint
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 550) * @ep: endpoint to register
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 551) * @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 552) * Return: 0 on success; negative error code on failure
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 553) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 554) * The specified endpoint must have the xmit function pointer set on call.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 555) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 556) int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 557) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 558) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 559)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 560) if (!ep || !ep->xmit)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 561) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 562)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 563) node = kzalloc(sizeof(*node), GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 564) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 565) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 566)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 567) kref_init(&node->ref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 568) mutex_init(&node->ep_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 569) skb_queue_head_init(&node->rx_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 570) node->nid = QRTR_EP_NID_AUTO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 571) node->ep = ep;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 572)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 573) INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 574) mutex_init(&node->qrtr_tx_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 575)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 576) qrtr_node_assign(node, nid);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 577)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 578) mutex_lock(&qrtr_node_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 579) list_add(&node->item, &qrtr_all_nodes);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 580) mutex_unlock(&qrtr_node_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 581) ep->node = node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 582)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 583) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 584) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 585) EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 586)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 587) /**
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 588) * qrtr_endpoint_unregister - unregister endpoint
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 589) * @ep: endpoint to unregister
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 590) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 591) void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 592) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 593) struct qrtr_node *node = ep->node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 594) struct sockaddr_qrtr src = {AF_QIPCRTR, node->nid, QRTR_PORT_CTRL};
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 595) struct sockaddr_qrtr dst = {AF_QIPCRTR, qrtr_local_nid, QRTR_PORT_CTRL};
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 596) struct radix_tree_iter iter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 597) struct qrtr_ctrl_pkt *pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 598) struct qrtr_tx_flow *flow;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 599) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 600) void __rcu **slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 601)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 602) mutex_lock(&node->ep_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 603) node->ep = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 604) mutex_unlock(&node->ep_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 605)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 606) /* Notify the local controller about the event */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 607) skb = qrtr_alloc_ctrl_packet(&pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 608) if (skb) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 609) pkt->cmd = cpu_to_le32(QRTR_TYPE_BYE);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 610) qrtr_local_enqueue(NULL, skb, QRTR_TYPE_BYE, &src, &dst);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 611) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 612)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 613) /* Wake up any transmitters waiting for resume-tx from the node */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 614) mutex_lock(&node->qrtr_tx_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 615) radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 616) flow = *slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 617) wake_up_interruptible_all(&flow->resume_tx);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 618) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 619) mutex_unlock(&node->qrtr_tx_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 620)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 621) qrtr_node_release(node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 622) ep->node = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 623) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 624) EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 625)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 626) /* Lookup socket by port.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 627) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 628) * Callers must release with qrtr_port_put()
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 629) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 630) static struct qrtr_sock *qrtr_port_lookup(int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 631) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 632) struct qrtr_sock *ipc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 633)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 634) if (port == QRTR_PORT_CTRL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 635) port = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 636)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 637) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 638) ipc = idr_find(&qrtr_ports, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 639) if (ipc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 640) sock_hold(&ipc->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 641) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 642)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 643) return ipc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 644) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 645)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 646) /* Release acquired socket. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 647) static void qrtr_port_put(struct qrtr_sock *ipc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 648) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 649) sock_put(&ipc->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 650) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 651)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 652) /* Remove port assignment. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 653) static void qrtr_port_remove(struct qrtr_sock *ipc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 654) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 655) struct qrtr_ctrl_pkt *pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 656) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 657) int port = ipc->us.sq_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 658) struct sockaddr_qrtr to;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 659)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 660) to.sq_family = AF_QIPCRTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 661) to.sq_node = QRTR_NODE_BCAST;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 662) to.sq_port = QRTR_PORT_CTRL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 663)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 664) skb = qrtr_alloc_ctrl_packet(&pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 665) if (skb) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 666) pkt->cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 667) pkt->client.node = cpu_to_le32(ipc->us.sq_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 668) pkt->client.port = cpu_to_le32(ipc->us.sq_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 669)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 670) skb_set_owner_w(skb, &ipc->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 671) qrtr_bcast_enqueue(NULL, skb, QRTR_TYPE_DEL_CLIENT, &ipc->us,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 672) &to);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 673) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 674)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 675) if (port == QRTR_PORT_CTRL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 676) port = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 677)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 678) __sock_put(&ipc->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 679)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 680) mutex_lock(&qrtr_port_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 681) idr_remove(&qrtr_ports, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 682) mutex_unlock(&qrtr_port_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 683)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 684) /* Ensure that if qrtr_port_lookup() did enter the RCU read section we
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 685) * wait for it to up increment the refcount */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 686) synchronize_rcu();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 687) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 688)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 689) /* Assign port number to socket.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 690) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 691) * Specify port in the integer pointed to by port, and it will be adjusted
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 692) * on return as necesssary.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 693) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 694) * Port may be:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 695) * 0: Assign ephemeral port in [QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET]
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 696) * <QRTR_MIN_EPH_SOCKET: Specified; requires CAP_NET_ADMIN
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 697) * >QRTR_MIN_EPH_SOCKET: Specified; available to all
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 698) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 699) static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 700) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 701) u32 min_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 702) int rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 703)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 704) mutex_lock(&qrtr_port_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 705) if (!*port) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 706) min_port = QRTR_MIN_EPH_SOCKET;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 707) rc = idr_alloc_u32(&qrtr_ports, ipc, &min_port, QRTR_MAX_EPH_SOCKET, GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 708) if (!rc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 709) *port = min_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 710) } else if (*port < QRTR_MIN_EPH_SOCKET && !capable(CAP_NET_ADMIN)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 711) rc = -EACCES;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 712) } else if (*port == QRTR_PORT_CTRL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 713) min_port = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 714) rc = idr_alloc_u32(&qrtr_ports, ipc, &min_port, 0, GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 715) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 716) min_port = *port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 717) rc = idr_alloc_u32(&qrtr_ports, ipc, &min_port, *port, GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 718) if (!rc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 719) *port = min_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 720) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 721) mutex_unlock(&qrtr_port_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 722)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 723) if (rc == -ENOSPC)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 724) return -EADDRINUSE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 725) else if (rc < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 726) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 727)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 728) sock_hold(&ipc->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 729)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 730) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 731) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 732)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 733) /* Reset all non-control ports */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 734) static void qrtr_reset_ports(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 735) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 736) struct qrtr_sock *ipc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 737) int id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 738)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 739) mutex_lock(&qrtr_port_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 740) idr_for_each_entry(&qrtr_ports, ipc, id) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 741) /* Don't reset control port */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 742) if (id == 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 743) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 744)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 745) sock_hold(&ipc->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 746) ipc->sk.sk_err = ENETRESET;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 747) ipc->sk.sk_error_report(&ipc->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 748) sock_put(&ipc->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 749) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 750) mutex_unlock(&qrtr_port_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 751) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 752)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 753) /* Bind socket to address.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 754) *
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 755) * Socket should be locked upon call.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 756) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 757) static int __qrtr_bind(struct socket *sock,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 758) const struct sockaddr_qrtr *addr, int zapped)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 759) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 760) struct qrtr_sock *ipc = qrtr_sk(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 761) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 762) int port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 763) int rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 764)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 765) /* rebinding ok */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 766) if (!zapped && addr->sq_port == ipc->us.sq_port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 767) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 768)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 769) port = addr->sq_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 770) rc = qrtr_port_assign(ipc, &port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 771) if (rc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 772) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 773)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 774) /* unbind previous, if any */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 775) if (!zapped)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 776) qrtr_port_remove(ipc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 777) ipc->us.sq_port = port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 778)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 779) sock_reset_flag(sk, SOCK_ZAPPED);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 780)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 781) /* Notify all open ports about the new controller */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 782) if (port == QRTR_PORT_CTRL)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 783) qrtr_reset_ports();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 784)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 785) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 786) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 787)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 788) /* Auto bind to an ephemeral port. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 789) static int qrtr_autobind(struct socket *sock)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 790) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 791) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 792) struct sockaddr_qrtr addr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 793)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 794) if (!sock_flag(sk, SOCK_ZAPPED))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 795) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 796)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 797) addr.sq_family = AF_QIPCRTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 798) addr.sq_node = qrtr_local_nid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 799) addr.sq_port = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 800)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 801) return __qrtr_bind(sock, &addr, 1);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 802) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 803)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 804) /* Bind socket to specified sockaddr. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 805) static int qrtr_bind(struct socket *sock, struct sockaddr *saddr, int len)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 806) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 807) DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 808) struct qrtr_sock *ipc = qrtr_sk(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 809) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 810) int rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 811)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 812) if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 813) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 814)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 815) if (addr->sq_node != ipc->us.sq_node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 816) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 817)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 818) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 819) rc = __qrtr_bind(sock, addr, sock_flag(sk, SOCK_ZAPPED));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 820) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 821)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 822) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 823) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 824)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 825) /* Queue packet to local peer socket. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 826) static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 827) int type, struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 828) struct sockaddr_qrtr *to)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 829) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 830) struct qrtr_sock *ipc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 831) struct qrtr_cb *cb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 832)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 833) ipc = qrtr_port_lookup(to->sq_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 834) if (!ipc || &ipc->sk == skb->sk) { /* do not send to self */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 835) if (ipc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 836) qrtr_port_put(ipc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 837) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 838) return -ENODEV;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 839) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 840)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 841) cb = (struct qrtr_cb *)skb->cb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 842) cb->src_node = from->sq_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 843) cb->src_port = from->sq_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 844)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 845) if (sock_queue_rcv_skb(&ipc->sk, skb)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 846) qrtr_port_put(ipc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 847) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 848) return -ENOSPC;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 849) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 850)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 851) qrtr_port_put(ipc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 852)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 853) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 854) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 855)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 856) /* Queue packet for broadcast. */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 857) static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 858) int type, struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 859) struct sockaddr_qrtr *to)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 860) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 861) struct sk_buff *skbn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 862)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 863) mutex_lock(&qrtr_node_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 864) list_for_each_entry(node, &qrtr_all_nodes, item) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 865) skbn = skb_clone(skb, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 866) if (!skbn)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 867) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 868) skb_set_owner_w(skbn, skb->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 869) qrtr_node_enqueue(node, skbn, type, from, to);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 870) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 871) mutex_unlock(&qrtr_node_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 872)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 873) qrtr_local_enqueue(NULL, skb, type, from, to);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 874)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 875) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 876) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 877)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 878) static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 879) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 880) DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 881) int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *, int,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 882) struct sockaddr_qrtr *, struct sockaddr_qrtr *);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 883) __le32 qrtr_type = cpu_to_le32(QRTR_TYPE_DATA);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 884) struct qrtr_sock *ipc = qrtr_sk(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 885) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 886) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 887) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 888) size_t plen;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 889) u32 type;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 890) int rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 891)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 892) if (msg->msg_flags & ~(MSG_DONTWAIT))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 893) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 894)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 895) if (len > 65535)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 896) return -EMSGSIZE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 897)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 898) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 899)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 900) if (addr) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 901) if (msg->msg_namelen < sizeof(*addr)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 902) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 903) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 904) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 905)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 906) if (addr->sq_family != AF_QIPCRTR) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 907) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 908) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 909) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 910)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 911) rc = qrtr_autobind(sock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 912) if (rc) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 913) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 914) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 915) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 916) } else if (sk->sk_state == TCP_ESTABLISHED) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 917) addr = &ipc->peer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 918) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 919) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 920) return -ENOTCONN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 921) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 922)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 923) node = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 924) if (addr->sq_node == QRTR_NODE_BCAST) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 925) if (addr->sq_port != QRTR_PORT_CTRL &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 926) qrtr_local_nid != QRTR_NODE_BCAST) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 927) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 928) return -ENOTCONN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 929) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 930) enqueue_fn = qrtr_bcast_enqueue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 931) } else if (addr->sq_node == ipc->us.sq_node) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 932) enqueue_fn = qrtr_local_enqueue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 933) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 934) node = qrtr_node_lookup(addr->sq_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 935) if (!node) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 936) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 937) return -ECONNRESET;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 938) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 939) enqueue_fn = qrtr_node_enqueue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 940) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 941)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 942) plen = (len + 3) & ~3;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 943) skb = sock_alloc_send_skb(sk, plen + QRTR_HDR_MAX_SIZE,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 944) msg->msg_flags & MSG_DONTWAIT, &rc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 945) if (!skb) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 946) rc = -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 947) goto out_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 948) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 949)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 950) skb_reserve(skb, QRTR_HDR_MAX_SIZE);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 951)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 952) rc = memcpy_from_msg(skb_put(skb, len), msg, len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 953) if (rc) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 954) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 955) goto out_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 956) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 957)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 958) if (ipc->us.sq_port == QRTR_PORT_CTRL) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 959) if (len < 4) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 960) rc = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 961) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 962) goto out_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 963) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 964)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 965) /* control messages already require the type as 'command' */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 966) skb_copy_bits(skb, 0, &qrtr_type, 4);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 967) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 968)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 969) type = le32_to_cpu(qrtr_type);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 970) rc = enqueue_fn(node, skb, type, &ipc->us, addr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 971) if (rc >= 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 972) rc = len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 973)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 974) out_node:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 975) qrtr_node_release(node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 976) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 977)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 978) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 979) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 980)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 981) static int qrtr_send_resume_tx(struct qrtr_cb *cb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 982) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 983) struct sockaddr_qrtr remote = { AF_QIPCRTR, cb->src_node, cb->src_port };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 984) struct sockaddr_qrtr local = { AF_QIPCRTR, cb->dst_node, cb->dst_port };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 985) struct qrtr_ctrl_pkt *pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 986) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 987) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 988) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 989)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 990) node = qrtr_node_lookup(remote.sq_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 991) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 992) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 993)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 994) skb = qrtr_alloc_ctrl_packet(&pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 995) if (!skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 996) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 997)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 998) pkt->cmd = cpu_to_le32(QRTR_TYPE_RESUME_TX);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 999) pkt->client.node = cpu_to_le32(cb->dst_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1000) pkt->client.port = cpu_to_le32(cb->dst_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1001)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1002) ret = qrtr_node_enqueue(node, skb, QRTR_TYPE_RESUME_TX, &local, &remote);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1003)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1004) qrtr_node_release(node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1005)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1006) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1007) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1008)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1009) static int qrtr_recvmsg(struct socket *sock, struct msghdr *msg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1010) size_t size, int flags)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1011) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1012) DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1013) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1014) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1015) struct qrtr_cb *cb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1016) int copied, rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1017)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1018) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1019)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1020) if (sock_flag(sk, SOCK_ZAPPED)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1021) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1022) return -EADDRNOTAVAIL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1023) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1024)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1025) skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1026) flags & MSG_DONTWAIT, &rc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1027) if (!skb) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1028) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1029) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1030) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1031) cb = (struct qrtr_cb *)skb->cb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1032)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1033) copied = skb->len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1034) if (copied > size) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1035) copied = size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1036) msg->msg_flags |= MSG_TRUNC;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1037) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1038)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1039) rc = skb_copy_datagram_msg(skb, 0, msg, copied);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1040) if (rc < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1041) goto out;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1042) rc = copied;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1043)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1044) if (addr) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1045) /* There is an anonymous 2-byte hole after sq_family,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1046) * make sure to clear it.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1047) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1048) memset(addr, 0, sizeof(*addr));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1049)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1050) addr->sq_family = AF_QIPCRTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1051) addr->sq_node = cb->src_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1052) addr->sq_port = cb->src_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1053) msg->msg_namelen = sizeof(*addr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1054) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1055)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1056) out:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1057) if (cb->confirm_rx)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1058) qrtr_send_resume_tx(cb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1059)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1060) skb_free_datagram(sk, skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1061) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1062)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1063) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1064) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1065)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1066) static int qrtr_connect(struct socket *sock, struct sockaddr *saddr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1067) int len, int flags)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1068) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1069) DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1070) struct qrtr_sock *ipc = qrtr_sk(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1071) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1072) int rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1073)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1074) if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1075) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1076)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1077) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1078)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1079) sk->sk_state = TCP_CLOSE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1080) sock->state = SS_UNCONNECTED;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1081)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1082) rc = qrtr_autobind(sock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1083) if (rc) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1084) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1085) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1086) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1087)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1088) ipc->peer = *addr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1089) sock->state = SS_CONNECTED;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1090) sk->sk_state = TCP_ESTABLISHED;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1091)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1092) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1093)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1094) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1095) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1096)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1097) static int qrtr_getname(struct socket *sock, struct sockaddr *saddr,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1098) int peer)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1099) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1100) struct qrtr_sock *ipc = qrtr_sk(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1101) struct sockaddr_qrtr qaddr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1102) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1103)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1104) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1105) if (peer) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1106) if (sk->sk_state != TCP_ESTABLISHED) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1107) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1108) return -ENOTCONN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1109) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1110)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1111) qaddr = ipc->peer;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1112) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1113) qaddr = ipc->us;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1114) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1115) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1116)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1117) qaddr.sq_family = AF_QIPCRTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1118)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1119) memcpy(saddr, &qaddr, sizeof(qaddr));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1120)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1121) return sizeof(qaddr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1122) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1123)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1124) static int qrtr_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1125) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1126) void __user *argp = (void __user *)arg;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1127) struct qrtr_sock *ipc = qrtr_sk(sock->sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1128) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1129) struct sockaddr_qrtr *sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1130) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1131) struct ifreq ifr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1132) long len = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1133) int rc = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1134)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1135) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1136)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1137) switch (cmd) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1138) case TIOCOUTQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1139) len = sk->sk_sndbuf - sk_wmem_alloc_get(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1140) if (len < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1141) len = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1142) rc = put_user(len, (int __user *)argp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1143) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1144) case TIOCINQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1145) skb = skb_peek(&sk->sk_receive_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1146) if (skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1147) len = skb->len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1148) rc = put_user(len, (int __user *)argp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1149) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1150) case SIOCGIFADDR:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1151) if (copy_from_user(&ifr, argp, sizeof(ifr))) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1152) rc = -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1153) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1154) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1155)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1156) sq = (struct sockaddr_qrtr *)&ifr.ifr_addr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1157) *sq = ipc->us;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1158) if (copy_to_user(argp, &ifr, sizeof(ifr))) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1159) rc = -EFAULT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1160) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1161) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1162) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1163) case SIOCADDRT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1164) case SIOCDELRT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1165) case SIOCSIFADDR:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1166) case SIOCGIFDSTADDR:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1167) case SIOCSIFDSTADDR:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1168) case SIOCGIFBRDADDR:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1169) case SIOCSIFBRDADDR:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1170) case SIOCGIFNETMASK:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1171) case SIOCSIFNETMASK:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1172) rc = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1173) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1174) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1175) rc = -ENOIOCTLCMD;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1176) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1177) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1178)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1179) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1180)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1181) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1182) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1183)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1184) static int qrtr_release(struct socket *sock)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1185) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1186) struct sock *sk = sock->sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1187) struct qrtr_sock *ipc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1188)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1189) if (!sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1190) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1191)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1192) lock_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1193)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1194) ipc = qrtr_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1195) sk->sk_shutdown = SHUTDOWN_MASK;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1196) if (!sock_flag(sk, SOCK_DEAD))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1197) sk->sk_state_change(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1198)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1199) sock_set_flag(sk, SOCK_DEAD);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1200) sock_orphan(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1201) sock->sk = NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1202)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1203) if (!sock_flag(sk, SOCK_ZAPPED))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1204) qrtr_port_remove(ipc);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1205)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1206) skb_queue_purge(&sk->sk_receive_queue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1207)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1208) release_sock(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1209) sock_put(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1210)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1211) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1212) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1213)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1214) static const struct proto_ops qrtr_proto_ops = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1215) .owner = THIS_MODULE,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1216) .family = AF_QIPCRTR,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1217) .bind = qrtr_bind,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1218) .connect = qrtr_connect,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1219) .socketpair = sock_no_socketpair,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1220) .accept = sock_no_accept,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1221) .listen = sock_no_listen,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1222) .sendmsg = qrtr_sendmsg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1223) .recvmsg = qrtr_recvmsg,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1224) .getname = qrtr_getname,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1225) .ioctl = qrtr_ioctl,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1226) .gettstamp = sock_gettstamp,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1227) .poll = datagram_poll,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1228) .shutdown = sock_no_shutdown,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1229) .release = qrtr_release,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1230) .mmap = sock_no_mmap,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1231) .sendpage = sock_no_sendpage,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1232) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1233)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1234) static struct proto qrtr_proto = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1235) .name = "QIPCRTR",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1236) .owner = THIS_MODULE,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1237) .obj_size = sizeof(struct qrtr_sock),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1238) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1239)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1240) static int qrtr_create(struct net *net, struct socket *sock,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1241) int protocol, int kern)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1242) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1243) struct qrtr_sock *ipc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1244) struct sock *sk;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1245)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1246) if (sock->type != SOCK_DGRAM)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1247) return -EPROTOTYPE;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1248)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1249) sk = sk_alloc(net, AF_QIPCRTR, GFP_KERNEL, &qrtr_proto, kern);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1250) if (!sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1251) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1252)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1253) sock_set_flag(sk, SOCK_ZAPPED);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1254)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1255) sock_init_data(sock, sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1256) sock->ops = &qrtr_proto_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1257)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1258) ipc = qrtr_sk(sk);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1259) ipc->us.sq_family = AF_QIPCRTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1260) ipc->us.sq_node = qrtr_local_nid;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1261) ipc->us.sq_port = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1262)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1263) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1264) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1265)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1266) static const struct net_proto_family qrtr_family = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1267) .owner = THIS_MODULE,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1268) .family = AF_QIPCRTR,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1269) .create = qrtr_create,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1270) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1271)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1272) static int __init qrtr_proto_init(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1273) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1274) int rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1275)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1276) rc = proto_register(&qrtr_proto, 1);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1277) if (rc)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1278) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1279)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1280) rc = sock_register(&qrtr_family);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1281) if (rc) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1282) proto_unregister(&qrtr_proto);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1283) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1284) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1285)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1286) qrtr_ns_init();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1287)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1288) return rc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1289) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1290) postcore_initcall(qrtr_proto_init);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1291)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1292) static void __exit qrtr_proto_fini(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1293) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1294) qrtr_ns_remove();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1295) sock_unregister(qrtr_family.family);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1296) proto_unregister(&qrtr_proto);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1297) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1298) module_exit(qrtr_proto_fini);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1299)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1300) MODULE_DESCRIPTION("Qualcomm IPC-router driver");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1301) MODULE_LICENSE("GPL v2");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1302) MODULE_ALIAS_NETPROTO(PF_QIPCRTR);
Orange Pi5 kernel
Deprecated Linux kernel 5.10.110 for OrangePi 5/5B/5+ boards
3 Commits
0 Branches
0 Tags