rollback to initial versino because last refactoring broken it.
will try refactoring tomorrow with CONFIG_USB_FIQ_ENABLED