+struct tegra_sor_params {
+ /* number of link clocks per line */
+ unsigned int num_clocks;
+ /* ratio between input and output */
+ u64 ratio;
+ /* precision factor */
+ u64 precision;
+
+ unsigned int active_polarity;
+ unsigned int active_count;
+ unsigned int active_frac;
+ unsigned int tu_size;
+ unsigned int error;
+};
+
+static int tegra_sor_compute_params(struct tegra_sor *sor,
+ struct tegra_sor_params *params,
+ unsigned int tu_size)
+{
+ u64 active_sym, active_count, frac, approx;
+ u32 active_polarity, active_frac = 0;
+ const u64 f = params->precision;
+ s64 error;
+
+ active_sym = params->ratio * tu_size;
+ active_count = div_u64(active_sym, f) * f;
+ frac = active_sym - active_count;
+
+ /* fraction < 0.5 */
+ if (frac >= (f / 2)) {
+ active_polarity = 1;
+ frac = f - frac;
+ } else {
+ active_polarity = 0;
+ }
+
+ if (frac != 0) {
+ frac = div_u64(f * f, frac); /* 1/fraction */
+ if (frac <= (15 * f)) {
+ active_frac = div_u64(frac, f);
+
+ /* round up */
+ if (active_polarity)
+ active_frac++;
+ } else {
+ active_frac = active_polarity ? 1 : 15;
+ }
+ }
+
+ if (active_frac == 1)
+ active_polarity = 0;
+
+ if (active_polarity == 1) {
+ if (active_frac) {
+ approx = active_count + (active_frac * (f - 1)) * f;
+ approx = div_u64(approx, active_frac * f);
+ } else {
+ approx = active_count + f;
+ }
+ } else {
+ if (active_frac)
+ approx = active_count + div_u64(f, active_frac);
+ else
+ approx = active_count;
+ }
+
+ error = div_s64(active_sym - approx, tu_size);
+ error *= params->num_clocks;
+
+ if (error <= 0 && abs64(error) < params->error) {
+ params->active_count = div_u64(active_count, f);
+ params->active_polarity = active_polarity;
+ params->active_frac = active_frac;
+ params->error = abs64(error);
+ params->tu_size = tu_size;
+
+ if (error == 0)
+ return true;
+ }
+
+ return false;
+}
+
+static int tegra_sor_calc_config(struct tegra_sor *sor,
+ struct drm_display_mode *mode,
+ struct tegra_sor_config *config,
+ struct drm_dp_link *link)
+{
+ const u64 f = 100000, link_rate = link->rate * 1000;
+ const u64 pclk = mode->clock * 1000;
+ u64 input, output, watermark, num;
+ struct tegra_sor_params params;
+ u32 num_syms_per_line;
+ unsigned int i;
+
+ if (!link_rate || !link->num_lanes || !pclk || !config->bits_per_pixel)
+ return -EINVAL;
+
+ output = link_rate * 8 * link->num_lanes;
+ input = pclk * config->bits_per_pixel;
+
+ if (input >= output)
+ return -ERANGE;
+
+ memset(¶ms, 0, sizeof(params));
+ params.ratio = div64_u64(input * f, output);
+ params.num_clocks = div_u64(link_rate * mode->hdisplay, pclk);
+ params.precision = f;
+ params.error = 64 * f;
+ params.tu_size = 64;
+
+ for (i = params.tu_size; i >= 32; i--)
+ if (tegra_sor_compute_params(sor, ¶ms, i))
+ break;
+
+ if (params.active_frac == 0) {
+ config->active_polarity = 0;
+ config->active_count = params.active_count;
+
+ if (!params.active_polarity)
+ config->active_count--;
+
+ config->tu_size = params.tu_size;
+ config->active_frac = 1;
+ } else {
+ config->active_polarity = params.active_polarity;
+ config->active_count = params.active_count;
+ config->active_frac = params.active_frac;
+ config->tu_size = params.tu_size;
+ }
+
+ dev_dbg(sor->dev,
+ "polarity: %d active count: %d tu size: %d active frac: %d\n",
+ config->active_polarity, config->active_count,
+ config->tu_size, config->active_frac);
+
+ watermark = params.ratio * config->tu_size * (f - params.ratio);
+ watermark = div_u64(watermark, f);
+
+ watermark = div_u64(watermark + params.error, f);
+ config->watermark = watermark + (config->bits_per_pixel / 8) + 2;
+ num_syms_per_line = (mode->hdisplay * config->bits_per_pixel) *
+ (link->num_lanes * 8);
+
+ if (config->watermark > 30) {
+ config->watermark = 30;
+ dev_err(sor->dev,
+ "unable to compute TU size, forcing watermark to %u\n",
+ config->watermark);
+ } else if (config->watermark > num_syms_per_line) {
+ config->watermark = num_syms_per_line;
+ dev_err(sor->dev, "watermark too high, forcing to %u\n",
+ config->watermark);
+ }
+
+ /* compute the number of symbols per horizontal blanking interval */
+ num = ((mode->htotal - mode->hdisplay) - 7) * link_rate;
+ config->hblank_symbols = div_u64(num, pclk);
+
+ if (link->capabilities & DP_LINK_CAP_ENHANCED_FRAMING)
+ config->hblank_symbols -= 3;
+
+ config->hblank_symbols -= 12 / link->num_lanes;
+
+ /* compute the number of symbols per vertical blanking interval */
+ num = (mode->hdisplay - 25) * link_rate;
+ config->vblank_symbols = div_u64(num, pclk);
+ config->vblank_symbols -= 36 / link->num_lanes + 4;
+
+ dev_dbg(sor->dev, "blank symbols: H:%u V:%u\n", config->hblank_symbols,
+ config->vblank_symbols);
+
+ return 0;
+}
+