	TITLE		"16F84 mobile Decoder Program Ver. 1.05"
;			For Decod10 PCB layout
;
;
;
; Assembly options
	LIST	P=16F84,r=hex,N=75,C=120,T=ON
;
;
	include 	"P16f84.inc"
	__IDLOCS	h'0001'
	__CONFIG	h'3FF6' ; PUTimer on, WDT on, HS clock
;
;	mods by MB	
;	added speed table values
;	added motor damping  23/1/00
;	error in Func_1 corrected  27/01/04
;-----------------------------------------
; constants and mem locations	
;
; FLG == |LV-7|VP-6|VB-5|LS-4|H-3|SC-0:2|
;
LV	equ 7		; Last bit value
VP	equ 6		; Valid Packet
VB	equ 5		; Valid Byte
H	equ 3		; Half-Bit
H_msk	equ b'00001000' ; mask of half-bit
LS	equ 4		; Last sample
SC_msb	equ 2		; sample count MSB
LS_msk	equ B'00010000' ; mask for last sample
;

;-----------------------------------------------
; FLG2 = 0000000|AM
AM	equ	0		; Analog Mode
;-----------------------------------------------
; Effect bits
;
EF_SPD		equ	7		; Speed related counter (by pwm)
EF_SPD_msk	equ	b'10000000'
EF_QA		equ	6		; Qtr Sec phase A
EF_QA_msk	equ	b'01000000'
EF_QB		equ	5		; Qtr Sec phase B
EF_QB_msk	equ 	b'00100000'
EF_Fwd		equ 	4		; Fwd ON
EF_Fwd_msk	equ	b'00010000'
EF_Rev		equ 	3
EF_Rev_msk	equ 	b'00001000'
EF_MARs 	equ 	2		; Off-Dim-On-Dim-
EF_MARs_msk 	equ 	b'00000100'
EF_ST		equ 	1		; Strobe Light
EF_ST_msk	equ 	b'00000010'
EF_Dim		equ 	0
EF_Dim_msk	equ 	b'00000001'

;-----------------------------------------------
; Logical Function control bits
;
#DEFINE 	FN_NUM	5	; set this to the total no. of functions (including fl/rl)

FN_FL		equ 0
FN_RL		equ 1
FN_F1		equ 2
FN_F2		equ 3
FN_F3		equ 4
FN_F4		equ 5
FN_F5		equ 6
FN_RST		equ 7


;-----------------------------------------------
; PIO bits
;
;  The following definitions should be changed 
;  according to pin assignments   
;
#DEFINE 	OUT_fl_port 	PORTB
OUT_fl		equ 	4			; forward light
OUT_fl_msk	equ b'00010000'
#DEFINE 	OUT_rl_port 	PORTB
OUT_rl		equ 5				; reverse light
OUT_rl_msk	equ b'00100000'
#DEFINE 	OUT_f1_port 	PORTB			;*
OUT_f1		equ 6				; Function 1		 *
OUT_f1_msk	equ b'01000000' 		;*
#DEFINE 	OUT_f2_port 	PORTA			;*
OUT_f2		equ 0				; Function 2		 *
OUT_f2_msk	equ b'00000001' 		;*
#DEFINE 	OUT_f3_port PORTA			;*
OUT_f3		equ 1				; Function 3		 *
OUT_f3_msk	equ b'00000010' 		;*

;
; Set the following mask as a combination of all the outputs controlled by logical Fn's in PORTA
;
PORTA_FN_msk	 equ OUT_f2_msk | OUT_f3_msk
PORTB_FN_msk	 equ OUT_fl_msk | OUT_rl_msk | OUT_f1_msk  ;*
;
;	functions f2 and f3 on port a  (MB dec10)
;	functions fl, rl and F1 on port B  (MB dec 10)
;
; 
;
#DEFINE 	IN_OverVolt_port	PORTB		;not used
IN_OverVolt	equ 7	; Over-voltage or Marklin analog detection circuit
IN_OverVolt_msk equ	b'10000000'
#DEFINE 	OUT_lmp_port	PORTB			;not used
OUT_lmp 	equ 1	; lights on/off
OUT_lmp_msk equ b'00000010'
#DEFINE 	OUT_PWM_port	PORTA
OUT_PWM 	equ 2	; pwm signal
OUT_PWM_msk equ b'00000100'
#DEFINE 	OUT_fwd_port	PORTB			;*
OUT_fwd 	equ 0	; forward dir		 *
OUT_fwd_msk equ b'00000001'
#DEFINE 	OUT_back_port	PORTB			;*
OUT_back	equ 1	; backward dir		 *
OUT_back_msk	equ b'00000010' 		;*
#DEFINE 	IN_port PORTB				;*
IN_bit		equ 2	; rail input pin	 *
IN_bit_msk	equ b'00000100'  		;*

; >>> added to control TRIS registers correctly Gil 7/10/2000
; These masks should have '1' in all input bits
;
; input pins
PORTA_Input_msk equ	0				; for normal operation
PORTB_Input_msk equ	IN_bit_msk | IN_OverVolt_msk  	
; input pins in analog mode
PORTA_In_Analog_msk equ	OUT_PWM_msk				; for analog mode - see set_power
PORTB_In_Analog_msk equ	IN_bit_msk | IN_OverVolt_msk | OUT_fwd_msk | OUT_back_msk
; pins that should be '1' in analog mode (CV13 not implemented yet)
PORTA_ON_Analog_msk equ	0
PORTB_ON_Analog_msk equ	OUT_fl_msk | OUT_rl_msk

; >>>> end new masks Gil 7/10/2000
;
;For MB system, OUT_back_port and OUT_fwd_port must be the same port
;
; >>> Gil set motor damping mask - to pre-initialized constant (it is not changed)
Motor_Dmp_msk	equ OUT_fwd_msk | OUT_back_msk

; Set the following mask as a combination of all the outputs controlled by logical Fn's in PORTB
;
;PORTB_FN_msk	equ OUT_f1_msk | OUT_f2_msk | OUT_f3_msk	;* set previously
;
; Set the following mask with 1's in every position where an output is active low
;
Out_Active_Low	equ 0	;OUT_fl_msk | OUT_rl_msk	all active high  *	
;

;------------------------
; Timing constants
;
FREQ		equ     8	; 8MHz, 12, 16, 20 or 24
SAMPLE_PRD	equ d'44'	; 44 usec
PWM_CNT_PRESET	equ d'80'	; the number of 44usec cycles to increment/decrement PWM threshold (256 increments) 
							;	 - based on formula t(incr) = (CV#3 + CV#23) * t = [(CV#3 + CV#23) * .896] / 256
							;	 - PWM_CNT_PRESET = t / 44us =	.896s / (256 * 44us)
CHAF_PRESET_CNT equ d'255'	; 255 * 80 * 44 == .9 sec
SVC_TO_PRESET	equ d'6'	;  6 * 80 * 44 ==  20 ms
STEP_MULTIPLIER equ d'151'	; 255*16/151 = 27
PREAMBLE_CNT	equ d'18'

;------------------------
; Command bits
;
CMD_dir 	equ 5
CMD_dir_msk equ b'00100000' 	
;-------------------------------------
; volatile memory
;
	CBLOCK	0x000C
; PWM variables
		PWM_step	; current step256		0c
		PWM_cnt 	; counter			0d
		PWM_step_tgt	; for accel/decel effect	0e
		PWM_thr 	; current threshold 	0f				
		PWM_Accel_cnt	; counts the 44us cycles between accel updates	10
		PWM_timer	; contents of VC#3 or VC#4, decremented to 0	11
		TStep		; time of speed step in usec			12
		PWM_t		; accumulated time of step			13
		state		; state machine state				14
		SVC_timer	; expiration of svc mode			15
		Effect_cnt	; rolls ~every sec				16
;---------------------------------------------------------------------------------
;	.5s | .25sA | .25sB | Fwd | Rev | Mars | Strb | Dim
;		  00000000 - On/Off all the time.
		ON_Effect	; mask for special effects			17
;
; Byte variables
		Buf 	; byte buffer	addr				18
		B_cnt		; counter for packet length 		19	
		Err 	; error byte					1a
; ----- Addr bytes ------
		Addr2_b 	; cover extended addressing 		1b
		Addr_b		; packet bytes - address			1c
; ----- Instruction bytes + error byte -------
		Cmd_b		; " - command					1d
		Chk_b		; " - check					1e
		Sup_1		; long instructions 			1f
		Sup_2		;						20
		LastAddr2_b ;						21
		LastAddr_b	;						22
		LastCmd_b	;						23
		LastChk_b	;						24
		LastSup1	;						25
		LastSup2	;						26
;---------------------------------------------------
; Configuration Vars
;

		FLG 		; Flag word (byte)				27
		FLG2		; second Flag					28
; volatile memory

		MEM_FILE_S:0	; start of file
		Active_addr 	;stores the active address of decoder		29
		Decoder_addr	; -"- decoder original addr			2a
		Arg1		; general purpose args				2b
		Arg2		;						2c
		Arg3		;						2d						
		Arg4		;						2e
		Arg5		;						2f
		Arg6		;						30
		DEC_STATUS	; Decoder status word				31
		Page_low	; Register paging - 53,54			32
		Page_high	;						33
		B_rem		; Bytes roll counter				34
		Last_spd	; Saved speed for half-steps			35
		_w		; save place for W				36
		_status 	; " - status					37
		Active_addr_x	; extended addressing				38
		Decoder_addr_x	; -"- decoder original addr			39
		DEC_config	; ram copy of Config_data_1 (CV29)		3a		
		Fn_control	; master on/off of functions			3b
		Sec_cnt 	; T/O counter (seconds) 			3c
		magic		; reset condition magic byte			3d
		Chaf_cnt	; for speed-dependent functions (sound) 	3e
 		Mmask		; unmask for direction				3f
	ENDC
;-------------------------------------
; Decoder status bits
; |SYNC|BP|DP|CR|NG|AC|CO|SV|
DEC_STAT_AC 	equ 2			; sent to consist addr
DEC_STAT_CO 	equ 1			; Consist Control Active bit
DEC_STAT_CO_msk 	equ b'00000010' ; mask
DEC_STAT_SV 	equ 0			; Service mode bit
DEC_STAT_SV_msk 	equ b'00000001'
DEC_STAT_NG 	equ 3			; negative number (for spline)
DEC_STAT_NG_msk 	equ b'00001000'
DEC_STAT_CR 	equ 4			; Consist reverse bit
DEC_STAT_CR_msk 	equ b'00010000'
DEC_STAT_DP 	equ 5			; Dup (repeat) packet
DEC_STAT_DP_msk 	equ b'00100000'
DEC_STAT_BP 	equ 6			; Begin of packet
DEC_STAT_BP_msk 	equ b'01000000'
DEC_STAT_SYNC		equ 7		; SYNC mode
DEC_STAT_SYNC_msk	equ b'10000000'
;-------------------------------------
; Decoder Config bits
; |DT|0|AA|ST|AK|PW|FL|DR|
DEC_CFG_DR		equ 0	; Direction. 0=Normal 1=Reversed
DEC_CFG_FL		equ 1	; Lights Control Bit. 0=in SpeedDir instruction 1=bit 4 of Fn group 1
DEC_CFG_PW		equ 2	; Analog Power conversion control
DEC_CFG_AK		equ 3	; Advanced Ack
DEC_CFG_ST		equ 4	; Speed table / polynomial
DEC_CFG_AA		equ 5	; Extended Addressing
DEC_CFG_DT		equ 7	; Decoder Type. 0=multifunction 1=accessory
;-------------------------------------
; EEPROM Data 
;
;****** warning: locations in this list are critical.
;****** add vars at the end only
;
	CBLOCK	h'0'		; EEPROM addresses

		PRIM_Addr	;CV1  = 0
		V_Start 	;CV2
		ACC_Rate	;CV3
		DEC_Rate	;CV4
		V_High		;CV5
		V_Mid		;CV6
		Version 	;CV7
		ManId		;CV8
		PWMTot		;CV9
		EMFCut		;CV10
		Packet_TO	;CV11 = 10
; CV14-16 reserved
		ExtAddr2	;CV17 = 11 MSB
		ExtAddr1	;CV18
		Consist_addr	;CV19 = 13
; CV20 reserved
		CAct_F1_F8	;CV21 = 14
		CAct_LMP	;CV22
		Accel_Adj	;CV23
		Decel_Adj	;CV24
		CABSPD_Step ;CV25 = 18
; CV26-28 reserved
		Config_data_1	;CV29 = 19
		Err_Info	;CV30
		Config_data_2	;CV31 - reserved
		Config_data_3	;CV32 - reserved = 22
		FL_loc		;CV33 = 23
		RL_loc		;CV34 = 24
		F1_loc		;CV35 = 25
		F2_loc		;CV36 = 26
		F3_loc		;CV37 = 27
		FL_Effect	;CV49 = 28
		RL_Effect	;CV50 = 29
		F1_Effect	;CV49 = 30
		F2_Effect	;CV52 = 31
		F3_Effect	;CV53 = 32
		Kick_Start	;CV65 = 33
		Fwd_Trim	;CV66 = 34
		SPD_Tbl:1C	;CV67 - 94 = 35 - 62 (28 values)
		Rev_Trim:0	;CV95	63		
		MEM_FILE_E	; end of file
	ENDC
	
	
	





;------------------------------------
; code starts here
;
	org 0
powerup
	clrf	PCLATH
	clrf	Page_high
	clrf	Page_low
	goto	init_start

	org 4
;----------------------------------------
; Interrupt every 22us during sync,
; then drop rate to 1/44us
;

Int
	movwf	_w					;0
	movf	STATUS, w				;1
	movwf	_status 				;2
	bcf STATUS, RP0 			;3
	bcf INTCON, T0IF				;4
	movlw	d'256' + d'11' - (SAMPLE_PRD * FREQ / 4);5
	btfsc	DEC_STATUS, DEC_STAT_SYNC		;6
	addlw	(SAMPLE_PRD * FREQ / 8)		;7
	movwf	TMR0			; restart count ;8
; pwm
	btfss	DEC_STATUS, DEC_STAT_SYNC		;9
	goto	l_Int_1 				;10
	movlw	H_msk					;11   in sync mode for pwm, divide sample rate by 2 
	xorwf	FLG,		F			;12
	btfss	FLG,		H			;13
	goto	l_Int_2
l_Int_1
	incf	PWM_Accel_cnt,	F			;  This is handled in pwm routine
	movlw	SAMPLE_PRD				; 
	addwf	PWM_t,	F				;  add time in us
l_Int_2 
;  ***	Change <1/17/00> to accomodate different input port/pin combinations
	movf	FLG,		W			; **
	btfsc	IN_port,	IN_bit			; **
	xorlw	LS_msk					; **
;  *** end change <1/17/00>
	andlw	LS_msk
	btfss	STATUS, 	Z			; sample == last sample
	goto	l_Int_state
	btfss	FLG,		SC_msb			; count up to 4 and stay at 4
	incf	FLG,		F	
	call	Int_pwm
	goto	Int_end
;
; state machine dispatch
;
l_Int_state
	clrwdt						; cleared only if signal is arriving
	xorwf	FLG,		F
	movf	state,		W
	addwf	PCL,		F
	goto	l_state_sync0				; reset sync
	goto	l_state_sync1
	goto	l_state_sync2
	goto	l_state_sync3
	goto	l_state_hbit
;	goto	l_state_bit
;
l_state_bit
	call	Int_pwm
	bcf FLG,		LV					;
	btfss	FLG,		SC_msb	; is samp no. >= 4		;
	bsf FLG,		LV
	call	byte
	addwf	PCL,		F
	goto	l_state_sync0		; ret w==0 for error
	decf	state,		F	; next wait for hbit
	goto	l_clr_count 


;---------------------------------------------
;
; calculate MARs
MARs	swapf	Effect_cnt ,	w  
		andlw	0x0F			
		addwf	PCL,	f			

		retlw	0x0F		   
		retlw	0x07
		retlw	0x07
		retlw	0x03
		retlw	0x03
		retlw	0x01
		retlw	0x01
		retlw	0x00
		retlw	0x00
		retlw	0x01
		retlw	0x01
		retlw	0x03
		retlw	0x03
		retlw	0x07
		retlw	0x07
		retlw	0X0F

;
;-----------------------------------------
;  Decoder control instructions
;
decod_ctl
	btfsc	Cmd_b,	4
	goto	consist_ctl
	movlw	h'f'
	andwf	Cmd_b,	W	; get low nibble
	addwf	PCL,F		; jump table
	goto	start		; 0000 decoder reset
	goto	hard_reset	; 0001 hard reset
	goto	l_roll		; 0010 reserved
	goto	l_roll		; 0011	"
	goto	l_roll		; 0100	"
	goto	l_roll		; 0101	"
	goto	clr_adv_ack ; 0110	adv ack not supported
	goto	set_adv_ack ; 0111	 "
	goto	l_roll		; 1000	reserved
	goto	l_roll		; 1001	"
	goto	clr_adv_addr	; 1010	ext address
	goto	set_adv_addr	; 1011	"
	goto	l_roll		; 1100	reserved
	goto	l_roll		; 1101	"
	goto	l_roll		; 1110	"
	goto	ack 	; 1111 acknowledge (adv ack n/a)
;--------------------------------
; main dispatcher
;
l_disp_2
	movf	Addr2_b,	W
	xorwf	Decoder_addr_x, W
	btfss	STATUS, 	Z
	goto	l_disp_svc
	movf	Addr_b, 	W
	xorwf	Decoder_addr,	W	; is it Decoder addr in consist mode
	btfss	STATUS, 	Z	;
	goto	l_disp_svc
	movf	Cmd_b,		W
	andlw	b'11000000' 	; is cmd == 10x (function group 1-2)
	xorlw	b'10000000'
	btfsc	STATUS, Z
	goto	l_match
l_disp_svc	
;***** fixed check for 2nd byte 0 in svc mode. Gil 7/30/00
	movf	Addr2_b,	W	; addr2_b has to be 0
	btfss	STATUS,		Z
	goto	p_delay_start
;****** end fix. Gil 7/30/00
	movlw	h'F0'
	andwf	Addr_b, W
	xorlw	h'70'		; test for service mode
	btfss	STATUS, Z	; if Addr_b == 0x70 (C==1) go to service mode
	goto	p_delay_start	; no match, release packet buffer
;-------------------------------------------
; Service Mode
;
svc_mode
	movlw	SVC_TO_PRESET	; preset the SVC mode timer
	movwf	SVC_timer
	bsf DEC_STATUS, DEC_STAT_SV
	clrf	Sec_cnt 	; reset timeout counter
	call	isRepeatPacket	; only act on repeat packets
	btfss	DEC_STATUS, DEC_STAT_DP
	goto	p_delay_start
	movf	Chk_b,	W
	movwf	Sup_1
	movf	Cmd_b,	W
	movwf	Chk_b
	movf	Addr_b, W
	movwf	Cmd_b
	btfsc	B_cnt,	1	; is B_cnt <= 1
	goto	l_cvsl_start
	goto	CV_access_short
;-----------------------------------------
; dispatch- main entry
;
l_dispatch
	btfsc	FLG2,		AM	; if analog mode and packet detected, reset!	
	goto	init_start				
l_disp_3	
	bcf DEC_STATUS, DEC_STAT_AC
	movf	Addr_b, 	W
	iorwf	Addr2_b,	W
	btfsc	STATUS, 	Z	; if addr == broadcast
	goto	l_match
	movf	Addr_b, 	W
	xorwf	Active_addr,	W	; is it the active addr (decoder/consist)?
	btfss	STATUS, 	Z
	goto	l_disp_2
	movf	Addr2_b,	W
	xorwf	Active_addr_x,	W	; support ext. addr.it
	btfss	STATUS, 	Z
	goto	l_disp_2
	btfsc	DEC_STATUS, 	DEC_STAT_CO
	bsf 	DEC_STATUS, 	DEC_STAT_AC
l_match 			; addr match
	clrf	Sec_cnt 	; clear the Timeout counter
	movlw	SVC_TO_PRESET	; preset the SVC mode timer
	movwf	SVC_timer
	call	isRepeatPacket	; is it a dup packet ?
	movf	Cmd_b,	F	; test if reset - if not, clear service mode bit
	btfss	STATUS, Z
	bcf 	DEC_STATUS, DEC_STAT_SV
l_parse
	clrf	B_rem
	incf	B_rem,	F	; count first byte to roll
	swapf	Cmd_b,	F	; get major opcode
	rrf 	Cmd_b,	W
	swapf	Cmd_b,	F
	andlw	h'07'
	addwf	PCL,F		; jump Table
	goto	decod_ctl	; 000 decoder control / consist control
	goto	adv_op		; 001 advanced op
	goto	speed_dir	; 010 speed & direction
	goto	speed_dir	; 011	"
	goto	func_1		; 100 Function group 1
	goto	func_2		; 101 Function group 2
	goto	p_sync_start	; 110 reserved
;	goto	CV_access_long	; 111 Config Var access, long form / short form
;----------------------------------------
; Direct access to CVs - long form
;
CV_access_long
	incf	B_rem,	F
	btfsc	Cmd_b,	4
	goto	CV_access_short
l_cvsl_start
	incf	B_rem,	F
	btfsc	DEC_STATUS, DEC_STAT_AC
	goto	l_roll			; not active if sent to consist address
	movf	Chk_b,	w		; get low CV# bits
	movwf	Arg1			
	movf	Cmd_b,	w
	andlw	b'00000011' 	; get high CV# bits
	call	CV_to_addr
	movwf	EEADR
	movf	Sup_1,	w
	movwf	Chk_b			; make same as CV access short
	rrf Cmd_b,	f		
	rrf Cmd_b,	w
	andlw	b'00000011' 	; get cmd bits
	addwf	PCL,	f
	goto	l_roll			; reserved
	goto	l_cvsl_vf
	goto	l_cvsl_bit
	goto	l_cvsl_wr

l_cvsl_wr
	btfss	DEC_STATUS, DEC_STAT_DP ;test for repeat packet
	goto	l_roll			;if got here, do nothing and wait for repeat
	movf	Chk_b,	w
	call	ee_write
	goto	ack

l_cvsl_vf1
	movwf	EEADR
l_cvsl_vf
	call	ee_read_1
	xorwf	Chk_b,	w
	btfsc	STATUS, Z
	goto	ack
	goto	l_roll

l_cvsl_bit
	movf	Chk_b,	w
	andlw	b'00000111'
	movwf	Arg4
	incf	Arg4,	f
	clrf	Arg3
	bsf STATUS, C
l_cvsl_lp1	
	rlf Arg3,	f	; get a 1 bit into position in Arg3
	decfsz	Arg4,	f
	goto	l_cvsl_lp1
	call	ee_read_1
	movwf	Arg4
	btfsc	Chk_b,	4
	goto	l_cvsl_wbit
	movf	Arg3,	w	; load bit mask-> w
	andwf	Arg4,	w
	btfsc	Chk_b,	3	; if we test for 0, the value in w should be 0
	xorwf	Arg3,	w	; if we test for 1, invert the bit
	btfsc	STATUS, Z
	goto	ack
	goto	l_roll
	
l_cvsl_wbit
	btfss	DEC_STATUS, DEC_STAT_DP
	goto	l_roll			;if got here, do nothing and wait for repeat
	movf	Arg3,	W	
	iorwf	Arg4,	W	; get the orig value and set bit to 1
	btfss	Chk_b,	3
	xorwf	Arg3,	W	; clear the bit
	call	ee_write
	goto	ack

;----------------------------------------
; Direct access to CVs - short form
;
CV_access_short
	movf	Cmd_b,	w
	andlw	0x7
	addwf	PCL,	F
	goto	l_cvs_data	; 000 data reg 0
	goto	l_cvs_data	; 001 data reg 1
	goto	l_cvs_data	; 010 data reg 2
	goto	l_cvs_data	; 011 data reg 3
	goto	l_cvs_config	; 100 Config CV29
	goto	l_cvs_Page	; 101 page reg
	goto	l_cvsl_vf1	; 110 version # CV7
	goto	l_cvsl_vf1	; 111 manufacturer ID CV8
;
l_cvs_data
	addwf	Page_low, W
	btfss	DEC_STATUS, DEC_STAT_SV 	; are we in service mode
	addlw	d'20'				; make it CV#21 - 24 (std is 23,24)
	movwf	Arg1
	movf	Page_high,W
	call	CV_to_addr
	goto	l_cvs_op
l_cvs_config
	movlw	Config_data_1	; modify basic config CV29 
	goto	l_cvs_op
l_cvs_Page
	decf	Chk_b,	F	; Page--
	clrf	Arg1		; (Page# - 1) *4, high & low
	bcf STATUS, C
	rlf Chk_b,	F
	rlf Arg1,	F
	rlf Chk_b,	F
	rlf Arg1,	F
	btfss	Cmd_b,	3	; page is in mem, not EEPROM
	goto	l_cvs_pg_vf
	movf	Chk_b,	W	; write value
	movwf	Page_low
	movf	Arg1,	W
	movwf	Page_high
	goto	ack
l_cvs_pg_vf
	movf	Chk_b,	W
	xorwf	Page_low, W
	btfss	STATUS, Z
	goto	l_roll
	movf	Arg1, W
	xorwf	Page_high, W
	btfss	STATUS, Z
	goto	l_roll		; not verified
	goto	ack
	
;
l_cvs_op
	movwf	EEADR
	btfss	Cmd_b,	3	; test op code
	goto	l_cvsl_vf
	goto	l_cvsl_wr

;-------------------------------------------
; bit recognizer state machine
;
l_state_sync0
	clrf	state
	bsf DEC_STATUS, DEC_STAT_SYNC
	movlw	PREAMBLE_CNT			; reset hbit count for sync
	movwf	B_cnt
	clrf	Err
	movlw	Addr2_b 		; reset 
	movwf	FSR 	
l_next_state
	incf	state,		F	; state = sync1
l_clr_count
	movlw	b'11111000'
	andwf	FLG,		F	; clear sample counter
	incf	FLG,		F	; count 1st sample
Int_end
	movf	_status,	W
	movwf	STATUS
	swapf	_w, 	F
	swapf	_w, 	W
	retfie
;----------------------------------
;  Interrupt PWM handling  1/26/00
;
Int_pwm
	btfsc	FLG2,	AM
	return
	movf	TStep,	W
	subwf	PWM_t,	W
	btfss	STATUS, C	; it's time to count a step
	return
	movwf	PWM_t		; get residue
	incf	PWM_cnt, F
	incf	PWM_cnt, F
	movf	PWM_thr, W
	subwf	PWM_cnt, W
	btfss	STATUS, 	C
	goto	l_Int_set_pwm
	bcf 	OUT_PWM_port, OUT_PWM
	movlw	Motor_Dmp_msk
	iorwf	OUT_fwd_port,	F	;set motor short
	return
l_Int_set_pwm
	movf	Mmask,		W	;clear motor short
	andwf	OUT_fwd_port,	F
	bsf 	OUT_PWM_port, OUT_PWM
	return
;
;
l_state_sync1
	btfsc	FLG,	SC_msb		; 0 or 1
	goto	l_state_sync0		; 0 hbit causes reset
	decfsz	B_cnt,		F
	goto	l_clr_count
	goto	l_next_state
;
l_state_sync2
	btfsc	FLG,	SC_msb
	incf	state,		F
	goto	l_clr_count
;
l_state_sync3
	btfss	FLG,	SC_msb
	goto	l_state_sync0		; 1 after 0 hbit of preamble not accepted	
	bcf DEC_STATUS, DEC_STAT_SYNC
	movlw	(SAMPLE_PRD * FREQ / 8) - 2 ;  *** Gil 2/19/01 fixed adjustment to high freq
	subwf	TMR0,		F	; adjust sample time	
	movlw	h'FE'
	movwf	Buf
	goto	l_next_state
;
l_state_hbit
	incf	FLG,		F	; add one count so that 3 samples are 4, over the threshold
	incf	state,		F	; do not reset the count on hbit. change state to l_state_bit.
	call	Int_pwm
	goto	Int_end 


;-------------------------------------------
;  Consist control, activation/ deactivation
;
consist_ctl
	incf	B_rem,	F	; 2 byte instruction
	movlw	h'0E'
	andwf	Cmd_b,	W
	xorlw	2		; 001x == consist fwd/back
	btfss	STATUS, Z
	goto	l_roll	
; >>>  Fixed an error and saved an instruction. Gil 7/6/00
	bcf	Chk_b,	7	; original bit 7 should be '0' ('1' is reserved)
	btfss	Cmd_b,	0	; is it normal or reversed ?
	bsf 	Chk_b,	7
	movlw	Consist_addr
	movwf	EEADR
	movf	Chk_b, W
	call	ee_write	; inside, calls refresh_ram and updates all registers/ flags
	goto	l_roll

;------------------------------------
; access routines - eeprom
;
;
ee_read 			; address in w
	bcf STATUS, RP0
	movwf	EEADR
ee_read_1			; if address already in EEADR
	bsf STATUS, RP0
	bsf EECON1, RD
	bcf STATUS, RP0
	movf	EEDATA, W
	return

ee_write		; address in Arg1, value is in W reg.
	bcf INTCON, GIE 	;disable int
	clrwdt
	bcf STATUS, RP0
	movwf	EEDATA
	movlw	Version		; check if addr == CV7 or CV8 (read only)
	xorwf	EEADR,	W
	andlw	0xFE		; mask bit 0
	btfsc	STATUS, Z
	goto	refresh_ram
	bsf STATUS, RP0
;	bcf EECON1, EEIF
	bsf EECON1, WREN	;enable write
	movlw	h'55'
	movwf	EECON2		; write 55
	movlw	h'AA'
	movwf	EECON2
	bsf EECON1, WR	; set wr bit, begin wr
ee_wr_wait			; make sure write completed & verified
	btfsc	EECON1, WR
	goto	ee_wr_wait
	bcf EECON1, WREN
; ee write verify
	bcf STATUS, RP0
;	movf	EEDATA, W
;	bsf STATUS, RP0
;	bsf EECON1, RD	; now read the value actually written
;	bcf STATUS, RP0
;	xorwf	EEDATA, F	
;	btfss	STATUS, Z
;	goto	ee_write
	bsf INTCON, GIE ;enable int
	goto	refresh_ram ; put all time-critical values in ram copies



;------------------------------------
; read saved CVs from eeprom
; Arg5 and Arg3 are changed
;
read_eeprom_file
	movlw	1
	movwf	PWM_timer
	movlw	b'11000000'
	movwf	DEC_STATUS
	movlw	b'10000000'
	movwf	FLG
;
refresh_ram
	movlw	PWMTot		; (CV9 << 2) ^ 0x83
	call	ee_read
	movwf	Arg5
	rlf Arg5,	F
	rlf Arg5,	F
	movlw	0x83
	iorwf	Arg5,	W
	movwf	TStep
;
	movlw	Config_data_1	; read config byte (CV29)
	call	ee_read
	movwf	DEC_config	; move to RAM copy
	btfss	DEC_config, DEC_CFG_AA	; is adv. addr on?
	goto	read_ef_1
	movlw	ExtAddr1
	call	ee_read
	movwf	Decoder_addr
	movwf	Active_addr
	movlw	ExtAddr2
	call	ee_read
	andlw	0xE7
	iorlw	0xC0
	movwf	Decoder_addr_x
	movwf	Active_addr_x
	goto	read_ef_2	
read_ef_1
	clrf	Decoder_addr_x
	clrf	Active_addr_x
	movlw	PRIM_Addr	; read primary address
	call	ee_read
	andlw	0x7F
	movwf	Decoder_addr
	movwf	Active_addr
read_ef_2
	bcf DEC_STATUS, DEC_STAT_CO
	movlw	Consist_addr	; read consist address
	call	ee_read
	movwf	Arg5
	bcf DEC_STATUS, DEC_STAT_CR
	btfsc	Arg5,	7
	bsf DEC_STATUS, DEC_STAT_CR
	andlw	b'01111111' ; is consist addr == 0 (inactive)
	btfsc	STATUS, Z
	return
	movwf	Active_addr ; activate consist
	clrf	Active_addr_x	; ext. byte is 0
	bsf DEC_STATUS, 	DEC_STAT_CO
	return
;---------------------------------------
; write contents of DEC_config into CV29
;
write_config
	movlw	Config_data_1	; CV29
	movwf	EEADR
	movf	DEC_config, W
	call	ee_write
	return
;------------------------------------
; Multiply
; M1 = W; M2 = Arg1; Prod = Arg2:Arg1
; Arg3 is cleared
;
multiply
	movwf	Arg2
	movlw	d'8'
	movwf	Arg3		; counter
	movf	Arg2,	W
	clrf	Arg2
l_mult_loop 
	rrf Arg1,	F
	btfsc	STATUS, C
	addwf	Arg2,	F
	rrf Arg2,	F
	decfsz	Arg3,	F
	goto l_mult_loop
	rrf Arg1,	F	; one additional rotate
	return
;------------------------------------
; spline calculation
; PWM_step = step256 [4 : 255]
; V(W) = {X1}  [W * (W - 128) * V_High] / (128 * 256) - 		   
;	 {X2}  [(W - 128)(256 - W)* V_Start] / (128 * 256) +
;	 {X3}  [W * (256 - W) * V_Mid ] / (64  * 256)
;
splineIt
	movf	PWM_step, W	; take care of stop and emergency stop
	btfss	STATUS, Z
	goto	l_spl_nz
	clrf	PWM_thr
	return
l_spl_nz
	btfss	DEC_config, DEC_CFG_ST
	goto	l_spl_start ; calculate curve
	movwf	Arg1
	movlw	d'27'		; get 28ss
	call	multiply	; Arg2 is step28, Arg1 is fraction
	movlw	SPD_Tbl + 1 ; get T[step28 +1]
	addwf	Arg2,	F
	movf	Arg2,	W
	call	ee_read 	; read speed from table
	movwf	Arg4
	decf	Arg2,	W
	call	ee_read
	movwf	Arg5
	subwf	Arg4,	W	; W == T[step28 + 1] - T[step28]
	call	multiply	; Arg2 = interpolation value
	movf	Arg2,	W
	addwf	Arg5,	W
l_spl_end
	movwf	PWM_thr
	return	
;
l_spl_start
	clrf	Arg6
	movlw	V_High
	call	ee_read
	andlw	h'FE'
	btfsc	STATUS, Z	; test for default case
	movlw	h'FF'
	movwf	Arg1
	bcf 	STATUS, C
	rrf 	Arg1,	F	; convert to 7 bit numbers
	movlw	V_Start
	call	ee_read
	movwf	Arg4		; save it for later
	bcf 	STATUS, C
	rrf 	Arg4,	F
	movf	Arg4,	W
	addwf	Arg1,	F	; guaranteed no overflow
	movf	PWM_step, W
	call	multiply	; now Arg2== E1== W*(V_High + V_Start) / 256
	movf	Arg4,	W
	subwf	Arg2,	W
	btfsc	STATUS, C
	goto	l_spl_c1
	incf	Arg6, F
	sublw	0
l_spl_c1
	movwf	Arg1		; get ready to multiply by (W - 128)/128
	movlw	d'128'		
	subwf	PWM_step, W
	btfsc	STATUS, C	; is result negative?
	goto	l_spl_c3	; no.
	sublw	0		; negate
	incf	Arg6,	F	; reverse sign
	bsf 	Arg6,	7	; remember graph area
l_spl_c3
	call	multiply
	rlf 	Arg1,	F	; to increase accuracy do not lose bits early	
	rlf 	Arg2,	W	; now w == E1 * |W - 128| / 128
	btfsc	Arg6,	0	; is result so far negative?
	sublw	0		; negate, so we get the real comp' 2 number
	movwf	Arg5
;
	movf	PWM_step, W	; PWM_step is [W + 1]
	movwf	Arg1
	sublw	0
	call	multiply
	movlw	V_Mid		; calc E3 == (W * (256-W) * V_Mid) / (64 * 256)
	call	ee_read
	andlw	h'FE'
	btfsc	STATUS, Z	; default case
	movlw	h'80'
	movwf	Arg1
	bcf 	STATUS, C
	rrf 	Arg1,	F
	movf	Arg2,	W
	call	multiply
	rlf 	Arg1,	F
	rlf 	Arg2,	F
	rlf 	Arg1,	F
	rlf 	Arg2,	W
	addwf	Arg5, F	; that's it!
	btfsc	Arg5, 7
	goto	l_spl_ovf
;	bsf 	STATUS,	C
;	btfsc	Arg6,	7	; *** Changed 2/1/2000 to prevent motor humming on 0 speed
;	bcf 	STATUS,	C
	rlf 	Arg5, 	W
	addlw	3
	goto	l_spl_end
l_spl_ovf
	movlw	0
	btfss	Arg6,	7
	movlw	h'FF'
	goto	l_spl_end

;------------------------------------
; start point

hard_reset
testest
;test******
;	clrf	PWM_step
;test_1
;	call	splineIt
;	movf	PWM_thr, W
;	incf	PWM_step, F
;	btfss	STATUS, Z
;	goto	test_1
;	nop
;	nop
; **** end test
;
;
	movlw	Consist_addr		; zero in hard-reset
	movwf	EEADR
	clrw
	call	ee_write
	clrf	DEC_config
	call	write_config
	clrw
	incf	EEADR,	f		; Clear error CV 
	call	ee_write
init_start
	bcf INTCON, GIE
	clrf	FLG2
	btfsc	STATUS, NOT_TO		; skip if a WDT reset
	goto	l_digital_mode
	movlw	Config_data_1
	call	ee_read
	movwf	DEC_config
	btfsc	DEC_config, DEC_CFG_PW		; get analog mode enable
	bsf FLG2,	AM
l_digital_mode
	bsf Fn_control, FN_RST	; signal 1st speed command is next, for momentary reset
	movlw	h'A5'
	xorwf	magic,	W
	btfsc	STATUS, Z
	goto	l_start_pulse
	xorwf	magic,	F
start
	call	read_eeprom_file
	bcf INTCON, GIE 	; temp. disable timer int
	movlw	b'00111111'
	movwf	ON_Effect
	clrf	PWM_step
	clrf	PWM_step_tgt
	clrf	PWM_thr
	clrf	PWM_cnt
	clrf	PWM_Accel_cnt
	clrf	PORTA
	clrf	PORTB
	
;	clrf	Mmask1			;mask for motor short >>> converted to constant
;	bsf Mmask1,OUT_fwd
;	bsf Mmask1,OUT_back
;	comf	Mmask1, 	W
	movlw	~Motor_Dmp_msk
	movwf	Mmask
	bcf OUT_fl_port,OUT_fl		; initially turn off lights  *
	bcf OUT_rl_port,OUT_rl		;*
	clrf	Sec_cnt
	clrf	Fn_control
l_start_pulse
	call	set_power
p_sync_start
	clrf	state		; reset
p_delay_start
	clrf	B_cnt
p_delay
	movlw	Addr2_b
	movwf	FSR 	; reset pointer
	clrf	Err
	clrf	Sup_1
	clrf	Sup_2
	bcf FLG,	VP	; release packet buffers
	bsf INTCON, T0IE
	bcf INTCON, T0IF
	bsf INTCON, GIE
;----------------------------------------
;  packet handler
; 
packet
	call	pwm
	btfsc	FLG, VP 	; if no valid packet, check time-out & return or sleep
	goto	l_dispatch
	movlw	Packet_TO	; read time-out value (sec)
	call	ee_read
	btfsc	STATUS, Z	; is T/O active?
	goto	packet
	subwf	Sec_cnt, W
	btfss	STATUS, C
	goto	packet
	goto	l_spd_emr_stop




;---------------------------------------------
; update effects
;
effect_update
	incf	Effect_cnt, F	; rolls ~every 1sec
	btfsc	STATUS, 	Z
	incf	Sec_cnt,	F
	movlw	b'00011000'
	andwf	ON_Effect,	F
	movlw	0x40
	andwf	Effect_cnt, W
	iorlw	0x83
	iorwf	ON_Effect,	F
	bcf 	ON_Effect,	EF_QB
	btfss	ON_Effect,	EF_QA
	bsf 	ON_Effect,	EF_QB
	call	MARs
	andwf	Effect_cnt, W
	btfss	STATUS, 	Z
	bsf ON_Effect,	EF_MARs
	movlw	0x06
	andwf	Effect_cnt, W
	btfsc	STATUS, 	Z
	bcf ON_Effect,	EF_Dim
	movlw	0x78
	andwf	Effect_cnt, W
	btfsc	STATUS, 	Z
	bcf ON_Effect,	EF_ST	
	decfsz	Chaf_cnt,	F
	goto	l_ef_func
	bcf ON_Effect,	EF_SPD	
	rrf PWM_thr,	W
	sublw	CHAF_PRESET_CNT
	movwf	Chaf_cnt
	
;
; handle functions
;
l_ef_func
	clrf	Arg5			; result output mask
	movlw	FN_NUM
	movwf	Arg6			; Fn CV ( reversed ) index
	movlw	1
	movwf	Arg3			; used as bit mask
;
l_ef_loop
	movf	Arg3,		W
	andwf	Fn_control, 	W
	btfsc	STATUS, 	Z
	goto	l_ef_c1
	movf	Arg6,		W
	sublw	FL_Effect + FN_NUM
	call	ee_read
	andwf	ON_Effect,	W	; if 0, turn on function outputs 
	btfss	STATUS, 	Z
	goto	l_ef_c1
	movf	Arg6,		W
	sublw	FL_loc + FN_NUM
	call	ee_read
	iorwf	Arg5,		F
l_ef_c1
	bcf STATUS, 	C
	rlf Arg3,		F
	decfsz	Arg6,		F
	goto	l_ef_loop
;  
;  when we finish this loop, Arg5 has the bits set for the output lines on/of status
;
	movlw	Out_Active_Low
	xorwf	Arg5,		F	; set correct active polarity
;
	movf	Arg5,		W
	xorwf	PORTA,		W
	andlw	PORTA_FN_msk
	xorwf	PORTA,		F

;no room for this bit!
;
;	rlf Arg5,		W	; problem if want to comply w/ RP	*
;
	movf	Arg5,		W       ;					*
	xorwf	PORTB,		W	;					*
	andlw	PORTB_FN_msk		;					*
	xorwf	PORTB,		F	;					*
;
	return

;---------------------------------------------
; PWM routine - handles acceleration / deceleration
;
pwm
	btfsc	FLG2,	AM
	retlw	0		; analog mode, no pwm
l_pwm_6
	movlw	PWM_CNT_PRESET		; compare to preset cycle value (for 256 increments)
	subwf	PWM_Accel_cnt,	W
	btfss	STATUS, 	C
	goto	l_pwm_4 		; no accel/decel
	movwf	PWM_Accel_cnt		; get the residue
;
;	effects are handled here
	call	effect_update
	
;
	decfsz	SVC_timer,	F	; is svc mode expired
	goto	l_pwm_2
	bcf DEC_STATUS, 	DEC_STAT_SV ; clear svc mode
l_pwm_2
	decfsz	PWM_timer,	F	; count down based on CV#3 + CV#23 or CV#4 + CV#24
	goto	l_pwm_4
	movf	PWM_step,	W
	subwf	PWM_step_tgt,	W
	btfsc	STATUS, 	Z	; if equal no acceleration needed
	goto	l_pwm_5

	btfss	STATUS, 	C	; if positive, accel needed; otherwise decel
	goto	l_pwm_decel 
	incf	PWM_step,	F	; increment now
	movlw	ACC_Rate
	call	ee_read
	movwf	PWM_timer		; preset to CV
	movlw	Accel_Adj		; add adjustment
	goto	l_pwm_1
l_pwm_decel
	decf	PWM_step,	F
	movlw	DEC_Rate
	call	ee_read
	movwf	PWM_timer
	movlw	Decel_Adj		; add adjustment
l_pwm_1
	call	ee_read
	movwf	Arg1
	andlw	h'7F'
	btfsc	Arg1,		7	; add or subtract?
	sublw	0			; W := 0 - W
	addwf	PWM_timer,	F
	btfss	STATUS, 	Z
	goto	l_pwm_7
	movf	PWM_step_tgt,	W
	movwf	PWM_step
l_pwm_7
	call	splineIt
l_pwm_5
	incf	PWM_timer,	F
l_pwm_4
	retlw	0

;----------------------------------------
;  byte constructor and error check
;
byte	
	rlf FLG,		W		; move LV -> C
	btfss	FLG,		VB		; skip if byte complete
	goto	l_rotate
	bcf FLG,		VB
	btfss	STATUS, 	C		; look for end-of-packet bit
	goto	l_rotate
	bsf DEC_STATUS, DEC_STAT_BP ; mark beginning of packet
	movf	Err,		F
	btfss	STATUS, 	Z
	retlw	0				; error
	movlw	4				; verify min. packet length (treat as ext. addr)
	subwf	B_cnt,		F
	btfss	STATUS, 	C
	retlw	0
;
	incf	B_cnt,		F		; B_cnt - 4 + 1, subtract addr bytes and err byte from length
	bsf FLG,		VP		; packet is ok
l_rotate
	rlf Buf,		F	
	btfsc	STATUS, 	C		; skip if bit == 0, start bit (end of byte)
	retlw	1				; not a byte 
	bsf FLG,		VB
	btfsc	FLG,		VP
	retlw	0				; if packet marked, drop byte on the floor and error
	movf	Buf,		W
	xorwf	Err,		F
	btfss	DEC_STATUS, DEC_STAT_BP
	goto	l_byte_3
	bcf DEC_STATUS, DEC_STAT_BP
	andlw	b'11000000' 		; test if high address bits are '11', extended address
	xorlw	b'11000000'
	btfsc	STATUS, 	Z		; if it is, start at Addr2_b
	goto	l_byte_2
	clrf	Addr2_b 	; ensures a comparison of short addr as ext. Addr will succeed (MSB is 0)
	incf	FSR,		F		; if not, skip the MSB of address, point to Addr_b
	incf	B_cnt,		F		;
l_byte_2
	movf	Buf,		W; put original addr back
l_byte_3
	movwf	INDF
	incf	FSR,		F
	incf	B_cnt,		F
	movlw	7				; pkt size < 7 bytes
	subwf	B_cnt,		W
	btfsc	STATUS, 	C
	retlw	0				; error
l_new_byte
	clrf	Buf
	comf	Buf,		F
	retlw	1



;----------------------------------------------------
; Is this a repeat packet
;  required for Service mode & CV access instructions
;
isRepeatPacket
	bsf DEC_STATUS, DEC_STAT_DP 
	movf	Addr2_b,	W
	xorwf	LastAddr2_b,	W
	btfss	STATUS, 	Z
	bcf DEC_STATUS, DEC_STAT_DP 	
	xorwf	LastAddr2_b,	F
	movf	Addr_b, 	W
	xorwf	LastAddr_b, W	; should be zero
	btfss	STATUS, 	Z
	bcf DEC_STATUS, DEC_STAT_DP 	
	xorwf	LastAddr_b, F
	movf	Cmd_b,		W
	xorwf	LastCmd_b,	W
	btfss	STATUS, Z
	bcf DEC_STATUS, DEC_STAT_DP 	
	xorwf	LastCmd_b,	F
	movf	Chk_b,		W
	xorwf	LastChk_b,	W
	btfss	STATUS, Z
	bcf DEC_STATUS, DEC_STAT_DP 	
	xorwf	LastChk_b,	F
	movf	Sup_1,		W
	xorwf	LastSup1,	W
	btfss	STATUS, Z
	bcf DEC_STATUS, DEC_STAT_DP 	
	xorwf	LastSup1,	F	
	movf	Sup_2,		W
	xorwf	LastSup2,	W
	btfss	STATUS, Z
	bcf DEC_STATUS, DEC_STAT_DP 	
	xorwf	LastSup2,	F	
	return


;--------------------------------------
; set advanced acknowledgement
;
set_adv_ack
;	bsf DEC_config, DEC_CFG_AK
;	goto	l_clr_ak_1

;--------------------------------------
; clr advanced acknowledgement
;
clr_adv_ack
;	bcf DEC_config, DEC_CFG_AK
l_clr_ak_1
	call	write_config
	goto	ack
;--------------------------------------
; set adv. addressing
;
set_adv_addr
	bsf DEC_config, DEC_CFG_AA
	goto	l_clr_ak_1
;--------------------------------------
; clr adv. addressing
;
clr_adv_addr
	bcf DEC_config, DEC_CFG_AA
l_clr_aa_1
	goto	l_clr_ak_1
;--------------------------------------
;  acknowledge request response. 
;  Note: this is implemented as a 5ms 
;  motor-on, basic ack
;
ack
	btfss	DEC_STATUS, DEC_STAT_SV ; if not in svc mode, do not ack
	goto	l_roll
	bcf INTCON, GIE 	; clear interrupts
	movlw	2 * FREQ	; adjust for frequency
	movwf	Arg3		; count 2 * Clock Freq * 256 * 3 ~= 6 milisec
	
	bsf OUT_fl_port,	OUT_fl						;*
	bsf OUT_rl_port,	OUT_rl
	bcf OUT_back_port,	OUT_back		;*						;*
	bsf OUT_fwd_port,	OUT_fwd 		;*
		 
	bsf OUT_PWM_port,	OUT_PWM ; turn on motor for >= 5msec		 *
;	bsf OUT_lmp_port,	OUT_lmp 					;*
l_ack_delay
	clrwdt
	clrf	Arg4
l_ack_loop
	decfsz	Arg4,		F
	goto	l_ack_loop	
	decfsz	Arg3,		F
	goto	l_ack_delay
;	bcf OUT_PWM_port,	OUT_PWM
	bcf OUT_fwd_port,	OUT_fwd 
	bcf OUT_fl_port,	OUT_fl						;*
	bcf OUT_rl_port,	OUT_rl						;*
;	bcf OUT_lmp_port,	OUT_lmp 					;*
	goto	p_sync_start		; no need to roll in svc mode

;----------------------------------------
; roll packet bytes
;	W : number of bytes
l_roll
	movf	B_rem,	W
	subwf	B_cnt,	F		; decrease byte count
	btfsc	STATUS, Z
	goto	p_delay 		; continue
	btfss	STATUS, C	
	goto	p_sync_start		; error, reset
l_roll_1
	movf	Chk_b,	W
	movwf	Cmd_b
	movf	Sup_1,	W
	movwf	Chk_b
	movf	Sup_2,	W
	movwf	Sup_1
	decfsz	B_rem,	F
	goto	l_roll_1
	goto	l_parse

		
;----------------------------------------
; Set Direction 
; Z-bit - direction
set_dir
	btfss	DEC_STATUS, DEC_STAT_AC ; is this a consist command?
	goto	l_setd_1
	btfsc	DEC_STATUS, DEC_STAT_CR
	xorwf	Arg1,		W
	goto	l_setd_2
l_setd_1	
	btfsc	DEC_config, DEC_CFG_DR	; is direction reversed
	xorwf	Arg1,		W		; reverse bit
l_setd_2
	andwf	Arg1,		W
	btfss	STATUS, 	Z
	goto	l_setd_fwd
	bcf 	Mmask,		OUT_fwd
	bsf 	ON_Effect,	EF_Fwd
	bsf 	Mmask,		OUT_back
	bcf 	ON_Effect,	EF_Rev
	return
l_setd_fwd
	bcf 	Mmask,		OUT_back
	bsf 	ON_Effect,	EF_Rev
	bsf 	Mmask,		OUT_fwd
	bcf 	ON_Effect,	EF_Fwd
	return
;----------------------------------------
; Set	lamp fl/rl
;
set_lmp
;	bsf OUT_lmp_port,	OUT_lmp 	; this is constantly on
	movlw	CAct_LMP			; read mask of lights that are controlled by consist addr.
	call	ee_read
	btfss	DEC_STATUS, 	DEC_STAT_AC 	; if not consist addr, then lights are controlled
	movlw	b'00000011'
	xorlw	h'FF'
	andwf	Fn_control, F			; first, mask controlled lights off
	xorlw	h'FF'
	btfss	Cmd_b,		4		; this is the FL bit
	clrw
	iorwf	Fn_control, F	
	bcf 	Cmd_b,		4		; for speed/dir: set bit for intermediate step to 0
	return				
	
;----------------------------------------
; set power mode (DCC/Analog)
;			
set_power
	bsf STATUS, RP0
	movlw	b'11001011'
	movwf	OPTION_REG
	btfsc	FLG2,		AM
	goto	l_sep_analog
	movlw	PORTA_Input_msk ; inputs
	movwf	TRISA
	movlw	PORTB_Input_msk	; must keep RB7 as input (marklin change direction) *
	movwf	TRISB
	goto	l_sep_end
l_sep_analog
	movlw	PORTA_In_Analog_msk 	; keeps functs output, pwm as input
	movwf	TRISA
	movlw	PORTB_In_Analog_msk 	; direction lines as input, functs output	
	movwf	TRISB
	bcf STATUS, RP0
	movlw	PORTA_ON_Analog_msk 	; turn them on	*
	movwf	PORTA			; turn lights on *
	movlw 	PORTB_ON_Analog_msk
	movwf	PORTB
l_sep_end
	bcf STATUS, RP0
	return

;----------------------------------------
; Advanced op
;
adv_op
	movf	Cmd_b,	w
	xorlw	b'00111111' ; 128 speed step
	btfss	STATUS, Z
	goto	l_roll
	incf	B_rem,	F	; 2 byte instruction
	movlw	b'10000000' ; direction bit mask for 128ss
	movwf	Arg1
	movf	Chk_b,	W
	call	set_dir
	rlf Chk_b,	W
	andlw	h'FE'
	btfsc	STATUS, Z	; is it stop?
	goto	l_spd_stop
	addlw	-2		; (2-254) -> (0-252)
	btfsc	STATUS, Z	; emergency stop?
	goto	l_spd_emr_stop
	addlw	-2		; (2-252) -> (0-250)
	goto	l_spd_calc
;----------------------------------------
; Speed and direction
;
speed_dir
	bcf	DEC_STATUS, DEC_STAT_NG
	movlw	b'00100000' ; direction bit mask for 14/28 ss
	movwf	Arg1
	movf	Cmd_b,	W
	call	set_dir
	btfss	DEC_config, DEC_CFG_FL
	call	set_lmp
l_speed
	movlw	b'00011111'
	andwf	Cmd_b,	F
	movf	Cmd_b,	W
	andlw	b'00001111'
	btfsc	STATUS, Z
	goto	l_spd_stop
	addlw	-1
	btfsc	STATUS, Z
	goto	l_spd_emr_stop
	bcf STATUS, C
	rlf Cmd_b,	F	; make 28 spd steps out of 16
	btfsc	Cmd_b,	5	; bit 4 controls intermediate step
	bsf Cmd_b,	0
	bcf Cmd_b,	5	; erase bit 4
	movf	Last_spd, W
	subwf	Cmd_b,	W
	btfsc	STATUS, C	; treat negative values
	goto	l_spd_delta
	bsf DEC_STATUS, DEC_STAT_NG
	addlw	2
l_spd_delta
	addlw	-1		; at this point, if |Cmd_b - Last_spd| == 1, then W == 0
	movwf	Arg6		; store it for later
	movf	Cmd_b,	W
	movwf	Last_spd	; keep last speed for half-steps
	addlw	-4		; convert 4 - 31 -> 0 - 27
	movwf	Arg1
	movlw	STEP_MULTIPLIER ; (step-4) * 256 / 27 == (step-4) * 151 / 16
	call	multiply
	swapf	Arg2,	F	; divide by 16
	swapf	Arg1,	W
	xorwf	Arg2,	W
	andlw	h'0F'
	xorwf	Arg2,	W
	movf	Arg6,	F	; test if delta speed is 1
	btfss	STATUS, Z
	goto	l_spd_calc

; add or subtract 5 from step256 ~= (256/27) / 2 

	btfss	DEC_STATUS, DEC_STAT_NG ; 2/17/00 fixed. when positive should subtract half step
	addlw	-d'10'
	addlw	d'5'
l_spd_calc
	addlw	1			; makes sure the step sent to splineIt is not '0' (which is stop)
	movwf	PWM_step_tgt
	btfsc	Fn_control, FN_RST	; this signals 1st speed packet after reset
	movwf	PWM_step		; if it is 1st speed packet, set immediately
	bcf 	Fn_control, FN_RST
	goto	l_spd_end
l_spd_emr_stop
	clrf	PWM_step
l_spd_stop
	clrf	PWM_step_tgt
l_spd_end
	call	splineIt
	goto	l_roll

;----------------------------------------
; enhanced special functions - group 1
;	100|FL|F4|F3|F2|F1
;  currently implemented FL, F1, F2
; 
func_1
	btfsc	DEC_config, 	DEC_CFG_FL	; if bit is set, control FL here
	call	set_lmp
;
	rlf	Cmd_b,		F
	rlf	Cmd_b,		F
	movf	Fn_control,	W
	xorwf	Cmd_b,		F
	movlw	CAct_F1_F8			; read mask for Fn controlled by consist
	call	ee_read
	movwf	Arg1
	rrf	Arg1,		F
	rrf	Arg1,		W
	andlw	b'00111100'
	btfss	DEC_STATUS, DEC_STAT_AC 	; if packet not to consist addr, mask is all Fn's controlled
	movlw	b'00111100'	
	andwf	Cmd_b,		W
	xorwf	Fn_control, 	F

;----------------------------------------
; enhanced special functions - group 1
;	1011|F8|F7|F6|F5
;  currently implemented none
; 

func_2
	goto	l_roll


;----------------------------------------
; translate CV# (0 based) to EEPROM address
; CV# bits 0-7 in Arg1
; CV# bits 8-9 in W (lsb)
;
CV_to_addr
	andlw	b'00000011' 		;check hi bits; have to be 0
	btfss	STATUS,	Z	
	retlw	0xFF
;
	movlw	d'11'
	subwf	Arg1,	w		; a-11
	btfsc	STATUS,	C		; CV <= 11 (a <= 10)
	goto	l_cva_2
	addlw	d'11'			; get 'a' back, it's the address
	return
l_cva_2					; CV17-19 (16 <= a <=18)
	movlw	d'16'
	subwf	Arg1,	w		; a-16 
	btfss	STATUS,	C
	retlw	0xFF			; CV < 17
	addlw	-3			; a-19
	btfsc	STATUS, C
	goto	l_cva_3
	addlw	d'14'			; 14+(a-19) == a-5
	return
l_cva_3					; CV21-25
	movlw	d'20'
	subwf	Arg1,	w		; a-20
	btfss	STATUS,	C
	retlw	0xFF			; CV < 21
	addlw	-5			; a-25
	btfsc	STATUS, C
	goto	l_cva_4
	addlw	d'19'			; 18+(a-24) == a-6
	return
l_cva_4					; CV29-37
	movlw	d'28'
	subwf	Arg1,	w		; a-28
	btfss	STATUS,	C
	retlw	0xFF			; CV < 29
	addlw	-9			; a-37
	btfsc	STATUS, C
	goto	l_cva_5
	addlw	d'28'			; 28+(a-37) == a-9
	return
l_cva_5					; CV49 - 53
	movlw	d'48'
	subwf	Arg1,	w		; a - 48
	btfss	STATUS, C
	retlw	0xFF			; CV < 49
	addlw	-5			; a - 53
	btfsc	STATUS, C
	goto	l_cva_6
	addlw	d'33'			; 33+(a-53) == a-20
	return
					
l_cva_6					; CV65-95	
	movlw	d'64'
	subwf	Arg1,	w		; a-64 
	btfss	STATUS,	C
	retlw	0xFF			; CV < 67
	addlw	-d'31'			; a-95 *** Corrected to decimal value Gil 6/8/2000
	btfsc	STATUS, C
	retlw	0xFF			; no more CVs supported
	addlw	d'64'			; 64+(a-95) == a-31
	return	
	
	
;----------------------------------------
;	DATA EEPROM preset values
;
	org 0x2100
	de	3		;PRIM_Addr		;CV1
	de	7		;V_Start	;CV2
	de	0		;ACC_Rate	;CV3
	de	0		;DEC_Rate	;CV4
	de	1		;V_High 	;CV5
	de	1		;V_Mid		;CV6
	de	d'105'		;Version	;CV7
	de	d'165'		;ManId		;CV8
	de	0		;PWMTot 	;CV9
	de	0		;EMFCut 	;CV10
	de	0		;Packet_TO	;CV11 = 10
; CV14-16 reserved
	de	0		;ExtAddr1	;CV17 = 11
	de	0		;ExtAddr2	;CV18
	de	0		;Consist_addr	;CV19 = 13
; CV20 reserved
	de	0		;CAct_F1_F8 	;CV21 = 14
	de	0		;CAct_LGT	;CV22
	de	0		;Accel_Adj	;CV23
	de	0		;Decel_Adj	;CV24
	de	1		;CABSPD_Step	;CV25 = 18
; CV26-28 reserved
	de	0		;Config_data_1	;CV29 = 19
	de	0		;Err_Info	;CV30
	de	0		;Config_data_2	;CV31 - reserved
	de	0		;Config_data_3	;CV32 - reserved = 22
	de	OUT_fl_msk 	;FL_loc 	;CV33 = 23
	de	OUT_rl_msk 	;RL_loc 	;CV34 = 24
	de	OUT_f1_msk 	;F1_loc 	;CV35 = 25	*
	de	OUT_f2_msk 	;F2_loc 	;CV36 = 26	*
	de	OUT_f3_msk 	;F3_loc 	;CV37 = 27	*
	de	EF_Fwd_msk | EF_MARs_msk	;FL_Effect	;CV49 = 28
	de	EF_Rev_msk | EF_ST_msk		;RL_Effect	;CV50 = 29
	de	EF_SPD_msk			;F1_Effect	;CV51 = 30
	de	0		;F2_Effect	;CV52 = 31
	de	0		;F3_Effect	;CV53 = 32

	org 	H'2100' + Fwd_Trim
				;Kick_Start ;CV65  = 33
				;Kick_Start 	;CV65  = 33
	de	d'128'		;Fwd_Trim	;CV66
				;SPD_Tbl:1C 	;CV67 - 94 = 35 - 62 (28 values)
	de	d'1'		;CV67
	de	d'6'		;CV68
	de	d'12'		;CV69
	de	d'16'		;CV70
	de	d'20'		;CV71
	de	d'24'		;CV72
	de	d'28'		;CV73
	de	d'32'		;CV74
	de	d'36'		;CV75
	de	d'42'		;CV76
	de	d'48'		;CV77
	de	d'54'		;CV78
	de	d'60'		;CV79
	de	d'68'		;CV80
	de	d'76'		;CV81
	de	d'84'		;CV82
	de	d'92'		;CV83
	de	d'102'		;CV84
	de	d'112'		;CV85
	de	d'124'		;CV86
	de	d'136'		;CV87
	de	d'152'		;CV88
	de	d'168'		;CV89
	de	d'188'		;CV90
	de	d'208'		;CV91
	de	d'230'		;CV92
	de	d'252'		;CV93
	de	d'255'		;CV94
	org H'2100' + Rev_Trim	;CV95	63
	de	d'128'		; Rev_Trim

	END

