Very little code is different between these two paths now, so extract
the common code.
Acked-by: Frank Hofmann <frank.hofmann@tomtom.com> Tested-by: Kevin Hilman <khilman@ti.com> Acked-by: Jean Pihet <j-pihet@ti.com> Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>