Commit 6bd0dcfa authored by Nick Dyer's avatar Nick Dyer Committed by Dmitry Torokhov

Input: synaptics-rmi4 - factor out functions from probe

Signed-off-by: default avatarNick Dyer <nick@shmanahar.org>
Signed-off-by: default avatarBenjamin Tissoires <benjamin.tissoires@redhat.com>
Tested-by: default avatarChris Healy <cphealy@gmail.com>
Signed-off-by: default avatarDmitry Torokhov <dmitry.torokhov@gmail.com>
parent ad338e8b
...@@ -39,6 +39,8 @@ static void rmi_free_function_list(struct rmi_device *rmi_dev) ...@@ -39,6 +39,8 @@ static void rmi_free_function_list(struct rmi_device *rmi_dev)
struct rmi_function *fn, *tmp; struct rmi_function *fn, *tmp;
struct rmi_driver_data *data = dev_get_drvdata(&rmi_dev->dev); struct rmi_driver_data *data = dev_get_drvdata(&rmi_dev->dev);
rmi_dbg(RMI_DEBUG_CORE, &rmi_dev->dev, "Freeing function list\n");
data->f01_container = NULL; data->f01_container = NULL;
/* Doing it in the reverse order so F01 will be removed last */ /* Doing it in the reverse order so F01 will be removed last */
...@@ -855,15 +857,90 @@ static inline int rmi_driver_of_probe(struct device *dev, ...@@ -855,15 +857,90 @@ static inline int rmi_driver_of_probe(struct device *dev,
} }
#endif #endif
static int rmi_probe_interrupts(struct rmi_driver_data *data)
{
struct rmi_device *rmi_dev = data->rmi_dev;
struct device *dev = &rmi_dev->dev;
int irq_count;
size_t size;
void *irq_memory;
int retval;
/*
* We need to count the IRQs and allocate their storage before scanning
* the PDT and creating the function entries, because adding a new
* function can trigger events that result in the IRQ related storage
* being accessed.
*/
rmi_dbg(RMI_DEBUG_CORE, dev, "%s: Counting IRQs.\n", __func__);
irq_count = 0;
retval = rmi_scan_pdt(rmi_dev, &irq_count, rmi_count_irqs);
if (retval < 0) {
dev_err(dev, "IRQ counting failed with code %d.\n", retval);
return retval;
}
data->irq_count = irq_count;
data->num_of_irq_regs = (data->irq_count + 7) / 8;
size = BITS_TO_LONGS(data->irq_count) * sizeof(unsigned long);
irq_memory = devm_kzalloc(dev, size * 4, GFP_KERNEL);
if (!irq_memory) {
dev_err(dev, "Failed to allocate memory for irq masks.\n");
return retval;
}
data->irq_status = irq_memory + size * 0;
data->fn_irq_bits = irq_memory + size * 1;
data->current_irq_mask = irq_memory + size * 2;
data->new_irq_mask = irq_memory + size * 3;
return retval;
}
static int rmi_init_functions(struct rmi_driver_data *data)
{
struct rmi_device *rmi_dev = data->rmi_dev;
struct device *dev = &rmi_dev->dev;
int irq_count;
int retval;
irq_count = 0;
rmi_dbg(RMI_DEBUG_CORE, dev, "%s: Creating functions.\n", __func__);
retval = rmi_scan_pdt(rmi_dev, &irq_count, rmi_create_function);
if (retval < 0) {
dev_err(dev, "Function creation failed with code %d.\n",
retval);
goto err_destroy_functions;
}
if (!data->f01_container) {
dev_err(dev, "Missing F01 container!\n");
retval = -EINVAL;
goto err_destroy_functions;
}
retval = rmi_read_block(rmi_dev,
data->f01_container->fd.control_base_addr + 1,
data->current_irq_mask, data->num_of_irq_regs);
if (retval < 0) {
dev_err(dev, "%s: Failed to read current IRQ mask.\n",
__func__);
goto err_destroy_functions;
}
return 0;
err_destroy_functions:
rmi_free_function_list(rmi_dev);
return retval;
}
static int rmi_driver_probe(struct device *dev) static int rmi_driver_probe(struct device *dev)
{ {
struct rmi_driver *rmi_driver; struct rmi_driver *rmi_driver;
struct rmi_driver_data *data; struct rmi_driver_data *data;
struct rmi_device_platform_data *pdata; struct rmi_device_platform_data *pdata;
struct rmi_device *rmi_dev; struct rmi_device *rmi_dev;
size_t size;
void *irq_memory;
int irq_count;
int retval; int retval;
rmi_dbg(RMI_DEBUG_CORE, dev, "%s: Starting probe.\n", rmi_dbg(RMI_DEBUG_CORE, dev, "%s: Starting probe.\n",
...@@ -929,35 +1006,11 @@ static int rmi_driver_probe(struct device *dev) ...@@ -929,35 +1006,11 @@ static int rmi_driver_probe(struct device *dev)
PDT_PROPERTIES_LOCATION, retval); PDT_PROPERTIES_LOCATION, retval);
} }
/*
* We need to count the IRQs and allocate their storage before scanning
* the PDT and creating the function entries, because adding a new
* function can trigger events that result in the IRQ related storage
* being accessed.
*/
rmi_dbg(RMI_DEBUG_CORE, dev, "Counting IRQs.\n");
irq_count = 0;
retval = rmi_scan_pdt(rmi_dev, &irq_count, rmi_count_irqs);
if (retval < 0) {
dev_err(dev, "IRQ counting failed with code %d.\n", retval);
goto err;
}
data->irq_count = irq_count;
data->num_of_irq_regs = (data->irq_count + 7) / 8;
mutex_init(&data->irq_mutex); mutex_init(&data->irq_mutex);
size = BITS_TO_LONGS(data->irq_count) * sizeof(unsigned long); retval = rmi_probe_interrupts(data);
irq_memory = devm_kzalloc(dev, size * 4, GFP_KERNEL); if (retval)
if (!irq_memory) {
dev_err(dev, "Failed to allocate memory for irq masks.\n");
goto err; goto err;
}
data->irq_status = irq_memory + size * 0;
data->fn_irq_bits = irq_memory + size * 1;
data->current_irq_mask = irq_memory + size * 2;
data->new_irq_mask = irq_memory + size * 3;
if (rmi_dev->xport->input) { if (rmi_dev->xport->input) {
/* /*
...@@ -974,36 +1027,16 @@ static int rmi_driver_probe(struct device *dev) ...@@ -974,36 +1027,16 @@ static int rmi_driver_probe(struct device *dev)
dev_err(dev, "%s: Failed to allocate input device.\n", dev_err(dev, "%s: Failed to allocate input device.\n",
__func__); __func__);
retval = -ENOMEM; retval = -ENOMEM;
goto err_destroy_functions; goto err;
} }
rmi_driver_set_input_params(rmi_dev, data->input); rmi_driver_set_input_params(rmi_dev, data->input);
data->input->phys = devm_kasprintf(dev, GFP_KERNEL, data->input->phys = devm_kasprintf(dev, GFP_KERNEL,
"%s/input0", dev_name(dev)); "%s/input0", dev_name(dev));
} }
irq_count = 0; retval = rmi_init_functions(data);
rmi_dbg(RMI_DEBUG_CORE, dev, "Creating functions."); if (retval)
retval = rmi_scan_pdt(rmi_dev, &irq_count, rmi_create_function); goto err;
if (retval < 0) {
dev_err(dev, "Function creation failed with code %d.\n",
retval);
goto err_destroy_functions;
}
if (!data->f01_container) {
dev_err(dev, "Missing F01 container!\n");
retval = -EINVAL;
goto err_destroy_functions;
}
retval = rmi_read_block(rmi_dev,
data->f01_container->fd.control_base_addr + 1,
data->current_irq_mask, data->num_of_irq_regs);
if (retval < 0) {
dev_err(dev, "%s: Failed to read current IRQ mask.\n",
__func__);
goto err_destroy_functions;
}
if (data->input) { if (data->input) {
rmi_driver_set_input_name(rmi_dev, data->input); rmi_driver_set_input_name(rmi_dev, data->input);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment