From 08ed25eb491c0f7a5111e1284843bd5445565a17 Mon Sep 17 00:00:00 2001
From: PaulStoffregen <paul@pjrc.com>
Date: Sat, 25 Feb 2017 13:40:31 -0800
Subject: [PATCH] Disconnect drivers and free device when cable unplugged

---
 USBHost.h       |  2 ++
 ehci.cpp        |  4 ++--
 enumeration.cpp | 55 +++++++++++++++++++++++++++++++++++++++++++++----
 print.cpp       | 21 +++++++++++++++++++
 4 files changed, 76 insertions(+), 6 deletions(-)

diff --git a/USBHost.h b/USBHost.h
index 1dc87d2..4e7a960 100644
--- a/USBHost.h
+++ b/USBHost.h
@@ -145,6 +145,7 @@ protected:
 	static bool queue_Data_Transfer(Pipe_t *pipe, void *buffer,
 		uint32_t len, USBDriver *driver);
 	static Device_t * new_Device(uint32_t speed, uint32_t hub_addr, uint32_t hub_port);
+	static void disconnect_Device(Device_t *dev);
 	static void enumeration(const Transfer_t *transfer);
 	static void driver_ready_for_device(USBDriver *driver);
 private:
@@ -166,6 +167,7 @@ protected:
 	static void print(const Transfer_t *first, const Transfer_t *last);
 	static void print_token(uint32_t token);
 	static void print(const Pipe_t *pipe);
+	static void print_driverlist(const char *name, const USBDriver *driver);
 	static void print_hexbytes(const void *ptr, uint32_t len);
 	static void print(const char *s)	{ Serial.print(s); }
 	static void print(int n)		{ Serial.print(n); }
diff --git a/ehci.cpp b/ehci.cpp
index d6c0a72..5f75b4f 100644
--- a/ehci.cpp
+++ b/ehci.cpp
@@ -275,7 +275,8 @@ void USBHost::isr()
 				println("    disconnect");
 				port_state = PORT_STATE_DISCONNECTED;
 				USBPHY_CTRL_CLR = USBPHY_CTRL_ENHOSTDISCONDETECT;
-				// TODO: delete & clean up device state...
+				disconnect_Device(rootdev);
+				rootdev = NULL;
 			}
 		}
 		if (portstat & USBHS_PORTSC_PEC) {
@@ -824,4 +825,3 @@ bool USBHost::allocate_interrupt_pipe_bandwidth(uint32_t speed, uint32_t maxlen,
 	return true;
 }
 
-
diff --git a/enumeration.cpp b/enumeration.cpp
index a879d51..d4e306d 100644
--- a/enumeration.cpp
+++ b/enumeration.cpp
@@ -29,6 +29,7 @@ static USBDriver *available_drivers = NULL;
 static uint8_t enumbuf[256] __attribute__ ((aligned(16)));
 static setup_t enumsetup __attribute__ ((aligned(16)));
 static uint16_t enumlen;
+static Device_t  *devlist=NULL;
 
 
 static uint32_t assign_addr(void);
@@ -84,6 +85,13 @@ Device_t * USBHost::new_Device(uint32_t speed, uint32_t hub_addr, uint32_t hub_p
 	// go onto a waiting list
 	mk_setup(enumsetup, 0x80, 6, 0x0100, 0, 8); // 6=GET_DESCRIPTOR
 	queue_Control_Transfer(dev, &enumsetup, enumbuf, NULL);
+	if (devlist == NULL) {
+		devlist = dev;
+	} else {
+		Device_t *p;
+		for (p = devlist; p->next; p = p->next) ; // walk devlist
+		p->next = dev;
+	}
 	return dev;
 }
 
@@ -317,9 +325,48 @@ static void pipe_set_addr(Pipe_t *pipe, uint32_t addr)
 	pipe->qh.capabilities[0] = (pipe->qh.capabilities[0] & 0xFFFFFF80) | addr;
 }
 
-//static uint32_t pipe_get_addr(Pipe_t *pipe)
-//{
-//	return pipe->qh.capabilities[0] & 0xFFFFFF80;
-//}
+
+void USBHost::disconnect_Device(Device_t *dev)
+{
+	if (!dev) return;
+	println("disconnect_Device:");
+
+	// Disconnect all drivers using this device.  If this device is
+	// a hub, the hub driver is responsible for recursively calling
+	// this function to disconnect its downstream devices.
+	print_driverlist("available_drivers", available_drivers);
+	print_driverlist("dev->drivers", dev->drivers);
+	for (USBDriver *p = dev->drivers; p; ) {
+		println("disconnect driver ", (uint32_t)p, HEX);
+		p->disconnect();
+		USBDriver *next = p->next;
+		p->next = available_drivers;
+		available_drivers = p;
+		p = next;
+	}
+	print_driverlist("available_drivers", available_drivers);
+
+	// TODO: halt all pipes, free their Transfer_t
+
+	// TODO: remove periodic scheduled pipes, free their Pipe_t
+
+	// TODO: remove async scheduled pipes, free their Pipe_t
+
+	// remove device from devlist and free its Device_t
+	Device_t *prev_dev = NULL;
+	for (Device_t *p = devlist; p; p = p->next) {
+		if (p == dev) {
+			if (prev_dev == NULL) {
+				devlist = p->next;
+			} else {
+				prev_dev->next = p->next;
+			}
+			println("removed Device_t from devlist");
+			free_Device(p);
+			break;
+		}
+		prev_dev = p;
+	}
+}
 
 
diff --git a/print.cpp b/print.cpp
index 9c54f89..29ad89a 100644
--- a/print.cpp
+++ b/print.cpp
@@ -125,6 +125,27 @@ void USBHost::print(const Pipe_t *pipe)
 	//Serial.print();
 }
 
+void USBHost::print_driverlist(const char *name, const USBDriver *driver)
+{
+	Serial.print("USBDriver (");
+	Serial.print(name);
+	Serial.print(") list: ");
+	if (driver == NULL) {
+		Serial.println("(empty");
+		return;
+	}
+	uint32_t count=0;
+	for (const USBDriver *p = driver; p; p = p->next) {
+		Serial.print((uint32_t)p, HEX);
+		if (p->next) Serial.print(" -> ");
+		if (++count > 30) {
+			Serial.println("abort:list too long");
+			return;
+		}
+	}
+	Serial.println();
+}
+
 
 void USBHost::print_hexbytes(const void *ptr, uint32_t len)
 {