static struct device_t * led_pwm_bl_probe(struct driver_t * drv, struct dtnode_t * n)
{
struct led_pwm_bl_pdata_t * pdat;
struct pwm_t * pwm;
struct led_t * led;
struct device_t * dev;
if(!(pwm = search_pwm(dt_read_string(n, "pwm-name", NULL))))
return NULL;
pdat = malloc(sizeof(struct led_pwm_bl_pdata_t));
if(!pdat)
return NULL;
led = malloc(sizeof(struct led_t));
if(!led)
{
free(pdat);
return NULL;
}
pdat->pwm = pwm;
pdat->regulator = strdup(dt_read_string(n, "regulator-name", NULL));
pdat->period = dt_read_int(n, "pwm-period-ns", 1000 * 1000);
pdat->polarity = dt_read_bool(n, "pwm-polarity", 0);
pdat->from = dt_read_int(n, "pwm-percent-from", 0) * pdat->period / 100;
pdat->to = dt_read_int(n, "pwm-percent-to", 100) * pdat->period / 100;
pdat->brightness = dt_read_int(n, "default-brightness", 0);
led->name = alloc_device_name(dt_read_name(n), dt_read_id(n));
led->set = led_pwm_bl_set,
led->get = led_pwm_bl_get,
led->priv = pdat;
if(pdat->brightness > 0)
regulator_enable(pdat->regulator);
else
regulator_disable(pdat->regulator);
led_pwm_bl_set_brightness(pdat, pdat->brightness);
if(!register_led(&dev, led))
{
regulator_disable(pdat->regulator);
led_pwm_bl_set_brightness(pdat, 0);
if(pdat->regulator)
free(pdat->regulator);
free_device_name(led->name);
free(led->priv);
free(led);
return NULL;
}
dev->driver = drv;
return dev;
}
static bool_t buzzer_pwm_register_buzzer(struct resource_t * res)
{
struct buzzer_pwm_data_t * rdat = (struct buzzer_pwm_data_t *)res->data;
struct buzzer_pwm_pdata_t * pdat;
struct buzzer_t * buzzer;
struct pwm_t * pwm;
char name[64];
pwm = search_pwm(rdat->pwm);
if(!pwm)
return FALSE;
pdat = malloc(sizeof(struct buzzer_pwm_pdata_t));
if(!pdat)
return FALSE;
buzzer = malloc(sizeof(struct buzzer_t));
if(!buzzer)
{
free(pdat);
return FALSE;
}
snprintf(name, sizeof(name), "%s.%d", res->name, res->id);
timer_init(&pdat->timer, buzzer_pwm_timer_function, buzzer);
pdat->beep = queue_alloc();
pdat->frequency = 0;
pdat->polarity = rdat->polarity;
pdat->pwm = pwm;
buzzer->name = strdup(name);
buzzer->init = buzzer_pwm_init;
buzzer->exit = buzzer_pwm_exit;
buzzer->set = buzzer_pwm_set,
buzzer->get = buzzer_pwm_get,
buzzer->beep = buzzer_pwm_beep,
buzzer->suspend = buzzer_pwm_suspend,
buzzer->resume = buzzer_pwm_resume,
buzzer->priv = pdat;
if(register_buzzer(buzzer))
return TRUE;
free(buzzer->priv);
free(buzzer->name);
free(buzzer);
return FALSE;
}
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 struct device_t * servo_pwm_probe(struct driver_t * drv, struct dtnode_t * n)
{
struct servo_pwm_pdata_t * pdat;
struct pwm_t * pwm;
struct servo_t * m;
struct device_t * dev;
if(!(pwm = search_pwm(dt_read_string(n, "pwm-name", NULL))))
return NULL;
pdat = malloc(sizeof(struct servo_pwm_pdata_t));
if(!pdat)
return NULL;
m = malloc(sizeof(struct servo_t));
if(!m)
{
free(pdat);
return NULL;
}
pdat->pwm = pwm;
pdat->period = dt_read_int(n, "pwm-period-ns", 20000 * 1000);
pdat->polarity = dt_read_bool(n, "pwm-polarity", 0);
pdat->from = dt_read_int(n, "pwm-duty-ns-from", 500 * 1000);
pdat->to = dt_read_int(n, "pwm-duty-ns-to", 2500 * 1000);
pdat->range = dt_read_int(n, "rotation-angle-range", 180);
pdat->angle = -360;
m->name = alloc_device_name(dt_read_name(n), dt_read_id(n));
m->enable = servo_pwm_enable;
m->disable = servo_pwm_disable;
m->set = servo_pwm_set;
m->priv = pdat;
servo_pwm_set(m, dt_read_int(n, "default-angle", 0));
if(!register_servo(&dev, m))
{
free_device_name(m->name);
free(m->priv);
free(m);
return NULL;
}
dev->driver = drv;
return dev;
}
static bool_t led_pwm_register_led(struct resource_t * res)
{
struct led_pwm_data_t * rdat = (struct led_pwm_data_t *)res->data;
struct led_pwm_private_data_t * dat;
struct led_t * led;
struct pwm_t * pwm;
char name[64];
pwm = search_pwm(rdat->pwm);
if(!pwm)
return FALSE;
dat = malloc(sizeof(struct led_pwm_private_data_t));
if(!dat)
return FALSE;
led = malloc(sizeof(struct led_t));
if(!led)
{
free(dat);
return FALSE;
}
snprintf(name, sizeof(name), "%s.%d", res->name, res->id);
dat->brightness = 0;
dat->pwm = pwm;
dat->rdat = rdat;
led->name = strdup(name);
led->init = led_pwm_init;
led->exit = led_pwm_exit;
led->set = led_pwm_set,
led->get = led_pwm_get,
led->suspend = led_pwm_suspend,
led->resume = led_pwm_resume,
led->priv = dat;
if(register_led(led))
return TRUE;
free(led->priv);
free(led->name);
free(led);
return FALSE;
}
