Working version. (pull request on eelco's node for bluetoothsp.

TODO: Tidy it up, make a good architecture and test.