Since my patch didn't follow GNU C style here's an update.
Sorry guys!