Added read/write configuration register functions.

Functions non-working, need debugging. Only frame for future.