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;
}
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;
}
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);
}
