Dear Wolfgang,

You are right, the patch was ugly.
The new one seems to be better.

Signed-off-by: Valeriy Glushkov <gvv@lstec.com>
Signed-off-by: Scott Wood <scottwood@freescale.com>