SW: Fixed functions reading/modifying monitor registers

- Added functions modifying configuration register
- Improved debugging messages