[picodrive.git] / pico / 32x / pwm.c

/*
 * PicoDrive
 * (C) notaz, 2009,2010,2013
 *
 * This work is licensed under the terms of MAME license.
 * See COPYING file in the top-level directory.
 */
#include "../pico_int.h"

static int pwm_cycles;
static int pwm_mult;
static int pwm_ptr;
static int pwm_irq_reload;

void p32x_pwm_ctl_changed(void)
{
  int control = Pico32x.regs[0x30 / 2];
  int cycles = Pico32x.regs[0x32 / 2];

  cycles = (cycles - 1) & 0x0fff;
  pwm_cycles = cycles;
  pwm_mult = 0x10000 / cycles;

  pwm_irq_reload = (control & 0x0f00) >> 8;
  pwm_irq_reload = ((pwm_irq_reload - 1) & 0x0f) + 1;

  if (Pico32x.pwm_irq_cnt == 0)
    Pico32x.pwm_irq_cnt = pwm_irq_reload;
}

static void do_pwm_irq(SH2 *sh2, unsigned int m68k_cycles)
{
  Pico32x.sh2irqs |= P32XI_PWM;
  p32x_update_irls(sh2, m68k_cycles);

  if (Pico32x.regs[0x30 / 2] & P32XP_RTP) {
    p32x_event_schedule(m68k_cycles, P32X_EVENT_PWM, pwm_cycles / 3 + 1);
    // note: might recurse
    p32x_dreq1_trigger();
  }
}

#define consume_fifo(sh2, m68k_cycles) { \
  int cycles_diff = ((m68k_cycles) * 3) - Pico32x.pwm_cycle_p; \
  if (cycles_diff >= pwm_cycles) \
    consume_fifo_do(sh2, m68k_cycles, cycles_diff); \
}

static void consume_fifo_do(SH2 *sh2, unsigned int m68k_cycles,
  int sh2_cycles_diff)
{
  int do_irq = 0;

  if (pwm_cycles == 0)
    return;

  elprintf(EL_PWM, "pwm: %u: consume %d/%d, %d,%d ptr %d",
    m68k_cycles, sh2_cycles_diff, sh2_cycles_diff / pwm_cycles,
    Pico32x.pwm_p[0], Pico32x.pwm_p[1], pwm_ptr);

  if (sh2_cycles_diff >= pwm_cycles * 17) {
    // silence/skip
    Pico32x.pwm_cycle_p = m68k_cycles * 3;
    Pico32x.pwm_p[0] = Pico32x.pwm_p[1] = 0;
    return;
  }

  while (sh2_cycles_diff >= pwm_cycles) {
    struct Pico32xMem *mem = Pico32xMem;
    short *fifo_l = mem->pwm_fifo[0];
    short *fifo_r = mem->pwm_fifo[1];

    if (Pico32x.pwm_p[0] > 0) {
      fifo_l[0] = fifo_l[1];
      fifo_l[1] = fifo_l[2];
      fifo_l[2] = fifo_l[3];
      Pico32x.pwm_p[0]--;
    }
    if (Pico32x.pwm_p[1] > 0) {
      fifo_r[0] = fifo_r[1];
      fifo_r[1] = fifo_r[2];
      fifo_r[2] = fifo_r[3];
      Pico32x.pwm_p[1]--;
    }

    mem->pwm[pwm_ptr * 2    ] = fifo_l[0];
    mem->pwm[pwm_ptr * 2 + 1] = fifo_r[0];
    pwm_ptr = (pwm_ptr + 1) & (PWM_BUFF_LEN - 1);

    sh2_cycles_diff -= pwm_cycles;

    if (--Pico32x.pwm_irq_cnt == 0) {
      Pico32x.pwm_irq_cnt = pwm_irq_reload;
      // irq also does dreq1, so call it after cycle update
      do_irq = 1;
      break;
    }
  }
  Pico32x.pwm_cycle_p = m68k_cycles * 3 - sh2_cycles_diff;

  if (do_irq)
    do_pwm_irq(sh2, m68k_cycles);
}

static int p32x_pwm_schedule_(SH2 *sh2, unsigned int m68k_now)
{
  unsigned int sh2_now = m68k_now * 3;
  int cycles_diff_sh2;

  if (pwm_cycles == 0)
    return 0;

  cycles_diff_sh2 = sh2_now - Pico32x.pwm_cycle_p;
  if (cycles_diff_sh2 >= pwm_cycles)
    consume_fifo_do(sh2, m68k_now, cycles_diff_sh2);

  if (Pico32x.sh2irqs & P32XI_PWM)
    return 0; // previous not acked
  if (!((Pico32x.sh2irq_mask[0] | Pico32x.sh2irq_mask[1]) & 1))
    return 0; // masked by everyone

  cycles_diff_sh2 = sh2_now - Pico32x.pwm_cycle_p;
  return (Pico32x.pwm_irq_cnt * pwm_cycles
           - cycles_diff_sh2) / 3 + 1;
}

void p32x_pwm_schedule(unsigned int m68k_now)
{
  int after = p32x_pwm_schedule_(NULL, m68k_now);
  if (after != 0)
    p32x_event_schedule(m68k_now, P32X_EVENT_PWM, after);
}

void p32x_pwm_schedule_sh2(SH2 *sh2)
{
  int after = p32x_pwm_schedule_(sh2, sh2_cycles_done_m68k(sh2));
  if (after != 0)
    p32x_event_schedule_sh2(sh2, P32X_EVENT_PWM, after);
}

void p32x_pwm_irq_event(unsigned int m68k_now)
{
  p32x_pwm_schedule(m68k_now);
}

unsigned int p32x_pwm_read16(unsigned int a, SH2 *sh2,
  unsigned int m68k_cycles)
{
  unsigned int d = 0;

  consume_fifo(sh2, m68k_cycles);

  a &= 0x0e;
  switch (a) {
    case 0: // control
    case 2: // cycle
      d = Pico32x.regs[(0x30 + a) / 2];
      break;

    case 4: // L ch
      if (Pico32x.pwm_p[0] == 3)
        d |= P32XP_FULL;
      else if (Pico32x.pwm_p[0] == 0)
        d |= P32XP_EMPTY;
      break;

    case 6: // R ch
    case 8: // MONO
      if (Pico32x.pwm_p[1] == 3)
        d |= P32XP_FULL;
      else if (Pico32x.pwm_p[1] == 0)
        d |= P32XP_EMPTY;
      break;
  }

  elprintf(EL_PWM, "pwm: %u: r16 %02x %04x (p %d %d)",
    m68k_cycles, a, d, Pico32x.pwm_p[0], Pico32x.pwm_p[1]);
  return d;
}

void p32x_pwm_write16(unsigned int a, unsigned int d,
  SH2 *sh2, unsigned int m68k_cycles)
{
  elprintf(EL_PWM, "pwm: %u: w16 %02x %04x (p %d %d)",
    m68k_cycles, a & 0x0e, d, Pico32x.pwm_p[0], Pico32x.pwm_p[1]);

  consume_fifo(sh2, m68k_cycles);

  a &= 0x0e;
  if (a == 0) { // control
    // supposedly we should stop FIFO when xMd is 0,
    // but mars test disagrees
    Pico32x.regs[0x30 / 2] = d;
    p32x_pwm_ctl_changed();
    Pico32x.pwm_irq_cnt = pwm_irq_reload; // ?
  }
  else if (a == 2) { // cycle
    Pico32x.regs[0x32 / 2] = d & 0x0fff;
    p32x_pwm_ctl_changed();
  }
  else if (a <= 8) {
    d = (d - 1) & 0x0fff;
    if (d > pwm_cycles)
      d = pwm_cycles;
    d = (d - pwm_cycles / 2) * pwm_mult;

    if (a == 4 || a == 8) { // L ch or MONO
      short *fifo = Pico32xMem->pwm_fifo[0];
      if (Pico32x.pwm_p[0] < 3)
        Pico32x.pwm_p[0]++;
      else {
        fifo[1] = fifo[2];
        fifo[2] = fifo[3];
      }
      fifo[Pico32x.pwm_p[0]] = d;
    }
    if (a == 6 || a == 8) { // R ch or MONO
      short *fifo = Pico32xMem->pwm_fifo[1];
      if (Pico32x.pwm_p[1] < 3)
        Pico32x.pwm_p[1]++;
      else {
        fifo[1] = fifo[2];
        fifo[2] = fifo[3];
      }
      fifo[Pico32x.pwm_p[1]] = d;
    }
  }
}

void p32x_pwm_update(int *buf32, int length, int stereo)
{
  short *pwmb;
  int step;
  int p = 0;
  int xmd;

  xmd = Pico32x.regs[0x30 / 2] & 0x0f;
  if ((xmd != 0x05 && xmd != 0x0a) || pwm_ptr <= 16)
    goto out;

  step = (pwm_ptr << 16) / length; // FIXME: division..
  pwmb = Pico32xMem->pwm;

  if (stereo)
  {
    if (xmd == 0x0a) {
      // channel swap
      while (length-- > 0) {
        *buf32++ += pwmb[1];
        *buf32++ += pwmb[0];

        p += step;
        pwmb += (p >> 16) * 2;
        p &= 0xffff;
      }
    }
    else {
      while (length-- > 0) {
        *buf32++ += pwmb[0];
        *buf32++ += pwmb[1];

        p += step;
        pwmb += (p >> 16) * 2;
        p &= 0xffff;
      }
    }
  }
  else
  {
    while (length-- > 0) {
      *buf32++ += pwmb[0];

      p += step;
      pwmb += (p >> 16) * 2;
      p &= 0xffff;
    }
  }

  elprintf(EL_PWM, "pwm_update: pwm_ptr %d, len %d, step %04x, done %d",
    pwm_ptr, length, step, (pwmb - Pico32xMem->pwm) / 2);

out:
  pwm_ptr = 0;
}

void p32x_pwm_state_loaded(void)
{
  int cycles_diff_sh2;

  p32x_pwm_ctl_changed();

  // for old savestates
  cycles_diff_sh2 = SekCycleCntT * 3 - Pico32x.pwm_cycle_p;
  if (cycles_diff_sh2 >= pwm_cycles || cycles_diff_sh2 < 0) {
    Pico32x.pwm_irq_cnt = pwm_irq_reload;
    Pico32x.pwm_cycle_p = SekCycleCntT * 3;
    p32x_pwm_schedule(SekCycleCntT);
  }
}

// vim:shiftwidth=2:ts=2:expandtab
Commit	Line	Data
	1	/*
	2	* PicoDrive
	3	* (C) notaz, 2009,2010,2013
	4	*
	5	* This work is licensed under the terms of MAME license.
	6	* See COPYING file in the top-level directory.
	7	*/
	8	#include "../pico_int.h"
	9
	10	static int pwm_cycles;
	11	static int pwm_mult;
	12	static int pwm_ptr;
	13	static int pwm_irq_reload;
	14
	15	void p32x_pwm_ctl_changed(void)
	16	{
	17	int control = Pico32x.regs[0x30 / 2];
	18	int cycles = Pico32x.regs[0x32 / 2];
	19
	20	cycles = (cycles - 1) & 0x0fff;
	21	pwm_cycles = cycles;
	22	pwm_mult = 0x10000 / cycles;
	23
	24	pwm_irq_reload = (control & 0x0f00) >> 8;
	25	pwm_irq_reload = ((pwm_irq_reload - 1) & 0x0f) + 1;
	26
	27	if (Pico32x.pwm_irq_cnt == 0)
	28	Pico32x.pwm_irq_cnt = pwm_irq_reload;
	29	}
	30
	31	static void do_pwm_irq(SH2 *sh2, unsigned int m68k_cycles)
	32	{
	33	Pico32x.sh2irqs \|= P32XI_PWM;
	34	p32x_update_irls(sh2, m68k_cycles);
	35
	36	if (Pico32x.regs[0x30 / 2] & P32XP_RTP) {
	37	p32x_event_schedule(m68k_cycles, P32X_EVENT_PWM, pwm_cycles / 3 + 1);
	38	// note: might recurse
	39	p32x_dreq1_trigger();
	40	}
	41	}
	42
	43	#define consume_fifo(sh2, m68k_cycles) { \
	44	int cycles_diff = ((m68k_cycles) * 3) - Pico32x.pwm_cycle_p; \
	45	if (cycles_diff >= pwm_cycles) \
	46	consume_fifo_do(sh2, m68k_cycles, cycles_diff); \
	47	}
	48
	49	static void consume_fifo_do(SH2 *sh2, unsigned int m68k_cycles,
	50	int sh2_cycles_diff)
	51	{
	52	int do_irq = 0;
	53
	54	if (pwm_cycles == 0)
	55	return;
	56
	57	elprintf(EL_PWM, "pwm: %u: consume %d/%d, %d,%d ptr %d",
	58	m68k_cycles, sh2_cycles_diff, sh2_cycles_diff / pwm_cycles,
	59	Pico32x.pwm_p[0], Pico32x.pwm_p[1], pwm_ptr);
	60
	61	if (sh2_cycles_diff >= pwm_cycles * 17) {
	62	// silence/skip
	63	Pico32x.pwm_cycle_p = m68k_cycles * 3;
	64	Pico32x.pwm_p[0] = Pico32x.pwm_p[1] = 0;
	65	return;
	66	}
	67
	68	while (sh2_cycles_diff >= pwm_cycles) {
	69	struct Pico32xMem *mem = Pico32xMem;
	70	short *fifo_l = mem->pwm_fifo[0];
	71	short *fifo_r = mem->pwm_fifo[1];
	72
	73	if (Pico32x.pwm_p[0] > 0) {
	74	fifo_l[0] = fifo_l[1];
	75	fifo_l[1] = fifo_l[2];
	76	fifo_l[2] = fifo_l[3];
	77	Pico32x.pwm_p[0]--;
	78	}
	79	if (Pico32x.pwm_p[1] > 0) {
	80	fifo_r[0] = fifo_r[1];
	81	fifo_r[1] = fifo_r[2];
	82	fifo_r[2] = fifo_r[3];
	83	Pico32x.pwm_p[1]--;
	84	}
	85
	86	mem->pwm[pwm_ptr * 2 ] = fifo_l[0];
	87	mem->pwm[pwm_ptr * 2 + 1] = fifo_r[0];
	88	pwm_ptr = (pwm_ptr + 1) & (PWM_BUFF_LEN - 1);
	89
	90	sh2_cycles_diff -= pwm_cycles;
	91
	92	if (--Pico32x.pwm_irq_cnt == 0) {
	93	Pico32x.pwm_irq_cnt = pwm_irq_reload;
	94	// irq also does dreq1, so call it after cycle update
	95	do_irq = 1;
	96	break;
	97	}
	98	}
	99	Pico32x.pwm_cycle_p = m68k_cycles * 3 - sh2_cycles_diff;
	100
	101	if (do_irq)
	102	do_pwm_irq(sh2, m68k_cycles);
	103	}
	104
	105	static int p32x_pwm_schedule_(SH2 *sh2, unsigned int m68k_now)
	106	{
	107	unsigned int sh2_now = m68k_now * 3;
	108	int cycles_diff_sh2;
	109
	110	if (pwm_cycles == 0)
	111	return 0;
	112
	113	cycles_diff_sh2 = sh2_now - Pico32x.pwm_cycle_p;
	114	if (cycles_diff_sh2 >= pwm_cycles)
	115	consume_fifo_do(sh2, m68k_now, cycles_diff_sh2);
	116
	117	if (Pico32x.sh2irqs & P32XI_PWM)
	118	return 0; // previous not acked
	119	if (!((Pico32x.sh2irq_mask[0] \| Pico32x.sh2irq_mask[1]) & 1))
	120	return 0; // masked by everyone
	121
	122	cycles_diff_sh2 = sh2_now - Pico32x.pwm_cycle_p;
	123	return (Pico32x.pwm_irq_cnt * pwm_cycles
	124	- cycles_diff_sh2) / 3 + 1;
	125	}
	126
	127	void p32x_pwm_schedule(unsigned int m68k_now)
	128	{
	129	int after = p32x_pwm_schedule_(NULL, m68k_now);
	130	if (after != 0)
	131	p32x_event_schedule(m68k_now, P32X_EVENT_PWM, after);
	132	}
	133
	134	void p32x_pwm_schedule_sh2(SH2 *sh2)
	135	{
	136	int after = p32x_pwm_schedule_(sh2, sh2_cycles_done_m68k(sh2));
	137	if (after != 0)
	138	p32x_event_schedule_sh2(sh2, P32X_EVENT_PWM, after);
	139	}
	140
	141	void p32x_pwm_irq_event(unsigned int m68k_now)
	142	{
	143	p32x_pwm_schedule(m68k_now);
	144	}
	145
	146	unsigned int p32x_pwm_read16(unsigned int a, SH2 *sh2,
	147	unsigned int m68k_cycles)
	148	{
	149	unsigned int d = 0;
	150
	151	consume_fifo(sh2, m68k_cycles);
	152
	153	a &= 0x0e;
	154	switch (a) {
	155	case 0: // control
	156	case 2: // cycle
	157	d = Pico32x.regs[(0x30 + a) / 2];
	158	break;
	159
	160	case 4: // L ch
	161	if (Pico32x.pwm_p[0] == 3)
	162	d \|= P32XP_FULL;
	163	else if (Pico32x.pwm_p[0] == 0)
	164	d \|= P32XP_EMPTY;
	165	break;
	166
	167	case 6: // R ch
	168	case 8: // MONO
	169	if (Pico32x.pwm_p[1] == 3)
	170	d \|= P32XP_FULL;
	171	else if (Pico32x.pwm_p[1] == 0)
	172	d \|= P32XP_EMPTY;
	173	break;
	174	}
	175
	176	elprintf(EL_PWM, "pwm: %u: r16 %02x %04x (p %d %d)",
	177	m68k_cycles, a, d, Pico32x.pwm_p[0], Pico32x.pwm_p[1]);
	178	return d;
	179	}
	180
	181	void p32x_pwm_write16(unsigned int a, unsigned int d,
	182	SH2 *sh2, unsigned int m68k_cycles)
	183	{
	184	elprintf(EL_PWM, "pwm: %u: w16 %02x %04x (p %d %d)",
	185	m68k_cycles, a & 0x0e, d, Pico32x.pwm_p[0], Pico32x.pwm_p[1]);
	186
	187	consume_fifo(sh2, m68k_cycles);
	188
	189	a &= 0x0e;
	190	if (a == 0) { // control
	191	// supposedly we should stop FIFO when xMd is 0,
	192	// but mars test disagrees
	193	Pico32x.regs[0x30 / 2] = d;
	194	p32x_pwm_ctl_changed();
	195	Pico32x.pwm_irq_cnt = pwm_irq_reload; // ?
	196	}
	197	else if (a == 2) { // cycle
	198	Pico32x.regs[0x32 / 2] = d & 0x0fff;
	199	p32x_pwm_ctl_changed();
	200	}
	201	else if (a <= 8) {
	202	d = (d - 1) & 0x0fff;
	203	if (d > pwm_cycles)
	204	d = pwm_cycles;
	205	d = (d - pwm_cycles / 2) * pwm_mult;
	206
	207	if (a == 4 \|\| a == 8) { // L ch or MONO
	208	short *fifo = Pico32xMem->pwm_fifo[0];
	209	if (Pico32x.pwm_p[0] < 3)
	210	Pico32x.pwm_p[0]++;
	211	else {
	212	fifo[1] = fifo[2];
	213	fifo[2] = fifo[3];
	214	}
	215	fifo[Pico32x.pwm_p[0]] = d;
	216	}
	217	if (a == 6 \|\| a == 8) { // R ch or MONO
	218	short *fifo = Pico32xMem->pwm_fifo[1];
	219	if (Pico32x.pwm_p[1] < 3)
	220	Pico32x.pwm_p[1]++;
	221	else {
	222	fifo[1] = fifo[2];
	223	fifo[2] = fifo[3];
	224	}
	225	fifo[Pico32x.pwm_p[1]] = d;
	226	}
	227	}
	228	}
	229
	230	void p32x_pwm_update(int *buf32, int length, int stereo)
	231	{
	232	short *pwmb;
	233	int step;
	234	int p = 0;
	235	int xmd;
	236
	237	xmd = Pico32x.regs[0x30 / 2] & 0x0f;
	238	if ((xmd != 0x05 && xmd != 0x0a) \|\| pwm_ptr <= 16)
	239	goto out;
	240
	241	step = (pwm_ptr << 16) / length; // FIXME: division..
	242	pwmb = Pico32xMem->pwm;
	243
	244	if (stereo)
	245	{
	246	if (xmd == 0x0a) {
	247	// channel swap
	248	while (length-- > 0) {
	249	*buf32++ += pwmb[1];
	250	*buf32++ += pwmb[0];
	251
	252	p += step;
	253	pwmb += (p >> 16) * 2;
	254	p &= 0xffff;
	255	}
	256	}
	257	else {
	258	while (length-- > 0) {
	259	*buf32++ += pwmb[0];
	260	*buf32++ += pwmb[1];
	261
	262	p += step;
	263	pwmb += (p >> 16) * 2;
	264	p &= 0xffff;
	265	}
	266	}
	267	}
	268	else
	269	{
	270	while (length-- > 0) {
	271	*buf32++ += pwmb[0];
	272
	273	p += step;
	274	pwmb += (p >> 16) * 2;
	275	p &= 0xffff;
	276	}
	277	}
	278
	279	elprintf(EL_PWM, "pwm_update: pwm_ptr %d, len %d, step %04x, done %d",
	280	pwm_ptr, length, step, (pwmb - Pico32xMem->pwm) / 2);
	281
	282	out:
	283	pwm_ptr = 0;
	284	}
	285
	286	void p32x_pwm_state_loaded(void)
	287	{
	288	int cycles_diff_sh2;
	289
	290	p32x_pwm_ctl_changed();
	291
	292	// for old savestates
	293	cycles_diff_sh2 = SekCycleCntT * 3 - Pico32x.pwm_cycle_p;
	294	if (cycles_diff_sh2 >= pwm_cycles \|\| cycles_diff_sh2 < 0) {
	295	Pico32x.pwm_irq_cnt = pwm_irq_reload;
	296	Pico32x.pwm_cycle_p = SekCycleCntT * 3;
	297	p32x_pwm_schedule(SekCycleCntT);
	298	}
	299	}
	300
	301	// vim:shiftwidth=2:ts=2:expandtab