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