void ev3_output_port_unregister(struct lego_port_device *port)
{
struct ev3_output_port_data *data;
/* port can be null if disabled via module parameter */
if (!port)
return;
data = container_of(port, struct ev3_output_port_data, out_port);
pwm_disable(data->pwm);
pwm_put(data->pwm);
hrtimer_cancel(&data->timer);
cancel_work_sync(&data->change_uevent_work);
cancel_work_sync(&data->work);
if (data->motor)
ev3_output_port_unregister_motor(&data->work);
if (port->mode == EV3_OUTPUT_PORT_MODE_RAW)
ev3_output_port_disable_raw_mode(data);
lego_port_unregister(&data->out_port);
ev3_output_port_float(data);
gpio_free_array(data->gpio, ARRAY_SIZE(data->gpio));
put_legoev3_analog(data->analog);
dev_set_drvdata(&port->dev, NULL);
kfree(data);
}
static int sicilia_pwm_power_off(struct pwm_device **dev)
{
pwm_disable(*dev);
pwm_put(*dev);
*dev = NULL;
return 0;
}
struct lego_port_device
*ev3_output_port_register(struct ev3_output_port_platform_data *pdata,
struct device *parent)
{
struct ev3_output_port_data *data;
struct pwm_device *pwm;
int err;
if (WARN(!pdata, "Platform data is required."))
return ERR_PTR(-EINVAL);
data = kzalloc(sizeof(struct ev3_output_port_data), GFP_KERNEL);
if (!data)
return ERR_PTR(-ENOMEM);
data->id = pdata->id;
data->analog = get_legoev3_analog();
if (IS_ERR(data->analog)) {
dev_err(parent, "Could not get legoev3-analog device.\n");
err = PTR_ERR(data->analog);
goto err_request_legoev3_analog;
}
data->gpio[GPIO_PIN1].gpio = pdata->pin1_gpio;
data->gpio[GPIO_PIN1].flags = GPIOF_IN;
data->gpio[GPIO_PIN1].label = "pin1";
data->gpio[GPIO_PIN2].gpio = pdata->pin2_gpio;
data->gpio[GPIO_PIN2].flags = GPIOF_IN;
data->gpio[GPIO_PIN2].label = "pin2";
data->gpio[GPIO_PIN5].gpio = pdata->pin5_gpio;
data->gpio[GPIO_PIN5].flags = GPIOF_IN;
data->gpio[GPIO_PIN5].label = "pin5";
data->gpio[GPIO_PIN5_INT].gpio = pdata->pin5_int_gpio;
data->gpio[GPIO_PIN5_INT].flags = GPIOF_IN;
data->gpio[GPIO_PIN5_INT].label = "pin5_tacho";
data->gpio[GPIO_PIN6_DIR].gpio = pdata->pin6_dir_gpio;
data->gpio[GPIO_PIN6_DIR].flags = GPIOF_IN;
data->gpio[GPIO_PIN6_DIR].label = "pin6";
err = gpio_request_array(data->gpio, ARRAY_SIZE(data->gpio));
if (err) {
dev_err(parent, "Requesting GPIOs failed.\n");
goto err_gpio_request_array;
}
data->out_port.name = ev3_output_port_type.name;
snprintf(data->out_port.port_name, LEGO_PORT_NAME_SIZE, "out%c",
data->id + 'A');
pwm = pwm_get(NULL, data->out_port.port_name);
if (IS_ERR(pwm)) {
dev_err(parent, "Could not get pwm! (%ld)\n", PTR_ERR(pwm));
err = PTR_ERR(pwm);
goto err_pwm_get;
}
err = pwm_config(pwm, 0, NSEC_PER_SEC / 10000);
if (err) {
dev_err(parent,
"Failed to set pwm duty percent and frequency! (%d)\n",
err);
goto err_pwm_config;
}
err = pwm_enable(pwm);
if (err) {
dev_err(parent, "Failed to start pwm! (%d)\n", err);
goto err_pwm_start;
}
/* This lets us set the pwm duty cycle in an atomic context */
pm_runtime_irq_safe(pwm->chip->dev);
data->pwm = pwm;
data->out_port.num_modes = NUM_EV3_OUTPUT_PORT_MODE;
data->out_port.mode_info = legoev3_output_port_mode_info;
data->out_port.set_mode = ev3_output_port_set_mode;
data->out_port.set_device = ev3_output_port_set_device;
data->out_port.get_status = ev3_output_port_get_status;
data->out_port.dc_motor_ops = &ev3_output_port_motor_ops;
data->out_port.context = data;
err = lego_port_register(&data->out_port, &ev3_output_port_type, parent);
if (err) {
dev_err(parent, "Failed to register lego_port_device. (%d)\n",
err);
goto err_lego_port_register;
}
INIT_WORK(&data->change_uevent_work, ev3_output_port_change_uevent_work);
INIT_WORK(&data->work, NULL);
data->con_state = CON_STATE_INIT;
hrtimer_init(&data->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
data->timer.function = ev3_output_port_timer_callback;
hrtimer_start(&data->timer, ktime_set(0, OUTPUT_PORT_POLL_NS),
HRTIMER_MODE_REL);
return &data->out_port;
err_lego_port_register:
pwm_disable(pwm);
err_pwm_start:
err_pwm_config:
pwm_put(pwm);
err_pwm_get:
gpio_free_array(data->gpio, ARRAY_SIZE(data->gpio));
err_gpio_request_array:
put_legoev3_analog(data->analog);
err_request_legoev3_analog:
kfree(data);
return ERR_PTR(err);
}
UBYTE pwm_send(UBYTE *resp, UBYTE len, UBYTE *msg, UBYTE msg_len)
{
UBYTE ret,i,j;
UBYTE message[20];
UBYTE done;
if (msg_len>6)
{
printf("Error: PWM message allows only 7 bytes\n\r");
return 0;
}
//OK we need to build the message
message[0]=0x61;
message[1]=0x6A;
message[2]=0xF1;
for(i=0; i<msg_len; i++)
{
message[3+i]=msg[i];
}
i=3+i;
message[i]=crc(message,i);
i++;
/*
printf("Sending: ");
for(ret=0; ret<i; ret++)
{
printf("%X ",message[ret]);
}
*/
//now we can send message
for(j=0; j<3; j++)
{
ret=pwm_put(message,i,2000);
if (ret==i)
break;
}
if(ret!=i)
{
printf("Error: PWM message sending problems %d\n\r",ret);
return 0;
}
//now lets get response
//delay_us(500);
//printf("calling PWM get\n\r");
//ret=pwm_get(resp,len,2000);
done=0;
i=0;
while(!done)
{
ret=pwm_rx(resp,len,2000);
if (ret>5)
{
if (resp[0]==0x41)
done=1;
}
i++;
if (i>20)
done=1;
}
//ret=pwm_rx(resp,len,2000);
//ret=pwm_rx(resp,len,2000);
/*
printf("\tRx: ");
for(i=0; i<ret; i++)
{
printf("%X ",resp[i]);
}
printf("\n\r");
*/
return ret;
}
static void pwm_release(void)
{
pwm_put(pwm_dev);
}
