ST-link v2 SWD some fixes