UBUNTU: Ubuntu-4.15.0-96.97

[mirror_ubuntu-bionic-kernel.git] / drivers / input / rmi4 / rmi_f03.c

diff --git a/drivers/input/rmi4/rmi_f03.c b/drivers/input/rmi4/rmi_f03.c
index ad71a5e768dc46432b18b84fb344a7bb514bb56c..7ccbb370a9a81ecd8098801453c0122c2bc99f8e 100644 (file)
--- a/drivers/input/rmi4/rmi_f03.c
+++ b/drivers/input/rmi4/rmi_f03.c
@@ -32,6 +32,7 @@ struct f03_data {
        struct rmi_function *fn;
 
        struct serio *serio;
+       bool serio_registered;
 
        unsigned int overwrite_buttons;
 
@@ -138,6 +139,37 @@ static int rmi_f03_initialize(struct f03_data *f03)
        return 0;
 }
 
+static int rmi_f03_pt_open(struct serio *serio)
+{
+       struct f03_data *f03 = serio->port_data;
+       struct rmi_function *fn = f03->fn;
+       const u8 ob_len = f03->rx_queue_length * RMI_F03_OB_SIZE;
+       const u16 data_addr = fn->fd.data_base_addr + RMI_F03_OB_OFFSET;
+       u8 obs[RMI_F03_QUEUE_LENGTH * RMI_F03_OB_SIZE];
+       int error;
+
+       /*
+        * Consume any pending data. Some devices like to spam with
+        * 0xaa 0x00 announcements which may confuse us as we try to
+        * probe the device.
+        */
+       error = rmi_read_block(fn->rmi_dev, data_addr, &obs, ob_len);
+       if (!error)
+               rmi_dbg(RMI_DEBUG_FN, &fn->dev,
+                       "%s: Consumed %*ph (%d) from PS2 guest\n",
+                       __func__, ob_len, obs, ob_len);
+
+       return fn->rmi_dev->driver->set_irq_bits(fn->rmi_dev, fn->irq_mask);
+}
+
+static void rmi_f03_pt_close(struct serio *serio)
+{
+       struct f03_data *f03 = serio->port_data;
+       struct rmi_function *fn = f03->fn;
+
+       fn->rmi_dev->driver->clear_irq_bits(fn->rmi_dev, fn->irq_mask);
+}
+
 static int rmi_f03_register_pt(struct f03_data *f03)
 {
        struct serio *serio;
@@ -148,6 +180,8 @@ static int rmi_f03_register_pt(struct f03_data *f03)
 
        serio->id.type = SERIO_PS_PSTHRU;
        serio->write = rmi_f03_pt_write;
+       serio->open = rmi_f03_pt_open;
+       serio->close = rmi_f03_pt_close;
        serio->port_data = f03;
 
        strlcpy(serio->name, "Synaptics RMI4 PS/2 pass-through",
@@ -184,17 +218,27 @@ static int rmi_f03_probe(struct rmi_function *fn)
                         f03->device_count);
 
        dev_set_drvdata(dev, f03);
-
-       error = rmi_f03_register_pt(f03);
-       if (error)
-               return error;
-
        return 0;
 }
 
 static int rmi_f03_config(struct rmi_function *fn)
 {
-       fn->rmi_dev->driver->set_irq_bits(fn->rmi_dev, fn->irq_mask);
+       struct f03_data *f03 = dev_get_drvdata(&fn->dev);
+       int error;
+
+       if (!f03->serio_registered) {
+               error = rmi_f03_register_pt(f03);
+               if (error)
+                       return error;
+
+               f03->serio_registered = true;
+       } else {
+               /*
+                * We must be re-configuring the sensor, just enable
+                * interrupts for this function.
+                */
+               fn->rmi_dev->driver->set_irq_bits(fn->rmi_dev, fn->irq_mask);
+       }
 
        return 0;
 }
@@ -204,7 +248,7 @@ static int rmi_f03_attention(struct rmi_function *fn, unsigned long *irq_bits)
        struct rmi_device *rmi_dev = fn->rmi_dev;
        struct rmi_driver_data *drvdata = dev_get_drvdata(&rmi_dev->dev);
        struct f03_data *f03 = dev_get_drvdata(&fn->dev);
-       u16 data_addr = fn->fd.data_base_addr;
+       const u16 data_addr = fn->fd.data_base_addr + RMI_F03_OB_OFFSET;
        const u8 ob_len = f03->rx_queue_length * RMI_F03_OB_SIZE;
        u8 obs[RMI_F03_QUEUE_LENGTH * RMI_F03_OB_SIZE];
        u8 ob_status;
@@ -226,8 +270,7 @@ static int rmi_f03_attention(struct rmi_function *fn, unsigned long *irq_bits)
                drvdata->attn_data.size -= ob_len;
        } else {
                /* Grab all of the data registers, and check them for data */
-               error = rmi_read_block(fn->rmi_dev, data_addr + RMI_F03_OB_OFFSET,
-                                      &obs, ob_len);
+               error = rmi_read_block(fn->rmi_dev, data_addr, &obs, ob_len);
                if (error) {
                        dev_err(&fn->dev,
                                "%s: Failed to read F03 output buffers: %d\n",
@@ -266,7 +309,8 @@ static void rmi_f03_remove(struct rmi_function *fn)
 {
        struct f03_data *f03 = dev_get_drvdata(&fn->dev);
 
-       serio_unregister_port(f03->serio);
+       if (f03->serio_registered)
+               serio_unregister_port(f03->serio);
 }
 
 struct rmi_function_handler rmi_f03_handler = {