github-mirror/linux
1
0
Fork 0
mirror of https://github.com/torvalds/linux.git synced 2026-07-30 11:11:26 +02:00
Code Issues Actions Packages Projects Releases Wiki Activity Graph

Merge "net: qrtr: Update data_len when padding large skbs"

Browse Source
This commit is contained in:
qctecmdr 2022-11-11 17:04:20 -08:00 committed by Gerrit - the friendly Code Review server
commit 04a37de2a2
7 changed files with 441 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
android/abi_gki_aarch64_qcom
View File

@ -1593,6 +1593,7 @@
sk_alloc
skb_clone
skb_copy_bits
skb_copy_datagram_from_iter
skb_copy_datagram_iter
skb_dequeue
skb_free_datagram

333
net/qrtr/af_qrtr.c
View File

@ -14,6 +14,7 @@
#include <linux/rwsem.h>
#include <linux/uidgid.h>
#include <linux/pm_wakeup.h>
#include <linux/of_device.h>
#include <linux/ipc_logging.h>
#include <net/sock.h>
@ -185,6 +186,13 @@ struct qrtr_node {
struct wakeup_source *ws;
void *ilc;
struct xarray no_wake_svc; /* services that will not wake up APPS */
};
struct qrtr_tx_flow_waiter {
struct list_head node;
struct sock *sk;
};
/**
@ -192,11 +200,16 @@ struct qrtr_node {
* @resume_tx: waiters for a resume tx from the remote
* @pending: number of waiting senders
* @tx_failed: indicates that a message with confirm_rx flag was lost
* @waiters: list of ports to notify when this flow resumes
* @lock: lock to protect flow variables
*/
struct qrtr_tx_flow {
struct wait_queue_head resume_tx;
int pending;
int tx_failed;
struct list_head waiters;
/* protect above flow variables */
spinlock_t lock;
};
#define QRTR_TX_FLOW_HIGH 10
@ -204,10 +217,10 @@ struct qrtr_tx_flow {
static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
int type, struct sockaddr_qrtr *from,
struct sockaddr_qrtr *to);
struct sockaddr_qrtr *to, unsigned int flags);
static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
int type, struct sockaddr_qrtr *from,
struct sockaddr_qrtr *to);
struct sockaddr_qrtr *to, unsigned int flags);
static struct qrtr_sock *qrtr_port_lookup(int port);
static void qrtr_port_put(struct qrtr_sock *ipc);
@ -393,6 +406,8 @@ static inline int kref_put_rwsem_lock(struct kref *kref,
static void __qrtr_node_release(struct kref *kref)
{
struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
struct qrtr_tx_flow_waiter *waiter;
struct qrtr_tx_flow_waiter *temp;
struct radix_tree_iter iter;
struct qrtr_tx_flow *flow;
unsigned long flags;
@ -417,11 +432,18 @@ static void __qrtr_node_release(struct kref *kref)
wakeup_source_unregister(node->ws);
/* Free tx flow counters */
mutex_lock(&node->qrtr_tx_lock);
radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
flow = *slot;
list_for_each_entry_safe(waiter, temp, &flow->waiters, node) {
list_del(&waiter->node);
sock_put(waiter->sk);
kfree(waiter);
}
radix_tree_iter_delete(&node->qrtr_tx_flow, &iter, slot);
kfree(flow);
}
mutex_unlock(&node->qrtr_tx_lock);
kfree(node);
}
@ -449,26 +471,48 @@ static void qrtr_node_release(struct qrtr_node *node)
*/
static void qrtr_tx_resume(struct qrtr_node *node, struct sk_buff *skb)
{
struct qrtr_tx_flow_waiter *waiter;
struct qrtr_tx_flow_waiter *temp;
struct qrtr_ctrl_pkt pkt = {0,};
struct qrtr_tx_flow *flow;
struct sockaddr_qrtr src;
struct qrtr_sock *ipc;
struct sk_buff *skbn;
unsigned long flags;
unsigned long key;
u64 remote_node;
u32 remote_port;
skb_copy_bits(skb, 0, &pkt, sizeof(pkt));
remote_node = le32_to_cpu(pkt.client.node);
remote_port = le32_to_cpu(pkt.client.port);
key = remote_node << 32 | remote_port;
if (le32_to_cpu(pkt.cmd) != QRTR_TYPE_RESUME_TX)
return;
rcu_read_lock();
src.sq_family = AF_QIPCRTR;
src.sq_node = le32_to_cpu(pkt.client.node);
src.sq_port = le32_to_cpu(pkt.client.port);
key = (u64)src.sq_node << 32 | src.sq_port;
mutex_lock(&node->qrtr_tx_lock);
flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
rcu_read_unlock();
if (flow) {
spin_lock(&flow->resume_tx.lock);
flow->pending = 0;
spin_unlock(&flow->resume_tx.lock);
wake_up_interruptible_all(&flow->resume_tx);
mutex_unlock(&node->qrtr_tx_lock);
if (!flow)
return;
spin_lock_irqsave(&flow->lock, flags);
flow->pending = 0;
wake_up_interruptible_all(&flow->resume_tx);
list_for_each_entry_safe(waiter, temp, &flow->waiters, node) {
list_del(&waiter->node);
skbn = alloc_skb(0, GFP_ATOMIC);
if (skbn) {
ipc = qrtr_sk(waiter->sk);
qrtr_local_enqueue(NULL, skbn, QRTR_TYPE_RESUME_TX,
&src, &ipc->us, 0);
}
sock_put(waiter->sk);
kfree(waiter);
}
spin_unlock_irqrestore(&flow->lock, flags);
consume_skb(skb);
}
@ -488,24 +532,31 @@ static void qrtr_tx_resume(struct qrtr_node *node, struct sk_buff *skb)
*
* Return: 1 if confirm_rx should be set, 0 otherwise or errno failure
*/
static int qrtr_tx_wait(struct qrtr_node *node, int dest_node, int dest_port,
int type)
static int qrtr_tx_wait(struct qrtr_node *node, struct sockaddr_qrtr *to,
struct sock *sk, int type, unsigned int flags)
{
unsigned long key = (u64)dest_node << 32 | dest_port;
unsigned long key = (u64)to->sq_node << 32 | to->sq_port;
struct qrtr_tx_flow_waiter *waiter;
struct qrtr_tx_flow *flow;
int confirm_rx = 0;
int ret;
long timeo;
long ret;
/* Never set confirm_rx on non-data packets */
if (type != QRTR_TYPE_DATA)
return 0;
/* Assume sk is set correctly for all data type packets */
timeo = sock_sndtimeo(sk, flags & MSG_DONTWAIT);
mutex_lock(&node->qrtr_tx_lock);
flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
if (!flow) {
flow = kzalloc(sizeof(*flow), GFP_KERNEL);
if (flow) {
INIT_LIST_HEAD(&flow->waiters);
init_waitqueue_head(&flow->resume_tx);
spin_lock_init(&flow->lock);
if (radix_tree_insert(&node->qrtr_tx_flow, key, flow)) {
kfree(flow);
flow = NULL;
@ -518,11 +569,13 @@ static int qrtr_tx_wait(struct qrtr_node *node, int dest_node, int dest_port,
if (!flow)
return 1;
spin_lock_irq(&flow->resume_tx.lock);
ret = wait_event_interruptible_locked_irq(flow->resume_tx,
flow->pending < QRTR_TX_FLOW_HIGH ||
flow->tx_failed ||
!node->ep);
spin_lock_irq(&flow->lock);
ret = wait_event_interruptible_lock_irq_timeout(flow->resume_tx,
flow->pending < QRTR_TX_FLOW_HIGH ||
flow->tx_failed ||
!node->ep,
flow->lock,
timeo);
if (ret < 0) {
confirm_rx = ret;
} else if (!node->ep) {
@ -530,11 +583,32 @@ static int qrtr_tx_wait(struct qrtr_node *node, int dest_node, int dest_port,
} else if (flow->tx_failed) {
flow->tx_failed = 0;
confirm_rx = 1;
} else if (!ret && flow->pending >= QRTR_TX_FLOW_HIGH) {
list_for_each_entry(waiter, &flow->waiters, node) {
if (waiter->sk == sk) {
spin_unlock_irq(&flow->lock);
return -EAGAIN;
}
}
waiter = kzalloc(sizeof(*waiter), GFP_ATOMIC);
if (!waiter) {
spin_unlock_irq(&flow->lock);
return -ENOMEM;
}
waiter->sk = sk;
sock_hold(sk);
list_add_tail(&waiter->node, &flow->waiters);
confirm_rx = -EAGAIN;
QRTR_INFO(node->ilc, "new waiter %s[%d] for [0x%x:0x%x]\n",
current->comm, current->pid,
to->sq_node, to->sq_port);
} else {
flow->pending++;
confirm_rx = flow->pending == QRTR_TX_FLOW_LOW;
}
spin_unlock_irq(&flow->resume_tx.lock);
spin_unlock_irq(&flow->lock);
return confirm_rx;
}
@ -558,20 +632,112 @@ static void qrtr_tx_flow_failed(struct qrtr_node *node, int dest_node,
unsigned long key = (u64)dest_node << 32 | dest_port;
struct qrtr_tx_flow *flow;
rcu_read_lock();
mutex_lock(&node->qrtr_tx_lock);
flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
rcu_read_unlock();
mutex_unlock(&node->qrtr_tx_lock);
if (flow) {
spin_lock_irq(&flow->resume_tx.lock);
spin_lock_irq(&flow->lock);
flow->tx_failed = 1;
spin_unlock_irq(&flow->resume_tx.lock);
spin_unlock_irq(&flow->lock);
}
}
static int qrtr_pad_word_pskb(struct sk_buff *skb)
{
unsigned int padding_len;
unsigned int padto;
int nfrags;
int count;
int i;
padto = ALIGN(skb->len, 4);
padding_len = padto - skb->len;
if (!padding_len)
return 0;
count = skb_headlen(skb);
nfrags = skb_shinfo(skb)->nr_frags;
for (i = 0; i < nfrags; i++) {
u32 p_off, p_len, copied;
u32 f_off, f_len;
u32 d_off, d_len;
skb_frag_t *frag;
struct page *p;
u8 *vaddr;
frag = &skb_shinfo(skb)->frags[i];
f_off = skb_frag_off(frag);
f_len = skb_frag_size(frag);
if (count + f_len < skb->len) {
count += f_len;
continue;
}
/* fragment can fit all padding */
if (count + f_len >= padto) {
skb_frag_foreach_page(frag, f_off, f_len, p, p_off,
p_len, copied) {
if (count + p_len < padto) {
count += p_len;
continue;
}
d_off = skb->len - count;
vaddr = kmap_atomic(p);
memset(vaddr + p_off + d_off, 0, padding_len);
kunmap_atomic(vaddr);
count += d_off + padding_len;
skb->len = padto;
skb->data_len += padding_len;
break;
}
} else {
/* messy case, padding split between pages */
skb_frag_foreach_page(frag, f_off, f_len, p, p_off,
p_len, copied) {
if (count + p_len < skb->len) {
count += p_len;
continue;
}
/* need to add padding into next page */
if (count + p_len < padto) {
d_off = skb->len - count;
d_len = p_len - d_off;
vaddr = kmap_atomic(p);
memset(vaddr + p_off + d_off, 0, d_len);
kunmap_atomic(vaddr);
count += p_len;
padding_len -= d_len;
skb->len += d_len;
skb->data_len += padding_len;
continue;
}
d_off = (count < skb->len) ? skb->len - count : 0;
vaddr = kmap_atomic(p);
memset(vaddr + p_off + d_off, 0, padding_len);
kunmap_atomic(vaddr);
count += d_off + padding_len;
skb->len += padding_len;
skb->data_len += padding_len;
}
}
if (skb->len == padto)
break;
}
WARN_ON(skb->len != padto);
return 0;
}
/* Pass an outgoing packet socket buffer to the endpoint driver. */
static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb,
int type, struct sockaddr_qrtr *from,
struct sockaddr_qrtr *to)
struct sockaddr_qrtr *to, unsigned int flags)
{
struct qrtr_hdr_v1 *hdr;
size_t len = skb->len;
@ -592,7 +758,7 @@ static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb,
confirm_rx = cb->confirm_rx;
} else {
confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type);
confirm_rx = qrtr_tx_wait(node, to, skb->sk, type, flags);
if (confirm_rx < 0) {
kfree_skb(skb);
return confirm_rx;
@ -616,10 +782,15 @@ static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb,
hdr->confirm_rx = !!confirm_rx;
qrtr_log_tx_msg(node, hdr, skb);
rc = skb_put_padto(skb, ALIGN(len, 4) + sizeof(*hdr));
/* word align the data and pad with 0s */
if (skb_is_nonlinear(skb))
rc = qrtr_pad_word_pskb(skb);
else
rc = skb_put_padto(skb, ALIGN(len, 4) + sizeof(*hdr));
if (rc)
pr_err("%s: failed to pad size %lu to %lu rc:%d\n", __func__,
len, ALIGN(len, 4) + sizeof(*hdr), rc);
skb->len, ALIGN(skb->len, 4), rc);
if (!rc) {
mutex_lock(&node->ep_lock);
@ -799,6 +970,7 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
unsigned int ver;
size_t hdrlen;
int errcode;
int svc_id;
if (len == 0 || len & 3)
return -EINVAL;
@ -890,6 +1062,7 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
/* All control packets and non-local destined data packets should be
* queued to the worker for forwarding handling.
*/
svc_id = qrtr_get_service_id(cb->src_node, cb->src_port);
if (cb->type != QRTR_TYPE_DATA || cb->dst_node != qrtr_local_nid) {
skb_queue_tail(&node->rx_queue, skb);
kthread_queue_work(&node->kworker, &node->read_data);
@ -906,8 +1079,8 @@ int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
goto err;
}
/* Force wakeup for all packets except for sensors */
if (node->nid != 9)
/* Force wakeup based on services */
if (!xa_load(&node->no_wake_svc, svc_id))
pm_wakeup_ws_event(node->ws, qrtr_wakeup_ms, true);
qrtr_port_put(ipc);
@ -998,7 +1171,7 @@ static void qrtr_fwd_ctrl_pkt(struct qrtr_node *src, struct sk_buff *skb)
to.sq_node = node->nid;
to.sq_port = QRTR_PORT_CTRL;
qrtr_node_enqueue(node, skbn, cb->type, &from, &to);
qrtr_node_enqueue(node, skbn, cb->type, &from, &to, 0);
}
up_read(&qrtr_epts_lock);
}
@ -1015,7 +1188,7 @@ static void qrtr_fwd_pkt(struct sk_buff *skb, struct qrtr_cb *cb)
return;
}
qrtr_node_enqueue(node, skb, cb->type, &from, &to);
qrtr_node_enqueue(node, skb, cb->type, &from, &to, 0);
qrtr_node_release(node);
}
@ -1106,7 +1279,7 @@ static void qrtr_hello_work(struct kthread_work *work)
pkt->cmd = cpu_to_le32(QRTR_TYPE_HELLO);
from.sq_node = qrtr_local_nid;
to.sq_node = node->nid;
qrtr_node_enqueue(node, skb, QRTR_TYPE_HELLO, &from, &to);
qrtr_node_enqueue(node, skb, QRTR_TYPE_HELLO, &from, &to, 0);
qrtr_port_put(ctrl);
}
@ -1115,13 +1288,16 @@ static void qrtr_hello_work(struct kthread_work *work)
* @ep: endpoint to register
* @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
* @rt: flag to notify real time low latency endpoint
* @no_wake: array of services to not wake up
* Return: 0 on success; negative error code on failure
*
* The specified endpoint must have the xmit function pointer set on call.
*/
int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id,
bool rt)
bool rt, struct qrtr_array *no_wake)
{
int rc, i;
size_t size;
struct qrtr_node *node;
struct sched_param param = {.sched_priority = 1};
@ -1151,6 +1327,17 @@ int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id,
if (rt)
sched_setscheduler(node->task, SCHED_FIFO, &param);
xa_init(&node->no_wake_svc);
size = no_wake ? no_wake->size : 0;
for (i = 0; i < size; i++) {
rc = xa_insert(&node->no_wake_svc, no_wake->arr[i], node,
GFP_KERNEL);
if (rc) {
kfree(node);
return rc;
}
}
INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL);
mutex_init(&node->qrtr_tx_lock);
@ -1219,7 +1406,7 @@ static void qrtr_fwd_del_proc(struct qrtr_node *src, unsigned int nid)
from.sq_node = src->nid;
to.sq_node = dst->nid;
qrtr_node_enqueue(dst, skb, QRTR_TYPE_DEL_PROC, &from, &to);
qrtr_node_enqueue(dst, skb, QRTR_TYPE_DEL_PROC, &from, &to, 0);
}
}
@ -1252,7 +1439,7 @@ void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
skb = qrtr_alloc_ctrl_packet(&pkt, GFP_ATOMIC);
if (skb) {
pkt->cmd = cpu_to_le32(QRTR_TYPE_BYE);
qrtr_local_enqueue(NULL, skb, QRTR_TYPE_BYE, &src, &dst);
qrtr_local_enqueue(NULL, skb, QRTR_TYPE_BYE, &src, &dst, 0);
}
spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
@ -1325,7 +1512,7 @@ static void qrtr_send_del_client(struct qrtr_sock *ipc)
skb_set_owner_w(skb, &ipc->sk);
if (ipc->state == QRTR_STATE_MULTI) {
qrtr_bcast_enqueue(NULL, skb, type, &ipc->us, &to);
qrtr_bcast_enqueue(NULL, skb, type, &ipc->us, &to, 0);
return;
}
@ -1341,11 +1528,11 @@ static void qrtr_send_del_client(struct qrtr_sock *ipc)
}
skb_set_owner_w(skbn, &ipc->sk);
qrtr_node_enqueue(node, skbn, type, &ipc->us, &to);
qrtr_node_enqueue(node, skbn, type, &ipc->us, &to, 0);
qrtr_node_release(node);
}
exit:
qrtr_local_enqueue(NULL, skb, type, &ipc->us, &to);
qrtr_local_enqueue(NULL, skb, type, &ipc->us, &to, 0);
}
/* Remove port assignment. */
@ -1383,7 +1570,7 @@ static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
if (!*port) {
rc = xa_alloc_cyclic(&qrtr_ports, port, ipc,
QRTR_EPH_PORT_RANGE, &qrtr_ports_next,
GFP_KERNEL);
GFP_ATOMIC);
} else if (*port < QRTR_MIN_EPH_SOCKET &&
!(capable(CAP_NET_ADMIN) ||
in_egroup_p(AID_VENDOR_QRTR) ||
@ -1511,7 +1698,7 @@ static int qrtr_bind(struct socket *sock, struct sockaddr *saddr, int len)
/* Queue packet to local peer socket. */
static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
int type, struct sockaddr_qrtr *from,
struct sockaddr_qrtr *to)
struct sockaddr_qrtr *to, unsigned int flags)
{
struct qrtr_sock *ipc;
struct qrtr_cb *cb;
@ -1556,7 +1743,7 @@ static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
/* Queue packet for broadcast. */
static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
int type, struct sockaddr_qrtr *from,
struct sockaddr_qrtr *to)
struct sockaddr_qrtr *to, unsigned int flags)
{
struct sk_buff *skbn;
@ -1569,11 +1756,11 @@ static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
if (!skbn)
break;
skb_set_owner_w(skbn, skb->sk);
qrtr_node_enqueue(node, skbn, type, from, to);
qrtr_node_enqueue(node, skbn, type, from, to, flags);
}
up_read(&qrtr_epts_lock);
qrtr_local_enqueue(NULL, skb, type, from, to);
qrtr_local_enqueue(NULL, skb, type, from, to, flags);
return 0;
}
@ -1582,7 +1769,8 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
{
DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *, int,
struct sockaddr_qrtr *, struct sockaddr_qrtr *);
struct sockaddr_qrtr *, struct sockaddr_qrtr *,
unsigned int);
__le32 qrtr_type = cpu_to_le32(QRTR_TYPE_DATA);
struct qrtr_sock *ipc = qrtr_sk(sock->sk);
struct sock *sk = sock->sk;
@ -1590,6 +1778,8 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
struct qrtr_node *node;
struct qrtr_node *srv_node;
struct sk_buff *skb;
int pdata_len = 0;
int data_len = 0;
size_t plen;
u32 type;
int rc;
@ -1651,8 +1841,17 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
}
plen = (len + 3) & ~3;
skb = sock_alloc_send_skb(sk, plen + QRTR_HDR_MAX_SIZE,
msg->msg_flags & MSG_DONTWAIT, &rc);
if (plen > SKB_MAX_ALLOC) {
data_len = min_t(size_t,
plen - SKB_MAX_ALLOC,
MAX_SKB_FRAGS * PAGE_SIZE);
pdata_len = PAGE_ALIGN(data_len);
BUILD_BUG_ON(SKB_MAX_ALLOC < PAGE_SIZE);
}
skb = sock_alloc_send_pskb(sk, QRTR_HDR_MAX_SIZE + (plen - data_len),
pdata_len, msg->msg_flags & MSG_DONTWAIT,
&rc, PAGE_ALLOC_COSTLY_ORDER);
if (!skb) {
rc = -ENOMEM;
goto out_node;
@ -1660,7 +1859,13 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
skb_reserve(skb, QRTR_HDR_MAX_SIZE);
rc = memcpy_from_msg(skb_put(skb, len), msg, len);
/* len is used by the enqueue functions and should remain accurate
* regardless of padding or allocation size
*/
skb_put(skb, len - data_len);
skb->data_len = data_len;
skb->len = len;
rc = skb_copy_datagram_from_iter(skb, 0, &msg->msg_iter, len);
if (rc) {
kfree_skb(skb);
goto out_node;
@ -1694,7 +1899,7 @@ static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
qrtr_node_release(srv_node);
}
rc = enqueue_fn(node, skb, type, &ipc->us, addr);
rc = enqueue_fn(node, skb, type, &ipc->us, addr, msg->msg_flags);
if (rc >= 0)
rc = len;
@ -1729,7 +1934,7 @@ static int qrtr_send_resume_tx(struct qrtr_cb *cb)
pkt->client.node = cpu_to_le32(cb->dst_node);
pkt->client.port = cpu_to_le32(cb->dst_port);
ret = qrtr_node_enqueue(node, skb, QRTR_TYPE_RESUME_TX, &local, &remote);
ret = qrtr_node_enqueue(node, skb, QRTR_TYPE_RESUME_TX, &local, &remote, 0);
qrtr_node_release(node);
@ -1996,10 +2201,30 @@ static const struct net_proto_family qrtr_family = {
.create = qrtr_create,
};
static void qrtr_update_node_id(void)
{
const char *compat = "qcom,qrtr";
struct device_node *np = NULL;
u32 node_id;
int ret;
while ((np = of_find_compatible_node(np, NULL, compat))) {
ret = of_property_read_u32(np, "qcom,node-id", &node_id);
of_node_put(np);
if (ret)
continue;
qrtr_local_nid = node_id;
break;
}
}
static int __init qrtr_proto_init(void)
{
int rc;
qrtr_update_node_id();
rc = proto_register(&qrtr_proto, 1);
if (rc)
return rc;

129
net/qrtr/gunyah.c
View File

@ -219,6 +219,60 @@ static void gunyah_tx_write(struct gunyah_pipe *pipe, const void *data,
*pipe->head = cpu_to_le32(head);
}
static size_t gunyah_sg_copy_toio(struct scatterlist *sg, unsigned int nents,
void *buf, size_t buflen, off_t skip)
{
unsigned int sg_flags = SG_MITER_ATOMIC | SG_MITER_FROM_SG;
struct sg_mapping_iter miter;
unsigned int offset = 0;
sg_miter_start(&miter, sg, nents, sg_flags);
if (!sg_miter_skip(&miter, skip))
return 0;
while ((offset < buflen) && sg_miter_next(&miter)) {
unsigned int len;
len = min(miter.length, buflen - offset);
memcpy_toio(buf + offset, miter.addr, len);
offset += len;
}
sg_miter_stop(&miter);
return offset;
}
static void gunyah_sg_write(struct gunyah_pipe *pipe, struct scatterlist *sg,
int offset, size_t count)
{
size_t len;
u32 head;
int rc = 0;
head = le32_to_cpu(*pipe->head);
len = min_t(size_t, count, pipe->length - head);
if (len) {
rc = gunyah_sg_copy_toio(sg, sg_nents(sg), pipe->fifo + head,
len, offset);
offset += rc;
}
if (len != count)
rc = gunyah_sg_copy_toio(sg, sg_nents(sg), pipe->fifo,
count - len, offset);
head += count;
if (head >= pipe->length)
head -= pipe->length;
smp_wmb();
*pipe->head = cpu_to_le32(head);
}
static void gunyah_set_tx_notify(struct qrtr_gunyah_dev *qdev)
{
*qdev->tx_pipe.read_notify = cpu_to_le32(1);
@ -249,17 +303,10 @@ static int qrtr_gunyah_send(struct qrtr_endpoint *ep, struct sk_buff *skb)
int chunk_size;
int left_size;
int offset;
int rc;
qdev = container_of(ep, struct qrtr_gunyah_dev, ep);
rc = skb_linearize(skb);
if (rc) {
kfree_skb(skb);
return rc;
}
left_size = skb->len;
offset = 0;
while (left_size > 0) {
@ -273,7 +320,22 @@ static int qrtr_gunyah_send(struct qrtr_endpoint *ep, struct sk_buff *skb)
else
chunk_size = left_size;
gunyah_tx_write(&qdev->tx_pipe, skb->data + offset, chunk_size);
if (skb_is_nonlinear(skb)) {
struct scatterlist sg[MAX_SKB_FRAGS + 1];
sg_init_table(sg, skb_shinfo(skb)->nr_frags + 1);
rc = skb_to_sgvec(skb, sg, 0, skb->len);
if (rc < 0) {
pr_err("failed skb_to_sgvec rc:%d\n", rc);
break;
}
gunyah_sg_write(&qdev->tx_pipe, sg, offset,
chunk_size);
} else {
gunyah_tx_write(&qdev->tx_pipe, skb->data + offset,
chunk_size);
}
offset += chunk_size;
left_size -= chunk_size;
@ -454,7 +516,8 @@ static int qrtr_gunyah_rm_cb(struct notifier_block *nb, unsigned long cmd,
if (vm_status_payload->vm_status == GH_RM_VM_STATUS_READY) {
qrtr_gunyah_fifo_init(qdev);
if (qrtr_endpoint_register(&qdev->ep, QRTR_EP_NET_ID_AUTO, false)) {
if (qrtr_endpoint_register(&qdev->ep, QRTR_EP_NET_ID_AUTO,
false, NULL)) {
pr_err("%s: endpoint register failed\n", __func__);
return NOTIFY_DONE;
}
@ -551,6 +614,24 @@ static struct device_node *qrtr_gunyah_svm_of_parse(struct qrtr_gunyah_dev *qdev
return shm_np;
}
static int qrtr_gunyah_alloc_fifo(struct qrtr_gunyah_dev *qdev)
{
struct device *dev = qdev->dev;
resource_size_t size;
size = FIFO_1_START + FIFO_SIZE;
qdev->base = dma_alloc_attrs(dev, size, &qdev->res.start, GFP_KERNEL,
DMA_ATTR_FORCE_CONTIGUOUS);
if (!qdev->base)
return -ENOMEM;
qdev->res.end = qdev->res.start + size - 1;
qdev->size = size;
return 0;
}
static int qrtr_gunyah_map_memory(struct qrtr_gunyah_dev *qdev)
{
struct device *dev = qdev->dev;
@ -558,8 +639,11 @@ static int qrtr_gunyah_map_memory(struct qrtr_gunyah_dev *qdev)
resource_size_t size;
int ret;
np = of_parse_phandle(dev->of_node, "shared-buffer", 0);
if (!np) {
if (qdev->master) {
np = of_parse_phandle(dev->of_node, "shared-buffer", 0);
if (!np)
return qrtr_gunyah_alloc_fifo(qdev);
} else {
np = qrtr_gunyah_svm_of_parse(qdev);
if (!np) {
dev_err(dev, "can't parse shared mem node!\n");
@ -649,7 +733,7 @@ static int qrtr_gunyah_probe(struct platform_device *pdev)
qdev->ep.xmit = qrtr_gunyah_send;
if (!qdev->master) {
ret = qrtr_endpoint_register(&qdev->ep, QRTR_EP_NET_ID_AUTO,
false);
false, NULL);
if (ret)
goto register_fail;
}
@ -678,11 +762,32 @@ static int qrtr_gunyah_probe(struct platform_device *pdev)
static int qrtr_gunyah_remove(struct platform_device *pdev)
{
struct qrtr_gunyah_dev *qdev = dev_get_drvdata(&pdev->dev);
struct device_node *np;
gh_vmid_t peer_vmid;
gh_vmid_t self_vmid;
cancel_work_sync(&qdev->work);
gh_dbl_tx_unregister(qdev->tx_dbl);
gh_dbl_rx_unregister(qdev->rx_dbl);
if (!qdev->master)
return 0;
if (gh_rm_get_vmid(qdev->peer_name, &peer_vmid))
return 0;
if (gh_rm_get_vmid(GH_PRIMARY_VM, &self_vmid))
return 0;
qrtr_gunyah_unshare_mem(qdev, self_vmid, peer_vmid);
np = of_parse_phandle(qdev->dev->of_node, "shared-buffer", 0);
if (np) {
of_node_put(np);
return 0;
}
dma_free_attrs(qdev->dev, qdev->size, qdev->base, qdev->res.start,
DMA_ATTR_FORCE_CONTIGUOUS);
return 0;
}

2
net/qrtr/mhi.c
View File

@ -130,7 +130,7 @@ static int qcom_mhi_qrtr_probe(struct mhi_device *mhi_dev,
qrtr_mhi_of_parse(mhi_dev, &net_id, &rt);
rc = qrtr_endpoint_register(&qdev->ep, net_id, rt);
rc = qrtr_endpoint_register(&qdev->ep, net_id, rt, NULL);
if (rc)
return rc;

28
net/qrtr/ns.c
View File

@ -105,7 +105,7 @@ int qrtr_get_service_id(unsigned int node_id, unsigned int port_id)
struct qrtr_node *node;
unsigned long index;
node = node_get(node_id);
node = xa_load(&nodes, node_id);
if (!node)
return -EINVAL;
@ -228,24 +228,26 @@ static int announce_servers(struct sockaddr_qrtr *sq)
struct qrtr_server *srv;
struct qrtr_node *node;
unsigned long index;
unsigned long node_idx;
int ret;
node = node_get(qrtr_ns.local_node);
if (!node)
return 0;
/* Announce the list of servers registered in this node */
xa_for_each(&node->servers, index, srv) {
ret = service_announce_new(sq, srv);
if (ret < 0) {
if (ret == -ENODEV)
continue;
xa_for_each(&nodes, node_idx, node) {
if (node->id == sq->sq_node) {
pr_info("Avoiding duplicate announce for NODE ID %u\n", node->id);
continue;
}
xa_for_each(&node->servers, index, srv) {
ret = service_announce_new(sq, srv);
if (ret < 0) {
if (ret == -ENODEV)
continue;
pr_err("failed to announce new service %d\n", ret);
return ret;
pr_err("failed to announce new service %d\n", ret);
return ret;
}
}
}
return 0;
}

14
net/qrtr/qrtr.h
View File

@ -26,8 +26,20 @@ struct qrtr_endpoint {
struct qrtr_node *node;
};
/**
* struct qrtr_array - array with size
* @arr: elements in the array
* @size: number of elements
*
* An array with its size provided.
*/
struct qrtr_array {
u32 *arr;
size_t size;
};
int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int net_id,
bool rt);
bool rt, struct qrtr_array *no_wake);
void qrtr_endpoint_unregister(struct qrtr_endpoint *ep);

16
net/qrtr/smd.c
View File

@ -66,8 +66,10 @@ static int qcom_smd_qrtr_send(struct qrtr_endpoint *ep, struct sk_buff *skb)
static int qcom_smd_qrtr_probe(struct rpmsg_device *rpdev)
{
struct qrtr_array svc_arr = {NULL, 0};
struct qrtr_smd_dev *qdev;
u32 net_id;
int size;
bool rt;
int rc;
@ -85,7 +87,19 @@ static int qcom_smd_qrtr_probe(struct rpmsg_device *rpdev)
rt = of_property_read_bool(rpdev->dev.of_node, "qcom,low-latency");
rc = qrtr_endpoint_register(&qdev->ep, net_id, rt);
size = of_property_count_u32_elems(rpdev->dev.of_node, "qcom,no-wake-svc");
if (size > 0) {
svc_arr.size = size;
svc_arr.arr = kmalloc_array(size, sizeof(u32), GFP_KERNEL);
if (!svc_arr.arr)
return -ENOMEM;
of_property_read_u32_array(rpdev->dev.of_node, "qcom,no-wake-svc",
svc_arr.arr, size);
}
rc = qrtr_endpoint_register(&qdev->ep, net_id, rt, &svc_arr);
kfree(svc_arr.arr);
if (rc) {
dev_err(qdev->dev, "endpoint register failed: %d, low-latency: %d\n", rc, rt);
return rc;