divert(-1)
| ************************ start of m68kexec file *************************
dnl (c)INRIA 1995  
dnl      contact: syndex-support@inria.fr
dnl               Wilfrid.Lefebvre@inria.fr
dnl          1996 
dnl		  Remy.Kocik@inria.fr
dnl		  Pierre.Lesoille@inria.fr	
dnl		  Marc.Chappellier@inria.fr
dnl		  Pascal.Daviet@inria.fr
dnl SynDEx v4.0 include file for Motorola 68332 macro-assembler

| ************************ comments *****************************************

dnl comment{comments with balanced curly braces}comment
define(`comment_', `changequote({,})patsubst({{$1}}, {^}, {| })changequote')
define(`comment', `pushdef(`comment', `changequote)`'popdef(`comment')
dnl')changequote({,})comment_(')

dnl Use this defining macro to generate a trace in the output:
define(`def', `define(`$1', `
|||' changequote({,}){`$}{0($}{*)'}changequote
  `$2')')


dnl Use this one if no trace is wanted in the output:
dnl define(`def', `define(`$1', `$2')')
define(`ArG332',0)
define(`ArD332',1)
define(`AvD332',2)
define(`AvG332',3)
define(`Dir332',4)

include(cpus.m4)

define(`set_param',` dnl
ifelse(num_noeud_m4,0,`.set $1, $2',dnl
`ifelse(num_noeud_m4,1,`.set $1, $3',dnl
`ifelse(num_noeud_m4,2,`.set $1, $4',dnl
`ifelse(num_noeud_m4,3,`.set $1, $5',`.set $1, $6')')')')
')

define(`forloop',`pushdef(`$1',`$2')_forloop(`$1',`$2',`$3',`$4')popdef(`$1')')
define(`_forloop',`$4`'ifelse($1,`$3',,`define(`$1',incr($1))_forloop(`$1',`$2',`$3',`$4')')')

def(`init_sequence_',`forloop(icpt,0,10,`ifelse(regexp(cpus,icpt),-1,,icpt,$1,`PBL_o_logical(0,1,_IOok_,0)'
,`PBL_n_logical(0,1)'
)')')

dnl processor_(name) ; place holder for future extensions
def(`processor_', `
define(`num_noeud_m4',$1)
.text	
.even
undivert') dnl insert here eventual diverted I/O support

| ******************** label generation *************************************

dnl pushlabel(lbl) ; Generator for unique labels
define(`labelnumber',0) dnl label counter
define(`pushlabel',    `dnl make a new unique label name based on argument
  pushdef(`$1', $1`_'labelnumber`_')dnl
  define(`labelnumber',incr(labelnumber))')
define(`poplabel', `popdef(`$1')')

| ******************** Memory allocation ************************************

dnl Data buffers for temporary signals, delays, windows and constants,
dnl are located in the uninitialized .usect "syndata" section and marked
dnl by the symbolic "label" given to the macroes.
dnl
dnl     logical :  8 bits
dnl     integer : 32 bits
dnl     float   : 32 bits (actuellement non implante)
dnl     dpfloat : 64 bits (actuellement non implante)
dnl

| -------------------- allocate ---------------------------------------------

dnl cell_(label, items) | creates a buffer for items memory cells
define(`cell_', `
.lcomm $1,$2 | uninitialized data buffer
')

dnl logical_(label,items) | allocate items booleans
def(`logical_', `cell_($1,eval(($2+1)&~1)) ')

dnl integer_(label, items) | allocates a buffer for items integers
def(`integer_', `cell_($1,eval($2*4))')

dnl real_(label, items) | allocates a buffer for items reals
def(`real_', `cell_($1,eval($2*4))')

dnl dpreal_(label, items) | allocates a buffer for items reals
def(`dpreal_', `cell_($1,eval($2*8))')


| -------------------- reallocate -------------------------------------------
dnl cell_alias_(newlabel,oldlabel,offset) | newlabel=oldlabel+offset
define(`cell_alias_', `
.set $1, $2+$3')

dnl logical_alias_(newlabel,oldlabel,offset) | newlabel=oldlabel+offset
def(`logical_alias_', `cell_alias_($@)')

dnl integer_alias_(newlabel,oldlabel,offset) | newlabel=oldlabel+offset
def(`integer_alias_', `cell_alias_($1,$2,eval($3*4))')

dnl real_alias_(newlabel,oldlabel,offset) | newlabel=oldlabel+offset
def(`real_alias_', `cell_alias_($1,$2,eval($3*4))')

dnl dpreal_alias_(newlabel,oldlabel,offset) | newlabel=oldlabel+offset
def(`dpreal_alias_', `cell_alias_($1,$2,eval($3*8))')


| -------------------- initialize -------------------------------------------

dnl cell__ini_({repeat[,step]}) | for use with <type>_ini_
define(`cell__ini_', `ifelse($1,, `
 .long 0 | iterators end mark', `
 .long $1, eval(cellSize*ifelse($2,,1,$2))cell__ini_(shift(shift($@)))')') 


dnl logical_ini_(value,label,offset, iter...) 
dnl  | initialize an array of booleans
define(`true', `1') define(`false', `0')
def(`logical_ini_', `pushdef(`cellSize',1)
 .long $2`'ifelse($#,2,, $3,0,, +$3)       
 .long MainInitLoop1
 .long $1 cell__ini_(shift(shift(shift($@)))) popdef(`cellSize')')


dnl integer_ini_(value,label,offset, iter...) 
dnl  | initialize an array of integers
def(`integer_ini_', `pushdef(`cellSize',4)
 .long $2`'ifelse($#,2,, $3,0,, +$3*4)  
 .long MainInitLoop4
 .long $1 cell__ini_(shift(shift(shift($@)))) popdef(`cellSize')')


dnl real_ini_(value,label,offset, iter...) 
dnl  | initialize an array of reals
def(`real_ini_', `pushdef(`cellSize',4)
 .long $2`'ifelse($#,2,, $3,0,, +$3*4)  
 .long MainInitLoop4
 .float $1 cell__ini_(shift(shift(shift($@)))) popdef(`cellSize')')


dnl dpreal_ini_(value,label,offset, iter...) 
dnl  |initialize an array of double precision reals
def(`dpreal_ini_', `pushdef(`cellSize',8)
 .long $2`'ifelse($#,2,, $3,0,, +$3*8)  
 .long MainInitLoop8
 .double $1 cell__ini_(shift(shift(shift($@)))) popdef(`cellSize')')
 

| -------------------- copy -------------------------------------------------

dnl logical_copy_(dstlabel,scelabel,items)
define(`logical_copy_', `ifelse(
eval($3),1, `
	move.b	($2),($1)',
eval($3),2, `
	move.w	($2),($1)', `
	lea.l	$2,%a0			| Adresse source dans A0
	lea.l	$1,%a1			| Adresse destination dans A1
	move.l	`#'eval($3-1),%d0	| -1 for DBRA
0:	move.b	(%a0)+,(%a1)+		| Copie octet par octet
	dbra	%d0,0b')')

dnl cell_copy_(dstlabel,scelabel,items)
define(`cell_copy_', `ifelse(
eval($3),1, `
	move.l	($2),($1)',
eval($3),2, `
	move.l	($2),($1)
	move.l	($2+4),($1+4)', `
	lea.l	$2,%a0			| Adresse source dans A0
	lea.l	$1,%a1			| Adresse destination dans A1
	move.l	`#'eval($3-1),%d0	| -1 for DBRA
0:	move.l	(%a0)+,(%a1)+		| Copie octet par octet
	dbra	%d0,0b')')

dnl integer_copy_(dstlabel,scelabel,items)
def(`integer_copy_', `cell_copy_($@)')

dnl real_copy_(dstlabel,scelabel,items)
def(`real_copy_', `cell_copy_($@)')

dnl dpreal_copy_(dstlabel,scelabel,items)
def(`dpreal_copy_', `cell_copy_($1,$2,eval($3*2))')


| -------------------- slidding windows -------------------------------------

dnl Slidding windows: samples array, oldest item first, newest last.
dnl Update: items-1 first items are "left"-shifted one item in memory,
dnl         before the last item is updated (not here).
dnl Each item may be an array of itemsize elements of the basic type.

dnl logical_window_(itemsize,items,base)
def(`logical_window_', `logical_copy_($3,$3+$1,eval(($2-1)*$1))')

dnl integer_window_(itemsize,items,base)
def(`integer_window_', `cell_copy_($3,$3+$1*4,eval(($2-1)*$1))')

dnl real_window_(size,itemsize,base)
def(`real_window_', `cell_copy_($3,$3+$1*4,eval(($2-1)*$1))')

dnl dpreal_window_(size,itemsize,base)
def(`dpreal_window_', `cell_copy_($3,$3+$1*8,eval(($2-1)*$1*2))')

| ******************** Desequencing *****************************************

dnl if_(log)
def(`if_', `pushlabel(`forward_')
	tst.b	($1)
	jeq	forward_')

dnl ifnot_(log)
def(`ifnot_', `pushlabel(`forward_')
	tst.b	($1)
	jne	forward_')

dnl else_()
def(`else_',
`pushdef(`swap_', forward_)poplabel(`forward_')pushlabel(`forward_')
	jra	forward_
swap_:	popdef(`swap_')')

dnl endif_()
def(`endif_', `
forward_:	poplabel(`forward_')')

dnl while_(log)
def(`while_', `pushlabel(`backward_')pushlabel(`forward_')
	jra	forward_
backward_:')

dnl endwhile_(log)
def(`endwhile_', `
forward_:
	tst.b	($1)
	jne	backward_ poplabel(`forward_')poplabel(`backward_')')

define(`h_term_', `
	move.b	($1),%d7 ifelse($#,1,,`
	jeq		forward_ h_term_(shift($@))')')

dnl hmul_(log, `log,...', `log,...')
def(`hmul_', `pushlabel(`forward_')h_term_($2)
	jeq		forward_ h_term_($3)
forward_:	poplabel(`forward_')
	move.b	%d7,($1)')

dnl hadd_(log, `log,...', `log,...')
def(`hadd_', `pushlabel(`forward_')h_term_($2)dnl
pushdef(`swap_', forward_)poplabel(`forward_')pushlabel(`forward_')
	jne		forward_
swap_:	popdef(`swap_')h_term_($3)
forward_:	poplabel(`forward_')
	move.b	%d7,($1)')


| ******************** Synchronizers ****************************************
dnl To synchronize main (priority=0) 
dnl  and communication sequences (priority=1).

dnl semaphores_(n) | allocate and initialize n semaphores
def(`semaphores_',
  `logical_(sem_,$1)divert(1)logical_ini_(0,sem_,0,$1)divert')

dnl thread_(linkNumber) | start of a communication sequence
def(`thread_', `
.text
com$1_thread_:')

dnl spawn_(linkNumber) | spawn the pseudo-parallel execution of a com.seq.
def(`spawn_', `
	jbsr	com$1_thread_
0:	tst.b (goodstart_)
        jeq 0b
')

dnl Pre0_(sem) | priority 1->0 precedence
def(`Pre0_', `
	move.b	`#'1,(sem_+$1)	| set semaphore $1')

dnl Suc0_(sem) | priority 0<-1 precedence
def(`Suc0_', `
0:	tst.b	(sem_+$1)	| test semaphore $1
	jeq	0b		| until set by `Pre0_' on interrupt
	clr.b	(sem_+$1)	| reset it poplabel(`Suc0')')

dnl Pre1_(sem) | priority 0->1 or 1->1 precedence
def(`Pre1_', `
	jbsr	Suc_$1_')

dnl Suc1_(sem) | priority 1<-0 or 1<-1 precedence
def(`Suc1_', `
Suc_$1_:	| target address of CALL in `Pre1_'
	eor.b	`#'1,(sem_+$1)	| Read Modify Writechange semaphore $1
	jeq	0f		| Continue if null
	rts			| if non-null (`Suc1_' 1st),
				| return within _main or comINTi_
0:')


| ******************** Communications ***************************************

def(`tree_up_')
def(`tree_dn_')
def(`tree_dn_end')

| ----------------- main ---------------------------------------------------- 

dnl main_ini_()
def(`main_ini_',` | machine initialisation:








| ----------------- Pseudo DMA Registers ---------------------------
integer_(_DMA_SIZE,1) dnl remaining count of bytes to transfer
integer_(_DMA_ADDR,1) dnl address of next byte to transfer
integer_(_DMA_IPTR,1) dnl address of pending instruction


| -----------------------------------------------------------------------
| -------------- Sous-programmes d appel des CAN IN, OUT & NOP ---------- 
| -----------------------------------------------------------------------
| La doc Philips dit que la sortie /INT du 80C200 est active tant que le
| registre d interruption IR est non nul (il est annule des qu il est lu).
| La doc dit que chaque source d interruption ne peut activer son bit de IR
| que si son bit de masquage dans le registre de controle CR est a 1.
| L occurence d une source d interruption, masquee ou non, est memorisee par
| un bit dans le registre de status SR.
| La doc laisse penser que le demasquage d une interruption dont une occurence
| a ete memorisee dans SR ne provoque pas l activation du bit correspondant de
| IR et donc n active pas la sortie /INT.
| Cette implantation des routines d interruption a ete faite selon cette
| interpretation de la documentation.
| Dans le cas contraire, l implantation serait plus simple :
| - can_o_shared serait identique a can_n_shared
| - le demasquage de l interruption de reception, si le message de synchro
|  est deja recu, provoquerait l interruption menant au cas can_int_sync.  
| WARNING: dans tous les cas de figure, il faut verifier qu un emetteur qui
| envoie un message avec un identifiant qu il reconnait en reception, ne   
| declanche pas une interruption de reception (pas clair dans la doc).

can_i_shared:
	move.b	`#'_TXDL_,_CANA_	| Tx Data Length register:
	move.b	`#'16,_CAND_		| Request To Receive = sync message
|	move.p  %d0,(%a0)
	move.b	`#'_CMR_,_CANA_		| Command register:
	move.b	`#'1,_CAND_		| Request_TX,
	move.l	(%sp)+,_DMA_IPTR	| save address of pending instruction
	move.l	%d0,_DMA_SIZE		| save data buffer length
	move.l	%a0,_DMA_ADDR		| save data buffer address
	move.b	`#'_CR_,_CANA_		| Control register:
	move.b	`#'0x1a,_CAND_		| enable Rx Err Ovr interrupt
	rts

 
can_n_shared:				| then fall into can_n_shared
	move.l	(%sp)+,_DMA_IPTR	| save address of pending instruction
	move.l	%d0,_DMA_SIZE		| save data buffer length
	move.l	%a0,_DMA_ADDR		| save data buffer address
	move.b	`#'_CR_,_CANA_		| Control register:
	move.b	`#'0x1a,_CAND_		| enable Rx Err Ovr interrupt

	move.w `#'0x2600,%sr            | zone critique ????

	move.b	`#'_SR_,_CANA_		| Status register:
	btst.b	`#'0,_CAND_		| synchro frame already received?
	jeq	can_n_shared_end	| if not, wait for synchro frame.
	move.b	`#'_CMR_,_CANA_ 	| Command register:
	move.b	`#'4,_CAND_		| release Rx buffer
	
	move.b	`#'_IR_,_CANA_		| Interrupt register:
	move.b	_CAND_,%d5		| read into %d0 (and reset when read)

can_n_shared_end:
	move.w `#'0x2400,%sr            | fin zone critique ????
	rts

can_o_shared:				| %d0=dataLength %a0=dataAddress
	move.l	(%sp)+,_DMA_IPTR	| save address of pending instruction
	move.l	%d0,_DMA_SIZE		| save data buffer length
	move.l	%a0,_DMA_ADDR		| save data buffer address
	move.b	`#'_CR_,_CANA_		| Control register:
	move.b	`#'0x1a,_CAND_		| enable Rx  it BEFORE get status:

	move.w `#'0x2600,%sr            | zone critique ????

	move.b	`#'_SR_,_CANA_		| Status register:
	btst.b	`#'0,_CAND_		| synchro frame already received?
	jeq	can_o_shared_end	| if not, wait for synchro frame.
	move.b	`#'_CMR_,_CANA_ 	| Command register:
	move.b	`#'4,_CAND_		| release Rx buffer
	
	move.b	`#'_IR_,_CANA_		| Interrupt register:
	move.b	_CAND_,%d5		| read into %d0 (and reset when read)
	
	move.w `#'0x2400,%sr            | fin zone critique ????


| The first frame sent must have a length between 1 and 8 bytes such that
| all remaining frames will be full 8 bytes frames.

can_o_first_frame:			| %d0=dataLength %a0=dataAddress
	subq.b	`#'1,%d0		| send minimum 1, maximum 8 bytes:
	andi.l	`#'7,%d0		| (d0-1)%8+1 bytes
	addq.b	`#'1,%d0
	sub.l	%d0,_DMA_SIZE		| _DMA_SIZE -= number of bytes to send
	add.l	%d0,_DMA_ADDR		| _DMA_ADDR += number of bytes sent
	move.b	`#'_TXDL_,_CANA_	| Tx Data Length register
	move.b	%d0,_CAND_		| setup to number of bytes to send
	move.b	`#'_TXBUF_,%d1		| first Tx Buffer register number
	subq.b	`#'1,%d0		| -1 for DBRA
can_o_loop:
	move.b	%d1,_CANA_		| setup register number
	move.b	(%a0)+,_CAND_		| read and set register value
	addq.b	`#'1,%d1		| next register number
	dbra	%d0,can_o_loop
	move.b	`#'_CR_,_CANA_		| Control register:
	move.b	`#'0x1c,_CAND_		| enable Tx Err Ovr interrupt
	move.b	`#'_CMR_,_CANA_		| Command register:
	move.b	`#'1,_CAND_		| Request_TX
	rts

can_o_shared_end:
	move.w `#'0x2400,%sr            | fin zone critique ????
	rts

| -----------------------------------------------------------------------
| -------------- Sous-programme d interruption CAN ----------------------
| -----------------------------------------------------------------------

int_can:
	movem.l	%d0-%d1/%a0,-(%sp)	| save registers used during interrupt
	move.b	`#'_IR_,_CANA_		| Interrupt register:
	move.b	_CAND_,%d0		| read into %d0 (and reset when read)
	btst.b	`#'0,%d0		| bit0 = frame just received
	jne	int_can_Rx
	btst.b	`#'1,%d0		| bit1 = frame just sent
	jne	int_can_Tx
	btst.b	`#'2,%d0		| bit2 = too many errors
	jne	int_can_Err
	btst.b	`#'3,%d0		| bit3 = reception overrun error
	jne	int_can_Ovr
	btst.b	`#'4,%d0		| bit4 = wakeup
	jne	int_can_Wake

int_can_Wake:
	jra int_can_end

int_can_Err:
	jra int_can_end

int_can_Ovr:
	jra int_can_end

int_can_Rx:
	clr.l	%d0			| clear high bytes of %d0
	move.b	`#'_RXDL_,_CANA_	| Rx Data Length register:
	move.b	_CAND_,%d0		| get number of received bytes into %d0
	btst.b	`#'4,%d0		| test RTR (bit4)
	jne	int_can_sync		| RTR = synchro (receiver ready)
|	andi.b	`#'15,%d0		| reset high bits of %d0
	sub.l	%d0,_DMA_SIZE		| _DMA_SIZE -= number of received bytes
	movea.l	_DMA_ADDR,%a0		| destination address of received bytes
	tst.l	%a0
	jeq	int_can_Rx_free		| if null, can_nop ignores received bytes
	move.b	`#'_RXBUF_,%d1		| first Rx Buffer register number
	subi.b	`#'1,%d0		| -1 for DBRA
int_can_Rx_loop:
	move.b	%d1,_CANA_		| setup register number
	move.b	_CAND_,(%a0)+		| get and store register value
	addi.b	`#'1,%d1		| next register number
	dbra	%d0,int_can_Rx_loop
	move.l	%a0,_DMA_ADDR		| _DMA_ADDR += number of received bytes
int_can_Rx_free:
	move.b	`#'_CMR_,_CANA_		| Command register:
	move.b	`#'4,_CAND_		| Free_RX
	tst.l	_DMA_SIZE		| still frames to receive?
	jne	int_can_end
int_can_resume:				| no more frame to transfer:
	move.b	`#'_CR_,_CANA_		| Control register:
	move.b	`#'0,_CAND_		| disable all CAN interrupts
	movea.l	(_DMA_IPTR),%a0
	jbsr	(%a0)			| resume pending instruction
int_can_end:
	movem.l (%sp)+,%d0-%d1/%a0	| restore saved registers
	rte

int_can_sync:
	move.b	`#'_CMR_,_CANA_		| Command register:
	move.b	`#'4,_CAND_		| release Rx buffer
	movea.l	_DMA_ADDR,%a0		| source address of bytes to send
	tst.l	%a0
	jeq	int_can_end		| if null it s can_nop, otherwise it s can_out
	move.l	_DMA_SIZE,%d0		| number of bytes to send
	jbsr	can_o_first_frame
	movem.l	(%sp)+,%d0-%d1/%a0 	| restore saved registers
	rte

int_can_Tx:
	subi.l	`#'8,_DMA_SIZE		| remaining number of bytes to send
	blt		int_can_resume	| finished when null
	move.b	`#'_TXDL_,_CANA_	| Tx Data Length register
	move.b	`#'8,_CAND_		| setup to number of bytes to send
	movea.l	_DMA_ADDR,%a0		| source address of bytes to send
	move.b	`#'_TXBUF_,%d1		| first Tx Buffer register number
	move.l	`#'7,%d0		| -1 for DBRA
int_can_Tx_loop:
	move.b	%d1,_CANA_		| setup register number
	move.b	(%a0)+,_CAND_		| read and set register value
	addi.b	`#'1,%d1		| next register number
	dbra	%d0,int_can_Tx_loop
	move.l	%a0,_DMA_ADDR		| _DMA_ADDR += number of bytes sent
	move.b	`#'_CMR_,_CANA_		| Command register:
	move.b	`#'1,_CAND_		| Request_TX
	movem.l	(%sp)+,%d0-%d1/%a0 	| restore saved registers
	rte

| -----------------------------------------------------------------------
| -------------- Sous-programme d interruption Timer ----------------------
| -----------------------------------------------------------------------

.set _PWMHI_INIT_A,	100     | init PWMA high value time
.set _PWM_PERIOD_A,	200     | PWMA period time 

set_param(_PWM_MAX_A,190 , 190 , 190 , 190 , 165)
set_param(_PWM_MIN_A,10 , 10 , 10 , 10 , 35)
set_param(_PWM_MAX_B,50 , 50 , 50 , 50 , 0)
set_param(_PWM_MIN_B,0 , 0 , 0 , 0 , 0)

.set _PWMHI_INIT_B,	0     | init PWMB high value time
.set _PWM_PERIOD_B,	200     | PWMB period time 

.set _PWM_TIME_VALID_,   25   |  enable power for 25 ms 
.set _VitSecuMin_,	200 | 0.2 m/s
.set _CurSecu_,		20000 | 20Amperes

set_param(_PWM_MAX_B,50 , 50 , 50 , 50 , 0)


it_timer:
	movem.l	%d0-%d2/%a0,-(%sp)	| save registers used during interrupt
	ifelse(num_noeud_m4,4,`	DisablePowB()')
	tst.w (valid_pui)
	jeq	1f	
	subq.w `#'1, (valid_pui)
	jmp	0f
1:	clr.w (enable_pui)


0:	tst.w	(_HSRR0)
	jne	0b
0:	tst.w	(_HSRR1)
	jne	0b 

	move.w  `#'0x20,_HSRR0  | write hsrr = 10 chanpri

	tst.w       (enable_pui)
        jeq 3f
        EnablePowA()
        jra 4f
3:   
ifelse(num_noeud_m4,4,`	DisablePowA()',`dnl
	cmp.w `#'_VitSecuMin_,(vitfiltr)
	jge 31f
	cmp.w `#'-_VitSecuMin_,(vitfiltr)
	jle 32f
	DisablePowA()
	jra 4f
31:	
	EnablePowA()
	move.w `#'-_CurSecu_,(PwmA_current_)
	jra 4f
32:	
	EnablePowA()
	move.w `#'_CurSecu_,(PwmA_current_)')
4:

	clr.l %d0
	move.w  0xffffa2,%d0	| d0=top
	move.l	%d0,%d1		| d1=top
	sub.w	(topold),%d0	| d0=dtop
	move.w	%d1,(topold)	| topold=top
	add.l	`#'1,(temps)	| inc compteur
0:	tst.w	(_HSRR0)
	jne	0b
	move.w  0xffffa4,%d1	| d1=tcr1
	move.l	%d1,%d2		| d2=tcr1 
	sub.w	(tcr1old),%d1	| d1=tcr1-tcr1old
	move.w	%d2,(tcr1old)	| tcr1old=tcr1
	move.l	%d1,(dtcr1)
 	clr.l %d2
	move.w	(vitfiltr),%d2	| d2=vitesse
	muls.w	`#'20000,%d2
	asl.w  `#'4,%d0		|d0=dtop*16
	muls.w	`#'15625,%d0
	add.l	%d2,%d0		|   
	addi.w	`#'20000,%d1    | to+dtcr1
	divs.w	%d1,%d0         | 
	move.w	%d0,(vitfiltr)	|
| calcul du PWM pour un courant PWM_current 
	muls.w `#'Keparam_,%d0   |fcem=v*keparam
	clr.l %d2
	move.w	`#'Rsparam_,%d2	| d2=Rsparam
        muls.w (PwmA_current_),%d2| d2=R*I 
        add.l %d2,%d0		| tension(micron V)
	asr.l `#'5,%d0             | tension /32
	divs.w	`#'Kpwmparam_,%d0| pwm
 	add.w  `#'_PWMHI_INIT_A,%d0      | %d0 + valeur € 0
 	cmp.w   `#'_PWM_MAX_A,%d0          
	jle 1f                          | si pwmhi <= pwmmax
	move.w  `#'_PWM_MAX_A,%d0         | sinon, pwmhi=pwmmax
	jra 2f
1:      cmp.w   `#'_PWM_MIN_A,%d0         
	jge     2f                      | si pwmhi >= pwmmin
	move.w  `#'_PWM_MIN_A,%d0         | sinon, pwmhi=pwmmin
2:      move.w  %d0,0xffff24
   
	move.w	%d0,(PwmA_Val_) 
 	movem.l	(%sp)+,%d0-%d2/%a0 	| restore saved registers
	rte

	.globl main
main:
	link.w	%a6,#0			| save frame pointer
	movem.l	%d0-%d7/%a0-%a6,-(%sp)	| and registers (stack frame)
|	jbsr	__main 
|

||========= init chip select 68332 ==============

.set _CSPAR0,	0xFFFA44	| cs pin assignement register 0
.set _CSPAR1,	0xFFFA46	| cs pin assignement register 1  
.set _CSBAR3,	0xFFFA58	| cs3 base address register
.set _CSOR3,	0xFFFA5A	| cs3 option register
.set _CSBAR4,	0xFFFA5C	| cs4 base address register
.set _CSOR4,	0xFFFA5E	| cs4 option register
.set _CSBAR9,	0xFFFA70	| cs9 base address register
.set _CSOR9,	0xFFFA72	| cs9 option register
.set _CSBAR10,	0xFFFA74	| cs10 base address register
.set _CSOR10,	0xFFFA76	| cs10 option register

| 82C200 CS pin (chip select) is connected to the 68332 CS9 pin
| CS9 is selected for the address range _CANA_,_CANA_+2K
| 82C200 address and data are multiplexed on the same 8 bits bus
| 80C200 address/data is selected (ALE pin) by the 68332 A0 address pin 0/1


||====== Write Chip Select initialisation

|write cs pin assignement
	ori.w	`#'0xF00,_CSPAR0 | cspa0[4]=3 =>cs 3 flash pin ass=16bits
				 | cspa0[5]=3 =>cs 4 flash OE pin ass=16bits 
	ori.w	`#'0x3C0,_CSPAR1 | cspa1[3]=3 =>cs 9 can pin ass=16bits
				 | cspa1[4]=3 =>cs 10 A/D pin ass=16bits

|write cs base address
	move.w	`#'0x1006,_CSBAR3  | 0x1006  => flash address= 0x100000 
				   |	=> size =512K (x 8 bits)
	move.w	`#'0x1006,_CSBAR4  | 0x1006  => flash address OE = 0x10000
				   |	=> size =512K (x 8 bits)
	move.w	`#'0x0200,_CSBAR9  | 0x0200  => CAN address= 0x20000
				   |	=> size =2K (x 8 bits)
	move.w	`#'0x0100,_CSBAR10 | 0x0100  => A/D address= 0x1000
				   |	=> size =2K (8 bits)

|write cs option
	move.w	`#'0x70FE,_CSOR3	| asyn. mode, 16 bits Write acces,
					| interne DSACK 3 waite, S/U space,
					| IPL=111, AVEC=0
	move.w	`#'0x68FE,_CSOR4	| asyn. mode, 16 bits Read acces,
					| interne DSACK 3 waite, S/U space,
					| IPL=111, AVEC=0
	move.w	`#'0x79F0,_CSOR9	| asyn. mode, 16 bits R/W acces,
					| interne DSACK 7 wait, S/U space,
					| IPL=000, AVEC=0
	move.w	`#'0x7970,_CSOR10	| asyn. mode, 16 bits Write acces,
					| interne DSACK 5 waite, S/U space,
					| IPL=000, AVEC=0


|====================== set interrupt vectors ==============================
	move.w	`#'0x2700,%sr		| Masque toutes les interruptions
	move.l	`#'int_can,0x78		| Initialisation du V.I CAN It_auto6
	move.l	`#'it_timer,0x140	| Initialisation du V.I it timer

|====================== port F ==============================
.set _PFPAR_,  0xFFFA1F | port F pin Assignment Register
.set _DDRF_,   0xFFFA1D | port F pin Data direction Register
.set _PORTF0_, 0xFFFA19 | port F0 Data Register 
.set _PORTF1_, 0xFFFA1B | port F1 Data Register 

	move.b `#'0x40, _PFPAR_ |  0x01000000  : PIN6 for IRQ6 others for port F
	move.b `#'0x38, _DDRF_  |  0x00111000  : IRQ3,4,5 as output
	move.b `#'0x00, _PORTF0_|  pin IRQ3,4,5 low
	
||========================= INIT CAN ====================================

.set _CANA_,	0x20001		| A1-A0=01 select Address
.set _CAND_,	_CANA_+2	| A1-A0=11 select Data

|----------------- CAN 80C200 registers numbers --------------------
.set _CR_,	0x00	| control
.set _CMR_,	0x01	| command
.set _SR_,	0x02	| status
.set _IR_,	0x03	| interrupt
.set _ACR_,	0x04	| acceptance code
.set _AMR_,	0x05	| acceptance mask
.set _BTR0_,	0x06	| bus timing 0
.set _BTR1_,	0x07	| bus timing 1
.set _OCR_,	0x08	| output control
.set _TSTR_,	0x09	| test, inutilise ici !
.set _TXID_,	0x0A	| TX identifier (8 MSBits)
.set _TXDL_,	0x0B	| TX id(3 LSBits)+RTR+dataLength(0-8)
.set _TXBUF_,	0x0C	| first byte of TX buffer (max 8)
.set _RXID_,	0x14	| RX identifier (8 MSBits)
.set _RXDL_,	0x15	| RX id(3 LSBits)+RTR+dataLength(0-8)
.set _RXBUF_,	0x16	| first byte of RX buffer (max 8)
.set _CDR_,	0x1F	| clock divide


|------------------- init 82C200 ---------------------------------
|	move.w	`#'0x0034,0xfffc08	| init rs 232 clock 
|	move.w	`#'0x7c08,0xfffa04
	move.b	`#'_CR_,_CANA_		| Request CAN reset
	move.b	`#'0x01,_CAND_
	move.b	`#'_ACR_,_CANA_		| Set code name: 0x11
	move.b	`#'0x11,_CAND_
	move.b	`#'_AMR_,_CANA_		| Set Mask : 0xff (all messages)
	move.b	`#'0xff,_CAND_
	move.b	`#'_BTR0_,_CANA_	| Set Rate: 500 kbauds
	move.b	`#'0x0,_CAND_		| 0 for 500, 1M 1.6M
|	move.b	`#'0x67,_CAND_ 		| 67 for 10
	move.b	`#'_BTR1_,_CANA_
|	move.b	`#'0x1c,_CAND_		| for 500K
|	move.b	`#'0x14,_CAND_		| for 1M
	move.b	`#'0x18,_CAND_		| for 750k
|	move.b	`#'0x2f,_CAND_		| for 10K
	move.b	`#'_OCR_,_CANA_
	move.b	`#'0xfa,_CAND_
	move.b	`#'_CR_,_CANA_		| 1 front de synchro et autorisation it
	move.b	`#'0x20,_CAND_
	move.b	`#'_TXID_,_CANA_	|TxID 8 MSBits of 11 bits identifier
|	move.b	`#'0x11,_CAND_
	move.b   `#'1<<num_noeud_m4,_CAND_


	
||================ init TIMER IT ==================================
.set _PITR_,	0xfffa24	| Periodic Interrupt Timer Register
.set _PICR_,	0xfffa22	| Periodic Interrupt Control Register

	move.w	`#'0x8,_PITR_	| set period of periodic timer
				| PTP=0 => period_prescaler = 512
				| PITM=8 		
				| period = (4 * period_prescaler * PITM)/16MHz
				| period = (4 * 512 * 8 ) / 16Mhz = 1.024ms
|	move.w	`#'0x550,_PICR_	|  15      11 10       8 7                 0
				| |     0    |   PIRQL  | 	PITM	   | 
				| PIRQL=5 => level 5
				| PIV=50

	move.w	`#'0x550,_PICR_
||=============== lecture du numero du noeud ====================||
.set _FLSH_WR_ADDR_, 	0x100000
.set _NUM_ADDR_,	0x104000
 
           
        cmp.w `#'num_noeud_m4,(_NUM_ADDR_)
	jne 0f
	move.b	`#'1,(flag_goodnum_)
0:


set_param(Keparam_,2640 , 3280 , 2641 , 2800 , -9600)
set_param(Rsparam_,500, 500, 500, 500 , 1200)
set_param(Kpwmparam_,14531 , 14531 , 14531 , 14531 , 14531) 
set_param(GAnaparam_,174 , 128 , 140 , 140 , 140) 
set_param(CAnaparam_,-8 , -12 ,-11 ,-9 , 0) 


.text 1

vitfiltr:
	.word 0
topold:		| old fqd position
	.word 0
tcr1old:	| old tcr1 value from fqd read
	.word 0
dtcr1:		| dt between 2 fqd read
	.long 0
temps:		| time compter inc bye it_timer (1ms)
	.long 0

numero:
	.word	num_noeud_m4

flag_goodnum_:	
	.word 0
goodstart_:
	.word 0
PwmA_Val_:
	.word 0
PwmA_current_:
	.word 0
enable_pui:
	.word 0
valid_pui:
	.word 0
.even
.text 0

|----------- wait init can end ? -------------------------------
|	move.l	`#'500,%d0
| 0:	dbra	%d0,0b



||-------------------------------------------------------------
|----------------- INIT TPU -----------------------------------

.set _TBASADR,	0x0B0000	| base address of internal RAM   
.set _TRAMBAR,	0xFFFB04	| base address register  
.set _TPUMCR,	0xFFFE00	| TPU module configuration register
.set  LLC_EMU_ADR_DATA , 0x0760 | shift base address of emu data

.text 1

|Tables pour le TPU generee par tpumasm +convert
_LLC_EMU:
.word 0x3FFF,0xFFFE,0x7FFF,0xFEFE,0xBFFF,0x07C4,0x6739
.word 0xFEFF,0x3EFF,0xC00F,0xB407,0xFFFF,0x3EFF,0xF00F
.word 0x387F,0xF80A,0x6739,0xFFFF,0x103D,0xF813,0xB60D
.word 0xFEFF,0x10FF,0xCA03,0x35FD,0xCFFF,0x8C01,0xFEFF
.word 0x303C,0xF00F,0x1CFF,0xF817,0xB218,0xFFFF,0x367F
.word 0xD203,0xB618,0xFEFF,0x35FD,0xCFFF,0xAC18,0xFEFF
.word 0xE73E,0x21CF,0xE73F,0xE1FF,0xBC18,0xF2FD,0x8E27
.word 0xFEDF,0x1E1F,0xD203,0x30FE,0x4203,0x7FFF,0xFFD6
.word 0x7E03,0xDFFF,0xAE24,0xFFFF,0x063F,0xF817,0x9A22
.word 0xF6FF,0xE73E,0x21C7,0x30FF,0xCFFF,0x353F,0xF00F
.word 0xBE24,0xFEFC,0x367F,0xDFFF,0x9A1A,0xFEFF,0x1207
.word 0xF203,0x30FE,0x7202,0xE1E4,0x01C7,0x8E2C,0xFEF8
.word 0x7859,0xFEFF,0x7A59,0xFEFF,0x3C7F,0xF807,0xD42F
.word 0xFFFF,0x525C,0xB5FA,0x163F,0xF00B,0x101D,0xF80F
.word 0x8636,0xFFFF,0x36FE,0xB013,0x37FC,0x4FFF,0x8437
.word 0xFFFF,0xD9FF,0x1FFF,0x545C,0xF18A,0x545C,0xF14A
.word 0xA42C,0xFFFF,0xD02D,0xFFFF,0xA43D,0xFEFF,0x9C3D
.word 0xFEFF,0x3C7F,0xF807,0xD42F,0xFFFF,0x545C,0xF3FA
.word 0x505D,0xF3FE,0xAC47,0xFFFF,0xBFFF,0x0FFC,0x585F
.word 0xFFFF,0xFFFF,0xFFFF,0xB047,0xFEC3,0xCFFF,0x300B
.word 0x5C5E,0x31FF,0xB64B,0xFFFF,0x1FFF,0xF007,0x70E9
.word 0xFEFB,0x3FFF,0xF006,0x70E9,0xFEFB,0x30FF,0xC006
.word 0xBC49,0x40BF,0xBC4B,0x407F,0x231F,0xFFFF,0xE31E
.word 0x21FF,0xE31E,0x41FF,0xE31E,0x61FF,0xE31E,0x81FF
.word 0xE31E,0xA1FF,0xE31E,0xC1FF,0xE31E,0xE1FF,0x035F
.word 0xF80F,0x367F,0xD80F,0x007F,0xF00F,0x103F,0xF00B
.word 0x9E5E,0xFFFF,0x11FC,0x8A03,0x3C7F,0xFFFF,0x8460
.word 0xFFFF,0xD069,0xF817,0xAE63,0xFEFF,0x3FFF,0xF813
.word 0xC44F,0xF004,0xB268,0xFFFF,0x7FFF,0xFFFB,0xBC69
.word 0xF0FF,0xBC68,0xFEC0,0xBFFF,0xFFC8,0x3E7F,0xF80F
.word 0x7FF9,0xFEFE,0x119F,0xF00B,0xBFFF,0xFFF9,0x387F
.word 0xF813,0x673F,0xFEFB,0x3A7F,0xF817,0xB471,0xFFFF
.word 0x3E7F,0xF80E,0x3E7F,0xC80E,0xD26B,0xFFFF,0x11BF
.word 0xF00B,0x0000

_LLC_EMU_DATA:
.word 0x406B,0x406B,0x806A,0xA072,0xFE00,0xFE00,0xFE00
.word 0xFE00,0x406B,0x406B,0x406B,0x406B,0x406B,0x406B
.word 0x406B,0x406B,0x1066,0x1066,0x1067,0xFE01,0x3057
.word 0x3057,0x3057,0x3057,0xFE01,0xFE01,0xFE01,0xFE01
.word 0xFE01,0xFE01,0xFE01,0xFE01,0x304E,0x304E,0x304D
.word 0x1040,0x3049,0x4046,0x304B,0x4046,0x1001,0x1001
.word 0x1001,0x1001,0x3049,0x4046,0x304B,0x4046,0x283A
.word 0x2838,0x0028,0xFE00,0x903F,0xFE3C,0x302C,0x302C
.word 0xFE01,0xFE01,0xFE01,0xFE01,0xFE01,0xFE01,0xFE01
.word 0xFE01,0xFE01,0xFE01,0xEE07,0xEE02,0x6008,0x601C
.word 0x6008,0x601C,0xFE01,0xFE01,0xFE01,0xFE01,0xFE01
.word 0xFE01,0xFE01,0xFE01,0x0000
|

.text 0 
.even

|======== initialisation of TPU in emulation mode==========
| setting TPURAM base address in TRAMBAR 
	move.w	`#'_TBASADR>>8,_TRAMBAR   

| loading TPU code array in base tpu address
	lea.l	_TBASADR,%a1
	lea.l	_LLC_EMU,%a0
0:	move.w	(%a0)+,(%a1)+
	tst.w	(%a0)
	jne	0b
	lea.l	_TBASADR+LLC_EMU_ADR_DATA,%a1
	lea.l	_LLC_EMU_DATA,%a0
1:	move.w	(%a0)+,(%a1)+
	tst.w	(%a0)
	jne	1b

|setting TPUMCR
	move.w	`#'0x04C0,_TPUMCR | 15=0: normal mode
				  | 14-13=00: Tcr1p divide by 1
				  | 12-11=00: Tcr2p divide by 1
				  | 10=1: TPURAM in emulation mode
				  | 9=0: TCR2 pin is clock source for TCR2
				  | 8: read only
				  | 7=1: registers restricted in user mode
				  | 6=1: system clock/4 input to TCR1
				  | 5-4: not used
			          | 3-0=0000: no Interrup arbitration 


|_CFSR assign function to channel 
| 0xd = dio (Discrete IN/OUT)
| 0xe = pwm (Pulse-width Modulation)
| 0xf = fqd     (fast quadrature decode)

.set _CFSR3,    0xfffe12        | channels 0-3 function register
.set _CFSR2,    0xfffe10        | channels 4-7 function register				
.set _CFSR1,    0xfffe0e	| channels 8-11 function register
.set _CFSR0,    0xfffe0c	| channels 12-15 function register


| _CPR channel priority register
| 00 => channel disabled
| 01 => Low priority
| 10 => Middle priority
| 11 => High priority

.set _CPR1,	0xfffe1e | channels 0-7 priority register
			 |  15      12        8         4         0
			 | | ch7 ch6 | ch5 ch4 | ch3 ch2 | ch1 ch0 |

.set _CPR0,	0xfffe1c | channels 8-15 priority register
			 |  15      12        8         4         0
			 | |ch15 ch14|ch13 ch12|ch11 ch10|ch9 ch8 |
	

| _HSRR channel host service request
| 00 => No Host Service (Reset Condition)
| 01 => Type 1 host service
| 10 => Type 2 host service
| 11 => Type 3 host service

.set _HSRR1,    0xfffe1a        | channels 0-7 host service request register
				|  15      12        8         4         0
				| | ch7 ch6 | ch5 ch4 | ch3 ch2 | ch1 ch0 | | channels 0-7 host service request register
				|  15      12        8         4         0
				| | ch7 ch6 | ch5 ch4 | ch3 ch2 | ch1 ch0 |
	
.set _HSRR0,    0xfffe18        | channels 15-8 host service request register
				|  15      12        8         4         0
				| |ch15 ch14|ch13 ch12|ch11 ch10|ch9 ch8 |

| _HSQR channel host sequence registers

.set _HSQR1,    0xfffe16        | channels 0-7 host service request register
				|  15      12        8         4         0
				| | ch7 ch6 | ch5 ch4 | ch3 ch2 | ch1 ch0 |
	
.set _HSQR0,    0xfffe14        | channels 15-8 host service request register
				|  15      12        8         4         0
				| |ch15 ch14|ch13 ch12|ch11 ch10|ch9 ch8 |

.set _CIER_,	0xfffe0A        |Channel Interrupt enable register
	move.w 0x0000,_CIER_    |no interrupt

||********* initialization of TPU Channels ******************
		    
|| PWM function:
|       Parameters:
|               15  14  13  12  11  10  9  8  7  6  5  4  3  2  1  0  
|      ffffx0 |-------------------------|      CHANNEL  CONTROL    |
|      ffffx2 |                    OLDRIS                          |
|      ffffx4 |                    PWMHI                           |
|      ffffx6 |                    PWMPER                          |
|      ffffx8 |                    PWMRIS                          | 
|
|     x=channel number
|
|  CHANNEL CONTROL:
|                15         9 8    5 4   2 1  0
|               |  NOT USED  | TBS  | PAC | PSC |
|
|               TBS :   0100: Output: Capture TCR1, Compare TCR1
|                       0111: Output: Capture TCR2, Compare TCR2
|               PAC :   1xx : DO NOT CHANGE PAC
|               PSC :   01  : force Pin High
|                       10  : force Pin Low
|
|  Hsr type fonction:
|       00: no host service request
|       01: Immediate Update
|       10: Initialization
|       11: undifined
|
| !! before changing parameters or performing a host request 
|    CPU must ensure that previous TPU service request has been serviced,
|    indicated by the HSR bits of channel cleared by the TPU (HSR=00)
|
|  Initialization Sequence:
|       1 -> writing CHANNEL CONTROL
|       2 -> writing PWMHI
|       3 -> writing PWMPER
|       4 -> Issuing HSR 10 for initialization
|       5 -> enabling channel by assigning high, middle or low priority (CPR)
|


|| DIO function:
|       Parameters:
|               15  14  13  12  11  10  9  8  7  6  5  4  3  2  1  0  
|      ffffx0 |-------------------------|      CHANNEL  CONTROL    |
|      ffffx2 |                    PIN_LEVEL                       |
|      ffffx4 |                    MATCH_RATE                      |
|
|     x=channel number
|
|  CHANNEL CONTROL:
|                15         9 8    5 4   2 1  0
|               |  NOT USED  | TBS  | PAC | PSC |
|
|               TBS :   0000: input : Capture TCR1, Match TCR1
|                       0001: input : Capture TCR1, Match TCR2
|                       0010: input : Capture TCR2, Match TCR1 
|                       0011: input : Capture TCR2, Match TCR2 
|               PAC :   000: Do not detect transition
|                       001: Detect Rising Edge
|                       010: Detect Falling Edge
|                       011: Detect Either Edge
|               PSC :   11: Do not force Pin
|
|  Hsr type fonction:                           Hsqr  Host Sequence Bit
|   00: no host service request          Trans Mode Record Pin on transition
|   01: Force High Output                Match Mode Record Pin at MATCH_RATE
|   10: Force Low Output                 Record Pin State on
|   11: Init as per Host Sequence Bits
|
| !! before changing parameters or performing a host request 
|    CPU must ensure that previous TPU service request has been serviced,
|    indicated by the HSR bits of channel cleared by the TPU (HSR=00)
|
|  Initialization Sequence for input:
|       1 -> writing CHANNEL CONTROL
|       2 -> writing MATCH_RATE
|       3 -> writing HSQR to configure trans or match mode
|       4 -> Issuing HSR 11 for initialization
|       5 -> enabling channel by assigning high, middle or low priority (CPR)
|
|  Initialization Sequence for output:
|       1 -> writing 01 to HSR for high level on pin
|                  or 10 to HSR for low level on pin
|       2 -> enabling channel by assigning high, middle or low priority (CPR)




|| ===== ASSIGN FUNCTIONS TO CHANNELS =======
	move.w `#'0xdede,_CFSR3 |  |  ch3   |  ch2   |  ch1   |  ch0   |
				|  |  0xd   |  0xe   |  0xd   |  0xe   |
				|  | DIO A  | PWM A  | DIO B  | PWM B  |

	move.w `#'0xdd0d,_CFSR2 |  |  ch7   |  ch6   |  ch5   |  ch4   |
				|  |  DI7   |  DI 6  |  0     |  0xd   |
				|  |  0xd   |  0xd   |        | MLI B  |
	
	move.w `#'0xffdd,_CFSR1 |  |  ch11  |  ch10  |  ch9   |  ch8   |
				|  |  0xf   |  0xf   |  0xd   |  0xd   |
				|  | FQDsec | FQDpri |  DO 9  |  DO 8  |

	move.w `#'0x0000,_CFSR0 |  |  ch15  |  ch14  |  ch3   |  ch12   |
				|  |  0     |  0     |  0     |  0      |

||=======  IO INITIALIZATION ============

|| DI (input) 6-7   => 0xffff6x  0xffff7x
|
	andi.w  `#'0x0fff,_HSQR1| 1000 xxxx xxxx xxxx update only on transition :tp6 
				| update on HSR11 on tp7
	ori.w  `#'0x8000,_HSQR1 |

	move.w `#'0xf,0xffff60 | PSC= 11 do not force output ??
			       | PAC= 11 detect either edge
	move.w `#'0xf,0xffff70 | PSC= 11 do not force output ??
			       | PAC= 11 detect either edge
	
| set to Low 
| 1 ->Issuing HSR 11 for init  1111 xxxx xxxx xxxx
    move.w   `#'0xF000,_HSRR1          | 1111 xxxx xxxx xxxx 
| 2 -> assigning priority  1111 xxxx xxxx xxxx  
    ori.w  `#'0xF000,_CPR1             | 1111 xxxx xxxx xxxx






|| DO (output) 8-9 (channel 8-9)  => 0xffff8x  0xffff9x
| set to Low tp9 and to high tp8
| 1 ->Issuing HSR 10  output low 01 output high  xxxx xxxx xxxx 1001
    move.w   `#'0x09,_HSRR0          | xxxx xxxx xxxx 1001
| 2 -> assigning priority  xxxx xxxx xxxx 1111  
    ori.w  `#'0x0F,_CPR0            | xxxx xxxx xxxx 1111





||======= MOTOR A INITIALIZATION ============

|| DIO A (channel 3)  => 0xffff3x
| set to Low 
| 1 ->Issuing HSR 10 for output low  xxxx xxxx 10xx xxxx
    move.w   `#'0x80,_HSRR1          | xxxx xxxx 10xx xxxx
| 2 -> assigning priority  xxxx xxxx 11xx xxxx  
	ori.w  `#'0xc0,_CPR1         | xxxx xxxx 11xx xxxx


|| pwm A (channel 2)  => 0xffff2x
| 1 step -> writing CHANNEL_CONTROL =>0xffff20 
	move.w `#'0x92,0xffff20 | PSC= 10 => force PIN Low at initialization
				| PAC= 100
				| TBS= Capture TCR1, Compare TCR1
| 2 step -> writing PWMHI => 0xffff24                
	move.w  `#'_PWMHI_INIT_A,0xffff24 
| 3 step -> writing PWMPER => 0xffff26     
	move.w  `#'_PWM_PERIOD_A,0xffff26
| 4 step -> Issuing HSR 10 for initialization  xxxx xxxx xx10 xxxx
	move.w   `#'0x20,_HSRR1          | xxxx xxxx xx10 xxxx
| 5 step -> assigning priority  xxxx xxxx xx11 xxxx  
	ori.w  `#'0x30,_CPR1            | xxxx xxxx xx11 xxxx
| 6 wait end initialization
0:	move.w	_HSRR1,%d0
	jne	0b

||============= MOTOR B INITIALIZATION ================


|| DIO B (channel 1)  => 0xffff1x
| set to Low 
| 1 ->Issuing HSR 10 for output low  xxxx xxxx xxxx 10xx
	move.w   `#'0x8,_HSRR1          | xxxx xxxx xxxx 10xx
| 2 -> assigning priority  xxxx xxxx xxxx 11xx  
	ori.w  `#'0xc,_CPR1            | xxxx xxxx xxxx 11xx

|| DIO MLI B (channel 4)  => 0xffff4x
| set to Low 
| 1 ->Issuing HSR 10 for output low  xxxx xx10 xxxx xxxx
	move.w   `#'0x200,_HSRR1          | xxxx xx10 xxxx xxxx
| 2 -> assigning priority  xxxx xx11 xxxx xxxx  
	ori.w  `#'0x300,_CPR1            | xxxx xx11 xxxx xxxx

|| pwm B (channel 0)  => 0xffff0x
| 1 step -> writing CHANNEL_CONTROL =>0xffff00 
	move.w `#'0x92,0xffff00 | PSC= 10 => force PIN Low at initialization
				| PAC= 100
				| TBS= Capture TCR1, Compare TCR1
| 2 step -> writing PWMHI => 0xffff04                
	move.w  `#'_PWMHI_INIT_B,0xffff04 
| 3 step -> writing PWMPER => 0xffff06     
	move.w  `#'_PWM_PERIOD_B,0xffff06
| 4 step -> Issuing HSR 10 for initialization  xxxx xxxx xxxx xx10
	move.w   `#'0x2,_HSRR1          | xxxx xxxx xxxx xx10
| 5 step -> assigning priority  xxxx xxxx xxxx xx11  
	ori.w  `#'0x3,_CPR1            | xxxx xxxx xxxx xx11
| 6 wait end initialization
0:	move.w	_HSRR1,%d0
	jne	0b

||============ Quadratures intialization ============
| 1 step -> writing corr pin state addr 0xffffa8 et 0xffffb8
	move.w  `#'0xb6,0xffffa8 | write addr of chan 11 pin state param
	move.w  `#'0xa6,0xffffb8 | write addr of chan 10 pin state param
| 2 step -> writing edge time lsb
	move.w  `#'0xa1,0xffffaa | lsb edge-time on chan 10
	move.w  `#'0xa1,0xffffba | lsb edge-time on chan 10

| 3 step -> configure position count primary channel => 0xffffa2
	move.w  `#'0,0xffffa2

| 4 step -> write hsqr
	andi.w  `#'0xff0f,_HSQR0| xxxx xxxx 0000 xxxx
	ori.w   `#'0x40,_HSQR0  | xxxx xxxx 0100 xxxx
				| write hsqr =00 chanpri
				| write hsqr =01 chansec
| 5 step -> Issuing HSR 11 for initialization
	move.w  `#'0xf0,_HSRR0  | write hsrr = 11 chanpri et chan sec
| 6 step -> assigning priority
	ori.w   `#'0xf0,_CPR0   | write pri chanpri chansec
| 7 wait end initialization
0:	move.w	_HSRR0,%d0
	jne	0b

||------------------- ADC ------------------


.set _AR_UR_IN_,    7	|entree IO_STATUS arret d urgence
.set _AR_UR_MEM_,   2   |entree IO_STATUS arret d urgence memorise
.set _FREIN_DES_, 0      |butee frein desserre 
.set _FREIN_SER_, 1      |butee frein serre 
.set _ANAL_ADR,  0x10000 | ADC converter address
.set _POT_VERIN_, 3       | analog input AIN4 
.set _JOY_DIR_,   1	  | analog input AIN2 
.set _TENSION_BATT_, 2   | Tension batterie
.set _JOY_TRA_,   2       | analog input AIN3 
.set _TEMP_BATT_, 0      | analog input AIN0 ?? 
.set _GYROMETRE_, 1      | analog input AIN0 ?? 
.set _CUR_POWA_,  4       | analog input AIN5
.set _CUR_POWB_,  6       | analog input AIN7
.set _JOY_CENTER_TRA_,	265	| zero joystick traction
.set _JOY_CENTER_DIR_,	270	| zero joystick direction
.set _JOY_MAX_DIR_,	210	| max joystick direction
.set _JOY_MIN_DIR_,	-210	| min joystick direction
.set _JOY_MAX_TRA_,	220	| max joystick traction
.set _JOY_MIN_TRA_,	-220	| min joystick traction

.set _POTAR_CENTER_,	-12	| potar center 
.set _PORT_VENTIL_,     1       | port ventilation
	move.b	`#'_CDR_,_CANA_ | set external CAN clock for adc
	move.b	`#'0,_CAND_




|----------- memory initializations ----------------------------

	lea.l	MainInits,%a0   
	jra	MainFirstInit

MainInitLoop1:				| loop: logical
	move.b	%d0,(%a1)		| initialize 1 byte cell
	adda.l	(4,%a0),%a1		| step to next cell
	subi.l	`#1',%d1
	jne	MainInitLoop1		| iterate while counter>0
	rts

MainInitLoop4:				| loop: integer, float
	move.l	%d0,(%a1)		| initialize 4 bytes cell
	adda.l	(4,%a0),%a1		| step to next cell
	subi.l	`#'1,%d1
	jne	MainInitLoop4		| iterate while counter>0
	rts

MainInitLoop8:				| loop: double
	move.l	%d0,(%a1)		| initialize 8 bytes cell
	move.l	%d2,(4,%a1)
	adda.l	(4,%a0),%a1		| step to next cell
	subi.l	`#'1,%d1
	jne	MainInitLoop8		| iterate while counter>0
	rts

MainSingleInit:
	moveq.l	`#'1,%d1
	jbsr	(%a2)

MainNextInit:
	move.l	(%a0)+,%d0
	jne	MainNextInit		| until list end mark found

MainFirstInit:
	movea.l	(%a0)+,%a1		| a1=address of 1st cell to initialize
	tst.l	%a1
	jeq	MainLastInit		| finished when =0 
	movea.l	(%a0)+,%a2		| a2=MainInitLoopx subroutine address 
	move.l	(%a0)+,%d0		| d0=initialization value
	cmpa.l	`#'MainInitLoop8,%a2
	jne	MainAnyCase
	move.l	(%a0)+,%d2		| d2=high word of 8 bytes double real

MainAnyCase:
	move.l	(%a0),%d1		| d1=iterator repeat count
	jeq	MainSingleInit		| null repeat count: only one cell to init     
	movea.l	%a0,%a3			| a3=iterator list offset
	
MainPushInitIter:
	move.l	%d1,-(%sp)		| save current iteration counter
	addq.l	`#'8,%a0		| step forward iteration repeat, step
	move.l	(%a0),%d1		| fetch next iteration initial repeat count
	jne	MainPushInitIter	| until null end marker found
	suba.l	`#'8,%a0		| step back iterator repeat, step
	move.l	(%sp)+,%d1		| d1=inner iteration repeat count
	jbsr	(%a2)			| vectorized inner iteration

MainPopInitIter:			| current iteration finished:
	cmpa.l	%a3,%a0			| outer iteration finished?
	jeq	MainNextInit
	suba.l	`#'8,%a0		| step back iterator repeat, step
	adda.l	(4,%a0),%a1		| complement step forward
	move.l	(%sp)+,%d1		| restore current iteration counter
	subi.l	`#'1,%d1
	jne	MainPushInitIter	| iterate while counter>0
	jra	MainPopInitIter

MainInits:	| data buffer initialization (see *_ini_ macroes)
undivert
')

dnl data_ini_end() | terminate initialization triplets list
def(`data_ini_end', `
 .long 0	| initializers end mark (null address)
MainLastInit:
||----------- Demask all it on CPU ------------------------------
|	move.w `#'0x2000,%sr
	move.w `#'0x2400,%sr

')


dnl main_end_() | end of the main sequence (of computations)
def(`main_end_', `
	movem.l	(%a7)+,%d0-%d7/%a0-%a6	| unlink stack frame
	unlk %a6			| restore frame pointer
	rts				| end of main
		
')


dnl end_() | end of the code generated by SynDEx
def(`end_', `
	.globl exit
exit:
|	fin du programme apres fin du main
|
')


| ----------------- Macros d'appel des CAN IN, OUT & NOP --------------------

dnl PBL_ini_() 
def(`PBL_ini_',`
|------------------Attente d un RTR venant du PC ----------------------------|
0:	move.b `#'_SR_,_CANA_
	btst.b	`#'0,_CAND_	| frame received ?
	jeq	0b		| no ,wait
	move.b	`#'_RXDL_,_CANA_	| Rx Data Length register:
	move.b	_CAND_,%d0		| get number of received bytes into %d0
	move.b	`#'_CMR_,_CANA_ | yes, release buffer and start prg 
	move.b	`#'4,_CAND_
	btst.b	`#'4,%d0		| test RTR (bit4)
	jeq	0b

	move.b (flag_goodnum_) , (_IOok_)	

init_sequence_(num_noeud_m4)


	
	move.b `#'1,(goodstart_)
')

dnl PBL_end_() | Fin de la sequence de communication CAN
def(`PBL_end_',`
|--------- disable CAN interrupt on 68332 ------------
	move.w	`#'0x2700,%sr
	move.b	`#'_CR_,_CANA_
	move.b	`#'0x20,_CAND_
	move.w	`#'0x2000,%sr
	rts
')

dnl PBL_o_<type>(link,size,addr,slot)
dnl send size <type> items, read from address addr
dnl dontCare link (only one, the CAN) and slot (multicast)
define(`Can_Out',`dnl  Parametres : count,adr       
	move.l	`#'eval($1),%d0	| data buffer length
	lea.l	$2,%a0		| data buffer address
	jbsr	can_o_shared')
def(`PBL_o_logical',	`Can_Out($2,  $3)')
def(`PBL_o_integer',	`Can_Out($2*4,$3)')
def(`PBL_o_real',	`Can_Out($2*4,$3)')
def(`PBL_o_dpreal',	`Can_Out($2*8,$3)')

dnl PBL_i_<type>(link,size,addr,slot)
dnl receive size <type> items, store from address addr
dnl dontCare link (only one, the CAN) and slot (multicast)
define(`Can_In',`dnl  Parametres : count,adr
	move.l	`#'eval($1),%d0	| data buffer length
	lea.l	$2,%a0		| data buffer address
	jbsr	can_i_shared')
define(`Can_Inb',`dnl  Parametres : count,adr
	move.l	`#'eval($1),%d0	| data buffer length
	lea.l	$2,%a0		| data buffer address
	jbsr	can_n_shared')
def(`PBL_i_logical',    `Can_In($2,  $3)')
def(`PBL_ib_logical',    `Can_Nop($2)')
dnl def(`PBL_ib_logical',    `Can_Inb($2,$3)')
def(`PBL_i_integer',    `Can_In($2*4,$3)')
def(`PBL_i_real',       `Can_In($2*4,$3)')
def(`PBL_i_dpreal',     `Can_In($2*8,$3)')


dnl PBL_n_<type>(link,size)
dnl ignore size <type> items
dnl dontCare link (only one, the CAN)
define(`Can_Nop',`dnl  Parametre : count
	move.l	`#'eval($1),%d0 | data buffer length
	movea.l	`#'0,%a0  | data buffer address null for can_nop
	jbsr    can_n_shared')
def(`PBL_n_logical',    `Can_Nop($2)')
def(`PBL_n_integer',    `Can_Nop($2*4)')
def(`PBL_n_real',       `Can_Nop($2*4)')
def(`PBL_n_dpreal',     `Can_Nop($2*8)')
 

| ******************** Subroutine calls *************************************
dnl For interfacing C functions with parameters passed by stack.
dnl Change these macroes for other parameter passing models.

define(`args_',0)dnl initialize args for the first arg_x

dnl arg_logical_(arg) | fetch and push a logical
define(`arg_logical_', `define(`args_', incr(args_))
	clr.l	-(%sp)
	move.b	$1,(%sp)')

dnl arg_integer_(arg) | fetch and push an integer
define(`arg_integer_', `define(`args_', incr(args_))
	move.l	$1,-(%sp)')

dnl arg_real_(arg) | fetch and push a real
define(`arg_real_', `define(`args_', incr(args_))
	move.l	$1,-(%sp)')

dnl arg_dpreal_(arg) | fetch and push a real
define(`arg_dpreal_', `define(`args_', eval(args_+2))
	move.l	$1+4,-(%sp)
	move.l	$1,-(%sp)')

dnl arg_addr_(arg) | fetch and push a pointer
define(`arg_addr_', `define(`args_',incr(args_))
	pea	$1')

dnl call_args_(name) | call a subroutine, then cleanup stack
define(`call_args_', ``
	jbsr	$1'
ifelse(eval(args_!=0),1, `dnl if any args pushed:
	adda.l	`#'eval(4*args_),%sp define(`args_',0)')')

dnl ret_logical_(ret) | store integer returned from subroutine
define(`ret_logical_', `
	move.b	%d0,$1')

dnl ret_integer_(ret) | store integer returned from subroutine
define(`ret_integer_', `
	move.l	%d0,$1')

dnl ret_real_(ret) | store real returned from subroutine
define(`ret_real_', `
	move.l	%d0,$1')

dnl ret_dpreal_(ret) | store dpreal returned from subroutine
define(`ret_dpreal_', `
	move.l	%d0,$1
	move.l	%d1,$1+4')

dnl push_args([params]) | 
define(`push_args_', `dnl
	ifelse($1,,,`arg_addr_($1) push_args_(shift($@)) dnl')
')

define(`externc_',`def(`$1',`callc_('``$1''`,'$`'@`)')')

dnl callc_(fonction name ,[params] ) | 
define(`callc_', `dnl
	push_args_(reverse(shift($@))) dnl
	call_args_(`$1')')
define(`reverse', `ifelse($#,0,,$#,1,``$1'',`reverse(shift($@)),`$1'')')

| ******************** Standard I/O *****************************************
dnl The label SynDExRoot must be defined on the root processor.
dnl The printf_ macro is enabled only on the root processor.
dnl Example uses (push converted arguments in reverse order):
dnl printf_(`hi !') | equivalent C source: printf("hi !\n")|
dnl arg_integer_(i) | push last argument first
dnl printf_(`i=%d') | equivalent C source: printf("i=%d\n", i)|

def(`printf_', `pushlabel(`formatString')
.text 1
formatString: 
.ascii "$1" 
.asciz "\n"

.text 0
	.even
arg_addr_(formatString) poplabel(`formatString')
call_args_(`print')')

| ******************** Chronometric functions *******************************
dnl assumed declarations at the very beginning of .m4 file:
dnl tree_up_(x)  | link connected to parent
dnl tree_dn_(y)  | link connected to descendant
dnl tree_dn_(z)  | link connected to descendant
dnl ...
dnl tree_dn_end()  | marks end of links list


| ******************** User operations **************************************
dnl The following sets of macroes may be extended at the user's convenience.
dnl Other user macro-operations must be given in the "application.inc" file.

| -------------------- ALU operations ---------------------------------------

dnl logNot(e1,ret) | ret=!e1
def(`logNot', `ifelse($2,$1,,`
	move.b	$1,$2')
	eor.b	`#'1,$2')

dnl logAnd(e1,e2,ret) | ret=e1&&e2
def(`logAnd', `ifelse($3,$2,,`
	move.b	$2,$3')
	and.b	$1,$3')

dnl logOr(e1,e2,ret) | ret=e1||e2
def(`logOr', `ifelse($3,$2,,`
	move.b	$2,$3')
	or.b	$1,$3')

dnl intEqu(e1,e2,ret) | ret=e1==e2
def(`intEqu', `
	cmp.l	$1,$2
	seq.b	$3')

dnl realEqu(e1,e2,ret) | ret=e1==e2
def(`realEqu', `
	cmp.l	$1,$2
	seq.b	$3')

dnl intNeq(e1,e2,ret) | ret=e1!=e2
def(`intNeq', `
	cmp.l	$1,$2
	sne.b	$3')

dnl realNeq(e1,e2,ret) | ret=e1!=e2
def(`realNeq', `
	cmp.l	$1,$2
	sne.b	$3')

dnl intGeq(e1,e2,ret) | ret=e1>=e2
def(`intGeq', `
	cmp.l	$1,$2
	sge.b	$3')

dnl intNge(e1,e2,ret) | ret=e1<e2
def(`intNge', `
	cmp.l	$1,$2
	sls.b	$3')

| ---------------- Add, Sub, Mul ---------------------------

dnl intAdd(e1,e2,ret) | 3+3/ ret=e1+e2
def(`intAdd', `ifelse($3,$2,,`
	move.l	$2,$3')
	add.l	$1,$3')

dnl intSub(e1,e2,ret) | 3+3/ ret=e1-e2
def(`intSub', `ifelse($3,$2,,`
	move.l	$2,$3')
	neg.l	$3
	add.l	$1,$3')

dnl intMul(e1,e2,ret) | 3+3/ ret=e1*e2 multiplication signee
def(`intMul', `ifelse($3,$2,,`
	move.l	$2,$3')
	muls.l	$1,$3')

dnl intDecr(i) | 3+1/ i--
def(`intDecr', `
	subq.l	`#'1,$1')

dnl intIncr(i) | 3+1/ i++
def(`intIncr', `
	addq.l	`#'1,$1')


dnl realAdd(e1,e2,ret) | 3+3/
def(`realAdd', `| fonction non definie !!!')

dnl realSub(e1,e2,ret) | 3+3/
def(`realSub', `| fonction non definie !!!')

dnl realMul(e1,e2,ret) | 3+3/
def(`realMul', `| fonction non definie !!!')

dnl realAAdd(size,i1,i2,o) | 7+3/ o[size]=i1[size]+i2[size]
def(`realAAdd', `| fonction non definie !!!')

dnl realASub(size,i1,i2,o) | 7+3/ o[size]=i1[size]-i2[size]
def(`realASub', `| fonction non definie !!!')

dnl realAMul(size,i1,i2,o) | 7+3/ o[size]=i1[size]*i2[size]
def(`realAMul', `| fonction non definie !!!')  

| -------------------- Array operations -------------------------------------

dnl intAAdd(size,i1,i2,o) | 7+3/ o[size]=i1[size]+i2[size]
def(`intAAdd', `
	lea.l	$4,%a0
	lea.l	$2,%a1
	lea.l	$3,%a2
	move.l	$1,%d0
	subq.l	`#'1,%d0
0:	move.l	(%a1)+,%d1
	add.l	(%a2)+,%d1
	move.l	%d1,(%a0)+
	dbra	%d0,0b')


dnl intASub(size,i1,i2,o) | 7+3/ o[size]=i1[size]-i2[size]
def(`intASub', `
	lea.l	$4,%a0
	lea.l	$2,%a1
	lea.l	$3,%a2
	move.l	$1,%d0
	subq.l	`#'1,%d0
0:	move.l	(%a1)+,%d1
	sub.l	(%a2)+,%d1
	move.l	%d1,(%a0)+
	dbra	%d0,0b')


dnl intAMul(size,i1,i2,o) | 7+3/ o[size]=i1[size]*i2[size]
def(`intAMul', `
	lea.l	$4,%a0
	lea.l	$2,%a1
	lea.l	$3,%a2
	move.l	$1,%d0
	subq.l	`#'1,%d0
0:	move.l	(%a1)+,%d1
	muls.l	(%a2)+,%d1
	move.l	%d1,(%a0)+
	dbra	%d0,0b')





|============= I/O PRAXICAR =====================

||------------------- ADC ---------------

dnl --------- Read ADC Convertor -----------
dnl Read_ADC(canalNumber,StoreAddr)
define(`Read_ADC',`
	move.w `#'0x2600,%sr
	move.w	`#'$1,_ANAL_ADR	| selection canal
 	move.l 	`#'3,%d0		| 2 wait for optimal 3 for secure
0:	dbra	%d0,0b			| wait conversion
 	move.w	_ANAL_ADR,%d0
	move.w `#'0x2400,%sr
	andi.w	`#'0x3ff,%d0	| read only 10 bits
	btst.l	`#'9,%d0
	jeq	1f
	ori.l	`#'0xfffffe00,%d0	| if <0 signed extension 1111 1111 1111 1111  1111 111x xxxx xxxx
1:	move.l	%d0,$2  
')



dnl ---- CourantVerin -------------------
def(`CourantVerin_ini_',`')
def(`CourantVerin_get_',`Read_ADC(_CUR_POWB_,$1)')
def(`CourantVerin_end_',`')

dnl ---- CourantMoteur ------------------
def(`CourantMoteur_ini_',`')
def(`CourantMoteur_get_',`Read_ADC(_CUR_POWA_,$1)
	move.l $1,%d0
	sub.l `#'CAnaparam_,%d0
	muls.l `#'GAnaparam_,%d0
	move.l %d0,$1
')
def(`CourantMoteur_end_',`')

dnl ---- TemperatureBatterie -------------------
def(`TempBatt_ini_',`')
def(`TempBatt_get_',`Read_ADC(_TEMP_BATT_,$1)')
def(`TempBatt_end_',`')

dnl ---- Gyrometre -------------------
def(`Gyrometre_ini_',`')
def(`Gyrometre_get_',`Read_ADC(_GYROMETRE_,$1)')
def(`Gyrometre_end_',`')

dnl ---- PositionPotentiometreVerin ------------
def(`PositionPotentiometreVerin_ini_',`')
def(`PositionPotentiometreVerin_get_',`Read_ADC(_POT_VERIN_,$1)')
def(`PositionPotentiometreVerin_end_',`')

dnl ----- JoystickDirection ---------------
def(`JoystickDirection_ini_',`')
def(`JoystickDirection_get_',`dnl
Read_ADC(_JOY_DIR_,$1)
	neg.l $1
')
def(`JoystickDirection_end_',`')


dnl ------ JoystickTraction ----------------
def(`JoystickTraction_ini_',`')
def(`JoystickTraction_get_',
`Read_ADC(_JOY_TRA_,$1)
	move.l $1,%d0  
	sub.w	`#'_JOY_CENTER_TRA_,%d0
	cmp.w   `#'_JOY_MAX_TRA_,%d0
	jle 1f                          | si joy_tra <= joymax
	move.w  `#'_JOY_MAX_TRA_,%d0         | sinon, joy_tra=joymax
	jra 2f
1:      cmp.w   `#'_JOY_MIN_TRA_,%d0         
	jge     2f                      | si joy_dir >= joymin
	move.w  `#'_JOY_MIN_TRA_,%d0         | sinon, joy_dir=joymin
2:      ext.l   %d0
	move.l 	%d0,$1
')
def(`JoystickTraction_end_',`')

dnl ------ JoyEtPotDirection ---------------
def(`JoyEtPotDirection_ini_',`')
def(`JoyEtPotDirection_get_',dnl
 `Read_ADC(_JOY_DIR_,$1)
	move.l $1,%d0  
	sub.w	`#'_JOY_CENTER_DIR_,%d0
	neg.w %d0
	cmp.w   `#'_JOY_MAX_DIR_,%d0
	jle 1f                          | si pwmhi <= pwmmax
	move.w  `#'_JOY_MAX_DIR_,%d0         | sinon, joy_dir=joymax
	jra 2f
1:      cmp.w   `#'_JOY_MIN_DIR_,%d0         
	jge     2f                      | si joy_dir >= joymin
	move.w  `#'_JOY_MIN_DIR_,%d0         | sinon, joy_dir=joymin
2:      ext.l   %d0
	move.l 	%d0,$1
	  	 
  Read_ADC(_POT_VERIN_,$1+4)
	sub.l	`#'_POTAR_CENTER_,$1+4
 	neg.l $1+4
	move.l `#'3,%d0
0:	dbra %d0,0b

')
def(`JoyEtPotDirection_end_',`')

|| ------------------ PWM -----------------------

dnl -------- PwmA_Change ---------------------
define(`PwmA_Change',`
	move.w  $1,%d0                  | %d0=consigne
	addq.w  `#'_PWMHI_INIT_A,%d0      | %d0 + valeur € 0
	cmp.w   `#'_PWM_MAX_A,%d0          
	jle 1f                          | si pwmhi <= pwmmax
	move.w  `#'_PWM_MAX_A,%d0         | sinon, pwmhi=pwmmax
	jra 2f
1:      cmp.w   `#'_PWM_MIN_A,%d0         
	jge     2f                      | si pwmhi >= pwmmin
	move.w  `#'_PWM_MIN_A,%d0         | sinon, pwmhi=pwmmin
2:      move.w  %d0,0xffff24            | change pwmhi canal 2
	')

dnl -------- PwmB_Change -----------------------
define(`PwmB_Change',`
	move.w  $1,%d0                    | %d0=consigne
	jgt	0f	| si >0
	neg.w	%d0	| |%d0|
	move.w	`#'0x200,_HSRR1   | xxxx xx10 xxxx xxxx  chan4->10 (dir -)
	jra	4f
0:	move.w	`#'0x100,_HSRR1   | xxxx xx01 xxxx xxxx  chan4->01 (dir +)	
4:	add.w  `#'_PWMHI_INIT_B,%d0      | %d0 + valeur € 0
	cmp.w   `#'_PWM_MAX_B,%d0         
	jle 1f                          | si pwmhi <= pwmmax
	move.w  `#'_PWM_MAX_B,%d0         | sinon, pwmhi=pwmmax
	jra 2f
1:      cmp.w   `#'_PWM_MIN_B,%d0         
	jge     2f                      | si pwmhi >= pwmmin
	move.w  `#'_PWM_MIN_B,%d0         | sinon, pwmhi=pwmmin
2:      tst.w	(_HSRR1)
	jne	2b
	move.w  %d0,0xffff04            | change pwmhi canal 0
	')

dnl -------- PwmMoteur --------------------------
def(`PwmMoteur_ini_',`')
def(`PwmMoteur_put_',`PwmA_Change($1)')
def(`PwmMoteur_end_',`')

dnl -------- PwmVerin ---------------------------
def(`PwmVerin_ini_',`')
def(`PwmVerin_put_',`PwmB_Change($1)')
def(`PwmVerin_end_',`')


def(`LectureValeurPwm_ini_',`')
def(`LectureValeurPwm_get_',`dnl
	clr.l %d0
	move.w (PwmA_Val_),%d0
	ext.l %d0
	move.l  %d0,$1
	clr.l %d0
	move.w (enable_pui),%d0
	ext.l %d0
	move.l %d0,$1+4
')

def(`LectureValeurPwm_end_',`')

def(`CmdCourantPwm',`dnl
	move.w $1+2,(PwmA_current_)
	move.w $1+6,(enable_pui)
	move.w `#' _PWM_TIME_VALID_ ,(valid_pui)  | valide la puissance pendant 25 millisec
')


def(`CmdCourantPwm_ini_',`')
def(`CmdCourantPwm_put_',`dnl
	move.w $1+2,(PwmA_current_)
	move.w $1+6,(enable_pui)	
	move.w `#' _PWM_TIME_VALID_ ,(valid_pui)  | valide la puissance pendant 25 millisec
')

def(`CmdCourantPwm_end_',`')

def(`CmdAllPwm_ini_',`')
def(`CmdAllPwm_put_',`dnl
	move.w $1,(PwmA_current_)
	move.b $1+2,%d0
	move.b %d0,%d1
	and.w `#'1,%d0 
	move.w %d0,(enable_pui)
	and.b `#'2,%d1
	jeq 1f
	EnablePowB()
	jmp 2f
1:	
	DisablePowB()
2:
	move.b $1+3,%d1
	ext.w %d1
	PwmB_Change(%d1)
	move.w `#' _PWM_TIME_VALID_ ,(valid_pui)  | valide la puissance pendant 25 millisec
')
def(`CmdAllPwm_end_',`')

def(`CmdAllPwmVent_ini_',`')

def(`CmdAllPwmVent_end_',`')
def(`CmdAllPwmVent_put_',`dnl
CmdAllPwm_put_($1)
	cmp.w `#'0,(enable_pui)
	jne	1f  | label 0 used in IO_PUT
	IO_PUT_(_PORT_VENTIL_,0)
	jmp 2f
1:	IO_PUT_(_PORT_VENTIL_,1)
2:
')
def(`CmdCourantPwmVent_ini_',`')
def(`CmdCourantPwmVent_put_',`dnl
	move.w $1+2,(PwmA_current_)
	move.w $1+6,(enable_pui)	
	move.w `#' _PWM_TIME_VALID_ ,(valid_pui)  | valide la puissance pendant 25 millisec
	cmp.w `#'0,(enable_pui)
	jne	1f  | label 0 used in IO_PUT
	IO_PUT_(_PORT_VENTIL_,0)
	jmp 2f
1:	IO_PUT_(_PORT_VENTIL_,1)
2:
')
def(`CmdCourantPwmVent_end_',`')


def(`VENTIL_ini_',`')
def(`VENTIL_get_',`dnl
IO_GET_((_PORT_VENTIL_+4),$1)')
def(`VENTIL_end_',`')

def(`FREINSTATUS_ini_',`')
def(`FREINSTATUS_get_',`dnl
IO_GET_(_FREIN_DES_,$1)
	move.b $1,%d1  | d1 not used in IO_GET
	eor.b `#'1,%d1 | inverse 0 et 1 
IO_GET_(_FREIN_SER_,$1)
	move.b $1,%d2
	eor.b `#'1,%d2
	lsl.b `#'1,%d2
	add.b %d2,%d1
	move.b %d1,$1
')
def(`FREINSTATUS_end_',`')


|--------------- DIO GENERAL ----------------------

dnl IO_PUT_(Num_port,valeur)

def(`IO_PUT_',`
	move.w   `#'0x08,_HSRR0  |xxxx xxxx xxxx 10xx tpu9 a 0
0:	tst.w	(_HSRR0)
	jne	0b
 	move.b `#'$2*0x20+$1*0x8, _PORTF0_ |  (IRQ4,IRQ3)=$1 IRQ5=$2
	move.w   `#'0x04,_HSRR0  |xxxx xxxx xxxx 01xx tpu9 a 1
0:	tst.w	(_HSRR0)
	jne	0b
	move.w   `#'0x08,_HSRR0  |xxxx xxxx xxxx 10xx tpu9 a 0
0:	tst.w	(_HSRR0)
	jne	0b')

dnl IO_GET_(Num_port,valeur) : valeur : byte  

def(`IO_GET_',`
 	move.b `#'$1*0x8, _PORTF0_ |  (IRQ5,IRQ4,IRQ3)=$1 
	move.w   `#'0xC000,_HSRR1  |11xx xxxx xxxx xxxx HSR 11 sur tpu7
0:	tst.w	(_HSRR1)
	jne	0b
	move.w  0xffff72,%d0
	andi.w `#'0x8000,%d0
	rol.w `#'1,%d0
	move.b %d0,$2 ')

dnl IO_STATUS_GET_(Num_port,valeur) : valeur : byte  

def(`IO_STATUS_GET_',`dnl
	| 1 ->Issuing HSR 10  output low on tp8  xxxx xxxx xxxx xx10
    	move.w   `#'0x02,_HSRR0
	move.b `#'$1*0x8, _PORTF0_ |  (IRQ5,IRQ4,IRQ3)=$1 
0:	tst.w	(_HSRR0)
	jne	0b          
 	move.w   `#'0xC000,_HSRR1  |11xx xxxx xxxx xxxx HSR 11 sur tpu7
0:	tst.w	(_HSRR1)
	jne	0b
	| 1 ->Issuing HSR 01  output low on tp8  xxxx xxxx xxxx xx01
	move.w   `#'0x01,_HSRR0
	move.w  0xffff72,%d0
	andi.w `#'0x8000,%d0
	rol.w `#'1,%d0
	move.b %d0,$2 
0:	tst.w	(_HSRR0)
	jne	0b')


dnl STATUS_GET(valeur) : valeur : byte  

def(`STATUS_GET_',`dnl
	move.w  0xffff62,%d0   |lecture de tp6 : vaut 1 si tout va bien
	andi.w `#'0x8000,%d0
	rol.w `#'1,%d0
	move.b %d0,$1')


def(`ARUR_ini_',`')
dnl ARUR_get(valeur) : valeur : byte  
def(`ARUR_get_',`dnl
	STATUS_GET_($1)
	tst.b $1
	jeq 0f
	clr.b $1
	jmp 1f
0:
	IO_STATUS_GET_( _AR_UR_MEM_ ,$1)
	move.b $1,%d1  | d1 not used in IO_STATUS_GET
	eor.b `#'1,%d1 | inverse 0 et 1 
	IO_STATUS_GET_( _AR_UR_IN_ ,$1)
	move.b $1,%d2
	eor.b `#'1,%d2
	lsl.b `#'1,%d2
	add.b %d2,%d1
	move.b %d1,$1
1:
')
def(`ARUR_end_',`')


|--------------- DIO PUISSANCE --------------------

dnl ------- DisablePowA ----------------------------
def(`DisablePowA',`dnl
move.w `#'0x80,_HSRR1   | disable: xxxx xxxx 10xx xxxx  chan3->10
0:	tst.w	(_HSRR1)
	jne	0b   ')

dnl ------- EnablePowA ------------------------------
def(`EnablePowA',`dnl
move.w  `#'0x40,_HSRR1  | enable : xxxx xxxx 01xx xxxx chan3->01
0:	tst.w	(_HSRR1)
	jne	0b ')

dnl ------- DisablePowB ----------------------------
def(`DisablePowB',`dnl
move.w `#'0x8,_HSRR1    | xxxx xxxx xxxx 10xx  chan1->10
0:	tst.w	(_HSRR1)
	jne	0b ')

dnl -------- EnablePowB  -----------------------------
def(`EnablePowB',`dnl
move.w  `#'0x4,_HSRR1   | xxxx xxxx xxxx 01xx chan1->01
0:	tst.w	(_HSRR1)
	jne	0b ')


dnl------- enable puissance verin ------------------
dnl   logical ?enable -> si 1 enable si -> disable
def(`EnableVerin_ini_',`')
def(`EnableVerin_put_',`
    tst.b       ($1)
    jeq 1f
    EnablePowB()
    jra 2f
1:   
    DisablePowB()
2:
')
def(`EnableVerin_end_',`')

dnl------- enable puissance moteur ------------------
dnl   logical ?enable -> si 1 enable si0 -> disable
def(`EnableMoteur_ini_',`')
def(`EnableMoteur_put_',`
    tst.b       ($1)
    jeq 1f
    EnablePowA()
    jra 2f
1:   
    DisablePowA()
2:
')
def(`EnableVerin_end_',`')

||----------------- FQD ---------------------

|-- LectureCodeur ---
def(`LectureCodeur_ini_',`
fqd_write(`0')
')
def(`LectureCodeur_get_',`
fqd_read($1)
')
def(`LectureCodeur_end_')

define(`fqd_read',`
move.w  0xffffa2,%d0
ext.l	%d0
move.l	%d0,$1
')

|-- fqd_write ---
define(`fqd_write',`
move.w  `#'$1,0xffffa2
')

|------ vitesse codeur -----------
def(`VitesseCodeur_ini_')
def(`VitesseCodeur_get_',`
	clr.l %d0
        move.w	(vitfiltr),%d0
        ext.l %d0
	move.l	%d0,$1
')

def(`VitesseCodeurOpt_ini_')
def(`VitesseCodeurOpt_get_',`
	clr.l %d0
        move.w	(vitfiltr),%d0
	move.w	%d0,$1
')


def(`VitesseCodeurOpt_end_')
def(`VitesseCodeur_end_')

|------ vitesse codeur dir  -----------
def(`VitesseCodeurDir_ini_')
def(`VitesseCodeurDir_get_',`
	clr.l %d0
        move.w	(vitfiltr),%d0
        ext.l %d0
	neg.l %d0
	move.l	%d0,$1
')
def(`VitesseCodeurDir_end_')

def(`VitesseCodeurDirOpt_ini_')
def(`VitesseCodeurDirOpt_get_',`
	clr.l %d0
        move.w	(vitfiltr),%d0
	neg.w %d0
	move.w	%d0,$1
')
def(`VitesseCodeurDirOpt_end_')


def(`FVoltmetre_ini_')

def(`FVoltmetre_get_',`dnl
	Read_ADC(_TENSION_BATT_,$1)
')
def(`FVoltmetre_end_')






def(`externpc_')



| ************************ end of m68kexec file ***************************
 divert`'dnl End of macro definitions