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