+/* sm501_calc_clock
+ *
+ * Calculates the nearest discrete clock frequency that
+ * can be achieved with the specified input clock.
+ * the maximum divisor is 3 or 5
+ */
+
+static int sm501_calc_clock(unsigned long freq,
+ struct sm501_clock *clock,
+ int max_div,
+ unsigned long mclk,
+ long *best_diff)
+{
+ int ret = 0;
+ int divider;
+ int shift;
+ long diff;
+
+ /* try dividers 1 and 3 for CRT and for panel,
+ try divider 5 for panel only.*/
+
+ for (divider = 1; divider <= max_div; divider += 2) {
+ /* try all 8 shift values.*/
+ for (shift = 0; shift < 8; shift++) {
+ /* Calculate difference to requested clock */
+ diff = sm501fb_round_div(mclk, divider << shift) - freq;
+ if (diff < 0)
+ diff = -diff;
+
+ /* If it is less than the current, use it */
+ if (diff < *best_diff) {
+ *best_diff = diff;
+
+ clock->mclk = mclk;
+ clock->divider = divider;
+ clock->shift = shift;
+ ret = 1;
+ }
+ }
+ }
+
+ return ret;
+}
+
+/* sm501_calc_pll
+ *
+ * Calculates the nearest discrete clock frequency that can be
+ * achieved using the programmable PLL.
+ * the maximum divisor is 3 or 5
+ */
+
+static unsigned long sm501_calc_pll(unsigned long freq,
+ struct sm501_clock *clock,
+ int max_div)
+{
+ unsigned long mclk;
+ unsigned int m, n, k;
+ long best_diff = 999999999;
+
+ /*
+ * The SM502 datasheet doesn't specify the min/max values for M and N.
+ * N = 1 at least doesn't work in practice.
+ */
+ for (m = 2; m <= 255; m++) {
+ for (n = 2; n <= 127; n++) {
+ for (k = 0; k <= 1; k++) {
+ mclk = (24000000UL * m / n) >> k;
+
+ if (sm501_calc_clock(freq, clock, max_div,
+ mclk, &best_diff)) {
+ clock->m = m;
+ clock->n = n;
+ clock->k = k;
+ }
+ }
+ }
+ }
+
+ /* Return best clock. */
+ return clock->mclk / (clock->divider << clock->shift);
+}
+