Now use the same algorithm as described in the ARM ARM. Signed-off-by: Christophe Lyon <christophe.lyon@st.com> Reviewed-by: Peter Maydell <peter.maydell@linaro.org> Signed-off-by: Aurelien Jarno <aurelien@aurel32.net>