int qpnp_vib_set(struct qpnp_vib *vib, int on)
{
int rc;
u8 val;
if (on) {
val = vib->reg_vtg_ctl;
val &= ~QPNP_VIB_VTG_SET_MASK;
val |= (vib->vtg_level & QPNP_VIB_VTG_SET_MASK);
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_vtg_ctl = val;
val = vib->reg_en_ctl;
val |= QPNP_VIB_EN;
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = val;
} else {
val = vib->reg_en_ctl;
val &= ~QPNP_VIB_EN;
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = val;
}
return rc;
}
/* Begin Immersion changes */
int qpnp_vib_set_with_vtglevel(struct qpnp_vib *vib, int vtglevel, int on)
{
int rc;
u8 val;
if(vtglevel < QPNP_VIB_MIN_LEVEL) vtglevel = QPNP_VIB_MIN_LEVEL;
if(vtglevel > QPNP_VIB_MAX_LEVEL) vtglevel = QPNP_VIB_MAX_LEVEL;
if (on) {
val = vib->reg_vtg_ctl;
val &= ~QPNP_VIB_VTG_SET_MASK;
val |= (vtglevel & QPNP_VIB_VTG_SET_MASK);
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_vtg_ctl = val;
val = vib->reg_en_ctl;
val |= QPNP_VIB_EN;
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = val;
} else {
val = vib->reg_en_ctl;
val &= ~QPNP_VIB_EN;
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = val;
}
return rc;
}
static int qpnp_vib_set(struct qpnp_vib *vib, int on)
{
int rc;
u8 val;
if (on) {
val = vib->reg_vtg_ctl;
val &= ~QPNP_VIB_VTG_SET_MASK;
val |= (vib->vtg_level & QPNP_VIB_VTG_SET_MASK);
printk(KERN_INFO "[VIB] on, reg0=0x%x.\n", val);
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_vtg_ctl = val;
val = vib->reg_en_ctl;
val |= QPNP_VIB_EN;
printk(KERN_INFO "[VIB] on, reg1=0x%x.\n", val);
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = val;
} else {
val = vib->reg_en_ctl;
val &= ~QPNP_VIB_EN;
printk(KERN_INFO "[VIB] off, reg1=0x%x.\n", val);
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = val;
}
return rc;
}
static int qpnp_vibrator_config(struct qpnp_vib *vib)
{
u8 reg = 0;
int rc;
/* Configure the VTG CTL regiser */
rc = qpnp_vib_read_u8(vib, ®, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0)
return rc;
reg &= ~QPNP_VIB_VTG_SET_MASK;
reg |= (vib->vtg_level & QPNP_VIB_VTG_SET_MASK);
rc = qpnp_vib_write_u8(vib, ®, QPNP_VIB_VTG_CTL(vib->base));
if (rc)
return rc;
vib->reg_vtg_ctl = reg;
/* Configure the VIB ENABLE regiser */
rc = qpnp_vib_read_u8(vib, ®, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
reg |= (!!vib->active_low) << QPNP_VIB_LOGIC_SHIFT;
if (vib->mode != QPNP_VIB_MANUAL) {
vib->pwm_info.pwm_dev = pwm_request(vib->pwm_info.pwm_channel,
"qpnp-vib");
if (IS_ERR_OR_NULL(vib->pwm_info.pwm_dev)) {
dev_err(&vib->spmi->dev, "vib pwm request failed\n");
return -ENODEV;
}
rc = pwm_config(vib->pwm_info.pwm_dev, vib->pwm_info.duty_us,
vib->pwm_info.period_us);
if (rc < 0) {
dev_err(&vib->spmi->dev, "vib pwm config failed\n");
pwm_free(vib->pwm_info.pwm_dev);
return -ENODEV;
}
reg |= BIT(vib->mode - 1);
}
rc = qpnp_vib_write_u8(vib, ®, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = reg;
return rc;
}
static ssize_t qpnp_vib_level_store(struct device *dev,
struct device_attribute *attr,
const char *buf, size_t count)
{
struct timed_output_dev *tdev = dev_get_drvdata(dev);
struct qpnp_vib *vib = container_of(tdev, struct qpnp_vib,
timed_dev);
int val;
int rc;
u8 reg = 0;
rc = kstrtoint(buf, 10, &val);
if (rc) {
pr_err("%s: error getting level\n", __func__);
return -EINVAL;
}
if (val < QPNP_VIB_MIN_LEVEL) {
pr_err("%s: level %d not in range (%d - %d), using min.", __func__, val, QPNP_VIB_MIN_LEVEL, QPNP_VIB_MAX_LEVEL);
val = QPNP_VIB_MIN_LEVEL;
} else if (val > QPNP_VIB_MAX_LEVEL) {
pr_err("%s: level %d not in range (%d - %d), using max.", __func__, val, QPNP_VIB_MIN_LEVEL, QPNP_VIB_MAX_LEVEL);
val = QPNP_VIB_MAX_LEVEL;
}
vib->vtg_level = val;
/* Configure the VTG CTL regiser */
rc = qpnp_vib_read_u8(vib, ®, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0) {
pr_info("qpnp: error while reading vibration control register\n");
}
reg &= ~QPNP_VIB_VTG_SET_MASK;
reg |= (vib->vtg_level & QPNP_VIB_VTG_SET_MASK);
rc = qpnp_vib_write_u8(vib, ®, QPNP_VIB_VTG_CTL(vib->base));
if (rc)
pr_info("qpnp: error while writing vibration control register\n");
return strnlen(buf, count);
}
static int qpnp_vib_set(struct qpnp_vib *vib, int on)
{
int rc;
u8 val;
if (on) {
val = vib->reg_vtg_ctl;
val &= ~QPNP_VIB_VTG_SET_MASK;
val |= (vib->vtg_level & QPNP_VIB_VTG_SET_MASK);
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_vtg_ctl = val;
val = vib->reg_en_ctl;
val |= QPNP_VIB_EN;
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
#ifdef CONFIG_QPNP_SCVIBRATOR
if (rc < 0) {
return rc;
} else {
#ifdef CONFIG_SHTERM
shterm_k_set_info( SHTERM_INFO_VIB, 1 );
#endif /* CONFIG_SHTERM */
}
#else /* CONFIG_QPNP_SCVIBRATOR */
if (rc < 0)
return rc;
#endif /* CONFIG_QPNP_SCVIBRATOR */
vib->reg_en_ctl = val;
} else {
val = vib->reg_en_ctl;
val &= ~QPNP_VIB_EN;
rc = qpnp_vib_write_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
#ifdef CONFIG_QPNP_SCVIBRATOR
if (rc < 0) {
return rc;
} else {
#ifdef CONFIG_SHTERM
shterm_k_set_info( SHTERM_INFO_VIB, 0 );
#endif /* CONFIG_SHTERM */
}
#else /* CONFIG_QPNP_SCVIBRATOR */
if (rc < 0)
return rc;
#endif /* CONFIG_QPNP_SCVIBRATOR */
vib->reg_en_ctl = val;
}
return rc;
}
int qpnp_vibrator_config(struct qpnp_vib_config *vib_cfg)
{
u8 reg = 0;
int rc = -EINVAL, level;
if (!vib_dev) {
pr_err("%s: vib_dev is NULL\n", __func__);
rc = -ENODEV;
goto gen_err;
}
level = vib_cfg->drive_mV / QPNP_VIB_UV_PER_MV;
if (level) {
if ((level < QPNP_VIB_MIN_LEVEL) ||
(level > QPNP_VIB_MAX_LEVEL)) {
dev_err(&vib_dev->spmi->dev, "Invalid voltage level\n");
goto gen_err;
}
} else {
dev_err(&vib_dev->spmi->dev, "Voltage level not specified\n");
goto gen_err;
}
/* Configure the VTG CTL regiser */
reg = vib_dev->reg_vtg_ctl;
reg &= ~QPNP_VIB_VTG_SET_MASK;
reg |= (level & QPNP_VIB_VTG_SET_MASK);
rc = qpnp_vib_write_u8(vib_dev, ®, QPNP_VIB_VTG_CTL(vib_dev->base));
if (rc)
goto gen_err;
vib_dev->reg_vtg_ctl = reg;
/* Configure the VIB ENABLE regiser */
reg = vib_dev->reg_en_ctl;
reg |= (!!vib_cfg->active_low) << QPNP_VIB_LOGIC_SHIFT;
if (vib_cfg->enable_mode == QPNP_VIB_MANUAL)
reg |= QPNP_VIB_EN;
else
reg |= BIT(vib_cfg->enable_mode - 1);
rc = qpnp_vib_write_u8(vib_dev, ®, QPNP_VIB_EN_CTL(vib_dev->base));
if (rc < 0)
goto gen_err;
vib_dev->reg_en_ctl = reg;
gen_err:
return rc;
}
static int __devinit qpnp_vibrator_probe(struct spmi_device *spmi)
{
struct qpnp_vib *vib;
struct resource *vib_resource;
int rc;
u8 val;
u32 temp_val;
vib = devm_kzalloc(&spmi->dev, sizeof(*vib), GFP_KERNEL);
if (!vib)
return -ENOMEM;
vib->spmi = spmi;
vib->timeout = QPNP_VIB_DEFAULT_TIMEOUT;
rc = of_property_read_u32(spmi->dev.of_node,
"qcom,vib-timeout-ms", &temp_val);
if (!rc) {
vib->timeout = temp_val;
} else if (rc != EINVAL) {
dev_err(&spmi->dev, "Unable to read vib timeout\n");
return rc;
}
vib->vtg_level = QPNP_VIB_DEFAULT_VTG_LVL;
rc = of_property_read_u32(spmi->dev.of_node,
"qcom,vib-vtg-level-mV", &temp_val);
if (!rc) {
vib->vtg_level = temp_val;
} else if (rc != -EINVAL) {
dev_err(&spmi->dev, "Unable to read vtg level\n");
return rc;
}
vib->vtg_level /= 100;
#ifdef CONFIG_VIBRATOR_PM8941_HAPTIC
vib->vtg_haptic_level = QPNP_VIB_MAX_LEVEL;
#endif
vib_resource = spmi_get_resource(spmi, 0, IORESOURCE_MEM, 0);
if (!vib_resource) {
dev_err(&spmi->dev, "Unable to get vibrator base address\n");
return -EINVAL;
}
vib->base = vib_resource->start;
/* save the control registers values */
rc = qpnp_vib_read_u8(vib, &val, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_vtg_ctl = val;
rc = qpnp_vib_read_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = val;
mutex_init(&vib->lock);
INIT_WORK(&vib->work, qpnp_vib_update);
hrtimer_init(&vib->vib_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
vib->vib_timer.function = qpnp_vib_timer_func;
vib->timed_dev.name = "vibrator";
vib->timed_dev.get_time = qpnp_vib_get_time;
vib->timed_dev.enable = qpnp_vib_enable;
#ifdef CONFIG_VIBRATOR_PM8941_HAPTIC
vib->timed_dev.set_vtLevel = qpnp_vib_set_level;
vib->timed_dev.get_vtLevel = qpnp_vib_get_level;
#endif
dev_set_drvdata(&spmi->dev, vib);
rc = timed_output_dev_register(&vib->timed_dev);
if (rc < 0)
return rc;
vib_dev = vib;
return rc;
}
static int __devinit qpnp_vibrator_probe(struct spmi_device *spmi)
{
struct qpnp_vib *vib;
const __be32 *temp_dt;
struct resource *vib_resource;
int rc;
u8 val;
vib = devm_kzalloc(&spmi->dev, sizeof(*vib), GFP_KERNEL);
if (!vib)
return -ENOMEM;
vib->spmi = spmi;
temp_dt = of_get_property(spmi->dev.of_node,
"qcom,qpnp-vib-timeout-ms", NULL);
if (temp_dt)
vib->timeout = be32_to_cpu(*temp_dt);
else
vib->timeout = QPNP_VIB_DEFAULT_TIMEOUT;
temp_dt = of_get_property(spmi->dev.of_node,
"qcom,qpnp-vib-vtg-level-mV", NULL);
if (temp_dt)
vib->vtg_level = be32_to_cpu(*temp_dt);
else
vib->vtg_level = QPNP_VIB_DEFAULT_VTG_LVL;
vib->vtg_level /= 100;
vib_resource = spmi_get_resource(spmi, 0, IORESOURCE_MEM, 0);
if (!vib_resource) {
dev_err(&spmi->dev, "Unable to get vibrator base address\n");
return -EINVAL;
}
vib->base = vib_resource->start;
rc = qpnp_vib_read_u8(vib, &val, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_vtg_ctl = val;
rc = qpnp_vib_read_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
return rc;
vib->reg_en_ctl = val;
spin_lock_init(&vib->lock);
INIT_WORK(&vib->work, qpnp_vib_update);
hrtimer_init(&vib->vib_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
vib->vib_timer.function = qpnp_vib_timer_func;
vib->timed_dev.name = "vibrator";
vib->timed_dev.get_time = qpnp_vib_get_time;
vib->timed_dev.enable = qpnp_vib_enable;
dev_set_drvdata(&spmi->dev, vib);
rc = timed_output_dev_register(&vib->timed_dev);
if (rc < 0)
return rc;
#ifdef CONFIG_VIB_TRIGGERS
vib->enabler.name = "qpnp-vibrator";
vib->enabler.default_trigger = "vibrator";
vib->enabler.enable = qpnp_vib_trigger_enable;
vib->enabler.trigger_data = vib;
vib_trigger_enabler_register(&vib->enabler);
#endif
vib_dev = vib;
return rc;
}
static int __devinit qpnp_vibrator_probe(struct spmi_device *spmi)
{
struct qpnp_vib *vib;
const __be32 *temp_dt;
struct resource *vib_resource;
int rc = -ENOMEM;
u8 val;
vib = devm_kzalloc(&spmi->dev, sizeof(*vib), GFP_KERNEL);
if (!vib)
goto gen_err;
vib->spmi = spmi;
temp_dt = of_get_property(spmi->dev.of_node,
"qcom,qpnp-vib-timeout-ms", NULL);
if (temp_dt)
vib->timeout = be32_to_cpu(*temp_dt);
else
vib->timeout = QPNP_VIB_DEFAULT_TIMEOUT;
temp_dt = of_get_property(spmi->dev.of_node,
"qcom,qpnp-vib-vtg-level-mV", NULL);
if (temp_dt)
vib->vtg_level = be32_to_cpu(*temp_dt);
else
vib->vtg_level = QPNP_VIB_DEFAULT_VTG_LVL;
vib->vtg_level /= QPNP_VIB_UV_PER_MV;
vib_resource = spmi_get_resource(spmi, 0, IORESOURCE_MEM, 0);
if (!vib_resource) {
dev_err(&spmi->dev, "Unable to get vibrator base address\n");
rc = -EINVAL;
goto gen_err;
}
vib->base = vib_resource->start;
/* save the control registers values */
rc = qpnp_vib_read_u8(vib, &val, QPNP_VIB_VTG_CTL(vib->base));
if (rc < 0)
goto gen_err;
vib->reg_vtg_ctl = val;
rc = qpnp_vib_read_u8(vib, &val, QPNP_VIB_EN_CTL(vib->base));
if (rc < 0)
goto gen_err;
vib->reg_en_ctl = val;
spin_lock_init(&vib->lock);
INIT_WORK(&vib->work, qpnp_vib_update);
hrtimer_init(&vib->vib_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
vib->vib_timer.function = qpnp_vib_timer_func;
vib->timed_dev.name = "vibrator";
vib->timed_dev.get_time = qpnp_vib_get_time;
vib->timed_dev.enable = qpnp_vib_enable;
rc = timed_output_dev_register(&vib->timed_dev);
if (rc < 0)
goto gen_err;
vib->sysfs_dev.init_name = "qpnp_vib";
rc = device_register(&vib->sysfs_dev);
if (rc < 0) {
dev_err(&spmi->dev, "%s: device_register failed %d\n",
__func__, rc);
goto error_dev_register;
}
rc = device_create_file(&vib->sysfs_dev, &qpnp_vib_attr);
if (rc < 0) {
dev_err(&spmi->dev, "%s: device_create_file failed %d\n",
__func__, rc);
goto error_device_create_file;
}
dev_set_drvdata(&spmi->dev, vib);
vib_dev = vib;
return rc;
error_device_create_file:
device_unregister(&vib->sysfs_dev);
error_dev_register:
timed_output_dev_unregister(&vib->timed_dev);
gen_err:
return rc;
}
