As these 2 functions code are 95% similar, factorize them.
Signed-off-by: NPatrice Chotard <patrice.chotard@st.com>
Acked-by: NMaxime Coquelin <maxime.coquelin@st.com>
Signed-off-by: NLinus Walleij <linus.walleij@linaro.org>