devices/grspwrtr: add clkdiv

spacewire/grspwrtr/grspwrtr.c

@@ -39,8 +39,9 @@
 #define LOG_ERROR(fmt, ...) fprintf(stderr, "spacewire-router: " fmt "\n", ##__VA_ARGS__)
 /* clang-format on */
 
-#define SPWRTR_PRIO  3
-#define SPWRTR_PORTS 4
+#define SPWRTR_PRIO         3
+#define SPWRTR_MAX_PHY_PORT 31
+#define SPWRTR_PORT_CNT     (SPWRTR_SPW_CNT + SPWRTR_AMBA_CNT)
 
 #define R_RESERVED(start, end) uint32_t rsvd_##start##_##end[(end - start) / sizeof(uint32_t)] /* reserved address */
 #define BIT(i)                 (1u << (i))                                                     /* bitmask for single bit */
@@ -154,11 +155,11 @@ static struct {
 
 static int spwrtr_setPortMapping(spwrtr_dev_t *dev, uint8_t port, uint32_t enPorts)
 {
-	if (port == 0) {
+	if ((port == 0 || port > SPWRTR_PORT_CNT) && port <= SPWRTR_MAX_PHY_PORT) {
 		return -EINVAL;
 	}
 
-	if ((enPorts >> (SPWRTR_PORTS + 1)) > 0) {
+	if ((enPorts >> (SPWRTR_PORT_CNT + 1)) > 0) {
 		return -EINVAL;
 	}
 
@@ -171,7 +172,7 @@ static int spwrtr_setPortMapping(spwrtr_dev_t *dev, uint8_t port, uint32_t enPor
 
 static int spwrtr_getPortMapping(spwrtr_dev_t *dev, uint8_t port, uint32_t *enPorts)
 {
-	if (port == 0) {
+	if ((port == 0 || port > SPWRTR_PORT_CNT) && port <= SPWRTR_MAX_PHY_PORT) {
 		return -EINVAL;
 	}
 
@@ -194,6 +195,39 @@ static int spwrtr_resetDevice(spwrtr_dev_t *dev)
 }
 
 
+static int spwrtr_setClockDiv(spwrtr_dev_t *dev, uint8_t port, uint8_t clkdiv)
+{
+	if (port == 0 || port > SPWRTR_SPW_CNT) {
+		return -EINVAL;
+	}
+
+	uint32_t pctrl;
+
+	pctrl = dev->mmio->rtr_pctrl[port];
+	pctrl &= ~SPWRTR_PCTRL_RD_MASK;
+	pctrl |= (clkdiv << SPWRTR_PCTRL_RD_SHIFT) & SPWRTR_PCTRL_RD_MASK;
+
+	dev->mmio->rtr_pctrl[port] = pctrl;
+
+	TRACE("port: %d set clockdiv: %d", port, clkdiv);
+
+	return 0;
+}
+
+
+static int spwrtr_getClockDiv(spwrtr_dev_t *dev, uint8_t port, uint8_t *clkdiv)
+{
+	if (port == 0 || port > SPWRTR_SPW_CNT) {
+		return -EINVAL;
+	}
+
+	*clkdiv = (dev->mmio->rtr_pctrl[port] & SPWRTR_PCTRL_RD_MASK) >> SPWRTR_PCTRL_RD_SHIFT;
+	TRACE("port: %d clockdiv: %d", port, *clkdiv);
+
+	return 0;
+}
+
+
 /* Message handling */
 
 
@@ -212,6 +246,13 @@ static void spwrtr_handleDevCtl(msg_t *msg)
 			err = spwrtr_getPortMapping(&spwrtr_common.dev, ictl->task.mapping.port, &octl->val);
 			break;
 
+		case spwrtr_clkdiv_set:
+			err = spwrtr_setClockDiv(&spwrtr_common.dev, ictl->task.clkdiv.port, ictl->task.clkdiv.div);
+			break;
+		case spwrtr_clkdiv_get:
+			err = spwrtr_getClockDiv(&spwrtr_common.dev, ictl->task.clkdiv.port, (uint8_t *)&octl->val);
+			break;
+
 		case spwrtr_reset:
 			err = spwrtr_resetDevice(&spwrtr_common.dev);
 			break;

spacewire/grspwrtr/grspwrtr.h

@@ -20,13 +20,17 @@
 
 typedef struct {
 	/* clang-format off */
-	enum { spwrtr_pmap_set = 0, spwrtr_pmap_get, spwrtr_pctrl, spwrtr_reset } type;
+	enum { spwrtr_pmap_set = 0, spwrtr_pmap_get, spwrtr_clkdiv_set, spwrtr_clkdiv_get, spwrtr_reset } type;
 	/* clang-format on */
 	union {
 		struct {
 			uint8_t port;
 			uint32_t enPorts;
 		} mapping;
+		struct {
+			uint8_t port;
+			uint8_t div;
+		} clkdiv;
 	} task;
 } spwrtr_t;