Reorganised C code a little