Svc and thread fixes

Some misc. fixes to SVC/threading code.
* Changed SleepThread to yield to the next thread
* Added a debug log in for ArbitrateAddress
* Updated Thread VerifyWait function to ensure that thread is actually in wait state.