/*
 *  mpa_dematrix.s
 *
 *  MPEG/audio multichannel decoder.
 *	Copyright (C) 2003 Optical Player P/J, Samsung Electronics.
 *
 */
 
.include	"mpa.h"

.if( _DECODER_TYPE != MPEG_1_AUDIO )

.secFILE_mpa_dematrix


/********************************************************************
	Select dematrix algorithm by channel configuration.
 
  	index AABB 4 bits
    	AA - surround mode
 	   	BB - center mode
********************************************************************/ 
dematrix_process_table01:
				dl		_end_dematrixing_
				dl		dp0_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp0_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp0_channel_config_30		; 3/0 (+2/0) and 2/1

				dl		dp0_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp0_channel_config_31		; 3/1
				dl		dp0_channel_config_31		; 3/1
				dl		dp0_channel_config_31		; 3/1

				dl		dp0_channel_config_22		; 2/2
				dl		dp0_channel_config_32		; 3/2
				dl		dp0_channel_config_32		; 3/2
				dl		dp0_channel_config_32		; 3/2

				dl		_end_dematrixing_
				dl		dp0_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp0_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp0_channel_config_30		; 3/0 (+2/0) and 2/1

dematrix_process_table2:
				dl		_end_dematrixing_
				dl		dp2_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp2_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp2_channel_config_30		; 3/0 (+2/0) and 2/1

				dl		dp2_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp2_channel_config_31		; 3/1
				dl		dp2_channel_config_31		; 3/1
				dl		dp2_channel_config_31		; 3/1

				dl		dp2_channel_config_22		; 2/2
				dl		dp2_channel_config_32		; 3/2
				dl		dp2_channel_config_32		; 3/2
				dl		dp2_channel_config_32		; 3/2

				dl		_end_dematrixing_
				dl		dp2_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp2_channel_config_30		; 3/0 (+2/0) and 2/1
				dl		dp2_channel_config_30		; 3/0 (+2/0) and 2/1
				
				

/********************************************************************

 Function:		MC_II_dematricing

 Description:	Dematrix equations are used in ISO/IEC 13818-3:1997.
 					
 Arguments:		a	dematrix procedure No.

				index registers:
				@bank0
						d1		d0		s1		s0
				---------------------------------------
				sd0: 	32		96	   -96		96
				sd1: 	 x		 x	     x		 0
				sd3: 	 x		 x	    32	   -95

********************************************************************/
MC_II_dematricing::
				push		a14

				es			op

				er			xsd
				eld			sd0, #0x0000
				eld			sd3, #0x0001
				es			xsd
				eld			sd0, #0x26a6
				eld			sd3, #0x002a
				esd1		s0, #0

				eld			rp3, #fraction
				eld			mc0, #sbgr_table
				eld			mc1, #tc_alloc
				ld			r8, #GRANULES				; gr count
				ld			r7, #PARTS-1				; part count
				ld			r6, #0						; sb count
				
; search channel mode
				eld			a, rpd0.surround
				eld			b, rpd0.center
				esla		a
				esla		a
				eadd		a, b

check_dematrix_procedure
				eld			c, rpd0.dematrix_procedure
				ecld		r2, c
				cmp			eq, r2, #3
				jpt			_end_dematrixing_

				cmp			gt, r2, #1					; dematrix procedure 0, 1
				brf			dematrix_procedure_0_1
				cmp			eq, r2, #2					; dematrix procedure 2
				brt			dematrix_procedure_2


;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::;
;::                                                                ::;
;::                   Dematrix procedure 0 and 1                   ::;
;::                                                                ::;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::;
dematrix_procedure_0_1
				esla		a
				esla		a
				ecld		r3, a
				ld			a13, #dematrix_process_table01
				add			a13, r3
				ldc			r1, @a13
				add			a13, #2
				ldc			r0, @a13
				ld			e13, r1
				ld			r13, r0
				jmp			a13
				

;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::;
;:::::::::::::::::::::[  channel mode 3 / 2  ]:::::::::::::::::::::::;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::;
dp0_channel_config_32
/*loop_start*/	eld			rp0, rp3
				ecld		a, r6
				eld			rp1, mc0
				erpn		rp1, a
				eld			a, @rp1						; the number of sbgr.
				eld			rp1, mc1
				erpn		rp1, a
				eld			a, @rp1						; tc_alloc[sbgr]
				ecld		r5, a
; check_tc_alloc
				cmp			eq, r5, #0					; tc_alloc
				brt			dp0_ch_32_tc_alloc_0 
				cmp			eq, r5, #1
				brt			dp0_ch_32_tc_alloc_1	
				cmp			eq, r5, #2
				brt			dp0_ch_32_tc_alloc_2	
				cmp			eq, r5, #3
				brt			dp0_ch_32_tc_alloc_3	
				cmp			eq, r5, #4
				brt			dp0_ch_32_tc_alloc_4 
				cmp			eq, r5, #5
				brt			dp0_ch_32_tc_alloc_5	
				cmp			eq, r5, #6
				brt			dp0_ch_32_tc_alloc_6	
				cmp			eq, r5, #7
				brt			dp0_ch_32_tc_alloc_7
.ifdef _DEBUG_
/** debug **/	bra			$
.endif
				
dp0_ch_32_tc_alloc_0
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		a, d, d, @rp0+s0 
				esub		b, c
				esub		b, d
				erpn		rp0, #-(96*5)
				eld			@rp0+d0, a					; T0 = T0 - T2 - T3
				eld			@rp0+d0, b					; T1 = T1 - T2 - T4
				erps		rp3+s1
				bnzd		r7, dp0_ch_32_tc_alloc_0
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0
dp0_ch_32_tc_alloc_1
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		a, d, d, @rp0+s0 
				esub		b, a
				esub		b, d
				erpn		rp0, #-(96*5)
				eld			@rp0+d0, c
				eld			@rp0+d0, b
				eld			@rp0+d0, a
				erps		rp3+s1
				bnzd		r7, dp0_ch_32_tc_alloc_1
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0				
dp0_ch_32_tc_alloc_2
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		b, c, d, @rp0+s0 
				esub		a, d, d, @rp0+s0 
				esub		b, d 
				esub		a, b
				erpn		rp0, #-(96*5)
				eld			@rp0+d0, a
				eld			@rp0+d0, c
				eld			@rp0+d0, b
				erps		rp3+s1
				bnzd		r7, dp0_ch_32_tc_alloc_2
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0				
dp0_ch_32_tc_alloc_3
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		b, c, c, @rp0+s0  
				esub		a, d 
				esub		b, c
				erpn		rp0, #-(96*5)
				eld			@rp0+d0, d
				eld			@rp0+d0, b
				erpd		rp0+d0
				eld			@rp0+d0, a
				erps		rp3+s1
				bnzd		r7, dp0_ch_32_tc_alloc_3
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0				
dp0_ch_32_tc_alloc_4
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		a, d, d, @rp0+s0  
				esub		b, c 
				esub		b, d
				erpn		rp0, #-(96*5)
				eld			@rp0+d0, a
				eld			@rp0+d0, d
				erpd		rp0+d0
				erpd		rp0+d0
				eld			@rp0+d0, b
				erps		rp3+s1
				bnzd		r7, dp0_ch_32_tc_alloc_4
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0				
dp0_ch_32_tc_alloc_5
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		b, c 
				esub		a, d, c, @rp0+s0  
				esub		b, c
				erpn		rp0, #-(96*5)
				eld			@rp0+d0, d
				eld			@rp0+d0, c
				erpd		rp0+d0
				eld			@rp0+d0, a
				eld			@rp0+d0, b
				erps		rp3+s1
				bnzd		r7, dp0_ch_32_tc_alloc_5
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0				
dp0_ch_32_tc_alloc_6
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		b, c, d, @rp0+s0 
				esub		a, d, d, @rp0+s1
				esub		b, d, d, @rp0+s0
				esub		a, b
				erpn		rp0, #-(96*4)
				eld			@rp0+d0, d
				eld			@rp0+d0, c
				eld			@rp0+d0, b
				eld			@rp0+d0, a
				erps		rp3+s1
				bnzd		r7, dp0_ch_32_tc_alloc_6
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0				
dp0_ch_32_tc_alloc_7
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		a, d, d, @rp0+s0
				esub		b, d
				esub		b, a
				erpn		rp0, #-(96*5)
				eld			@rp0+d0, c
				eld			@rp0+d0, d
				eld			@rp0+d0, a
				erpd		rp0+d0
				eld			@rp0+d0, b
				erps		rp3+s1
				bnzd		r7, dp0_ch_32_tc_alloc_7
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0				
				
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::;
;:::::::::::::::::::::[  channel mode 3 / 1  ]:::::::::::::::::::::::;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::;
dp0_channel_config_31
/*loop_start*/	eld			rp0, rp3
				ecld		a, r6
				eld			rp1, mc0
				erpn		rp1, a
				eld			a, @rp1						; the number of sbgr.
				eld			rp1, mc1
				erpn		rp1, a
				eld			a, @rp1						; tc_alloc[sbgr]
				ecld		r5, a
; check_tc_alloc
				cmp			eq, r5, #0					; tc_alloc
				brt			dp0_ch_31_tc_alloc_0 
				cmp			eq, r5, #1
				brt			dp0_ch_31_tc_alloc_1	
				cmp			eq, r5, #2
				brt			dp0_ch_31_tc_alloc_2	
				cmp			eq, r5, #3
				brt			dp0_ch_31_tc_alloc_3	
				cmp			eq, r5, #4
				brt			dp0_ch_31_tc_alloc_4 
.ifdef _DEBUG_
/** debug **/	bra			$
.endif
				
dp0_ch_31_tc_alloc_0
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		a, d
				esub		b, c
				esub		b, d
				erpn		rp0, #-(96*4)
				eld			@rp0+d0, a
				eld			@rp0+d0, b
				erps		rp3+s1
				bnzd		r7, dp0_ch_31_tc_alloc_0
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0
dp0_ch_31_tc_alloc_1
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		a, d
				esub		b, a
				esub		b, d
				erpn		rp0, #-(96*4)
				eld			@rp0+d0, c
				eld			@rp0+d0, b
				eld			@rp0+d0, a
				erps		rp3+s1
				bnzd		r7, dp0_ch_31_tc_alloc_1
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0
dp0_ch_31_tc_alloc_2
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		b, c, d, @rp0+s0 
				esub		b, d
				esub		a, b
				esub		a, d
				erpn		rp0, #-(96*4)
				eld			@rp0+d0, a
				eld			@rp0+d0, c
				eld			@rp0+d0, b
				erps		rp3+s1
				bnzd		r7, dp0_ch_31_tc_alloc_2
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0
dp0_ch_31_tc_alloc_3
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c, d, @rp0+s0 
				esub		a, d
				esub		b, c
				esub		b, a
				erpn		rp0, #-(96*4)
				eld			@rp0+d0, d
				eld			@rp0+d0, b
				erpd		rp0+d0
				eld			@rp0+d0, a
				erps		rp3+s1
				bnzd		r7, dp0_ch_31_tc_alloc_3
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0
dp0_ch_31_tc_alloc_4
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		b, c, d, @rp0+s0 
				esub		b, d
				esub		a, c
				esub		a, b
				erpn		rp0, #-(96*4)
				eld			@rp0+d0, a
				eld			@rp0+d0, d
				erpd		rp0+d0
				eld			@rp0+d0, b
				erps		rp3+s1
				bnzd		r7, dp0_ch_31_tc_alloc_4
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0

				
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::;
;:::::::::::::::[  channel mode 3/0 (+2/0) and 2/1  ]::::::::::::::::;
;::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::;
dp0_channel_config_30
/*loop_start*/	eld			rp0, rp3
				ecld		a, r6
				eld			rp1, mc0
				erpn		rp1, a
				eld			a, @rp1						; the number of sbgr.
				eld			rp1, mc1
				erpn		rp1, a
				eld			a, @rp1						; tc_alloc[sbgr]
				ecld		r5, a
; check_tc_alloc
				cmp			eq, r5, #0					; tc_alloc
				brt			dp0_ch_30_tc_alloc_0 
				cmp			eq, r5, #1
				brt			dp0_ch_30_tc_alloc_1	
				cmp			eq, r5, #2
				brt			dp0_ch_30_tc_alloc_2	
.ifdef _DEBUG_
/** debug **/	bra			$
.endif

dp0_ch_30_tc_alloc_0
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c
				esub		b, c
				erpn		rp0, #-(96*3)
				eld			@rp0+d0, a
				eld			@rp0+d0, b
				erps		rp3+s1
				bnzd		r7, dp0_ch_30_tc_alloc_0
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0
dp0_ch_30_tc_alloc_1
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		a, c
				esub		b, a
				erpn		rp0, #-(96*3)
				eld			@rp0+d0, c
				eld			@rp0+d0, b
				eld			@rp0+d0, a
				erps		rp3+s1
				bnzd		r7, dp0_ch_30_tc_alloc_1
				eld			rp0, rp3				
				brad		dp0_loop_sblimit
				erps		rp3+s0
dp0_ch_30_tc_alloc_2
				eld			a, @rp0+s0
				eld			b, @rp0+s0
				eld			c, @rp0+s0
				esub		b, c
				esub		a, b
				erpn		rp0, #-(96*3)
				eld			@rp0+d0, a
				eld			@rp0+d0, c