Reorganized stuff

Probe function started looking a bit like spaghetti so I chopped it up.
Added options which can easily be changed later for Kconfig.

I also removed rumble support again for testing. It seems to be
causing a kernel hang although I, once again, have no idea why.