Former patch still has some bugs, so rollback it and enhance the original code. Cc: Ruiyu NI <ruiyu.ni@intel.com> Reviewed-by: Jiewen Yao <jiewen.yao@intel.com> Contributed-under: TianoCore Contribution Agreement 1.0 Signed-off-by: Eric Dong <eric.dong@intel.com>