Fixed/Improved TEE_wait function

Signed-off-by: Tanel Dettenborn <dettenbo@cs.helsinki.fi>