From 89d13ce8f8b1314862bd10fd429ef5453dd9cf6e Mon Sep 17 00:00:00 2001
From: Werner Almesberger <werner@almesberger.net>
Date: Fri, 10 Jun 2011 17:12:57 -0300
Subject: [PATCH] atusb/fw/: SET_INTERFACE can now select among interfaces
 (changes user_setup)

- ep0.c (ep0_init), usb/dfu.c (dfu_init): set user_setups[0] instead
  of user_setup
- usb/atu2.c (usb_poll): reset user_setup on bus reset
- usb/usb.h (user_setups), usb/usb.c (user_setups): array of
  interface-specific setup functions
- usb/usb.c (handle_setup): in SET_INTERFACE, select setup function
  from user_setups according to interface
- usb/usb.c (handle_setup): if user_setup is not set (e.g., the
  optional SET_INTERFACE was never issued), fall back to user_setups[0]
---
 atusb/fw/ep0.c      |  2 +-
 atusb/fw/usb/atu2.c |  1 +
 atusb/fw/usb/dfu.c  |  2 +-
 atusb/fw/usb/usb.c  | 26 ++++++++++++++++++++------
 atusb/fw/usb/usb.h  |  1 +
 5 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/atusb/fw/ep0.c b/atusb/fw/ep0.c
index 81c2a3f..b41f85f 100644
--- a/atusb/fw/ep0.c
+++ b/atusb/fw/ep0.c
@@ -209,5 +209,5 @@ static int my_setup(const struct setup_request *setup)
 
 void ep0_init(void)
 {
-	user_setup = my_setup;
+	user_setups[0] = my_setup;
 }
diff --git a/atusb/fw/usb/atu2.c b/atusb/fw/usb/atu2.c
index 6ab9146..f626864 100644
--- a/atusb/fw/usb/atu2.c
+++ b/atusb/fw/usb/atu2.c
@@ -190,6 +190,7 @@ void usb_poll(void)
 	if (flags & (1 << EORSTI)) {
 		if (user_reset)
 			user_reset();
+		user_setup = NULL;
 		ep_init();
 		UDINT &= ~(1 << EORSTI);
 	}
diff --git a/atusb/fw/usb/dfu.c b/atusb/fw/usb/dfu.c
index 350b3cf..9ede826 100644
--- a/atusb/fw/usb/dfu.c
+++ b/atusb/fw/usb/dfu.c
@@ -288,7 +288,7 @@ static void my_reset(void)
 
 void dfu_init(void)
 {
-	user_setup = my_setup;
+	user_setups[0] = my_setup;
 	user_get_descriptor = my_descr;
 	user_reset = my_reset;
 }
diff --git a/atusb/fw/usb/usb.c b/atusb/fw/usb/usb.c
index 134ffc7..b127069 100644
--- a/atusb/fw/usb/usb.c
+++ b/atusb/fw/usb/usb.c
@@ -36,6 +36,7 @@ extern void panic(void);
 #endif
 
 int (*user_setup)(const struct setup_request *setup);
+int (*user_setups[2])(const struct setup_request *setup);
 int (*user_get_descriptor)(uint8_t type, uint8_t index,
     const uint8_t **reply, uint8_t *size);
 void (*user_reset)(void);
@@ -135,10 +136,21 @@ int handle_setup(const struct setup_request *setup)
 		{
 			const uint8_t *interface_descriptor =
 			    config_descriptor+9;
+			const uint8_t *p;
+			int i;
 
-			if (setup->wIndex != interface_descriptor[2] ||
-			    setup->wValue != interface_descriptor[3])
-				return 0;
+			i = 0;
+			for (p = interface_descriptor;
+			    p != config_descriptor+config_descriptor[2];
+			    p += p[0]) {
+				if (p[2] == setup->wIndex &&
+				    p[3] == setup->wValue) {
+					user_setup = user_setups[i];
+					return 1;
+				}
+				i++;
+			}
+			return 0;
 		}
 		break;
 
@@ -156,9 +168,11 @@ int handle_setup(const struct setup_request *setup)
 		return 0;
 
 	default:
-		if (!user_setup)
-			return 0;
-		return user_setup(setup);
+		if (user_setup)
+			return user_setup(setup);
+		if (user_setups[0])
+			return user_setups[0](setup);
+		return 0;
 	}
 
 	return 1;
diff --git a/atusb/fw/usb/usb.h b/atusb/fw/usb/usb.h
index 09d9c61..8808e06 100644
--- a/atusb/fw/usb/usb.h
+++ b/atusb/fw/usb/usb.h
@@ -133,6 +133,7 @@ extern const uint8_t config_descriptor[];
 extern struct ep_descr eps[];
 
 extern int (*user_setup)(const struct setup_request *setup);
+extern int (*user_setups[2])(const struct setup_request *setup);
 extern int (*user_get_descriptor)(uint8_t type, uint8_t index,
     const uint8_t **reply, uint8_t *size);
 extern void (*user_reset)(void);