optimize the algorithm

Signed-off-by: Haley Guo(B51762) <B51762@freescale.com>