Wrap the existing code to write configurations into registers in
a function.

Added error handling to the function.
Signed-off-by: Iker Perez del Palomar Sustatxa <iker.perez@codethink.co.uk>
Link: https://lore.kernel.org/r/20190808080246.8371-3-iker.perez@codethink.co.ukSigned-off-by: Guenter Roeck <linux@roeck-us.net>