/*
* Allocate and initialize serio structure for subsequent registration
* with serio core.
*/
static int rpckbd_probe(struct platform_device *dev)
{
struct rpckbd_data *rpckbd;
struct serio *serio;
int tx_irq, rx_irq;
rx_irq = platform_get_irq(dev, 0);
if (rx_irq <= 0)
return rx_irq < 0 ? rx_irq : -ENXIO;
tx_irq = platform_get_irq(dev, 1);
if (tx_irq <= 0)
return tx_irq < 0 ? tx_irq : -ENXIO;
serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
rpckbd = kzalloc(sizeof(*rpckbd), GFP_KERNEL);
if (!serio || !rpckbd) {
kfree(rpckbd);
kfree(serio);
return -ENOMEM;
}
rpckbd->rx_irq = rx_irq;
rpckbd->tx_irq = tx_irq;
serio->id.type = SERIO_8042;
serio->write = rpckbd_write;
serio->open = rpckbd_open;
serio->close = rpckbd_close;
serio->dev.parent = &dev->dev;
serio->port_data = rpckbd;
strlcpy(serio->name, "RiscPC PS/2 kbd port", sizeof(serio->name));
strlcpy(serio->phys, "rpckbd/serio0", sizeof(serio->phys));
platform_set_drvdata(dev, serio);
serio_register_port(serio);
return 0;
}
static int __init q40kbd_init(void)
{
int maxread = 100;
if (!MACH_IS_Q40)
return -EIO;
/* allocate the IRQ */
request_irq(Q40_IRQ_KEYBOARD, q40kbd_interrupt, 0, "q40kbd", NULL);
/* flush any pending input */
while (maxread-- && (Q40_IRQ_KEYB_MASK & master_inb(INTERRUPT_REG)))
master_inb(KEYCODE_REG);
/* off we go */
master_outb(-1,KEYBOARD_UNLOCK_REG);
master_outb(1,KEY_IRQ_ENABLE_REG);
serio_register_port(&q40kbd_port);
printk(KERN_INFO "serio: Q40 kbd registered\n");
return 0;
}
static int __devinit ct82c710_probe(struct platform_device *dev)
{
ct82c710_port = kzalloc(sizeof(struct serio), GFP_KERNEL);
if (!ct82c710_port)
return -ENOMEM;
ct82c710_port->id.type = SERIO_8042;
ct82c710_port->dev.parent = &dev->dev;
ct82c710_port->open = ct82c710_open;
ct82c710_port->close = ct82c710_close;
ct82c710_port->write = ct82c710_write;
strlcpy(ct82c710_port->name, "C&T 82c710 mouse port",
sizeof(ct82c710_port->name));
snprintf(ct82c710_port->phys, sizeof(ct82c710_port->phys),
"isa%16llx/serio0", (unsigned long long)CT82C710_DATA);
serio_register_port(ct82c710_port);
// printk(KERN_INFO "serio: C&T 82c710 mouse port at %#llx irq %d\n",
;
return 0;
}
static int l4bus_probe(const char *name)
{
struct l4bus_port *port;
struct serio *io;
int ret;
port = kmalloc(sizeof(struct l4bus_port), GFP_KERNEL);
io = kmalloc(sizeof(struct serio), GFP_KERNEL);
if (!port || !io) {
ret = -ENOMEM;
goto out;
}
memset(port, 0, sizeof(struct l4bus_port));
memset(io, 0, sizeof(struct serio));
io->type = SERIO_L4;
io->write = l4bus_write;
//io->read = 0;
io->open = l4bus_open;
io->close = l4bus_close;
strlcpy(io->name, name, sizeof(io->name));
strlcpy(io->phys, name, sizeof(io->phys));
io->port_data = port;
//io->dev.parent = &dev->dev;
port->io = io;
port->irq = -1;
serio_register_port(io);
return 0;
out:
kfree(io);
kfree(port);
return ret;
}
int __init nvec_ps2(struct nvec_chip *nvec)
{
struct serio *ser_dev = kzalloc(sizeof(struct serio), GFP_KERNEL);
ser_dev->id.type=SERIO_8042;
ser_dev->write=ps2_sendcommand;
ser_dev->open=ps2_startstreaming;
ser_dev->close=ps2_stopstreaming;
strlcpy(ser_dev->name, "NVEC PS2", sizeof(ser_dev->name));
strlcpy(ser_dev->phys, "NVEC I2C slave", sizeof(ser_dev->phys));
ps2_dev.ser_dev = ser_dev;
ps2_dev.notifier.notifier_call = nvec_ps2_notifier;
ps2_dev.nvec = nvec;
nvec_register_notifier(nvec, &ps2_dev.notifier, 0);
serio_register_port(ser_dev);
/* mouse reset */
nvec_write_async(nvec, "\x06\x01\xff\x03", 4);
return 0;
}
static int __init psif_probe(struct platform_device *pdev)
{
struct resource *regs;
struct psif *psif;
struct serio *io;
struct clk *pclk;
int irq;
int ret;
psif = kzalloc(sizeof(struct psif), GFP_KERNEL);
if (!psif) {
dev_dbg(&pdev->dev, "out of memory\n");
ret = -ENOMEM;
goto out;
}
psif->pdev = pdev;
io = kzalloc(sizeof(struct serio), GFP_KERNEL);
if (!io) {
dev_dbg(&pdev->dev, "out of memory\n");
ret = -ENOMEM;
goto out_free_psif;
}
psif->io = io;
regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
if (!regs) {
dev_dbg(&pdev->dev, "no mmio resources defined\n");
ret = -ENOMEM;
goto out_free_io;
}
psif->regs = ioremap(regs->start, resource_size(regs));
if (!psif->regs) {
ret = -ENOMEM;
dev_dbg(&pdev->dev, "could not map I/O memory\n");
goto out_free_io;
}
pclk = clk_get(&pdev->dev, "pclk");
if (IS_ERR(pclk)) {
dev_dbg(&pdev->dev, "could not get peripheral clock\n");
ret = PTR_ERR(pclk);
goto out_iounmap;
}
psif->pclk = pclk;
/* Reset the PSIF to enter at a known state. */
ret = clk_enable(pclk);
if (ret) {
dev_dbg(&pdev->dev, "could not enable pclk\n");
goto out_put_clk;
}
psif_writel(psif, CR, PSIF_BIT(CR_SWRST));
clk_disable(pclk);
irq = platform_get_irq(pdev, 0);
if (irq < 0) {
dev_dbg(&pdev->dev, "could not get irq\n");
ret = -ENXIO;
goto out_put_clk;
}
ret = request_irq(irq, psif_interrupt, IRQF_SHARED, "at32psif", psif);
if (ret) {
dev_dbg(&pdev->dev, "could not request irq %d\n", irq);
goto out_put_clk;
}
psif->irq = irq;
io->id.type = SERIO_8042;
io->write = psif_write;
io->open = psif_open;
io->close = psif_close;
snprintf(io->name, sizeof(io->name), "AVR32 PS/2 port%d", pdev->id);
snprintf(io->phys, sizeof(io->phys), "at32psif/serio%d", pdev->id);
io->port_data = psif;
io->dev.parent = &pdev->dev;
psif_set_prescaler(psif);
spin_lock_init(&psif->lock);
serio_register_port(psif->io);
platform_set_drvdata(pdev, psif);
dev_info(&pdev->dev, "Atmel AVR32 PSIF PS/2 driver on 0x%08x irq %d\n",
(int)psif->regs, psif->irq);
return 0;
out_put_clk:
clk_put(psif->pclk);
out_iounmap:
iounmap(psif->regs);
out_free_io:
kfree(io);
out_free_psif:
kfree(psif);
out:
return ret;
}
static int __devinit xps2_of_probe(struct platform_device *ofdev)
{
struct resource r_irq;
struct resource r_mem;
struct xps2data *drvdata;
struct serio *serio;
struct device *dev = &ofdev->dev;
resource_size_t remap_size, phys_addr;
int error;
dev_info(dev, "Device Tree Probing \'%s\'\n",
ofdev->dev.of_node->name);
error = of_address_to_resource(ofdev->dev.of_node, 0, &r_mem);
if (error) {
dev_err(dev, "invalid address\n");
return error;
}
if (!of_irq_to_resource(ofdev->dev.of_node, 0, &r_irq)) {
dev_err(dev, "no IRQ found\n");
return -ENODEV;
}
drvdata = kzalloc(sizeof(struct xps2data), GFP_KERNEL);
if (!drvdata) {
dev_err(dev, "Couldn't allocate device private record\n");
return -ENOMEM;
}
dev_set_drvdata(dev, drvdata);
spin_lock_init(&drvdata->lock);
drvdata->irq = r_irq.start;
phys_addr = r_mem.start;
remap_size = resource_size(&r_mem);
if (!request_mem_region(phys_addr, remap_size, DRIVER_NAME)) {
dev_err(dev, "Couldn't lock memory region at 0x%08llX\n",
(unsigned long long)phys_addr);
error = -EBUSY;
goto failed1;
}
drvdata->base_address = ioremap(phys_addr, remap_size);
if (drvdata->base_address == NULL) {
dev_err(dev, "Couldn't ioremap memory at 0x%08llX\n",
(unsigned long long)phys_addr);
error = -EFAULT;
goto failed2;
}
out_be32(drvdata->base_address + XPS2_IPIER_OFFSET, 0);
out_be32(drvdata->base_address + XPS2_SRST_OFFSET, XPS2_SRST_RESET);
dev_info(dev, "Xilinx PS2 at 0x%08llX mapped to 0x%p, irq=%d\n",
(unsigned long long)phys_addr, drvdata->base_address,
drvdata->irq);
serio = &drvdata->serio;
serio->id.type = SERIO_8042;
serio->write = sxps2_write;
serio->open = sxps2_open;
serio->close = sxps2_close;
serio->port_data = drvdata;
serio->dev.parent = dev;
snprintf(serio->name, sizeof(serio->name),
"Xilinx XPS PS/2 at %08llX", (unsigned long long)phys_addr);
snprintf(serio->phys, sizeof(serio->phys),
"xilinxps2/serio at %08llX", (unsigned long long)phys_addr);
serio_register_port(serio);
return 0;
failed2:
release_mem_region(phys_addr, remap_size);
failed1:
kfree(drvdata);
dev_set_drvdata(dev, NULL);
return error;
}
/*
* Add one device to this driver.
*/
static int __devinit ps2_probe(struct sa1111_dev *dev)
{
struct ps2if *ps2if;
struct serio *serio;
int ret;
ps2if = kzalloc(sizeof(struct ps2if), GFP_KERNEL);
serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
if (!ps2if || !serio) {
ret = -ENOMEM;
goto free;
}
serio->id.type = SERIO_8042;
serio->write = ps2_write;
serio->open = ps2_open;
serio->close = ps2_close;
strlcpy(serio->name, dev_name(&dev->dev), sizeof(serio->name));
strlcpy(serio->phys, dev_name(&dev->dev), sizeof(serio->phys));
serio->port_data = ps2if;
serio->dev.parent = &dev->dev;
ps2if->io = serio;
ps2if->dev = dev;
sa1111_set_drvdata(dev, ps2if);
spin_lock_init(&ps2if->lock);
/*
* Request the physical region for this PS2 port.
*/
if (!request_mem_region(dev->res.start,
dev->res.end - dev->res.start + 1,
SA1111_DRIVER_NAME(dev))) {
ret = -EBUSY;
goto free;
}
/*
* Our parent device has already mapped the region.
*/
ps2if->base = dev->mapbase;
sa1111_enable_device(ps2if->dev);
/* Incoming clock is 8MHz */
sa1111_writel(0, ps2if->base + SA1111_PS2CLKDIV);
sa1111_writel(127, ps2if->base + SA1111_PS2PRECNT);
/*
* Flush any pending input.
*/
ps2_clear_input(ps2if);
/*
* Test the keyboard interface.
*/
ret = ps2_test(ps2if);
if (ret)
goto out;
/*
* Flush any pending input.
*/
ps2_clear_input(ps2if);
sa1111_disable_device(ps2if->dev);
serio_register_port(ps2if->io);
return 0;
out:
sa1111_disable_device(ps2if->dev);
release_mem_region(dev->res.start, resource_size(&dev->res));
free:
sa1111_set_drvdata(dev, NULL);
kfree(ps2if);
kfree(serio);
return ret;
}
/**
* xps2_of_probe - probe method for the PS/2 device.
* @of_dev: pointer to OF device structure
* @match: pointer to the structure used for matching a device
*
* This function probes the PS/2 device in the device tree.
* It initializes the driver data structure and the hardware.
* It returns 0, if the driver is bound to the PS/2 device, or a negative
* value if there is an error.
*/
static int xps2_of_probe(struct platform_device *ofdev)
{
struct resource r_mem; /* IO mem resources */
struct xps2data *drvdata;
struct serio *serio;
struct device *dev = &ofdev->dev;
resource_size_t remap_size, phys_addr;
unsigned int irq;
int error;
dev_info(dev, "Device Tree Probing \'%s\'\n",
ofdev->dev.of_node->name);
/* Get iospace for the device */
error = of_address_to_resource(ofdev->dev.of_node, 0, &r_mem);
if (error) {
dev_err(dev, "invalid address\n");
return error;
}
/* Get IRQ for the device */
irq = irq_of_parse_and_map(ofdev->dev.of_node, 0);
if (!irq) {
dev_err(dev, "no IRQ found\n");
return -ENODEV;
}
drvdata = kzalloc(sizeof(struct xps2data), GFP_KERNEL);
serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
if (!drvdata || !serio) {
error = -ENOMEM;
goto failed1;
}
spin_lock_init(&drvdata->lock);
drvdata->irq = irq;
drvdata->serio = serio;
drvdata->dev = dev;
phys_addr = r_mem.start;
remap_size = resource_size(&r_mem);
if (!request_mem_region(phys_addr, remap_size, DRIVER_NAME)) {
dev_err(dev, "Couldn't lock memory region at 0x%08llX\n",
(unsigned long long)phys_addr);
error = -EBUSY;
goto failed1;
}
/* Fill in configuration data and add them to the list */
drvdata->base_address = ioremap(phys_addr, remap_size);
if (drvdata->base_address == NULL) {
dev_err(dev, "Couldn't ioremap memory at 0x%08llX\n",
(unsigned long long)phys_addr);
error = -EFAULT;
goto failed2;
}
/* Disable all the interrupts, just in case */
out_be32(drvdata->base_address + XPS2_IPIER_OFFSET, 0);
/* Reset the PS2 device and abort any current transaction, to make sure
* we have the PS2 in a good state */
out_be32(drvdata->base_address + XPS2_SRST_OFFSET, XPS2_SRST_RESET);
dev_info(dev, "Xilinx PS2 at 0x%08llX mapped to 0x%p, irq=%d\n",
(unsigned long long)phys_addr, drvdata->base_address,
drvdata->irq);
serio->id.type = SERIO_8042;
serio->write = sxps2_write;
serio->open = sxps2_open;
serio->close = sxps2_close;
serio->port_data = drvdata;
serio->dev.parent = dev;
snprintf(serio->name, sizeof(serio->name),
"Xilinx XPS PS/2 at %08llX", (unsigned long long)phys_addr);
snprintf(serio->phys, sizeof(serio->phys),
"xilinxps2/serio at %08llX", (unsigned long long)phys_addr);
serio_register_port(serio);
platform_set_drvdata(ofdev, drvdata);
return 0; /* success */
failed2:
release_mem_region(phys_addr, remap_size);
failed1:
kfree(serio);
kfree(drvdata);
return error;
}
/*
* Add one device to this driver.
*/
static int __devinit altera_ps2_probe(struct platform_device *pdev)
{
struct ps2if *ps2if;
struct serio *serio;
int error, irq;
ps2if = kzalloc(sizeof(struct ps2if), GFP_KERNEL);
serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
if (!ps2if || !serio) {
error = -ENOMEM;
goto err_free_mem;
}
serio->id.type = SERIO_8042;
serio->write = altera_ps2_write;
serio->open = altera_ps2_open;
serio->close = altera_ps2_close;
strlcpy(serio->name, dev_name(&pdev->dev), sizeof(serio->name));
strlcpy(serio->phys, dev_name(&pdev->dev), sizeof(serio->phys));
serio->port_data = ps2if;
serio->dev.parent = &pdev->dev;
ps2if->io = serio;
ps2if->iomem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
if (ps2if->iomem_res == NULL) {
error = -ENOENT;
goto err_free_mem;
}
irq = platform_get_irq(pdev, 0);
if (irq < 0) {
error = -ENXIO;
goto err_free_mem;
}
ps2if->irq = irq;
if (!request_mem_region(ps2if->iomem_res->start,
resource_size(ps2if->iomem_res), pdev->name)) {
error = -EBUSY;
goto err_free_mem;
}
ps2if->base = ioremap(ps2if->iomem_res->start,
resource_size(ps2if->iomem_res));
if (!ps2if->base) {
error = -ENOMEM;
goto err_free_res;
}
error = request_irq(ps2if->irq, altera_ps2_rxint, 0, pdev->name, ps2if);
if (error) {
dev_err(&pdev->dev, "could not allocate IRQ %d: %d\n",
ps2if->irq, error);
goto err_unmap;
}
dev_info(&pdev->dev, "base %p, irq %d\n", ps2if->base, ps2if->irq);
serio_register_port(ps2if->io);
platform_set_drvdata(pdev, ps2if);
return 0;
err_unmap:
iounmap(ps2if->base);
err_free_res:
release_mem_region(ps2if->iomem_res->start,
resource_size(ps2if->iomem_res));
err_free_mem:
kfree(ps2if);
kfree(serio);
return error;
}
static int amba_kmi_driver_probe(struct vmm_device *dev,
const struct vmm_devtree_nodeid *devid)
{
struct amba_kmi_port *kmi;
struct serio *io;
int ret;
kmi = kzalloc(sizeof(struct amba_kmi_port), GFP_KERNEL);
io = kzalloc(sizeof(struct serio), GFP_KERNEL);
if (!kmi || !io) {
ret = -ENOMEM;
goto out;
}
io->id.type = SERIO_8042;
io->write = amba_kmi_write;
io->open = amba_kmi_open;
io->close = amba_kmi_close;
if (strlcpy(io->name, dev->name, sizeof(io->name)) >=
sizeof(io->name)) {
ret = -EOVERFLOW;
goto out;
}
if (strlcpy(io->phys, dev->name, sizeof(io->phys)) >=
sizeof(io->phys)) {
ret = -EOVERFLOW;
goto out;
}
io->port_data = kmi;
io->dev.parent = dev;
kmi->io = io;
ret = vmm_devtree_request_regmap(dev->node,
(virtual_addr_t *)&kmi->base, 0, "AMBA KMI");
if (ret) {
ret = -ENOMEM;
goto out;
}
kmi->clk = clk_get(dev, "KMIREFCLK");
if (IS_ERR(kmi->clk)) {
ret = PTR_ERR(kmi->clk);
goto unmap;
}
kmi->irq = irq_of_parse_and_map(dev->node, 0);
if (!kmi->irq) {
ret = -EFAIL;
goto unmap;
}
vmm_devdrv_set_data(dev, kmi);
serio_register_port(kmi->io);
return VMM_OK;
unmap:
vmm_devtree_regunmap_release(dev->node, (virtual_addr_t)kmi->base, 0);
out:
if (kmi) kfree(kmi);
if (io) kfree(io);
return ret;
}
/* Initialize one APBPS2 PS/2 core */
static int apbps2_of_probe(struct platform_device *ofdev)
{
struct apbps2_priv *priv;
int irq, err;
u32 freq_hz;
struct resource *res;
priv = devm_kzalloc(&ofdev->dev, sizeof(*priv), GFP_KERNEL);
if (!priv) {
dev_err(&ofdev->dev, "memory allocation failed\n");
return -ENOMEM;
}
/* Find Device Address */
res = platform_get_resource(ofdev, IORESOURCE_MEM, 0);
priv->regs = devm_ioremap_resource(&ofdev->dev, res);
if (IS_ERR(priv->regs))
return PTR_ERR(priv->regs);
/* Reset hardware, disable interrupt */
iowrite32be(0, &priv->regs->ctrl);
/* IRQ */
irq = irq_of_parse_and_map(ofdev->dev.of_node, 0);
err = devm_request_irq(&ofdev->dev, irq, apbps2_isr,
IRQF_SHARED, "apbps2", priv);
if (err) {
dev_err(&ofdev->dev, "request IRQ%d failed\n", irq);
return err;
}
/* Get core frequency */
if (of_property_read_u32(ofdev->dev.of_node, "freq", &freq_hz)) {
dev_err(&ofdev->dev, "unable to get core frequency\n");
return -EINVAL;
}
/* Set reload register to core freq in kHz/10 */
iowrite32be(freq_hz / 10000, &priv->regs->reload);
priv->io = kzalloc(sizeof(struct serio), GFP_KERNEL);
if (!priv->io)
return -ENOMEM;
priv->io->id.type = SERIO_8042;
priv->io->open = apbps2_open;
priv->io->close = apbps2_close;
priv->io->write = apbps2_write;
priv->io->port_data = priv;
strlcpy(priv->io->name, "APBPS2 PS/2", sizeof(priv->io->name));
snprintf(priv->io->phys, sizeof(priv->io->phys),
"apbps2_%d", apbps2_idx++);
dev_info(&ofdev->dev, "irq = %d, base = 0x%p\n", irq, priv->regs);
serio_register_port(priv->io);
platform_set_drvdata(ofdev, priv);
return 0;
}
static ssize_t userio_char_write(struct file *file, const char __user *buffer,
size_t count, loff_t *ppos)
{
struct userio_device *userio = file->private_data;
struct userio_cmd cmd;
int error;
if (count != sizeof(cmd)) {
dev_warn(userio_misc.this_device, "Invalid payload size\n");
return -EINVAL;
}
if (copy_from_user(&cmd, buffer, sizeof(cmd)))
return -EFAULT;
error = mutex_lock_interruptible(&userio->mutex);
if (error)
return error;
switch (cmd.type) {
case USERIO_CMD_REGISTER:
if (!userio->serio->id.type) {
dev_warn(userio_misc.this_device,
"No port type given on /dev/userio\n");
error = -EINVAL;
goto out;
}
if (userio->running) {
dev_warn(userio_misc.this_device,
"Begin command sent, but we're already running\n");
error = -EBUSY;
goto out;
}
userio->running = true;
serio_register_port(userio->serio);
break;
case USERIO_CMD_SET_PORT_TYPE:
if (userio->running) {
dev_warn(userio_misc.this_device,
"Can't change port type on an already running userio instance\n");
error = -EBUSY;
goto out;
}
userio->serio->id.type = cmd.data;
break;
case USERIO_CMD_SEND_INTERRUPT:
if (!userio->running) {
dev_warn(userio_misc.this_device,
"The device must be registered before sending interrupts\n");
error = -ENODEV;
goto out;
}
serio_interrupt(userio->serio, cmd.data, 0);
break;
default:
error = -EOPNOTSUPP;
goto out;
}
out:
mutex_unlock(&userio->mutex);
return error ?: count;
}
/** Shared device initialization code */
static int xps2_setup(
struct device *dev,
int id,
struct resource *r_mem,
struct resource *r_irq) {
XPs2_Config xps2_cfg;
struct xps2data *drvdata;
unsigned long remap_size;
int retval;
if (!dev)
return -EINVAL;
drvdata = kzalloc(sizeof(struct xps2data), GFP_KERNEL);
if (!drvdata) {
dev_err(dev, "Couldn't allocate device private record\n");
return -ENOMEM;
}
spin_lock_init(&drvdata->lock);
dev_set_drvdata(dev, (void *)drvdata);
if (!r_mem || !r_irq) {
dev_err(dev, "IO resource(s) not found\n");
retval = -EFAULT;
goto failed1;
}
drvdata->irq = r_irq->start;
remap_size = r_mem->end - r_mem->start + 1;
if (!request_mem_region(r_mem->start, remap_size, DRIVER_NAME)) {
dev_err(dev, "Couldn't lock memory region at 0x%08x\n",
r_mem->start);
retval = -EBUSY;
goto failed1;
}
/* Fill in cfg data and add them to the list */
drvdata->phys_addr = r_mem->start;
drvdata->remap_size = remap_size;
xps2_cfg.DeviceId = id;
xps2_cfg.BaseAddress = (u32) ioremap(r_mem->start, remap_size);
if (xps2_cfg.BaseAddress == 0) {
dev_err(dev, "Couldn't ioremap memory at 0x%08x\n",
r_mem->start);
retval = -EFAULT;
goto failed2;
}
/* Tell the Xilinx code to bring this PS/2 interface up. */
down(&cfg_sem);
if (XPs2_CfgInitialize(&drvdata->ps2, &xps2_cfg, xps2_cfg.BaseAddress)
!= XST_SUCCESS) {
up(&cfg_sem);
dev_err(dev, "Could not initialize device.\n");
retval = -ENODEV;
goto failed3;
}
up(&cfg_sem);
/* Set up the interrupt handler. */
XPs2_SetHandler(&drvdata->ps2, sxps2_handler, drvdata);
dev_info(dev, "Xilinx PS2 at 0x%08X mapped to 0x%08X, irq=%d\n",
drvdata->phys_addr,
drvdata->ps2.BaseAddress,
drvdata->irq);
drvdata->serio.id.type = SERIO_8042;
drvdata->serio.write = sxps2_write;
drvdata->serio.open = sxps2_open;
drvdata->serio.close = sxps2_close;
drvdata->serio.port_data = drvdata;
drvdata->serio.dev.parent = dev;
snprintf(drvdata->serio.name, sizeof(drvdata->serio.name),
XPS2_NAME_DESC, id);
snprintf(drvdata->serio.phys, sizeof(drvdata->serio.phys),
XPS2_PHYS_DESC, id);
serio_register_port(&drvdata->serio);
return 0; /* success */
failed3:
iounmap((void *)(xps2_cfg.BaseAddress));
failed2:
release_mem_region(r_mem->start, remap_size);
failed1:
kfree(drvdata);
dev_set_drvdata(dev, NULL);
return retval;
}
