We have essentially the same code in each of two different
loops, so we can refactor it into a little helper function.

This also reduces the amount of work done during startup,
as we now only reprogram h/w found to be in a state other
than that expected, and so avoid the overhead of setting
doorbell registers to the state they're already in.
Signed-off-by: Dave Gordon <david.s.gordon@intel.com>
Reviewed-by: Tvrtko Ursulin <tvrtko.ursulin@intel.com>
Signed-off-by: Tvrtko Ursulin <tvrtko.ursulin@intel.com>