static void secvib_enable(struct timed_output_dev *dev, int value)
{
struct secvib_data *secvib =
container_of(dev, struct secvib_data, dev);
pr_debug("secvib: vibrator force:%d\n", value);
mutex_lock(&secvib->lock);
/* cancel previous timer and set GPIO according to value */
hrtimer_cancel(&secvib->timer);
if (value) {
secvib_vibrator_enable(secvib, 0, 1);
if (value > 0) {
if (value > secvib->max_timeout)
value = secvib->max_timeout;
hrtimer_start(&secvib->timer,
ktime_set(value / 1000,
(value % 1000) * 1000000),
HRTIMER_MODE_REL);
}
} else
secvib_vibrator_enable(secvib, 0, 0);
mutex_unlock(&secvib->lock);
}
static int secvib_forceout_set_samples(struct secvib_data *secvib,
u_int8_t actr_index,
u_int16_t bit_depth,
u_int16_t bufsize,
int8_t *buf)
{
int8_t vib_force;
static int8_t pre_vib_force;
switch (bit_depth) {
case 8:
/* buf is expected to contain 1 byte */
if (bufsize != 1) {
pr_err("secvib: wrong bufsize = %d\n", bufsize);
goto error_vibe;
}
vib_force = buf[0];
break;
case 16:
/* buf is expected to contain 2 byte */
if (bufsize != 2)
goto error_vibe;
/* Map 16-bit value to 8-bit */
vib_force = ((int16_t *)buf)[0] >> 8;
break;
default:
/* Unexpected bit depth */
goto error_vibe;
}
if (vib_force == 0)
/* Set 50% duty cycle or disable amp */
secvib_vibrator_enable(secvib, actr_index, 0);
else if (pre_vib_force != vib_force) {
secvib->pdata->pwm_set(actr_index, vib_force);
secvib_vibrator_enable(secvib, actr_index, 1);
}
pre_vib_force = vib_force;
return 0;
error_vibe:
return -1;
}
static void secvib_timer_worker(struct work_struct *work)
{
struct secvib_data *secvib =
container_of(work, struct secvib_data, vib_work);
secvib_vibrator_enable(secvib, 0, 0);
}
static enum hrtimer_restart secvib_timer_func(struct hrtimer *timer)
{
struct secvib_data *secvib =
container_of(timer, struct secvib_data, timer);
secvib_vibrator_enable(secvib, 0, 0);
return HRTIMER_NORESTART;
}
/* ioctl - I/O control */
static long secvib_ioctl(struct file *file,
unsigned int cmd, unsigned long arg)
{
int ret;
struct secvib_data *secvib =
container_of(file->private_data, struct secvib_data, miscdev);
pr_debug("secvib: %s[%d]\n", __func__, __LINE__);
mutex_lock(&secvib->lock);
switch (cmd) {
case TSPDRV_STOP_KERNEL_TIMER:
/*
* As we send one sample ahead of time, we need
* to finish playing the last sample
* before stopping the timer. So we just set a flag here.
*/
if (secvib->enabled == true)
secvib->stop_requested = true;
/* Last data processing to disable amp and stop timer */
ret = secvib_process_vib_data(secvib);
break;
case TSPDRV_MAGIC_NUMBER:
secvib->magic_no = TSPDRV_MAGIC_NUMBER;
ret = 0;
break;
case TSPDRV_ENABLE_AMP:
ret = secvib_vibrator_enable(secvib, arg, 1);
pr_debug("secvib: vibrator enabled\n");
break;
case TSPDRV_DISABLE_AMP:
/*
* Small fix for now to handle proper combination
* of TSPDRV_STOP_KERNEL_TIMER and
* TSPDRV_DISABLE_AMP together
* If a stop was requested, ignore the request as
* the amp will be disabled by the timer
* proc when it's ready
*/
if (!secvib->stop_requested)
ret = secvib_vibrator_enable(secvib, arg, 0);
else
ret = 0;
break;
case TSPDRV_GET_NUM_ACTUATORS:
ret = secvib->pdata->num_actuators;
break;
default:
ret = -EINVAL;
break;
}
mutex_unlock(&secvib->lock);
return ret;
}
static int secvib_process_vib_data(struct secvib_data *secvib)
{
int ret, i;
int actr_not_playing = 0;
for (i = 0; i < secvib->pdata->num_actuators; i++) {
struct actuator_samples_buffer *tmp = &(secvib->actr_buf[i]);
if (tmp->playingbuf_idx == -1) {
actr_not_playing++;
if ((secvib->pdata->num_actuators == actr_not_playing)
&& ((++secvib->wtchdg_cnt)
> WATCHDOG_TIMEOUT)) {
int8_t zero[1] = {0};
/*
* Nothing to play for all actuators, turn off
* the timer when we reach the watchdog tick
* count limit
*/
secvib_forceout_set_samples(secvib,
i, 8, 1, zero);
secvib_vibrator_enable(secvib, i, 0);
secvib_stop_haptic_timer(secvib);
/* Reset watchdog counter */
secvib->wtchdg_cnt = 0;
}
} else {
int8_t idx = (int)tmp->playingbuf_idx;
/* Play the current buffer */
ret = secvib_forceout_set_samples(
secvib,
tmp->samples_buf[idx].actr_index,
tmp->samples_buf[idx].bit_depth,
tmp->samples_buf[idx].bufsize,
tmp->samples_buf[idx].buf);
if (unlikely(ret < 0))
hrtimer_forward_now(&secvib->haptic_timer,
ktime_set(0, 5000000));
tmp->outputval_idx += tmp->samples_buf[idx].bufsize;
if (tmp->outputval_idx
>= tmp->samples_buf[idx].bufsize) {
/* Reach the end of the current buffer */
tmp->samples_buf[idx].bufsize = 0;
/* Switch buffer */
tmp->playingbuf_idx ^= 1;
tmp->outputval_idx = 0;
/* Finished playing, disable amp for actuator */
if (secvib->stop_requested) {
tmp->playingbuf_idx = -1;
secvib_vibrator_enable(secvib, i, 0);
}
}
}
}
/* If finished playing, stop timer */
if (secvib->stop_requested) {
secvib_stop_haptic_timer(secvib);
/* Reset watchdog counter */
secvib->wtchdg_cnt = 0;
if (secvib_is_vib_sem(&secvib->vib_sem))
up(&secvib->vib_sem);
/* tell the caller this is the last iteration */
return 1;
}
return 0;
}
