SoftPwm: make code nicer and maybe faster