diff --git a/hardware/arduino/avr/cores/arduino/PluggableUSB.cpp b/hardware/arduino/avr/cores/arduino/PluggableUSB.cpp
index 68a6c8af1..857a27fe9 100644
--- a/hardware/arduino/avr/cores/arduino/PluggableUSB.cpp
+++ b/hardware/arduino/avr/cores/arduino/PluggableUSB.cpp
@@ -34,7 +34,6 @@ extern u8 _initEndpoints[];
 static u8 modules_count = 0;
 
 static PUSBListNode* rootNode = NULL;
-static PUSBListNode* lastNode = NULL;
 
 int PUSB_GetInterface(u8* interfaceNum)
 {
@@ -77,9 +76,12 @@ int8_t PUSB_AddFunction(PUSBListNode *node, u8* interface)
 
 	if (modules_count == 0) {
 		rootNode = node;
-		lastNode = node;
 	} else {
-		lastNode->next = node;
+		PUSBListNode *current = rootNode;
+		while(current->next != NULL) {
+			current = current->next;
+		}
+		current->next = node;
 	}
 
 	*interface = lastIf;
diff --git a/hardware/arduino/avr/cores/arduino/PluggableUSB.h b/hardware/arduino/avr/cores/arduino/PluggableUSB.h
index 647980b85..d89040eb4 100644
--- a/hardware/arduino/avr/cores/arduino/PluggableUSB.h
+++ b/hardware/arduino/avr/cores/arduino/PluggableUSB.h
@@ -25,7 +25,7 @@
 
 #if defined(USBCON)
 
-typedef struct
+typedef struct __attribute__((packed))
 {
   bool (*setup)(USBSetup& setup, u8 i);
   int (*getInterface)(u8* interfaceNum);