refactor register handling API to use a single function for each SCPI callback

[jancumps]

Each custom register handler has its own almost similar code body.
Can I simplify this into one function for writing, one for reading registers?

Avoids duplicate logic (and duplicate code lines, although that's not the driver)