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