static int ev3_motor_probe(struct lego_device *ldev)
{
struct ev3_motor_data *data;
int err;
if (!ldev->port->tacho_motor_ops) {
dev_err(&ldev->dev, "Port '%s' does not support tacho motor.",
ldev->port->port_name);
return -EINVAL;
}
data = kzalloc(sizeof(struct ev3_motor_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
data->tm.driver_name = ldev->name;
data->tm.port_name = ldev->port->port_name;
data->tm.ops = ldev->port->tacho_motor_ops;
data->tm.context = ldev->port->context;
dev_set_drvdata(&ldev->dev, data);
err = register_tacho_motor(&data->tm, &ldev->dev);
if (err)
goto err_register_tacho_motor;
return 0;
err_register_tacho_motor:
dev_set_drvdata(&ldev->dev, NULL);
kfree(data);
return err;
}
int ms_nxtmmx_probe_cb(struct nxt_i2c_sensor_data *data)
{
struct ms_nxtmmx_data *mmx;
int i, err;
mmx = kzalloc(sizeof(struct ms_nxtmmx_data) * 2, GFP_KERNEL);
if (!mmx)
return -ENOMEM;
data->callback_data = mmx;
for (i = 0; i < 2; i++) {
mmx[i].tm.driver_name = data->info->name;
snprintf(mmx[i].port_name, LEGO_PORT_NAME_SIZE, "%s:i2c%d:mux%d",
data->port_name, data->client->addr, i + 1);
mmx[i].tm.port_name = mmx[i].port_name;
mmx[i].tm.ops = &ms_nxtmmx_tacho_motor_ops;
mmx[i].tm.context = &mmx[i];
mmx[i].i2c = data;
mmx[i].index = i;
mmx[i].command_flags = CMD_FLAGS_DEFAULT_VALUE;
}
err = register_tacho_motor(&mmx[0].tm, &data->client->dev);
if (err)
goto err_register_tacho_motor0;
err = register_tacho_motor(&mmx[1].tm, &data->client->dev);
if (err)
goto err_register_tacho_motor1;
data->poll_ms = 1000;
return 0;
err_register_tacho_motor1:
unregister_tacho_motor(&mmx[0].tm);
err_register_tacho_motor0:
data->callback_data = NULL;
kfree(mmx);
return err;
}