Page MenuHomeFreeBSD
Differential D8821 Diff 23015 sys/arm/allwinner/clk/aw_modclk.c

Changeset View

Standalone View

View Options

sys/arm/allwinner/clk/aw_modclk.c

Show First 20 Lines • Show All 62 Lines • ▼ Show 20 Lines
static struct ofw_compat_data compat_data[] = { static struct ofw_compat_data compat_data[] = {
{ "allwinner,sun4i-a10-mod0-clk", 1 }, { "allwinner,sun4i-a10-mod0-clk", 1 },
{ NULL, 0 } { NULL, 0 }
}; };
struct aw_modclk_sc { struct aw_modclk_sc {
device_t clkdev; device_t clkdev;
bus_addr_t reg; bus_addr_t reg;
u_int parent_cnt;
}; };
#define MODCLK_READ(sc, val) CLKDEV_READ_4((sc)->clkdev, (sc)->reg, (val)) #define MODCLK_READ(sc, val) CLKDEV_READ_4((sc)->clkdev, (sc)->reg, (val))
#define MODCLK_WRITE(sc, val) CLKDEV_WRITE_4((sc)->clkdev, (sc)->reg, (val)) #define MODCLK_WRITE(sc, val) CLKDEV_WRITE_4((sc)->clkdev, (sc)->reg, (val))
#define DEVICE_LOCK(sc) CLKDEV_DEVICE_LOCK((sc)->clkdev) #define DEVICE_LOCK(sc) CLKDEV_DEVICE_LOCK((sc)->clkdev)
#define DEVICE_UNLOCK(sc) CLKDEV_DEVICE_UNLOCK((sc)->clkdev) #define DEVICE_UNLOCK(sc) CLKDEV_DEVICE_UNLOCK((sc)->clkdev)
static int static int
Show All 17 Lines
static int static int
aw_modclk_set_mux(struct clknode *clk, int index) aw_modclk_set_mux(struct clknode *clk, int index)
{ {
struct aw_modclk_sc *sc; struct aw_modclk_sc *sc;
uint32_t val; uint32_t val;
sc = clknode_get_softc(clk); sc = clknode_get_softc(clk);
if (index < 0 || index >= sc->parent_cnt) if (index < 0 || index >= clknode_get_parents_num(clk))
return (ERANGE); return (ERANGE);
DEVICE_LOCK(sc); DEVICE_LOCK(sc);
MODCLK_READ(sc, &val); MODCLK_READ(sc, &val);
val &= ~CLK_SRC_SEL; val &= ~CLK_SRC_SEL;
val |= (index << CLK_SRC_SEL_SHIFT); val |= (index << CLK_SRC_SEL_SHIFT);
MODCLK_WRITE(sc, val); MODCLK_WRITE(sc, val);
DEVICE_UNLOCK(sc); DEVICE_UNLOCK(sc);
▲ Show 20 Lines • Show All 41 Lines • ▼ Show 20 Linesaw_modclk_recalc_freq(struct clknode *clk, uint64_t *freq)
return (0); return (0);
} }
static int static int
aw_modclk_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout, aw_modclk_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
int flags, int *stop) int flags, int *stop)
{ {
struct aw_modclk_sc *sc; struct aw_modclk_sc *sc;
struct clknode *parent_clk, *best_parent;
const char **parent_names;
uint32_t val, m, n, src, best_m, best_n, best_src; uint32_t val, m, n, src, best_m, best_n, best_src;
uint64_t cur_freq; uint64_t cur_freq;
int64_t best_diff, cur_diff; int64_t best_diff, cur_diff;
int error; int error;
sc = clknode_get_softc(clk); sc = clknode_get_softc(clk);
best_n = best_m = 0; best_n = best_m = 0;
best_diff = (int64_t)*fout; best_diff = (int64_t)*fout;
best_src = 0; best_src = 0;
for (src = 0; src < sc->parent_cnt; src++) { parent_names = clknode_get_parent_names(clk);
error = clknode_set_parent_by_idx(clk, src); for (src = 0; src < clknode_get_parents_num(clk); src++) {
if (error != 0) parent_clk = clknode_find_by_name(parent_names[src]);
if (parent_clk == NULL)
continue; continue;
error = clknode_get_freq(clknode_get_parent(clk), &fin); error = clknode_get_freq(parent_clk, &fin);
if (error != 0) if (error != 0)
continue; continue;
for (n = 0; n <= CLK_RATIO_N_MAX; n++) for (n = 0; n <= CLK_RATIO_N_MAX; n++)
for (m = 0; m <= CLK_RATIO_M_MAX; m++) { for (m = 0; m <= CLK_RATIO_M_MAX; m++) {
cur_freq = fin / (1 << n) / (m + 1); cur_freq = fin / (1 << n) / (m + 1);
cur_diff = (int64_t)*fout - cur_freq; cur_diff = (int64_t)*fout - cur_freq;
if (cur_diff >= 0 && cur_diff < best_diff) { if (cur_diff >= 0 && cur_diff < best_diff) {
best_src = src; best_src = src;
best_parent = parent_clk;
best_diff = cur_diff; best_diff = cur_diff;
best_m = m; best_m = m;
best_n = n; best_n = n;
} }
} }
} }
if (best_diff == (int64_t)*fout) if (best_diff == (int64_t)*fout)
return (ERANGE); return (ERANGE);
error = clknode_set_parent_by_idx(clk, best_src); error = clknode_get_freq(best_parent, &fin);
if (error != 0) if (error != 0)
return (error); return (error);
error = clknode_get_freq(clknode_get_parent(clk), &fin);
*fout = fin / (1 << best_n) / (best_m + 1);
*stop = 1;
if ((flags & CLK_SET_DRYRUN) != 0)
return (0);
error = clknode_set_parent_by_idx(clk, best_src);
if (error != 0) if (error != 0)
return (error); return (error);
DEVICE_LOCK(sc); DEVICE_LOCK(sc);
MODCLK_READ(sc, &val); MODCLK_READ(sc, &val);
val &= ~(CLK_RATIO_N | CLK_RATIO_M); val &= ~(CLK_RATIO_N | CLK_RATIO_M);
val |= (best_n << CLK_RATIO_N_SHIFT); val |= (best_n << CLK_RATIO_N_SHIFT);
val |= (best_m << CLK_RATIO_M_SHIFT); val |= (best_m << CLK_RATIO_M_SHIFT);
MODCLK_WRITE(sc, val); MODCLK_WRITE(sc, val);
DEVICE_UNLOCK(sc); DEVICE_UNLOCK(sc);
*fout = fin / (1 << best_n) / (best_m + 1);
*stop = 1;
return (0); return (0);
} }
static clknode_method_t aw_modclk_clknode_methods[] = { static clknode_method_t aw_modclk_clknode_methods[] = {
/* Device interface */ /* Device interface */
CLKNODEMETHOD(clknode_init, aw_modclk_init), CLKNODEMETHOD(clknode_init, aw_modclk_init),
CLKNODEMETHOD(clknode_set_gate, aw_modclk_set_gate), CLKNODEMETHOD(clknode_set_gate, aw_modclk_set_gate),
CLKNODEMETHOD(clknode_set_mux, aw_modclk_set_mux), CLKNODEMETHOD(clknode_set_mux, aw_modclk_set_mux),
▲ Show 20 Lines • Show All 71 Lines • ▼ Show 20 Lines if (clk == NULL) {
device_printf(dev, "cannot create clknode\n"); device_printf(dev, "cannot create clknode\n");
error = ENXIO; error = ENXIO;
goto fail; goto fail;
} }
sc = clknode_get_softc(clk); sc = clknode_get_softc(clk);
sc->reg = paddr; sc->reg = paddr;
sc->clkdev = device_get_parent(dev); sc->clkdev = device_get_parent(dev);
sc->parent_cnt = def.parent_cnt;
clknode_register(clkdom, clk); clknode_register(clkdom, clk);
if (clkdom_finit(clkdom) != 0) { if (clkdom_finit(clkdom) != 0) {
device_printf(dev, "cannot finalize clkdom initialization\n"); device_printf(dev, "cannot finalize clkdom initialization\n");
error = ENXIO; error = ENXIO;
goto fail; goto fail;
} }
Show All 28 Lines