Add reference C implementation