Based on code from "Martin Fuzzey" <mfuzzey@gmail.com>
Signed-off-by: NHolger Schurig <hs4233@mail.mn-solutions.de>
Signed-off-by: NSascha Hauer <s.hauer@pengutronix.de>