1 files changed, 20 insertions, 109 deletions

diff --git a/openpcd/firmware/src/usb_handler.c b/openpcd/firmware/src/usb_handler.c
index bb5f5d4..4252369 100644
--- a/openpcd/firmware/src/usb_handler.c
+++ b/openpcd/firmware/src/usb_handler.c
@@ -9,132 +9,43 @@
 #include <openpcd.h>
 
 #include "pcd_enumerate.h"
+#include "usb_handler.h"
 #include "openpcd.h"
 #include "rc632.h"
 #include "led.h"
 #include "dbgu.h"
 
-#define MAX_PAYLOAD_LEN	(64 - sizeof(struct openpcd_hdr))
+static usb_cmd_fn *cmd_hdlrs[16];
+
+int usb_hdlr_register(usb_cmd_fn *hdlr, u_int8_t class)
+{
+	cmd_hdlrs[class] = hdlr;
+}
+
+void usb_hdlr_unregister(u_int8_t class)
+{
+	cmd_hdlrs[class] = NULL;
+}
 
 static int usb_in(struct req_ctx *rctx)
 {
 	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
 	struct openpcd_hdr *pih = (struct openpcd_hdr *) &rctx->tx.data[0];
-	u_int16_t len = rctx->rx.tot_len;
+	usb_cmd_fn *hdlr;
 
-	DEBUGP("usb_in ");
+	DEBUGP("usb_in(cls=%d) ", OPENPCD_CMD_CLS(poh->cmd));
 
-	if (len < sizeof(*poh))
+	if (rctx->rx.tot_len < sizeof(*poh))
 		return -EINVAL;
 	
-	len -= sizeof(*poh);
-
 	memcpy(pih, poh, sizeof(*poh));
 	rctx->tx.tot_len = sizeof(*poh);
 
-	switch (poh->cmd) {
-	case OPENPCD_CMD_READ_REG:
-		rc632_reg_read(RAH, poh->reg, &pih->val);
-		DEBUGP("READ REG(0x%02x)=0x%02x ", poh->reg, pih->val);
-		goto respond;
-		break;
-	case OPENPCD_CMD_READ_FIFO:
-		{
-		u_int16_t req_len = poh->val, remain_len, pih_len;
-		if (req_len > MAX_PAYLOAD_LEN) {
-			pih_len = MAX_PAYLOAD_LEN;
-			remain_len -= pih_len;
-			rc632_fifo_read(RAH, pih_len, pih->data);
-			rctx->tx.tot_len += pih_len;
-			DEBUGP("READ FIFO(len=%u)=%s ", req_len,
-				hexdump(pih->data, pih_len));
-			req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
-			udp_refill_ep(2, rctx);
-
-			/* get and initialize second rctx */
-			rctx = req_ctx_find_get(RCTX_STATE_FREE,
-						RCTX_STATE_MAIN_PROCESSING);
-			if (!rctx) {
-				DEBUGPCRF("FATAL_NO_RCTX!!!\n");
-				break;
-			}
-			poh = (struct openpcd_hdr *) &rctx->rx.data[0];
-			pih = (struct openpcd_hdr *) &rctx->tx.data[0];
-			memcpy(pih, poh, sizeof(*poh));
-			rctx->tx.tot_len = sizeof(*poh);
-
-			pih_len = remain_len;
-			rc632_fifo_read(RAH, pih_len, pih->data);
-			rctx->tx.tot_len += pih_len;
-			DEBUGP("READ FIFO(len=%u)=%s ", pih_len,
-				hexdump(pih->data, pih_lenl));
-			/* don't set state of second rctx, main function
-			 * body will do this after switch statement */
-		} else {
-			pih_len = poh->val;
-			rc632_fifo_read(RAH, poh->val, pih->data);
-			rctx->tx.tot_len += pih_len;
-			DEBUGP("READ FIFO(len=%u)=%s ", poh->val,
-				hexdump(pih->data, poh->val));
-		}
-		goto respond;
-		break;
-		}
-	case OPENPCD_CMD_WRITE_REG:
-		DEBUGP("WRITE_REG(0x%02x, 0x%02x) ", poh->reg, poh->val);
-		rc632_reg_write(RAH, poh->reg, poh->val);
-		break;
-	case OPENPCD_CMD_WRITE_FIFO:
-		DEBUGP("WRITE FIFO(len=%d) ", len);
-		if (len <= 0)
-			return -EINVAL;
-		rc632_fifo_write(RAH, len, poh->data, 0);
-		break;
-	case OPENPCD_CMD_READ_VFIFO:
-		DEBUGP("READ VFIFO ");
-		DEBUGP("NOT IMPLEMENTED YET ");
-		goto respond;
-		break;
-	case OPENPCD_CMD_WRITE_VFIFO:
-		DEBUGP("WRITE VFIFO(len=%d) ", len);
-		if (len <= 0)
-			return -EINVAL;
-		DEBUGP("NOT IMPLEMENTED YET ");
-		break;
-	case OPENPCD_CMD_REG_BITS_CLEAR:
-		DEBUGP("CLEAR BITS ");
-		pih->val = rc632_clear_bits(RAH, poh->reg, poh->val);
-		break;
-	case OPENPCD_CMD_REG_BITS_SET:
-		DEBUGP("SET BITS ");
-		pih->val = rc632_set_bits(RAH, poh->reg, poh->val);
-		break;
-	case OPENPCD_CMD_SET_LED:
-		DEBUGP("SET LED(%u,%u) ", poh->reg, poh->val);
-		led_switch(poh->reg, poh->val);
-		break;
-	case OPENPCD_CMD_DUMP_REGS:
-		DEBUGP("DUMP REGS ");
-		DEBUGP("NOT IMPLEMENTED YET ");
-		goto respond;
-		break;
-	default:
-		/* FIXME: add a method how other firmware modules can
-		 * register callback functions for additional commands */
-		DEBUGP("UNKNOWN ");
-		return -EINVAL;
-		break;
-	}
-	req_ctx_put(rctx);
-	return 0;
-
-respond:
-	req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
-	/* FIXME: we could try to send this immediately */
-	udp_refill_ep(2, rctx);
-	DEBUGPCR("");
-
-	return 1;
+	hdlr = cmd_hdlrs[OPENPCD_CMD_CLS(poh->cmd)];
+	if (hdlr) 
+		return (hdlr)(rctx);
+	else
+		DEBUGPCR("no handler for this class\n");
 }
 
 void usb_out_process(void)