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

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

drivers/input/rmi4/rmi_driver.c

+86 −53
Original line number Diff line number Diff line
@@ -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);