Pull out the block of register programming into a separate function. Signed-off-by: Marek Vasut <marex@denx.de>