1 files changed, 71 insertions, 0 deletions
diff --git a/fmdriver/fmdriver_common.c b/fmdriver/fmdriver_common.c
new file mode 100644
index 0000000..26e6def
--- /dev/null
+++ b/fmdriver/fmdriver_common.c
@@ -0,0 +1,71 @@
+#include "fmdriver/fmdriver_common.h"
+
+uint8_t fmdriver_fm_freq2key(uint16_t freq) {
+  int block = freq >> (8+3);
+  int f_num = freq & ((1<<(8+3))-1);
+  if (!f_num) return 0x00;
+  while (!(f_num & (1<<(8+3-1)))) {
+    f_num <<= 1;
+    block--;
+  }
+  static const uint16_t freqtab[0xc] = {
+    0x042e, // < 9 (a)
+    0x046e,
+    0x04b1,
+    0x04f9,
+    0x0544,
+    0x0595,
+    0x05ea,
+    0x0644,
+    0x06a3,
+    0x0708,
+    0x0773,
+    0x07e4,
+  };
+  int note = 0;
+  for (; note < 12; note++) {
+    if (f_num < freqtab[note]) break;
+  }
+  note += 9;
+  block += (note/12);
+  note %= 12;
+
+  if (block < 0) return 0x00;
+  if (block > 8) return 0x8b;
+  return (block << 4) | note;
+}
+
+uint8_t fmdriver_ssg_freq2key(uint16_t freq) {
+  if (!freq) return 0x00;
+  int octave = -5;
+  while (!(freq & 0x8000)) {
+    freq <<= 1;
+    octave++;
+  }
+  // 7987200.0 / (64*440*(2**((i+2+0.5)/12))/(1<<8))
+  static const uint16_t freqtab[0xc] = {
+    0xf57f, // > 0 (c)
+    0xe7b8,
+    0xdab7,
+    0xce70,
+    0xc2da,
+    0xb7ea,
+    0xad98,
+    0xa3da,
+    0x9aa7,
+    0x91f9,
+    0x89c8,
+    0x820c,
+  };
+  int note = 0;
+  for (; note < 12; note++) {
+    if (freq > freqtab[note]) break;
+  }
+  note += 11;
+  octave += (note/12);
+  note %= 12;
+
+  if (octave < 0) return 0x00;
+  if (octave > 8) return 0x8b;
+  return (octave << 4) | note;
+}