Pull out the block of register programming into a separate function.

Signed-off-by: Marek Vasut <marex@denx.de>