//----------------------------------------------------------------
// DSU (Digital Signal Co-processor) Register Definition
//----------------------------------------------------------------
.equ 	DSCR,	0x00
.equ	DSUEN,	7
.equ	MM,	6
.equ	DSD1,	5
.equ	DSD0,	4
.equ	DSN,	2
.equ	DSZ,	1
.equ	DSC,	0

.equ 	DSIR,	0x01
.equ 	DSSD,	0x02

.equ 	DSDX,	0x10
.equ 	DSDY,	0x11

.equ 	DSAL,	0x38
.equ 	DSAH,	0x39

.equ	DX_MULSU_DY,		0x64
.equ	DA_ADD_DX_MULSS_DY,	0x76
.equ	DY_ASR_15,		0xff
.equ	NEG_DA,			0x85


/// State variable filter adaptation from 
///	https://github.com/pichenettes/eurorack/blob/master/peaks/drums/svf.h
/// Input:
///	r24:r25 - Filter structure
/// 	r22:r23 - input sample  as int16
.global svf
svf:
	push	r14
	push	r15
	push	r16
	push	r17
	push	r28
	push	r29
	// filt->lp += f * filt->bp >> 15;
	movw	r30, r24
	ld	r14, Z
	ldd	r15, Z+1
	ldd	r20, Z+2
	ldd	r21, Z+3
	ldd	r25, Z+5
	ldd	r24, Z+4
	out	DSDX, r24
	out	DSDY, r14
	ldi	r18, DX_MULSU_DY
	out	DSIR, r18
	ldi	r25, DY_ASR_15
	out	DSIR, r25
	ldd	r27, Z+7
	ldd	r26, Z+6
	out	DSDY, r26
	ldi	r27, 0x00
	ldi	r26, 0x01
	out	DSDX, r26
	ldi	r24, DA_ADD_DX_MULSS_DY
	out	DSIR, r24
	ldd	r17, Z+5
	// filt->lp = CLIP(filt->lp);
	in	r28, DSSD
	std	Z+7, r29
	std	Z+6, r28
	// int32_t notch = in - (filt->bp * damp >> 15);
	ldd	r16, Z+4
	out	DSDX, r16
	out	DSDY, r20
	out	DSIR, r18
	out	DSIR, r25
	ldi	r19, NEG_DA
	out	DSIR, r19
	out	DSDY, r22
	out	DSDX, r26
	out	DSIR, r24
	ldd	r21, Z+7
	in	r28, DSSD
	std	Z+9, r29
	std	Z+8, r28
	// int32_t hp = notch - filt->lp;
	ldd	r20, Z+6
	out	DSDY, r20
	ldi	r21, 0xFF
	ldi	r20, 0xFF
	out	DSDX, r20
	out	DSIR, r24
	out	DSDY, r14
	in	r28, DSSD
	std	Z+11, r29
	std	Z+10, r28
	// filt->bp += f * hp >> 15;
	out	DSDX, r28
	out	DSIR, r18
	out	DSIR, r25
	ldd	r29, Z+5
	ldd	r28, Z+4
	out	DSDY, r28
	out	DSDX, r26
	out	DSIR, r24
	pop	r29
	// filt->bp = CLIP(filt->bp);
	in	r24, DSSD
	std	Z+5, r25
	std	Z+4, r24
	pop	r28
	pop	r17
	pop	r16
	pop	r15
	pop	r14
	ret