Convert this from mostly inline assembler to mostly C.
Submitted by: Peter Grehan <peterg@ptree32.com.au>