- move usb incoming packet handling into respective subsystems

git-svn-id: https://svn.openpcd.org:2342/trunk@107 6dc7ffe9-61d6-0310-9af1-9938baff3ed1

10 files changed, 276 insertions, 131 deletions

diff --git a/openpcd/firmware/Makefile b/openpcd/firmware/Makefile
index a77662e..d5548eb 100644
--- a/openpcd/firmware/Makefile
+++ b/openpcd/firmware/Makefile
@@ -155,7 +155,7 @@ CSTANDARD = -std=gnu99
 CDEFS =  -D$(RUN_MODE) -D__MS_types__ -D__LIBRFID__ 
 
 ifdef DEBUG
-CDEF += -DDEBUG 
+CDEFS += -DDEBUG 
 endif
 
 ifdef OLIMEX
diff --git a/openpcd/firmware/include/openpcd.h b/openpcd/firmware/include/openpcd.h
index dee30c6..61bc205 100644
--- a/openpcd/firmware/include/openpcd.h
+++ b/openpcd/firmware/include/openpcd.h
@@ -6,29 +6,50 @@
 #include <sys/types.h>
 
 struct openpcd_hdr {
-	u_int8_t cmd;		/* command */
+	u_int8_t cmd;		/* command. high nibble: class, low nibble: cmd */
 	u_int8_t flags;
 	u_int8_t reg;		/* register */
 	u_int8_t val;		/* value (in case of write *) */
 	u_int8_t data[0];
 } __attribute__ ((packed));
 
-#define OPENPCD_REG_MAX			0x3f
-
-#define OPENPCD_CMD_WRITE_REG		0x01
-#define OPENPCD_CMD_WRITE_FIFO		0x02
-#define OPENPCD_CMD_WRITE_VFIFO		0x03
-#define OPENPCD_CMD_REG_BITS_CLEAR	0x04
-#define OPENPCD_CMD_REG_BITS_SET	0x05
-
-#define OPENPCD_CMD_READ_REG		0x11
-#define OPENPCD_CMD_READ_FIFO		0x12
-#define OPENPCD_CMD_READ_VFIFO		0x13
-#define OPENPCD_CMD_DUMP_REGS		0x14
-
-#define OPENPCD_CMD_SET_LED		0x21
-
-#define OPENPCD_CMD_IRQ			0x40	/* IRQ reported by RC632 */
+enum openpcd_cmd_class {
+	OPENPCD_CMD_CLS_RC632		= 0x1,
+	OPENPCD_CMD_CLS_LED		= 0x2,
+	OPENPCD_CMD_CLS_SSC		= 0x3,
+	OPENPCD_CMD_CLS_PWM		= 0x4,
+};
+
+#define OPENPCD_CMD_CLS(x)	(x >> 4)
+#define OPENPCD_CMD(x)		(x & 0xf)
+
+#define OPENPCD_CLS2CMD(x)	(x << 4)
+
+/* CMD_CLS_RC632 */
+#define OPENPCD_CMD_WRITE_REG		(0x1|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_WRITE_FIFO		(0x2|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_WRITE_VFIFO		(0x3|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_REG_BITS_CLEAR	(0x4|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_REG_BITS_SET	(0x5|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_READ_REG		(0x6|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_READ_FIFO		(0x7|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_READ_VFIFO		(0x8|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_DUMP_REGS		(0x9|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+#define OPENPCD_CMD_IRQ			(0xa|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_RC632))
+
+/* CMD_CLS_LED */
+#define OPENPCD_CMD_SET_LED		(0x1|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_LED))
+
+/* CMD_CLS_SSC */
+#define OPENPCD_CMD_SSC_READ		(0x1|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_SSC))
+#define OPENPCD_CMD_SSC_WRITE		(0x2|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_SSC))
+
+/* CMD_CLS_PWM */
+#define OPENPCD_CMD_PWM_ENABLE		(0x1|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_PWM))
+#define OPENPCD_CMD_PWM_DUTY_SET	(0x2|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_PWM))
+#define OPENPCD_CMD_PWM_DUTY_GET	(0x3|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_PWM))
+#define OPENPCD_CMD_PWM_FREQ_SET	(0x4|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_PWM))
+#define OPENPCD_CMD_PWM_FREQ_GET	(0x5|OPENPCD_CLS2CMD(OPENPCD_CMD_CLS_PWM))
 
 #define OPENPCD_VENDOR_ID	0x2342
 #define OPENPCD_PRODUCT_ID	0x0001
diff --git a/openpcd/firmware/src/led.c b/openpcd/firmware/src/led.c
index b3dcfc7..ec203e5 100644
--- a/openpcd/firmware/src/led.c
+++ b/openpcd/firmware/src/led.c
@@ -1,7 +1,10 @@
 
 #include <sys/types.h>
-#include <include/lib_AT91SAM7.h>
+#include <errno.h>
+#include <lib_AT91SAM7.h>
+#include <openpcd.h>
 #include "openpcd.h"
+#include "usb_handler.h"
 #include "dbgu.h"
 
 static int led2port(int led)
@@ -51,10 +54,31 @@ int led_toggle(int led)
 	return !on;
 }
 
+static int led_usb_rx(struct req_ctx *rctx)
+{
+	struct openpcd_hdr *poh = (struct openpcd_hdr *) &rctx->rx.data[0];
+	int ret = 1;
+
+	switch (poh->cmd) {
+	case OPENPCD_CMD_SET_LED:
+		DEBUGP("SET LED(%u,%u) ", poh->reg, poh->val);
+		led_switch(poh->reg, poh->val);
+		break;
+	default:
+		DEBUGP("UNKNOWN ");
+		ret = -EINVAL;
+		break;
+	}
+	req_ctx_put(rctx);
+	return 1;
+};
+
 void led_init(void)
 {
 	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_LED1);
 	AT91F_PIO_CfgOutput(AT91C_BASE_PIOA, OPENPCD_PIO_LED2);
 	led_switch(1, 0);
 	led_switch(2, 0);
+
+	usb_hdlr_register(&led_usb_rx, OPENPCD_CMD_CLS_LED);
 }
diff --git a/openpcd/firmware/src/main_analog.c b/openpcd/firmware/src/main_analog.c
index c60b64e..92de3a4 100644
--- a/openpcd/firmware/src/main_analog.c
+++ b/openpcd/firmware/src/main_analog.c
@@ -28,7 +28,7 @@ void _init_func(void)
 	DEBUGPCRF("initializing 14443A operation");
 	rc632_iso14443a_init(RAH);
 	/* Switch to 848kBps (1subcp / bit) */
-	rc632_clear_bits(RAH, RC632_REG_RX_CONTROL1, RC632_RXCTRL1_SUBCP_MASK);
+	//rc632_clear_bits(RAH, RC632_REG_RX_CONTROL1, RC632_RXCTRL1_SUBCP_MASK);
 }
 
 int _main_dbgu(char key)
diff --git a/openpcd/firmware/src/pcd_enumerate.c b/openpcd/firmware/src/pcd_enumerate.c
index d0edc60..a2cd31c 100644
--- a/openpcd/firmware/src/pcd_enumerate.c
+++ b/openpcd/firmware/src/pcd_enumerate.c
@@ -165,7 +165,7 @@ int udp_refill_ep(int ep, struct req_ctx *rctx)
 	DEBUGP("refilling EP%u ", ep);
 
 	/* FIXME: state machine to handle two banks of FIFO memory, etc */
-	if (atomic_read(&upcd.ep[ep].pkts_in_transit) >= 2)
+	if (atomic_read(&upcd.ep[ep].pkts_in_transit) >= 1)
 		return -EBUSY;
 		
 	/* fill FIFO/DPR */
diff --git a/openpcd/firmware/src/pwm.c b/openpcd/firmware/src/pwm.c
index 1b19355..8247921 100644
--- a/openpcd/firmware/src/pwm.c
+++ b/openpcd/firmware/src/pwm.c
@@ -8,6 +8,9 @@
 #include <AT91SAM7.h>
 #include <sys/types.h>
 #include <errno.h>
+#include <openpcd.h>
+#include "usb_handler.h"
+#include "pcd_enumerate.h"
 #include "dbgu.h"
 #include "openpcd.h"
 
@@ -100,6 +103,46 @@ void pwm_duty_set_percent(int channel, u_int16_t duty)
 	AT91F_PWMC_UpdateChannel(AT91C_BASE_PWMC, channel, tmp);
 }
 
+static int pwm_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_int32_t *freq;
+
+	switch (poh->cmd) {
+	case OPENPCD_CMD_PWM_ENABLE:
+		if (poh->val)
+			pwm_start(0);
+		else
+			pwm_stop(0);
+		break;
+	case OPENPCD_CMD_PWM_DUTY_SET:
+		pwm_duty_set_percent(0, poh->val);
+		break;
+	case OPENPCD_CMD_PWM_DUTY_GET:
+		goto respond;
+		break;
+	case OPENPCD_CMD_PWM_FREQ_SET:
+		if (rctx->rx.tot_len < sizeof(*poh)+4)
+			break;
+		freq = (void *) poh + sizeof(*poh);
+		pwm_freq_set(0, *freq);
+		break;
+	case OPENPCD_CMD_PWM_FREQ_GET:
+		goto respond;
+		break;
+	default:
+		break;
+	}
+
+	req_ctx_put(rctx);
+	return 0;
+respond:
+	req_ctx_set_state(rctx, RCTX_STATE_UDP_EP2_PENDING);
+	udp_refill_ep(2, rctx);
+	return 1;
+}
+
 void pwm_init(void)
 {
 	/* IMPORTANT: Disable PA17 (SSC TD) output */
@@ -110,9 +153,12 @@ void pwm_init(void)
 
 	/* Enable Clock for PWM controller */
 	AT91F_PWMC_CfgPMC();
+
+	usb_hdlr_register(&pwm_usb_in, OPENPCD_CMD_CLS_PWM);
 }
 
 void pwm_fini(void)
 {
+	usb_hdlr_unregister(OPENPCD_CMD_CLS_PWM);
 	AT91F_PMC_DisablePeriphClock(AT91C_BASE_PMC, (1 << AT91C_ID_PWMC));
 }
diff --git a/openpcd/firmware/src/rc632.c b/openpcd/firmware/src/rc632.c
index d37a407..0876a1b 100644
--- a/openpcd/firmware/src/rc632.c
+++ b/openpcd/firmware/src/rc632.c
@@ -9,7 +9,7 @@
  * */
 
 #include <string.h>
-
+#include <errno.h>
 #include <lib_AT91SAM7.h>
 #include <cl_rc632.h>
 #include <openpcd.h>
@@ -17,6 +17,7 @@
 #include "fifo.h"
 #include "dbgu.h"
 #include "pcd_enumerate.h"
+#include "usb_handler.h"
 #include "rc632.h"
 
 #define NOTHING  do {} while(0)
@@ -379,6 +380,107 @@ void rc632_reset(void)
 	rc632_reg_write(RAH, RC632_REG_PAGE0, 0x00);
 }
 
+static int rc632_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;
+
+	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->val, pih->data);
+			rctx->tx.tot_len += pih_len;
+			DEBUGP("READ FIFO(len=%u)=%s ", pih_len,
+				hexdump(pih->data, pih_len));
+			/* don't set state of second rctx, main function
+			 * body will do this after switch statement */
+		} else {
+			pih->val = 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=%u) ", len-sizeof(*poh));
+		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 ");
+		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_DUMP_REGS:
+		DEBUGP("DUMP REGS ");
+		DEBUGP("NOT IMPLEMENTED YET ");
+		goto respond;
+		break;
+	default:
+		DEBUGP("UNKNOWN ");
+		return -EINVAL;
+	}
+
+	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;
+}
+
 void rc632_init(void)
 {
 	//fifo_init(&rc632.fifo, 256, NULL, &rc632);
@@ -437,11 +539,13 @@ void rc632_init(void)
 	/* configure AUX to test signal four */
 	rc632_reg_write(RAH, RC632_REG_TEST_ANA_SELECT, 0x04);
 
+	usb_hdlr_register(&rc632_usb_in, OPENPCD_CMD_CLS_RC632);
 };
 
 #if 0
 void rc632_exit(void)
 {
+	usb_hdlr_unregister(OPENPCD_CMD_CLS_RC632);
 	AT91F_AIC_DisableIt(AT91C_BASE_AIC, OPENPCD_IRQ_RC632);
 	AT91F_AIC_DisableIt(AT91C_BASE_AIC, AT91C_ID_SPI);
 	AT91F_SPI_Disable(pSPI);
diff --git a/openpcd/firmware/src/ssc.c b/openpcd/firmware/src/ssc.c
index c9deb79..e79eda8 100644
--- a/openpcd/firmware/src/ssc.c
+++ b/openpcd/firmware/src/ssc.c
@@ -11,7 +11,9 @@
 #include <errno.h>
 #include <sys/types.h>
 #include <lib_AT91SAM7.h>
+#include <openpcd.h>
 
+#include "usb_handler.h"
 #include "openpcd.h"
 #include "dbgu.h"
 
@@ -24,6 +26,15 @@ struct ssc_state {
 
 static struct ssc_state ssc_state;
 
+static struct openpcd_hdr opcd_ssc_hdr = {
+	.cmd	= OPENPCD_CMD_SSC_READ,
+};
+
+static inline void init_opcdhdr(struct req_ctx *rctx)
+{
+	memcpy(&rctx->tx.data[0], rctx, sizeof(rctx));
+}
+
 /* Try to refill RX dma descriptors. Return values:
  *  0) no dma descriptors empty
  *  1) filled next/secondary descriptor
@@ -42,6 +53,7 @@ static int8_t ssc_rx_refill(void)
 	}
 
 	if (AT91F_PDC_IsRxEmpty(rx_pdc)) {
+		init_opcdhdr(rctx);
 		DEBUGPCRF("filling primary SSC RX dma ctx");
 		AT91F_PDC_SetRx(rx_pdc, &rctx->rx.data[MAX_HDRSIZE],
 				MAX_REQSIZE);
@@ -58,6 +70,7 @@ static int8_t ssc_rx_refill(void)
 
 	if (AT91F_PDC_IsNextRxEmpty(rx_pdc)) {
 		DEBUGPCRF("filling secondary SSC RX dma ctx");
+		init_opcdhdr(rctx);
 		AT91F_PDC_SetNextRx(rx_pdc, &rctx->rx.data[MAX_HDRSIZE],
 				    MAX_REQSIZE);
 		ssc_state.rx_ctx[1] = rctx;
@@ -131,6 +144,23 @@ void ssc_tx_init(void)
 			    0);
 }
 
+static int ssc_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];
+
+	/* FIXME: implement this */
+	switch (poh->cmd) {
+	case OPENPCD_CMD_SSC_READ:
+		break;
+	case OPENPCD_CMD_SSC_WRITE:
+		break;
+	}
+
+	req_ctx_put(rctx);
+	return -EINVAL;
+}
+
 void ssc_rx_init(void)
 {
 	rx_pdc = (AT91PS_PDC) &(ssc->SSC_RPR);
@@ -156,10 +186,13 @@ void ssc_rx_init(void)
 	AT91F_SSC_EnableIt(ssc, AT91C_SSC_OVRUN |
 			   AT91C_SSC_ENDRX | AT91C_SSC_RXBUFF);
 	AT91F_PDC_EnableRx(rx_pdc);
+
+	usb_hdlr_register(&ssc_usb_in, OPENPCD_CMD_CLS_SSC);
 }
 
 void ssc_fini(void)
 {
+	usb_hdlr_unregister(OPENPCD_CMD_CLS_SSC);
 	AT91F_PDC_DisableRx(rx_pdc);
 	AT91F_SSC_DisableTx(ssc);
 	AT91F_SSC_DisableRx(ssc);
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)
diff --git a/openpcd/firmware/src/usb_handler.h b/openpcd/firmware/src/usb_handler.h
index 70c9352..b4ecfbd 100644
--- a/openpcd/firmware/src/usb_handler.h
+++ b/openpcd/firmware/src/usb_handler.h
@@ -1,6 +1,12 @@
 #ifndef _USB_HANDLER_H
 #define _USB_HANDLER_H
 
+#define MAX_PAYLOAD_LEN	(64 - sizeof(struct openpcd_hdr))
+
+typedef int usb_cmd_fn(struct req_ctx *rctx);
+
+extern int usb_hdlr_register(usb_cmd_fn *hdlr, u_int8_t class);
+
 extern void usb_in_process(void);
 extern void usb_out_process(void);