diff --git a/cores/arduino/USB/USBCore.cpp b/cores/arduino/USB/USBCore.cpp
index b9ba77e4e88f47336aae8295926dd22d6be2ac66..557210c3e0e76e9cb64f9fbee23db0ab8e113e83 100644
--- a/cores/arduino/USB/USBCore.cpp
+++ b/cores/arduino/USB/USBCore.cpp
@@ -294,7 +294,7 @@ void USBDeviceClass::init()
 	while (GCLK->STATUS.bit.SYNCBUSY)
 		;
 
-	UHD_SetStack(&UDD_Handler);	
+	USB_SetHandler(&UDD_Handler);
 
 	// Reset USB Device
 	usbd.reset();
diff --git a/cores/arduino/USB/USB_host.h b/cores/arduino/USB/USB_host.h
index 390e3dd1eb36f4d09812288cb77c784ce9bec9c3..319dffea5861bc1becd325e588f7327703d9b1f6 100644
--- a/cores/arduino/USB/USB_host.h
+++ b/cores/arduino/USB/USB_host.h
@@ -48,7 +48,7 @@ typedef enum {
 
 extern void UHD_Init(void);
 extern void UHD_Handler(void);
-extern void UHD_SetStack(void (*pf_isr)(void)); 
+extern void USB_SetHandler(void (*pf_isr)(void));
 extern uhd_vbus_state_t UHD_GetVBUSState(void);
 extern uint32_t UHD_Pipe0_Alloc(uint32_t ul_add, uint32_t ul_ep_size);
 extern uint32_t UHD_Pipe_Alloc(uint32_t ul_dev_addr, uint32_t ul_dev_ep, uint32_t ul_type, uint32_t ul_dir, uint32_t ul_maxsize, uint32_t ul_interval, uint32_t ul_nb_bank);
diff --git a/cores/arduino/USB/USB_interrupt.c b/cores/arduino/USB/USB_interrupt.c
deleted file mode 100644
index 09f7a81a83ecadfb77eb178a75199bfb76ddd743..0000000000000000000000000000000000000000
--- a/cores/arduino/USB/USB_interrupt.c
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
-  Copyright (c) 2014 Arduino LLC.  All right reserved.
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-  See the GNU Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with this library; if not, write to the Free Software
-  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
-*/
-
-void (*gpf_isr)(void) = (0UL);
-
-void USB_Handler( void )
-{
-	if (gpf_isr)
-	gpf_isr();
-}
-
-void UHD_SetStack(void (*pf_isr)(void))
-{
-	gpf_isr = pf_isr;
-}
diff --git a/cores/arduino/USB/samd21_host.c b/cores/arduino/USB/samd21_host.c
index 350a38108e95733aa73f5043e7dd449f7ac258bb..229f738df72f3c693f096a4d10f177030ffb811e 100644
--- a/cores/arduino/USB/samd21_host.c
+++ b/cores/arduino/USB/samd21_host.c
@@ -63,7 +63,7 @@ void UHD_Init(void)
 	uint32_t pad_trim;
 	uint32_t i;
 
-	UHD_SetStack(&UHD_Handler);
+	USB_SetHandler(&UHD_Handler);
 
 	/* Enable USB clock */
 	PM->APBBMASK.reg |= PM_APBBMASK_USB;
diff --git a/cores/arduino/cortex_handlers.c b/cores/arduino/cortex_handlers.c
index fed128c4487cf86ada5c1c931f8de878d0ae2f79..435736c5983a9423bfa45c64b07dc1e983e4a894 100644
--- a/cores/arduino/cortex_handlers.c
+++ b/cores/arduino/cortex_handlers.c
@@ -16,8 +16,9 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
 
-#include "sam.h"
-#include "variant.h"
+#include <sam.h>
+#include <variant.h>
+#include <stdio.h>
 
 /* RTOS Hooks */
 extern void svcHook(void);
@@ -25,7 +26,6 @@ extern void pendSVHook(void);
 extern int sysTickHook(void);
 
 /* Default empty handler */
-
 void Dummy_Handler(void)
 {
 #if defined DEBUG
@@ -50,7 +50,7 @@ void RTC_Handler      (void) __attribute__ ((weak, alias("Dummy_Handler")));
 void EIC_Handler      (void) __attribute__ ((weak, alias("Dummy_Handler")));
 void NVMCTRL_Handler  (void) __attribute__ ((weak, alias("Dummy_Handler")));
 void DMAC_Handler     (void) __attribute__ ((weak, alias("Dummy_Handler")));
-void USB_Handler      (void) __attribute__ ((weak, alias("Dummy_Handler")));
+void USB_Handler      (void) __attribute__ ((weak));
 void EVSYS_Handler    (void) __attribute__ ((weak, alias("Dummy_Handler")));
 void SERCOM0_Handler  (void) __attribute__ ((weak, alias("Dummy_Handler")));
 void SERCOM1_Handler  (void) __attribute__ ((weak, alias("Dummy_Handler")));
@@ -176,3 +176,16 @@ void SysTick_Handler(void)
     return;
   SysTick_DefaultHandler();
 }
+
+static void (*usb_isr)(void) = NULL;
+
+void USB_Handler(void)
+{
+  if (usb_isr)
+    usb_isr();
+}
+
+void USB_SetHandler(void (*new_usb_isr)(void))
+{
+  usb_isr = new_usb_isr;
+}
diff --git a/cores/arduino/startup.c b/cores/arduino/startup.c
index 9a43b4a8ef6a26deddc644a5fa78085b97992420..4ffdae28f3d3d000da825e0c43b901092fc84161 100644
--- a/cores/arduino/startup.c
+++ b/cores/arduino/startup.c
@@ -19,6 +19,8 @@
 #include "sam.h"
 #include "variant.h"
 
+#include <stdio.h>
+
 /**
  * \brief SystemInit() configures the needed clocks and according Flash Read Wait States.
  * At reset:
@@ -222,4 +224,3 @@ void SystemInit( void )
 
   ADC->CALIB.reg = ADC_CALIB_BIAS_CAL(bias) | ADC_CALIB_LINEARITY_CAL(linearity);
 }
-