Current code does not work if count > 1, fix it.
Signed-off-by: Axel Lin <axel.lin@ingics.com>
Acked-by: Viresh Kumar <viresh.kumar@linaro.org>
Signed-off-by: Linus Walleij <linus.walleij@linaro.org>