If we reverse some of the logic and change the formula used,
we can simplify the function greatly.

It is intentional that this function is supplied and then re-worked
within the same patch-set.  The submission in the previous patch is
the tried and tested (i.e. in real releases) method written by ST.
This patch contains a simplification provided later.  It looks and
performs better, but doesn't have the same time-under-test that the
original method does.  The idea is that we keep some history in
order to provide an easy way back i.e. revert.
Signed-off-by: NLee Jones <lee.jones@linaro.org>
Signed-off-by: NMark Brown <broonie@kernel.org>