Code clean up

Code reordering is required in order to propperly defined the locations
in the code.
Refactory is neccesary in order to reduce code complexity

Signed-off-by: Mircea Pop <mircea.pop@freescale.com>