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