Fix XMEGA USB prescaler calculation

The USB prescaler calculation for the CLK.USBCTRL register is changed to
give valid results and set the prescaler correctly.

Signed-off-by: Bert van Hall <bert.vanhall@avionic-design.de>

diff --git a/LUFA/Drivers/USB/Core/XMEGA/USBController_XMEGA.c b/LUFA/Drivers/USB/Core/XMEGA/USBController_XMEGA.c
index f8b0fc6..b5de862 100644 (file)
--- a/LUFA/Drivers/USB/Core/XMEGA/USBController_XMEGA.c
+++ b/LUFA/Drivers/USB/Core/XMEGA/USBController_XMEGA.c
@@ -109,15 +109,25 @@ void USB_Disable(void)
 
 void USB_ResetInterface(void)
 {
 
 void USB_ResetInterface(void)
 {

+       uint8_t PrescalerNeeded;
+       uint8_t nbit = 0;
+

        #if defined(USB_DEVICE_OPT_FULLSPEED)
        if (USB_Options & USB_DEVICE_OPT_LOWSPEED)
        #if defined(USB_DEVICE_OPT_FULLSPEED)
        if (USB_Options & USB_DEVICE_OPT_LOWSPEED)

-         CLK.USBCTRL = (((F_USB / 6000000) - 1) << CLK_USBPSDIV_gp);
+         PrescalerNeeded = F_USB / 6000000;

        else
        else

-         CLK.USBCTRL = (((F_USB / 48000000) - 1) << CLK_USBPSDIV_gp);
+         PrescalerNeeded = F_USB / 48000000;

        #else
        #else

-       CLK.USBCTRL = (((F_USB / 6000000) - 1) << CLK_USBPSDIV_gp);
+       PrescalerNeeded = F_USB / 6000000;

        #endif
 
        #endif
 

+       while (PrescalerNeeded && nbit < 7) {
+               PrescalerNeeded >>= 1;
+               nbit++;
+       }
+
+       CLK.USBCTRL = (nbit - 1) << CLK_USBPSDIV_gp;
+

        if (USB_Options & USB_OPT_PLLCLKSRC)
          CLK.USBCTRL |= (CLK_USBSRC_PLL_gc   | CLK_USBSEN_bm);
        else
        if (USB_Options & USB_OPT_PLLCLKSRC)
          CLK.USBCTRL |= (CLK_USBSRC_PLL_gc   | CLK_USBSEN_bm);
        else