something maybe wrong with freq

[Jostarsama]

I think you can change it
uint32_t freq_reg = (float)freq * (float)((1 << 28) / FMCLK); // Tuning word
to
uint32_t freq_reg = ((((float)freq * (1 << 28)) / FMCLK) + 0.5); // Tuning word
It is more accurate.