Patch from Ben Dooks

This has been broken for a while now, so fix the
problems with the code, test and bring up to date.
This also makes the code conditional on an
Kconfig option
Signed-off-by: NBen Dooks <ben-linux@fluff.org>
Signed-off-by: NRussell King <rmk+kernel@arm.linux.org.uk>