More efficient software PWM ISR