USB_Endpoint_SelectedHandle->STATUS  = (Direction == ENDPOINT_DIR_IN) ? USB_EP_BUSNACK0_bm : 0;
        USB_Endpoint_SelectedHandle->CTRL    = Config;
        USB_Endpoint_SelectedHandle->CNT     = 0;
-       USB_Endpoint_SelectedHandle->DATAPTR = (intptr_t)&USB_Endpoint_SelectedFIFO->Data[0];
+       USB_Endpoint_SelectedHandle->DATAPTR = (intptr_t)USB_Endpoint_SelectedFIFO->Data;
        
        USB_Endpoint_SelectedFIFO->Length    = (Direction == ENDPOINT_DIR_IN) ? Size : 0;
        USB_Endpoint_SelectedFIFO->Position  = 0;
 
                                
                                if ((USB_Endpoint_SelectedHandle->CTRL & USB_EP_TYPE_gm) == USB_EP_TYPE_CONTROL_gc)
                                {
-                                       Endpoint_SelectEndpoint(USB_Endpoint_SelectedEndpoint |  ENDPOINT_DIR_IN);
+                                       Endpoint_SelectEndpoint(USB_Endpoint_SelectedEndpoint ^ ENDPOINT_DIR_IN);
                                        USB_Endpoint_SelectedHandle->CTRL |= USB_EP_STALL_bm;
-                                       Endpoint_SelectEndpoint(USB_Endpoint_SelectedEndpoint & ~ENDPOINT_DIR_IN);
                                }
                        }
 
                                return (USB_Endpoint_SelectedEndpoint & ENDPOINT_DIR_IN);
                        }
 
-                       /** Sets the direction of the currently selected endpoint.
-                        *
-                        *  \param[in] DirectionMask  New endpoint direction, as a \c ENDPOINT_DIR_* mask.
-                        */
-                       static inline void Endpoint_SetEndpointDirection(const uint8_t DirectionMask) ATTR_ALWAYS_INLINE;
-                       static inline void Endpoint_SetEndpointDirection(const uint8_t DirectionMask)
-                       {
-                               // TODO
-                       }
-
                        /** Reads one byte from the currently selected endpoint's bank, for OUT direction endpoints.
                         *
                         *  \ingroup Group_EndpointPrimitiveRW_XMEGA