static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len,
void *priv, u32 src)
{
struct rpmsg_ns_msg *msg = data;
struct rpmsg_channel *newch;
struct rpmsg_channel_info chinfo;
struct virtproc_info *vrp = priv;
struct device *dev = &vrp->vdev->dev;
int ret;
#if 0
print_hex_dump(KERN_DEBUG, __func__, DUMP_PREFIX_NONE, 16, 1,
data, len, true);
#endif
if (len != sizeof(*msg)) {
dev_err(dev, "malformed ns msg (%d)\n", len);
return;
}
/*
* the name service ept does _not_ belong to a real rpmsg channel,
* and is handled by the rpmsg bus itself.
* for sanity reasons, make sure a valid rpdev has _not_ sneaked
* in somehow.
*/
if (rpdev) {
dev_err(dev, "anomaly: ns ept has an rpdev handle\n");
return;
}
/* don't trust the remote processor for null terminating the name */
msg->name[RPMSG_NAME_SIZE - 1] = '\0';
dev_info(dev, "%sing channel %s addr 0x%x\n",
msg->flags & RPMSG_NS_DESTROY ? "destroy" : "creat",
msg->name, msg->addr);
strncpy(chinfo.name, msg->name, sizeof(chinfo.name));
chinfo.src = RPMSG_ADDR_ANY;
chinfo.dst = msg->addr;
if (msg->flags & RPMSG_NS_DESTROY) {
ret = rpmsg_destroy_channel_by_info(vrp, &chinfo);
if (ret)
dev_err(dev, "rpmsg_destroy_channel failed: %d\n", ret);
} else {
newch = rpmsg_create_channel(vrp, &chinfo);
if (!newch)
dev_err(dev, "rpmsg_create_channel failed\n");
}
}
static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len,
void *priv, u32 src)
{
struct rpmsg_ns_msg *msg = data;
struct rpmsg_channel *newch;
struct rpmsg_channel_info chinfo;
struct virtproc_info *vrp = priv;
struct device *dev = &vrp->vdev->dev;
int ret;
print_hex_dump(KERN_DEBUG, "NS announcement: ",
DUMP_PREFIX_NONE, 16, 1,
data, len, true);
if (len != sizeof(*msg)) {
dev_err(dev, "malformed ns msg (%d)\n", len);
return;
}
if (rpdev) {
dev_err(dev, "anomaly: ns ept has an rpdev handle\n");
return;
}
msg->name[RPMSG_NAME_SIZE - 1] = '\0';
dev_info(dev, "%sing channel %s addr 0x%x\n",
msg->flags & RPMSG_NS_DESTROY ? "destroy" : "creat",
msg->name, msg->addr);
strncpy(chinfo.name, msg->name, sizeof(chinfo.name));
chinfo.src = RPMSG_ADDR_ANY;
chinfo.dst = msg->addr;
if (msg->flags & RPMSG_NS_DESTROY) {
ret = rpmsg_destroy_channel(vrp, &chinfo);
if (ret)
dev_err(dev, "rpmsg_destroy_channel failed: %d\n", ret);
} else {
newch = rpmsg_create_channel(vrp, &chinfo);
if (!newch)
dev_err(dev, "rpmsg_create_channel failed\n");
}
}
static int rpmsg_probe(struct virtio_device *vdev)
{
vq_callback_t *vq_cbs[] = { rpmsg_recv_done, rpmsg_xmit_done };
const char *names[] = { "input", "output" };
struct virtqueue *vqs[2];
struct virtproc_info *vrp;
void *addr;
int err, i, num_bufs, buf_size, total_buf_size;
struct rpmsg_channel_info *ch;
vrp = kzalloc(sizeof(*vrp), GFP_KERNEL);
if (!vrp)
return -ENOMEM;
vrp->vdev = vdev;
idr_init(&vrp->endpoints);
spin_lock_init(&vrp->endpoints_lock);
mutex_init(&vrp->svq_lock);
mutex_init(&vrp->rm_lock);
init_waitqueue_head(&vrp->sendq);
/* We expect two virtqueues, rx and tx (in this order) */
err = vdev->config->find_vqs(vdev, 2, vqs, vq_cbs, names);
if (err)
goto free_vi;
vrp->rvq = vqs[0];
vrp->svq = vqs[1];
/* Platform must supply pre-allocated uncached buffers for now */
vdev->config->get(vdev, VPROC_BUF_ADDR, &addr, sizeof(addr));
vdev->config->get(vdev, VPROC_BUF_NUM, &num_bufs,
sizeof(num_bufs));
vdev->config->get(vdev, VPROC_BUF_SZ, &buf_size, sizeof(buf_size));
total_buf_size = num_bufs * buf_size;
dev_dbg(&vdev->dev, "%d buffers, size %d, addr 0x%x, total 0x%x\n",
num_bufs, buf_size, (unsigned int) addr, total_buf_size);
vrp->num_bufs = num_bufs;
vrp->buf_size = buf_size;
vrp->rbufs = addr;
vrp->sbufs = addr + total_buf_size / 2;
/* simulated addr base to make virt_to_page happy */
vdev->config->get(vdev, VPROC_SIM_BASE, &vrp->sim_base,
sizeof(vrp->sim_base));
/* set up the receive buffers */
for (i = 0; i < num_bufs / 2; i++) {
struct scatterlist sg;
void *tmpaddr = vrp->rbufs + i * buf_size;
void *simaddr = vrp->sim_base + i * buf_size;
sg_init_one(&sg, simaddr, buf_size);
err = virtqueue_add_buf_gfp(vrp->rvq, &sg, 0, 1, tmpaddr,
GFP_KERNEL);
WARN_ON(err < 0); /* sanity check; this can't really happen */
}
/* tell the remote processor it can start sending data */
virtqueue_kick(vrp->rvq);
/* suppress "tx-complete" interrupts */
virtqueue_disable_cb(vrp->svq);
vdev->priv = vrp;
dev_info(&vdev->dev, "rpmsg backend virtproc probed successfully\n");
/* if supported by the remote processor, enable the name service */
if (virtio_has_feature(vdev, VIRTIO_RPMSG_F_NS)) {
vrp->ns_ept = __rpmsg_create_ept(vrp, NULL, rpmsg_ns_cb,
vrp, RPMSG_NS_ADDR);
if (!vrp->ns_ept) {
dev_err(&vdev->dev, "failed to create the ns ept\n");
err = -ENOMEM;
goto vqs_del;
}
}
/* look for platform-specific static channels */
vdev->config->get(vdev, VPROC_STATIC_CHANNELS, &ch, sizeof(ch));
for (i = 0; ch && ch[i].name[0]; i++)
rpmsg_create_channel(vrp, &ch[i]);
return 0;
vqs_del:
vdev->config->del_vqs(vrp->vdev);
free_vi:
kfree(vrp);
return err;
}
