Refactor code in order to avoid identical code for different branches.

Addresses-Coverity-ID: 1248728
Signed-off-by: NGustavo A. R. Silva <gustavo@embeddedor.com>
Acked-by: NLarry Finger <Larry.Finger@lwfinger.net>
Signed-off-by: NKalle Valo <kvalo@codeaurora.org>