mutex_lock(&ocelot->tas_lock);
if (!taprio->enable) {
+ ocelot_port_mqprio(ocelot, port, &taprio->mqprio);
ocelot_rmw_rix(ocelot, 0, QSYS_TAG_CONFIG_ENABLE,
QSYS_TAG_CONFIG, port);
return 0;
}
+ ret = ocelot_port_mqprio(ocelot, port, &taprio->mqprio);
+ if (ret)
+ goto err_unlock;
+
if (taprio->cycle_time > NSEC_PER_SEC ||
taprio->cycle_time_extension >= NSEC_PER_SEC) {
ret = -EINVAL;
- goto err;
+ goto err_reset_tc;
}
if (taprio->num_entries > VSC9959_TAS_GCL_ENTRY_MAX) {
ret = -ERANGE;
- goto err;
+ goto err_reset_tc;
}
/* Enable guard band. The switch will schedule frames without taking
val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
if (val & QSYS_PARAM_STATUS_REG_8_CONFIG_PENDING) {
ret = -EBUSY;
- goto err;
+ goto err_reset_tc;
}
ocelot_rmw_rix(ocelot,
!(val & QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE),
10, 100000);
if (ret)
- goto err;
+ goto err_reset_tc;
ocelot_port->taprio = taprio_offload_get(taprio);
vsc9959_tas_guard_bands_update(ocelot, port);
-err:
+ mutex_unlock(&ocelot->tas_lock);
+
+ return 0;
+
+err_reset_tc:
+ taprio->mqprio.qopt.num_tc = 0;
+ ocelot_port_mqprio(ocelot, port, &taprio->mqprio);
+err_unlock:
mutex_unlock(&ocelot->tas_lock);
return ret;