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);
}
int ms_nxtmmx_register_out_port(struct ms_nxtmmx_data *mmx)
{
struct lego_port_device *port = &mmx->port;
int err;
port->name = ms_nxtmmx_out_port_type.name;
strncpy(port->address, mmx->address, LEGO_NAME_SIZE);
port->num_modes = NUM_MS_NXTMMX_OUT_PORT_MODES;
port->supported_modes = LEGO_PORT_ALL_MODES;
port->mode_info = ms_nxtmmx_out_port_mode_info;
port->set_mode = ms_nxtmmx_out_port_set_mode;
port->tacho_motor_ops = &ms_nxtmmx_tacho_motor_ops;
port->context = mmx;
err = lego_port_register(port, &ms_nxtmmx_out_port_type, &mmx->i2c_client->dev);
if (err)
return err;
err = ms_nxtmmx_out_port_set_mode(mmx, MS_NXTMMX_OUT_PORT_MODE_TACHO_MOTOR);
if (err) {
lego_port_unregister(port);
return err;
}
return 0;
}
int brickpi_register_in_ports(struct brickpi_channel_data *ch_data,
struct device *parent)
{
int i, err;
for (i = 0; i < NUM_BRICKPI_PORT; i++) {
struct lego_port_device *port = &ch_data->in_port[i].port;
port->name = brickpi_in_port_type.name;
snprintf(port->port_name, LEGO_PORT_NAME_SIZE, "%s:in%d",
dev_name(parent), ch_data->address * 2 + i - 1);
port->num_modes = NUM_BRICKPI_IN_PORT_MODES;
port->mode_info = brickpi_in_port_mode_info;
port->set_mode = brickpi_in_port_set_mode;
port->set_device = brickpi_in_port_set_device;
port->context = &ch_data->in_port[i];
port->nxt_analog_ops = &brickpi_in_port_nxt_analog_ops;
//port->nxt_i2c_ops = &brickpi_in_port_nxt_i2c_ops;
err = lego_port_register(port, &brickpi_in_port_type, parent);
if (err) {
dev_err(parent,
"Failed to register BrickPi input port. (%d)\n",
err);
for (i--; i >= 0; i--)
lego_port_unregister(port);
return err;
}
}
return 0;
}
void brickpi_unregister_in_ports(struct brickpi_channel_data *ch_data)
{
int i;
for (i = 0; i < NUM_BRICKPI_PORT; i++) {
brickpi_in_port_unregister_sensor(&ch_data->in_port[i]);
lego_port_unregister(&ch_data->in_port[i].port);
}
}
int brickpi_register_in_ports(struct brickpi_channel_data *ch_data,
struct device *parent)
{
int i, err;
for (i = 0; i < NUM_BRICKPI_PORT; i++) {
struct lego_port_device *port = &ch_data->in_port[i].port;
port->name = brickpi_in_port_type.name;
snprintf(port->address, LEGO_NAME_SIZE, "%s:S%d",
dev_name(parent), ch_data->address * 2 + i - 1);
port->num_modes = NUM_BRICKPI_IN_PORT_MODES;
port->supported_modes = BIT(BRICKPI_IN_PORT_MODE_NONE)
| BIT(BRICKPI_IN_PORT_MODE_NXT_ANALOG)
| BIT(BRICKPI_IN_PORT_MODE_NXT_COLOR)
| BIT(BRICKPI_IN_PORT_MODE_NXT_I2C);
/* only firmware version 2 supports EV3 sensors */
if (ch_data->fw_version >= 2) {
port->supported_modes |= BIT(BRICKPI_IN_PORT_MODE_EV3_ANALOG)
| BIT(BRICKPI_IN_PORT_MODE_EV3_UART);
}
port->mode_info = brickpi_in_port_mode_info;
port->set_mode = brickpi_in_port_set_mode;
port->set_device = brickpi_in_port_set_device;
port->ev3_uart_ops = &brickpi_ev3_uart_ops;
port->context = &ch_data->in_port[i];
port->nxt_analog_ops = &brickpi_in_port_nxt_analog_ops;
//port->nxt_i2c_ops = &brickpi_in_port_nxt_i2c_ops;
err = lego_port_register(port, &brickpi_in_port_type, parent);
if (err) {
dev_err(parent,
"Failed to register BrickPi input port. (%d)\n",
err);
for (i--; i >= 0; i--)
lego_port_unregister(port);
return err;
}
}
return 0;
}
void ms_nxtmmx_unregister_out_port(struct ms_nxtmmx_data *mmx)
{
ms_nxtmmx_out_port_unregister_motor(mmx);
lego_port_unregister(&mmx->port);
}
