in rmi4/rmi_f01.c [384:577]
static int rmi_f01_probe(struct rmi_function *fn)
{
struct rmi_device *rmi_dev = fn->rmi_dev;
struct rmi_driver_data *driver_data = dev_get_drvdata(&rmi_dev->dev);
struct rmi_device_platform_data *pdata = rmi_get_platform_data(rmi_dev);
struct f01_data *f01;
int error;
u16 ctrl_base_addr = fn->fd.control_base_addr;
u8 device_status;
u8 temp;
if (fn->dev.of_node) {
error = rmi_f01_of_probe(&fn->dev, pdata);
if (error)
return error;
}
f01 = devm_kzalloc(&fn->dev, sizeof(struct f01_data), GFP_KERNEL);
if (!f01)
return -ENOMEM;
f01->num_of_irq_regs = driver_data->num_of_irq_regs;
/*
* Set the configured bit and (optionally) other important stuff
* in the device control register.
*/
error = rmi_read(rmi_dev, fn->fd.control_base_addr,
&f01->device_control.ctrl0);
if (error) {
dev_err(&fn->dev, "Failed to read F01 control: %d\n", error);
return error;
}
switch (pdata->power_management.nosleep) {
case RMI_REG_STATE_DEFAULT:
break;
case RMI_REG_STATE_OFF:
f01->device_control.ctrl0 &= ~RMI_F01_CTRL0_NOSLEEP_BIT;
break;
case RMI_REG_STATE_ON:
f01->device_control.ctrl0 |= RMI_F01_CTRL0_NOSLEEP_BIT;
break;
}
/*
* Sleep mode might be set as a hangover from a system crash or
* reboot without power cycle. If so, clear it so the sensor
* is certain to function.
*/
if ((f01->device_control.ctrl0 & RMI_F01_CTRL0_SLEEP_MODE_MASK) !=
RMI_SLEEP_MODE_NORMAL) {
dev_warn(&fn->dev,
"WARNING: Non-zero sleep mode found. Clearing...\n");
f01->device_control.ctrl0 &= ~RMI_F01_CTRL0_SLEEP_MODE_MASK;
}
f01->device_control.ctrl0 |= RMI_F01_CTRL0_CONFIGURED_BIT;
error = rmi_write(rmi_dev, fn->fd.control_base_addr,
f01->device_control.ctrl0);
if (error) {
dev_err(&fn->dev, "Failed to write F01 control: %d\n", error);
return error;
}
/* Dummy read in order to clear irqs */
error = rmi_read(rmi_dev, fn->fd.data_base_addr + 1, &temp);
if (error < 0) {
dev_err(&fn->dev, "Failed to read Interrupt Status.\n");
return error;
}
error = rmi_f01_read_properties(rmi_dev, fn->fd.query_base_addr,
&f01->properties);
if (error < 0) {
dev_err(&fn->dev, "Failed to read F01 properties.\n");
return error;
}
dev_info(&fn->dev, "found RMI device, manufacturer: %s, product: %s, fw id: %d\n",
f01->properties.manufacturer_id == 1 ? "Synaptics" : "unknown",
f01->properties.product_id, f01->properties.firmware_id);
/* Advance to interrupt control registers, then skip over them. */
ctrl_base_addr++;
ctrl_base_addr += f01->num_of_irq_regs;
/* read control register */
if (f01->properties.has_adjustable_doze) {
f01->doze_interval_addr = ctrl_base_addr;
ctrl_base_addr++;
if (pdata->power_management.doze_interval) {
f01->device_control.doze_interval =
pdata->power_management.doze_interval;
error = rmi_write(rmi_dev, f01->doze_interval_addr,
f01->device_control.doze_interval);
if (error) {
dev_err(&fn->dev,
"Failed to configure F01 doze interval register: %d\n",
error);
return error;
}
} else {
error = rmi_read(rmi_dev, f01->doze_interval_addr,
&f01->device_control.doze_interval);
if (error) {
dev_err(&fn->dev,
"Failed to read F01 doze interval register: %d\n",
error);
return error;
}
}
f01->wakeup_threshold_addr = ctrl_base_addr;
ctrl_base_addr++;
if (pdata->power_management.wakeup_threshold) {
f01->device_control.wakeup_threshold =
pdata->power_management.wakeup_threshold;
error = rmi_write(rmi_dev, f01->wakeup_threshold_addr,
f01->device_control.wakeup_threshold);
if (error) {
dev_err(&fn->dev,
"Failed to configure F01 wakeup threshold register: %d\n",
error);
return error;
}
} else {
error = rmi_read(rmi_dev, f01->wakeup_threshold_addr,
&f01->device_control.wakeup_threshold);
if (error < 0) {
dev_err(&fn->dev,
"Failed to read F01 wakeup threshold register: %d\n",
error);
return error;
}
}
}
if (f01->properties.has_lts)
ctrl_base_addr++;
if (f01->properties.has_adjustable_doze_holdoff) {
f01->doze_holdoff_addr = ctrl_base_addr;
ctrl_base_addr++;
if (pdata->power_management.doze_holdoff) {
f01->device_control.doze_holdoff =
pdata->power_management.doze_holdoff;
error = rmi_write(rmi_dev, f01->doze_holdoff_addr,
f01->device_control.doze_holdoff);
if (error) {
dev_err(&fn->dev,
"Failed to configure F01 doze holdoff register: %d\n",
error);
return error;
}
} else {
error = rmi_read(rmi_dev, f01->doze_holdoff_addr,
&f01->device_control.doze_holdoff);
if (error) {
dev_err(&fn->dev,
"Failed to read F01 doze holdoff register: %d\n",
error);
return error;
}
}
}
error = rmi_read(rmi_dev, fn->fd.data_base_addr, &device_status);
if (error < 0) {
dev_err(&fn->dev,
"Failed to read device status: %d\n", error);
return error;
}
if (RMI_F01_STATUS_UNCONFIGURED(device_status)) {
dev_err(&fn->dev,
"Device was reset during configuration process, status: %#02x!\n",
RMI_F01_STATUS_CODE(device_status));
return -EINVAL;
}
dev_set_drvdata(&fn->dev, f01);
error = sysfs_create_group(&fn->rmi_dev->dev.kobj, &rmi_f01_attr_group);
if (error)
dev_warn(&fn->dev, "Failed to create sysfs group: %d\n", error);
return 0;
}