Add an API to set the clock frequency.

2 files changed, 62 insertions, 2 deletions

diff --git a/sys/dev/ofw/ofw_clock.c b/sys/dev/ofw/ofw_clock.c
index ef86d876bed..443b46795ba 100644
--- a/sys/dev/ofw/ofw_clock.c
+++ b/sys/dev/ofw/ofw_clock.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ofw_clock.c,v 1.3 2016/08/22 18:16:58 kettenis Exp $	*/
+/*	$OpenBSD: ofw_clock.c,v 1.4 2016/08/22 19:28:27 kettenis Exp $	*/
 /*
  * Copyright (c) 2016 Mark Kettenis
  *
@@ -74,6 +74,23 @@ clock_get_frequency_cells(uint32_t *cells)
 	return 0;
 }
 
+int
+clock_set_frequency_cells(uint32_t *cells, uint32_t freq)
+{
+	struct clock_device *cd;
+	uint32_t phandle = cells[0];
+
+	LIST_FOREACH(cd, &clock_devices, cd_list) {
+		if (cd->cd_phandle == phandle)
+			break;
+	}
+
+	if (cd && cd->cd_set_frequency)
+		return cd->cd_set_frequency(cd->cd_cookie, &cells[1], freq);
+
+	return -1;
+}
+
 void
 clock_enable_cells(uint32_t *cells, int on)
 {
@@ -176,6 +193,47 @@ clock_get_frequency(int node, const char *name)
 	return clock_get_frequency_idx(node, idx);
 }
 
+int
+clock_set_frequency_idx(int node, int idx, uint32_t freq)
+{
+	uint32_t *clocks;
+	uint32_t *clock;
+	int rv = -1;
+	int len;
+
+	len = OF_getproplen(node, "clocks");
+	if (len <= 0)
+		return 0;
+
+	clocks = malloc(len, M_TEMP, M_WAITOK);
+	OF_getpropintarray(node, "clocks", clocks, len);
+
+	clock = clocks;
+	while (clock && clock < clocks + (len / sizeof(uint32_t))) {
+		if (idx == 0) {
+			rv = clock_set_frequency_cells(clock, freq);
+			break;
+		}
+		clock = clock_next_clock(clock);
+		idx--;
+	}
+
+	free(clocks, M_TEMP, len);
+	return rv;
+}
+
+int
+clock_set_frequency(int node, const char *name, uint32_t freq)
+{
+	int idx;
+
+	idx = clock_index(node, name);
+	if (idx == -1)
+		return 0;
+
+	return clock_set_frequency_idx(node, idx, freq);
+}
+
 void
 clock_do_enable_idx(int node, int idx, int on)
 {
diff --git a/sys/dev/ofw/ofw_clock.h b/sys/dev/ofw/ofw_clock.h
index fdeff05d554..3e56a8ceefb 100644
--- a/sys/dev/ofw/ofw_clock.h
+++ b/sys/dev/ofw/ofw_clock.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ofw_clock.h,v 1.3 2016/08/22 18:16:58 kettenis Exp $	*/
+/*	$OpenBSD: ofw_clock.h,v 1.4 2016/08/22 19:28:27 kettenis Exp $	*/
 /*
  * Copyright (c) 2016 Mark Kettenis
  *
@@ -22,6 +22,7 @@ struct clock_device {
 	int	cd_node;
 	void	*cd_cookie;
 	uint32_t (*cd_get_frequency)(void *, uint32_t *);
+	int	(*cd_set_frequency)(void *, uint32_t *, uint32_t);
 	void	(*cd_enable)(void *, uint32_t *, int);
 
 	LIST_ENTRY(clock_device) cd_list;
@@ -33,6 +34,7 @@ void	clock_register(struct clock_device *);
 
 uint32_t clock_get_frequency(int, const char *);
 uint32_t clock_get_frequency_idx(int, int);
+int	clock_set_frequency(int, const char *, uint32_t);
 void	clock_enable(int, const char *);
 void	clock_enable_idx(int, int);
 void	clock_disable(int, const char *);