Refactored condvar wait to remove duplicated code

Signed-off-by: J. German Rivera <German.Rivera@freescale.com>