Initial implementation of discovery mode

First implementation. Not testet with several devices at once. Only
tested with single device.