Original diag_testmode with fix

Signed-off-by: Caio99BR <caiooliveirafarias0@gmail.com>