/*D:120 This is the core of the lguest bus: actually adding a new device.
* It's a separate function because it's neater that way, and because an
* earlier version of the code supported hotplug and unplug. They were removed
* early on because they were never used.
*
* As Andrew Tridgell says, "Untested code is buggy code".
*
* It's worth reading this carefully: we start with a pointer to the new device
* descriptor in the "lguest_devices" page, and the offset into the device
* descriptor page so we can uniquely identify it if things go badly wrong. */
static void add_lguest_device(struct lguest_device_desc *d,
unsigned int offset)
{
struct lguest_device *ldev;
/* Start with zeroed memory; Linux's device layer seems to count on
* it. */
ldev = kzalloc(sizeof(*ldev), GFP_KERNEL);
if (!ldev) {
printk(KERN_EMERG "Cannot allocate lguest dev %u type %u\n",
offset, d->type);
return;
}
/* This devices' parent is the lguest/ dir. */
ldev->vdev.dev.parent = lguest_root;
/* We have a unique device index thanks to the dev_index counter. */
ldev->vdev.id.device = d->type;
/* We have a simple set of routines for querying the device's
* configuration information and setting its status. */
ldev->vdev.config = &lguest_config_ops;
/* And we remember the device's descriptor for lguest_config_ops. */
ldev->desc = d;
/* register_virtio_device() sets up the generic fields for the struct
* virtio_device and calls device_register(). This makes the bus
* infrastructure look for a matching driver. */
if (register_virtio_device(&ldev->vdev) != 0) {
printk(KERN_ERR "Failed to register lguest dev %u type %u\n",
offset, d->type);
kfree(ldev);
}
}
/*
* adds a new device and register it with virtio
* appropriate drivers are loaded by the device model
*/
static int _vop_add_device(struct mic_device_desc __iomem *d,
unsigned int offset, struct vop_device *vpdev,
int dnode)
{
struct _vop_vdev *vdev, *reg_dev = NULL;
int ret;
u8 type = ioread8(&d->type);
vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
if (!vdev)
return -ENOMEM;
vdev->vpdev = vpdev;
vdev->vdev.dev.parent = &vpdev->dev;
vdev->vdev.dev.release = vop_virtio_release_dev;
vdev->vdev.id.device = type;
vdev->vdev.config = &vop_vq_config_ops;
vdev->desc = d;
vdev->dc = (void __iomem *)d + _vop_aligned_desc_size(d);
vdev->dnode = dnode;
vdev->vdev.priv = (void *)(unsigned long)dnode;
init_completion(&vdev->reset_done);
vdev->h2c_vdev_db = vpdev->hw_ops->next_db(vpdev);
vdev->virtio_cookie = vpdev->hw_ops->request_irq(vpdev,
vop_virtio_intr_handler, "virtio intr",
vdev, vdev->h2c_vdev_db);
if (IS_ERR(vdev->virtio_cookie)) {
ret = PTR_ERR(vdev->virtio_cookie);
goto kfree;
}
iowrite8((u8)vdev->h2c_vdev_db, &vdev->dc->h2c_vdev_db);
vdev->c2h_vdev_db = ioread8(&vdev->dc->c2h_vdev_db);
ret = register_virtio_device(&vdev->vdev);
reg_dev = vdev;
if (ret) {
dev_err(_vop_dev(vdev),
"Failed to register vop device %u type %u\n",
offset, type);
goto free_irq;
}
writeq((unsigned long)vdev, &vdev->dc->vdev);
dev_dbg(_vop_dev(vdev), "%s: registered vop device %u type %u vdev %p\n",
__func__, offset, type, vdev);
return 0;
free_irq:
vpdev->hw_ops->free_irq(vpdev, vdev->virtio_cookie, vdev);
kfree:
if (reg_dev)
put_device(&vdev->vdev.dev);
else
kfree(vdev);
return ret;
}
/*
* adds a new device and register it with virtio
* appropriate drivers are loaded by the device model
*/
static int mic_add_device(struct mic_device_desc __iomem *d,
unsigned int offset, struct mic_driver *mdrv)
{
struct mic_vdev *mvdev;
int ret;
int virtio_db;
u8 type = ioread8(&d->type);
mvdev = kzalloc(sizeof(*mvdev), GFP_KERNEL);
if (!mvdev) {
dev_err(mdrv->dev, "Cannot allocate mic dev %u type %u\n",
offset, type);
return -ENOMEM;
}
mvdev->mdev = &mdrv->mdev;
mvdev->vdev.dev.parent = mdrv->dev;
mvdev->vdev.dev.release = mic_virtio_release_dev;
mvdev->vdev.id.device = type;
mvdev->vdev.config = &mic_vq_config_ops;
mvdev->desc = d;
mvdev->dc = (void __iomem *)d + mic_aligned_desc_size(d);
init_completion(&mvdev->reset_done);
virtio_db = mic_next_card_db();
mvdev->virtio_cookie = mic_request_card_irq(mic_virtio_intr_handler,
"virtio intr", mvdev, virtio_db);
if (IS_ERR(mvdev->virtio_cookie)) {
ret = PTR_ERR(mvdev->virtio_cookie);
goto kfree;
}
iowrite8((u8)virtio_db, &mvdev->dc->h2c_vdev_db);
mvdev->c2h_vdev_db = ioread8(&mvdev->dc->c2h_vdev_db);
ret = register_virtio_device(&mvdev->vdev);
if (ret) {
dev_err(mic_dev(mvdev),
"Failed to register mic device %u type %u\n",
offset, type);
goto free_irq;
}
iowrite64((u64)mvdev, &mvdev->dc->vdev);
dev_dbg(mic_dev(mvdev), "%s: registered mic device %u type %u mvdev %p\n",
__func__, offset, type, mvdev);
return 0;
free_irq:
mic_free_card_irq(mvdev->virtio_cookie, mvdev);
kfree:
kfree(mvdev);
return ret;
}
static int __devinit virtio_mmio_probe(struct platform_device *pdev)
{
struct virtio_mmio_device *vm_dev;
struct resource *mem;
unsigned long magic;
mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
if (!mem)
return -EINVAL;
if (!devm_request_mem_region(&pdev->dev, mem->start,
resource_size(mem), pdev->name))
return -EBUSY;
vm_dev = devm_kzalloc(&pdev->dev, sizeof(*vm_dev), GFP_KERNEL);
if (!vm_dev)
return -ENOMEM;
vm_dev->vdev.dev.parent = &pdev->dev;
vm_dev->vdev.config = &virtio_mmio_config_ops;
vm_dev->pdev = pdev;
INIT_LIST_HEAD(&vm_dev->virtqueues);
spin_lock_init(&vm_dev->lock);
vm_dev->base = devm_ioremap(&pdev->dev, mem->start, resource_size(mem));
if (vm_dev->base == NULL)
return -EFAULT;
/* Check magic value */
magic = readl(vm_dev->base + VIRTIO_MMIO_MAGIC_VALUE);
if (memcmp(&magic, "virt", 4) != 0) {
dev_warn(&pdev->dev, "Wrong magic value 0x%08lx!\n", magic);
return -ENODEV;
}
/* Check device version */
vm_dev->version = readl(vm_dev->base + VIRTIO_MMIO_VERSION);
if (vm_dev->version != 1) {
dev_err(&pdev->dev, "Version %ld not supported!\n",
vm_dev->version);
return -ENXIO;
}
vm_dev->vdev.id.device = readl(vm_dev->base + VIRTIO_MMIO_DEVICE_ID);
vm_dev->vdev.id.vendor = readl(vm_dev->base + VIRTIO_MMIO_VENDOR_ID);
writel(PAGE_SIZE, vm_dev->base + VIRTIO_MMIO_GUEST_PAGE_SIZE);
platform_set_drvdata(pdev, vm_dev);
return register_virtio_device(&vm_dev->vdev);
}
/*
* adds a new device and register it with virtio
* appropriate drivers are loaded by the device model
*/
static void add_kvm_device(struct kvm_device_desc *d, unsigned int offset)
{
struct kvm_device *kdev;
kdev = kzalloc(sizeof(*kdev), GFP_KERNEL);
if (!kdev) {
printk(KERN_EMERG "Cannot allocate kvm dev %u type %u\n",
offset, d->type);
return;
}
kdev->vdev.dev.parent = kvm_root;
kdev->vdev.id.device = d->type;
kdev->vdev.config = &kvm_vq_configspace_ops;
kdev->desc = d;
if (register_virtio_device(&kdev->vdev) != 0) {
printk(KERN_ERR "Failed to register kvm device %u type %u\n",
offset, d->type);
kfree(kdev);
}
}
/*
* adds a new device and register it with virtio
* appropriate drivers are loaded by the device model
*/
static void add_kvm_device(struct kvm_device_desc *d, unsigned int offset)
{
struct kvm_device *kdev;
kdev = kzalloc(sizeof(*kdev), GFP_KERNEL);
if (!kdev) {
pr_emerg("Cannot allocate kvm dev %u type %u\n",
offset, d->type);
return;
}
kdev->vdev.dev.parent = kvm_root;
kdev->vdev.id.device = d->type;
kdev->vdev.config = &kvm_vq_config_ops;
kdev->desc = d;
kdev->desc_pa = PFN_PHYS(max_pfn) + offset;
if (register_virtio_device(&kdev->vdev) != 0) {
pr_err("Failed to register kvm device %u type %u\n",
offset, d->type);
kfree(kdev);
}
}
static int zynq_rpmsg_initialize(struct platform_device *pdev)
{
int ret = 0;
int index;
struct virtio_device *virtio_dev;
/* Register ipi handler. */
ret = set_ipi_handler(zynq_rpmsg_p->vring0, ipi_handler,
"Firmware kick");
if (ret) {
dev_err(&pdev->dev, "IPI handler already registered\n");
return -ENODEV;
}
/* Initialize work. */
INIT_WORK(&zynq_rpmsg_work, handle_event);
/* Memory allocations for vrings. */
ret = dma_declare_coherent_memory(&pdev->dev,
zynq_rpmsg_p->mem_start,
zynq_rpmsg_p->mem_start,
zynq_rpmsg_p->mem_end -
zynq_rpmsg_p->mem_start + 1,
DMA_MEMORY_IO);
if (!ret) {
dev_err(&pdev->dev, "dma_declare_coherent_memory failed\n");
return -ENODEV;
}
ret = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
if (ret) {
dev_err(&pdev->dev, "dma_set_coherent_mask: %d\n", ret);
return -ENODEV;
}
/* Initialize a mid-level device. Needed because of bad data structure
* handling and assumptions within the virtio rpmsg bus. We are doing it
* to just make sure that the virtio device has a parent device which
* then itself has a parent in the form of the platform device. */
device_initialize(&(zynq_rpmsg_p->mid_dev));
zynq_rpmsg_p->mid_dev.parent = &(pdev->dev);
zynq_rpmsg_p->mid_dev.type = &mid_level_type;
index = ida_simple_get(&rpmsg_zynq_dev_index, 0, 0, GFP_KERNEL);
if (index < 0) {
put_device(&(zynq_rpmsg_p->mid_dev));
return -ENODEV;
}
dev_set_name(&(zynq_rpmsg_p->mid_dev), "rpmsg_mid%d", index);
device_add(&(zynq_rpmsg_p->mid_dev));
/* Setup the virtio device structure. */
virtio_dev = &(zynq_rpmsg_p->virtio_dev);
virtio_dev->id.device = zynq_rpmsg_p->virtioid;
virtio_dev->config = &zynq_rpmsg_virtio_config_ops;
virtio_dev->dev.parent = &(zynq_rpmsg_p->mid_dev);
virtio_dev->dev.release = zynq_rpmsg_vdev_release;
/* Register the virtio device. */
ret = register_virtio_device(virtio_dev);
dev_info(&(zynq_rpmsg_platform->dev), "virtio device registered \r\n");
return ret;
}
