From 6fd10cdacb5cbe47a4fc339c20a733d4a9a384a1 Mon Sep 17 00:00:00 2001
From: Takamichi Horikawa <takamichiho@gmail.com>
Date: Sat, 26 Nov 2016 20:57:57 +0900
Subject: initial

---
 libopna/opnafm.c | 511 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 511 insertions(+)
 create mode 100644 libopna/opnafm.c

(limited to 'libopna/opnafm.c')

diff --git a/libopna/opnafm.c b/libopna/opnafm.c
new file mode 100644
index 0000000..c7512a8
--- /dev/null
+++ b/libopna/opnafm.c
@@ -0,0 +1,511 @@
+#include "opnafm.h"
+
+#include "opnatables.h"
+
+enum {
+  CH3_MODE_NORMAL = 0,
+  CH3_MODE_CSM    = 1,
+  CH3_MODE_SE     = 2
+};
+
+static void opna_fm_slot_reset(struct opna_fm_slot *slot) {
+  slot->phase = 0;
+  slot->env = 1023;
+  slot->env_count = 0;
+  slot->env_state = ENV_RELEASE;
+  slot->rate_shifter = 0;
+  slot->rate_selector = 0;
+  slot->rate_mul = 0;
+  slot->tl = 0;
+  slot->sl = 0;
+  slot->ar = 0;
+  slot->dr = 0;
+  slot->sr = 0;
+  slot->rr = 0;
+  slot->mul = 0;
+  slot->det = 0;
+  slot->ks = 0;
+  slot->keyon = false;
+}
+
+
+void opna_fm_chan_reset(struct opna_fm_channel *chan) {
+  for (int i = 0; i < 4; i++) {
+    opna_fm_slot_reset(&chan->slot[i]);
+  }
+
+  chan->fbmem1 = 0;
+  chan->fbmem2 = 0;
+  chan->alg_mem = 0;
+
+  chan->alg = 0;
+  chan->fb = 0;
+  chan->fnum = 0;
+  chan->blk = 0;
+}
+
+void opna_fm_reset(struct opna_fm *fm) {
+  for (int i = 0; i < 6; i++) {
+    opna_fm_chan_reset(&fm->channel[i]);
+    fm->lselect[i] = true;
+    fm->rselect[i] = true;
+  }
+  fm->blkfnum_h = 0;
+  fm->env_div3 = 0;
+  
+  fm->ch3.mode = CH3_MODE_NORMAL;
+  for (int i = 0; i < 3; i++) {
+    fm->ch3.fnum[i] = 0;
+    fm->ch3.blk[i] = 0;
+  }
+}
+#define LIBOPNA_ENABLE_HIRES
+// maximum output: 2042<<2 = 8168
+static int16_t opna_fm_slotout(struct opna_fm_slot *slot, int16_t modulation) {
+  unsigned pind = (slot->phase >> 10);
+  pind += modulation >> 1;
+  bool minus = pind & (1<<(LOGSINTABLEBIT+1));
+  bool reverse = pind & (1<<LOGSINTABLEBIT);
+  if (reverse) pind = ~pind;
+  pind &= (1<<LOGSINTABLEBIT)-1;
+
+#ifdef LIBOPNA_ENABLE_HIRES
+  unsigned pind_hires = (slot->phase >> 8);
+  pind_hires += modulation << 1;
+  minus = pind_hires & (1<<(LOGSINTABLEHIRESBIT+1));
+  reverse = pind_hires & (1<<LOGSINTABLEHIRESBIT);
+  if (reverse) pind_hires = ~pind_hires;
+  pind_hires &= (1<<LOGSINTABLEHIRESBIT)-1;
+
+  int logout = logsintable_hires[pind_hires] + (slot->env << 2) + (slot->tl << 5);
+#else
+  int logout = logsintable[pind] + (slot->env << 2) + (slot->tl << 5);
+#endif // LIBOPNA_ENABLE_HIRES
+
+  int selector = logout & ((1<<EXPTABLEBIT)-1);
+  int shifter = logout >> EXPTABLEBIT;
+  if (shifter > 13) shifter = 13; 
+
+  int16_t out = (exptable[selector] << 2) >> shifter;
+  if (minus) out = -out;
+  return out;
+}
+
+static unsigned blkfnum2freq(unsigned blk, unsigned fnum) {
+  return (fnum << blk) >> 1;
+}
+
+#define F(n) (!!(fnum & (1 << ((n)-1))))
+
+static unsigned blkfnum2keycode(unsigned blk, unsigned fnum) {
+  unsigned keycode = blk<<2;
+  keycode |= F(11) << 1;
+  keycode |= (F(11) && (F(10)||F(9)||F(8))) || ((!F(11))&&F(10)&&F(9)&&F(8));
+  return keycode;
+}
+
+#undef F
+
+static void opna_fm_slot_phase(struct opna_fm_slot *slot, unsigned freq) {
+// TODO: detune
+//  freq += slot->dt;
+  unsigned det = dettable[slot->det & 0x3][slot->keycode];
+  if (slot->det & 0x4) det = -det;
+  freq += det;
+  freq &= (1U<<17)-1;
+  int mul = slot->mul << 1;
+  if (!mul) mul = 1;
+  slot->phase += ((freq * mul)>>1);
+}
+
+void opna_fm_chan_phase(struct opna_fm_channel *chan) {
+  unsigned freq = blkfnum2freq(chan->blk, chan->fnum);
+  for (int i = 0; i < 4; i++) {
+    opna_fm_slot_phase(&chan->slot[i], freq);
+  }
+}
+
+static void opna_fm_chan_phase_se(struct opna_fm_channel *chan, struct opna_fm *fm) {
+  unsigned freq;
+  freq = blkfnum2freq(fm->ch3.blk[2], fm->ch3.fnum[1]);
+  opna_fm_slot_phase(&chan->slot[0], freq);
+  freq = blkfnum2freq(fm->ch3.blk[0], fm->ch3.fnum[0]);
+  opna_fm_slot_phase(&chan->slot[2], freq);
+  freq = blkfnum2freq(fm->ch3.blk[1], fm->ch3.fnum[2]);
+  opna_fm_slot_phase(&chan->slot[1], freq);
+  freq = blkfnum2freq(chan->blk, chan->fnum);
+  opna_fm_slot_phase(&chan->slot[3], freq);
+}
+
+int16_t opna_fm_chanout(struct opna_fm_channel *chan) {
+  int16_t fb = chan->fbmem1 + chan->fbmem2;
+  int16_t slot0 = chan->fbmem1;
+  chan->fbmem1 = chan->fbmem2;
+  if (!chan->fb) fb = 0;
+  chan->fbmem2 = opna_fm_slotout(&chan->slot[0], fb >> (9 - chan->fb));
+
+  int16_t slot2;
+  int16_t out = 0;
+
+  switch (chan->alg) {
+  case 0:
+    slot2 = opna_fm_slotout(&chan->slot[2], chan->alg_mem);
+    chan->alg_mem = opna_fm_slotout(&chan->slot[1], slot0);
+    out = opna_fm_slotout(&chan->slot[3], slot2);
+    break;
+  case 1:
+    slot2 = opna_fm_slotout(&chan->slot[2], chan->alg_mem);
+    chan->alg_mem = slot0;
+    chan->alg_mem += opna_fm_slotout(&chan->slot[1], 0);
+    out = opna_fm_slotout(&chan->slot[3], slot2);
+    break;
+  case 2:
+    slot2 = opna_fm_slotout(&chan->slot[2], chan->alg_mem);
+    chan->alg_mem = opna_fm_slotout(&chan->slot[1], 0);
+    out = opna_fm_slotout(&chan->slot[3], slot0 + slot2);
+    break;
+  case 3:
+    slot2 = opna_fm_slotout(&chan->slot[2], 0);
+    out = opna_fm_slotout(&chan->slot[3], slot2 + chan->alg_mem);
+    chan->alg_mem = opna_fm_slotout(&chan->slot[1], slot0);
+    break;
+  case 4:
+    out = opna_fm_slotout(&chan->slot[1], slot0);
+    slot2 = opna_fm_slotout(&chan->slot[2], 0);
+    out += opna_fm_slotout(&chan->slot[3], slot2);
+    break;
+  case 5:
+    out = opna_fm_slotout(&chan->slot[2], chan->alg_mem);
+    chan->alg_mem = slot0;
+    out += opna_fm_slotout(&chan->slot[1], slot0);
+    out += opna_fm_slotout(&chan->slot[3], slot0);
+    break;
+  case 6:
+    out = opna_fm_slotout(&chan->slot[1], slot0);
+    out += opna_fm_slotout(&chan->slot[2], 0);
+    out += opna_fm_slotout(&chan->slot[3], 0);
+    break;
+  case 7:
+    out = slot0;
+    out += opna_fm_slotout(&chan->slot[1], 0);
+    out += opna_fm_slotout(&chan->slot[2], 0);
+    out += opna_fm_slotout(&chan->slot[3], 0);
+    break;
+  }
+  
+  return out;
+}
+
+static void opna_fm_slot_setrate(struct opna_fm_slot *slot, int status) {
+  int r;
+  switch (status) {
+  case ENV_ATTACK:
+    r = slot->ar;
+    break;
+  case ENV_DECAY:
+    r = slot->dr;
+    break;
+  case ENV_SUSTAIN:
+    r = slot->sr;
+    break;
+  case ENV_RELEASE:
+    r = (slot->rr*2+1);
+    break;
+  default:
+    return;
+  }
+
+  if (!r) {
+    slot->rate_selector = 0;
+    slot->rate_mul = 0;
+    slot->rate_shifter = 0;
+    return;
+  }
+
+  int rate = 2*r + (slot->keycode >> (3 - slot->ks));
+
+  if (rate > 63) rate = 63;
+  int rate_shifter = 11 - (rate >> 2);
+  if (rate_shifter < 0) {
+    slot->rate_selector = (rate & ((1<<2)-1)) + 4;
+    slot->rate_mul = 1<<(-rate_shifter-1);
+    slot->rate_shifter = 0;
+  } else {
+    slot->rate_selector = rate & ((1<<2)-1);
+    slot->rate_mul = 1;
+    slot->rate_shifter = rate_shifter;
+  }
+}
+
+static void opna_fm_slot_env(struct opna_fm_slot *slot) {
+  slot->env_count++;
+  if (!(slot->env_count & ((1<<slot->rate_shifter)-1))) {
+    int rate_index = (slot->env_count >> slot->rate_shifter) & 7;
+    int env_inc = rateinctable[slot->rate_selector][rate_index];
+    env_inc *= slot->rate_mul;
+
+    switch (slot->env_state) {
+    int newenv;
+    int sl;
+    case ENV_ATTACK:
+      newenv = slot->env + (((-slot->env-1) * env_inc) >> 4);
+      if (newenv <= 0) {
+        slot->env = 0;
+        slot->env_state = ENV_DECAY;
+        opna_fm_slot_setrate(slot, ENV_DECAY);
+      } else {
+        slot->env = newenv;
+      }
+      break;
+    case ENV_DECAY:
+      slot->env += env_inc;
+      sl = slot->sl;
+      if (sl == 0xf) sl = 0x1f;
+      if (slot->env >= (sl << 5)) {
+        slot->env_state = ENV_SUSTAIN;
+        opna_fm_slot_setrate(slot, ENV_SUSTAIN);
+      }
+      break;
+    case ENV_SUSTAIN:
+      slot->env += env_inc;
+      if (slot->env >= 1023) slot->env = 1023;
+      break;
+    case ENV_RELEASE:
+      slot->env += env_inc;
+      if (slot->env >= 1023) {
+        slot->env = 1023;
+        slot->env_state = ENV_OFF;
+      }
+      break;
+    }
+  }
+}
+
+void opna_fm_slot_key(struct opna_fm_channel *chan, int slotnum, bool keyon) {
+  struct opna_fm_slot *slot = &chan->slot[slotnum];
+  if (keyon) {
+    if (!slot->keyon) {
+      slot->keyon = true;
+      slot->env_state = ENV_ATTACK;
+      slot->env_count = 0;
+      slot->phase = 0;
+      opna_fm_slot_setrate(slot, ENV_ATTACK);
+    }
+  } else {
+    if ((slot->env_state != ENV_OFF) && slot->keyon) {
+      slot->keyon = false;
+      slot->env_state = ENV_RELEASE;
+      opna_fm_slot_setrate(slot, ENV_RELEASE);
+    }
+  }
+}
+
+void opna_fm_slot_set_det(struct opna_fm_slot *slot, unsigned det) {
+  det &= 0x7;
+  slot->det = det;
+}
+
+void opna_fm_slot_set_mul(struct opna_fm_slot *slot, unsigned mul) {
+  mul &= 0xf;
+  slot->mul = mul;
+}
+
+void opna_fm_slot_set_tl(struct opna_fm_slot *slot, unsigned tl) {
+  tl &= 0x7f;
+  slot->tl = tl;
+}
+
+void opna_fm_slot_set_ks(struct opna_fm_slot *slot, unsigned ks) {
+  ks &= 0x3;
+  slot->ks = ks;
+}
+
+void opna_fm_slot_set_ar(struct opna_fm_slot *slot, unsigned ar) {
+  ar &= 0x1f;
+  slot->ar = ar;
+  if (slot->env_state == ENV_ATTACK) {
+    opna_fm_slot_setrate(slot, ENV_ATTACK);
+  }
+}
+
+void opna_fm_slot_set_dr(struct opna_fm_slot *slot, unsigned dr) {
+  dr &= 0x1f;
+  slot->dr = dr;
+  if (slot->env_state == ENV_DECAY) {
+    opna_fm_slot_setrate(slot, ENV_DECAY);
+  }
+}
+
+void opna_fm_slot_set_sr(struct opna_fm_slot *slot, unsigned sr) {
+  sr &= 0x1f;
+  slot->sr = sr;
+  if (slot->env_state == ENV_SUSTAIN) {
+    opna_fm_slot_setrate(slot, ENV_SUSTAIN);
+  }
+}
+
+void opna_fm_slot_set_sl(struct opna_fm_slot *slot, unsigned sl) {
+  sl &= 0xf;
+  slot->sl = sl;
+}
+
+void opna_fm_slot_set_rr(struct opna_fm_slot *slot, unsigned rr) {
+  rr &= 0xf;
+  slot->rr = rr;
+  if (slot->env_state == ENV_RELEASE) {
+    opna_fm_slot_setrate(slot, ENV_RELEASE);
+  }
+}
+
+void opna_fm_chan_set_blkfnum(struct opna_fm_channel *chan, unsigned blk, unsigned fnum) {
+  blk &= 0x7;
+  fnum &= 0x7ff;
+  chan->blk = blk;
+  chan->fnum = fnum;
+  for (int i = 0; i < 4; i++) {
+    chan->slot[i].keycode = blkfnum2keycode(chan->blk, chan->fnum);
+    opna_fm_slot_setrate(&chan->slot[i], chan->slot[i].env_state);
+  }
+}
+
+void opna_fm_chan_set_alg(struct opna_fm_channel *chan, unsigned alg) {
+  alg &= 0x7;
+  chan->alg = alg;
+}
+
+void opna_fm_chan_set_fb(struct opna_fm_channel *chan, unsigned fb) {
+  fb &= 0x7;
+  chan->fb = fb;
+}
+//#include <stdio.h>
+void opna_fm_writereg(struct opna_fm *fm, unsigned reg, unsigned val) {
+  val &= (1<<8)-1;
+
+  if (reg > 0x1ff) return;
+
+  switch (reg) {
+  case 0x27:
+    {
+      unsigned mode = val >> 6;
+      if (mode != fm->ch3.mode) {
+//        printf("0x27\n");
+//        printf("  mode = %d\n", mode);
+        fm->ch3.mode = mode;
+      }
+    }
+    return;
+  case 0x28:
+    {
+      int c = val & 0x3;
+      if (c == 3) return;
+      if (val & 0x4) c += 3;
+      for (int i = 0; i < 4; i++) {
+        opna_fm_slot_key(&fm->channel[c], i, (val & (1<<(4+i))));
+      }
+    }
+    return;
+  }
+
+  int c = reg & 0x3;
+  if (c == 3) return;
+  if (reg & (1<<8)) c += 3;
+  int s = ((reg & (1<<3)) >> 3) | ((reg & (1<<2)) >> 1);
+  struct opna_fm_channel *chan = &fm->channel[c];
+  struct opna_fm_slot *slot = &chan->slot[s];
+  switch (reg & 0xf0) {
+  case 0x30:
+    opna_fm_slot_set_det(slot, (val >> 4) & 0x7);
+    opna_fm_slot_set_mul(slot, val & 0xf);
+    break;
+  case 0x40:
+    opna_fm_slot_set_tl(slot, val & 0x7f);
+    break;
+  case 0x50:
+    opna_fm_slot_set_ks(slot, (val >> 6) & 0x3);
+    opna_fm_slot_set_ar(slot, val & 0x1f);
+    break;
+  case 0x60:
+    opna_fm_slot_set_dr(slot, val & 0x1f);
+    break;
+  case 0x70:
+    opna_fm_slot_set_sr(slot, val & 0x1f);
+    break;
+  case 0x80:
+    opna_fm_slot_set_sl(slot, (val >> 4) & 0xf);
+    opna_fm_slot_set_rr(slot, val & 0xf);
+    break;
+  case 0xa0:
+    {
+      unsigned blk = (fm->blkfnum_h >> 3) & 0x7;
+      unsigned fnum = ((fm->blkfnum_h & 0x7) << 8) | (val & 0xff);
+      switch (reg & 0xc) {
+      case 0x0:
+        opna_fm_chan_set_blkfnum(chan, blk, fnum);
+        break;
+      case 0x8:
+        c %= 3;
+        fm->ch3.blk[c] = blk;
+        fm->ch3.fnum[c] = fnum;
+        break;
+      case 0x4:
+      case 0xc:
+        fm->blkfnum_h = val & 0x3f;
+        break;
+      }
+    }
+    break;
+  case 0xb0:
+    switch (reg & 0xc) {
+    case 0x0:
+      opna_fm_chan_set_alg(chan, val & 0x7);
+      opna_fm_chan_set_fb(chan, (val >> 3) & 0x7);
+      break;
+    case 0x4:
+      fm->lselect[c] = val & 0x80;
+      fm->rselect[c] = val & 0x40;
+      break;
+    }
+    break;
+  }
+}
+
+void opna_fm_chan_env(struct opna_fm_channel *chan) {
+  for (int i = 0; i < 4; i++) {
+    opna_fm_slot_env(&chan->slot[i]);
+  }
+}
+
+void opna_fm_mix(struct opna_fm *fm, int16_t *buf, unsigned samples) {
+  for (unsigned i = 0; i < samples; i++) {
+    if (!fm->env_div3) {
+      for (int c = 0; c < 6; c++) {
+        opna_fm_chan_env(&fm->channel[c]);
+      }
+      fm->env_div3 = 3;
+    }
+    fm->env_div3--;
+    
+    int32_t lo = buf[i*2+0];
+    int32_t ro = buf[i*2+1];
+
+    for (int c = 0; c < 6; c++) {
+      int16_t o = opna_fm_chanout(&fm->channel[c]);
+      // TODO: CSM
+      if (c == 2 && fm->ch3.mode != CH3_MODE_NORMAL) {
+        opna_fm_chan_phase_se(&fm->channel[c], fm);
+      } else {
+        opna_fm_chan_phase(&fm->channel[c]);
+      }
+      o >>= 1;
+      if (fm->lselect[c]) lo += o;
+      if (fm->rselect[c]) ro += o;
+    }
+
+    if (lo < INT16_MIN) lo = INT16_MIN;
+    if (lo > INT16_MAX) lo = INT16_MAX;
+    if (ro < INT16_MIN) ro = INT16_MIN;
+    if (ro > INT16_MAX) ro = INT16_MAX;
+    buf[i*2+0] = lo;
+    buf[i*2+1] = ro;
+  }
+}
-- 
cgit v1.2.3