static int buzzer_pwm_timer_function(struct timer_t * timer, void * data) { struct buzzer_t * buzzer = (struct buzzer_t *)(data); struct buzzer_pwm_pdata_t * pdat = (struct buzzer_pwm_pdata_t *)buzzer->priv; struct beep_param_t * param = queue_pop(pdat->beep); if(!param) { buzzer_pwm_set(buzzer, 0); return 0; } buzzer_pwm_set(buzzer, param->frequency); timer_forward_now(&pdat->timer, ms_to_ktime(param->millisecond)); free(param); return 1; }
static struct device_t * buzzer_pwm_probe(struct driver_t * drv, struct dtnode_t * n) { struct buzzer_pwm_pdata_t * pdat; struct pwm_t * pwm; struct buzzer_t * buzzer; struct device_t * dev; if(!(pwm = search_pwm(dt_read_string(n, "pwm-name", NULL)))) return NULL; pdat = malloc(sizeof(struct buzzer_pwm_pdata_t)); if(!pdat) return NULL; buzzer = malloc(sizeof(struct buzzer_t)); if(!buzzer) { free(pdat); return NULL; } timer_init(&pdat->timer, buzzer_pwm_timer_function, buzzer); pdat->queue = queue_alloc(); pdat->pwm = pwm; pdat->polarity = dt_read_bool(n, "pwm-polarity", 0); pdat->frequency = -1; buzzer->name = alloc_device_name(dt_read_name(n), dt_read_id(n)); buzzer->set = buzzer_pwm_set; buzzer->get = buzzer_pwm_get; buzzer->beep = buzzer_pwm_beep; buzzer->priv = pdat; buzzer_pwm_set(buzzer, 0); if(!register_buzzer(&dev, buzzer)) { timer_cancel(&pdat->timer); queue_free(pdat->queue, iter_queue_node); free_device_name(buzzer->name); free(buzzer->priv); free(buzzer); return NULL; } dev->driver = drv; return dev; }
static void buzzer_pwm_beep(struct buzzer_t * buzzer, int frequency, int millisecond) { struct buzzer_pwm_pdata_t * pdat = (struct buzzer_pwm_pdata_t *)buzzer->priv; struct beep_param_t * param; if((frequency == 0) && (millisecond == 0)) { timer_cancel(&pdat->timer); queue_clear(pdat->beep, iter_beep_param); buzzer_pwm_set(buzzer, 0); return; } param = malloc(sizeof(struct beep_param_t)); if(!param) return; param->frequency = frequency; param->millisecond = millisecond; queue_push(pdat->beep, param); if(queue_avail(pdat->beep) == 1) timer_start_now(&pdat->timer, ms_to_ktime(1)); }