提交 6bd0dcfa 编写于 作者: N Nick Dyer 提交者: Dmitry Torokhov

Input: synaptics-rmi4 - factor out functions from probe

Signed-off-by: NNick Dyer <nick@shmanahar.org>
Signed-off-by: NBenjamin Tissoires <benjamin.tissoires@redhat.com>
Tested-by: NChris Healy <cphealy@gmail.com>
Signed-off-by: NDmitry Torokhov <dmitry.torokhov@gmail.com>
上级 ad338e8b
......@@ -39,6 +39,8 @@ static void rmi_free_function_list(struct rmi_device *rmi_dev)
struct rmi_function *fn, *tmp;
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;
/* 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,
}
#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)
{
struct rmi_driver *rmi_driver;
struct rmi_driver_data *data;
struct rmi_device_platform_data *pdata;
struct rmi_device *rmi_dev;
size_t size;
void *irq_memory;
int irq_count;
int retval;
rmi_dbg(RMI_DEBUG_CORE, dev, "%s: Starting probe.\n",
......@@ -929,35 +1006,11 @@ static int rmi_driver_probe(struct device *dev)
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);
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");
retval = rmi_probe_interrupts(data);
if (retval)
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) {
/*
......@@ -974,36 +1027,16 @@ static int rmi_driver_probe(struct device *dev)
dev_err(dev, "%s: Failed to allocate input device.\n",
__func__);
retval = -ENOMEM;
goto err_destroy_functions;
goto err;
}
rmi_driver_set_input_params(rmi_dev, data->input);
data->input->phys = devm_kasprintf(dev, GFP_KERNEL,
"%s/input0", dev_name(dev));
}
irq_count = 0;
rmi_dbg(RMI_DEBUG_CORE, dev, "Creating functions.");
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;
}
retval = rmi_init_functions(data);
if (retval)
goto err;
if (data->input) {
rmi_driver_set_input_name(rmi_dev, data->input);
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册