Finally I've ported @wilriker changes to CoreNG in 2.05.1 firmware and it is working!
I tested it on my desk and it works pretty well. I'm going to start testing that solution on my printers.
Thanks @Danal for tips but it still doesn't work for me. I only get "busy" state.
I suppose that C parameter in M581 is problem. You didn't use this parameter
Correct, define the pin with 950, then link to that with M581.