Add util.c and util.h