divert(-1)
define(`NOTRACEDEF')

# Robosoft MPC555 boards Serial Number (used by the download process)
define(`u555_CANID_',0x4014)
define(`copy',`ifelse(
MGC,`LOOP',`copy_($2,$1)')')



###################################################################################################################
#################################                             #####################################################														  #
#################################  GESTION CAPTEUR ULTRA SON  #####################################################										  #
#################################                             #####################################################														  #
###################################################################################################################



# CapteurCtr ajuste les parametres des ultra son en fct des besoins (quel capteur interroge t on, Portee des capteurs...
# Non implemente (le prg ne fonctionnant deja pas avec des valeurs fixees...)

define(`CapteurCtr',`ifelse(
MGC,LOOP,`'
)')


###############################################################################
# Capteur_us
#	Gere l initialisation (equivalent de main dans ceintult.c pour le programme sans syndex)
#	Lance le display pour affichage des resultats sur ecran
############################################################################

define(`capteur_us',`ifelse(
MGC,INIT,`
MPIOInit		# Initialisation des I/O
MPIODis1		# Capteur du groupe 1 eteint 
MPIOEn2			# Capteur du groupe 2 allume
MPIOEn3			# Capteur du groupe 3 allume
InitInterrupt		# Initialisation des interruptions
MPIOPWAR		# Le relais est mis en position arriere (les capteurs arrieres peuvent etre alimentes)
PWMInit			# Initialisation du PWM1
	li r31,0			# Initialise une valeur dans un registre
	addis	r30,r31,0x0030   	# Ne sert que pour avoir un retour des calculs fait par le mpc555
	ori	r30,r30,0x609A
	li	r29,0x2
	sth	r29,0x0(r30)
PWMEnInt		# Autorise les interruptions du PWM1
',
MGC,LOOP,`

	li 	r31,0x0			# Recupere la valeur du registre (Ne sert que pour avoir un retour sur ce que fait
	addis 	r30,r31,0x0030		# le MPC555
	ori 	r30,r30,0x609A
	lhz	r29,0x0(r30)
		
	B(stw r29,$1)			# Affichage de la valeur a l'ecran (DISPLAY) (c est la 1ere fct defini plus bas dans rtai)
')')


#*******************************************************************
#PWMInit
#	Initialisation de PWM1 (Seul PWM utilise pour les ultra son)
#********************************************************************
define(`PWMInit',`
	li	r31,0x0	
	
	addis	r30,r31,0x0030  # Set COUNTER PRESCALER MCPSM Status/Control Register	
	ori	r30,r30,0x6816
	li	r29,-0x7F09
	sth	r29,0x0(r30)
	

	addis	r30,r31,0x0030  # Set CHANNEL 1 PWM Period Register 	
	ori	r30,r30,0x6008
	li	r29,0x7FFF
	sth	r29,0x0(r30)
	
	addis	r30,r31,0x0030  # Set CHANNEL 1 PWM Pulse  Register	
	ori	r30,r30,0x600A
	li	r29,0x51EA	# Fixe la portee maximale des capteurs ultra-son
	sth	r29,0x0(r30)
	
	addis	r30,r31,0x0030 	# Set CHANNEL 1 PWM Status/Control Register
	ori	r30,r30,0x600E
	li	r29,0x54F6
	sth	r29,0x0(r30)	

	addis	r30,r31,0x0030  # Disable Interrupt on PWM1	
	ori	r30,r30,0x6C04
	#lhz	r29,0(r30)
	#andi.	r29,r29,0xFFFD
	li 	r29,0x0000
	sth	r29,0x0(r30)
	
	addis	r30,r31,0x0030  # Read Interrupt Status Register (MIOS1SR0)	
	ori	r30,r30,0x6C00
	lhz	r29,0x0(r30)
	sth	r31,0x0(r30)    # Clear interrupt status register   	
	
	addis	r30,r31,0x0030 	# Enable servicing by asigning a Priority
	ori	r30,r30,0x6C30
	lhz	r29,0x0(r30)
	ori	r29,r29,0x700
	sth	r29,0x0(r30)	

')

#******************************************************	
#PWMDIsInt
#	Masque les interruptions liees au PWM
#******************************************************
define(`PWMDisInt',`

	li	r31,0x0        		# Set r31=0x0
	
	addis	r30,r31,0x0030	    	# Disable Interrupt on PWM1
	ori	r30,r30,0x6C04
	lhz	r29,0x0(r30)
	andi.	r29,r29,0xFFFD
	sth	r29,0x0(r30)
	
')

#******************************************************	
#PWMENInt
#	Autorise les interruptions liees au PWM
#******************************************************	 
define(`PWMEnInt',`
	
	li	r31,0x0 
	
	addis	r30,r31,0x0030		# Enable interrupt on PWM1
	ori	r30,r30,0x6C04
	lhz	r29,0x0(r30)
	ori	r29,r29,0x2
	sth	r29,0x0(r30)
		
')

#####################################################
#MPIOPWAR
# Active l alimentation des capteur ultra son arriere
#####################################################	
define(`MPIOPWAR',`	
	li	r31,0x0
	addis	r30,r31,0x0030
	ori	r30,r30,0x6100
	lhz	r31,0x0(r30)
	ori	r31,r31,0x400	# Met a 1 MPIO10
	sth	r31,0x0(r30)	

')

#####################################################
#MPIOPWAV
# Active l alimentation des capteur ultra son avant
#####################################################
define(`MPIOPWMAV',`	
	li	r31,0x0
	addis	r30,r31,0x0030
	ori	r30,r30,0x6100
	lhz	r31,0x0(r30)
	andi.	r31,r31,0xFBFF	# Met a 0 de MPIO10
	sth	r31,0x0(r30)	

')

#########################################
#MPIOEn1
# Met a 1 (active) l I/O du groupe 1 (MPIO 9)
#########################################
define(`MPIOEn1',`
	
	li	r31,0x0
	addis	r30,r31,0x0030
	ori	r30,r30,0x6100
	lhz	r31,0x0(r30)
	ori	r31,r31,0x200
	sth	r31,0x0(r30)
')



#########################################
#MPIOEn2
# Met a 1 (active) l I/O du groupe 2 (MPIO 6)
#########################################
define(`MPIOEn2',`
	
	li	r31,0x0
	addis	r30,r31,0x0030
	ori	r30,r30,0x6100
	lhz	r31,0x0(r30)
	ori	r31,r31,0x40
	sth	r31,0x0(r30)

')

#########################################
#MPIOEn3
# Met a 1 (active) l I/O du groupe 3 (MPIO 7)
#########################################
define(`MPIOEn3',`
	
	li	r31,0x0
	addis	r30,r31,0x0030
	ori	r30,r30,0x6100
	lhz	r31,0x0(r30)
	ori	r31,r31,0x80
	sth	r31,0x0(r30)

')

#########################################
#MPIODis2
# Met a 0 (desactive) l I/O du groupe 2 (MPIO 6)
#########################################
define(`MPIODis2',`
	
	li	r31,0x0
	addis	r30,r31,0x0030
	ori	r30,r30,0x6100
	lhz	r31,0x0(r30)
	andi.	r31,r31,0xFFBF
	sth	r31,0x0(r30)	

')

#########################################
#MPIODis1
# Met a 0 (desactive) l I/O du groupe 1 (MPIO 9)
#########################################
define(`MPIODis1',`
	
	li	r31,0x0
	addis	r30,r31,0x0030
	ori	r30,r30,0x6100
	lhz	r31,0x0(r30)
	andi.	r31,r31,0xFDFF
	sth	r31,0x0(r30)

')

#########################################
#MPIODis3
# Met a 0 (desactive) l I/O du groupe 3 (MPIO 7)
#########################################
define(`MPIODis3',`	
	li	r31,0x0
	addis	r30,r31,0x0030
	ori	r30,r30,0x6100
	lhz	r31,0x0(r30)
	andi.	r31,r31,0xFF7F
	sth	r31,0x0(r30)
	
')


#*****************************************************************************
#MPIOInit
#	Initialisation des entrees sorties
#*****************************************************************************
define(`MPIOInit',`	
        li      r31,0x0  
        addis   r31,r31,0x0030	   # r31 = Pointer for MIOS Module (0x0030 0000)
	ori	r31,r31,0x6102
        
	li      r30,0x0FFF         # b0..b3 Input, b4..b31 Output       
	sth	r30,0x0(r31)	   # write DDR register

')

#*****************************************************************************
#InitInterrupt
#	Initialisation des interruptions
#*****************************************************************************
define(`InitInterrupt',`

        li      r31,0x0
	addis   r31,r31,0x002F	   # A_SIUBase
	ori	r31,r31,0xC000	   # A_SIUOffset

	li	r30,0x0800         # pin IRQn as SGPIO instead of IRQ !!
	stw	r30,0x0000(r31)    # WARNING IRQ0 is a NON maskable interrupt    A_SIUMCR

        li      r30,0x0480         # PIT irq Disable, and Stopped, set IRQ Level to 4
        sth     r30,0x0240(r31)	   # A_PISCR

        li      r30,0x5555         # Enable only Internal INTERRUPT
        sth     r30,0x0014(r31)    # A_SIMASK

	mfmsr	r30
	ori	r30,r30,0xA042	   # Enable external Interrupt, FP enable, FFF00000 for IT enable
	andi.	r30,r30,0xFFFF	   # keep only the 16 Low bit
        mtmsr   r30
	
')

############################# TPU########################


##########################################################################
#TPU3Init
# Initilisation des canaux TPU
# Equivalent de TPU3Inittcr1 + TPUInitUltra dans le programme sans syndex
##########################################################################
define(`TPU3Init',`	

	.macro SetReg AdRgLo,Value
		addis	r11,r0,0x0030
		ori	r11,r11,\AdRgLo
		li	r12,\Value
		sth	r12,0(r11)
	.endm
	
	.macro SetRegOr AdRgLo,Value
		addis	r11,r0,0x0030
		ori	r11,r11,\AdRgLo
		lhz	r12,0(r11)
		ori	r12,r12,\Value
		sth	r12,0(r11)
	.endm
	
	#configure with TPUMCRX
	SetRegOr 0x442A	, 0x004e	
	SetRegOr 0x4400	, 0x4000	
	SetRegOr 0x4428	, 0x00A2
	
	SetReg 0x441E ,0x0000       # Disable Clannels
	SetReg 0x4410 ,0x6666	    # Select FQD funtion
	SetReg 0x4412 ,0x6666

	SetReg 0x4508 ,0x0016	    # Initialise CORR_PIN_STATE_ADDR
	SetReg 0x450A ,0x0001	    # Initialise EDGE_TIME_LSB_ADDR  
	SetReg 0x4518 ,0x0006
	SetReg 0x451A ,0x0001

	SetReg 0x4528 ,0x0036
	SetReg 0x452A ,0x0021
	SetReg 0x4538 ,0x0026
	SetReg 0x453A ,0x0021

	SetReg 0x4548 ,0x0056
	SetReg 0x454A ,0x0041
	SetReg 0x4558 ,0x0046
	SetReg 0x455A ,0x0041

	SetReg 0x4568 ,0x0076
	SetReg 0x456A ,0x0061
	SetReg 0x4578 ,0x0066
	SetReg 0x457A ,0x0061

	SetReg 0x4502 ,0x0000	     # Initialse Position Counts
	SetReg 0x4522 ,0x0000
	SetReg 0x4542 ,0x0000
	SetReg 0x4562 ,0x0000

	SetReg 0x4416 ,0x4444	     # Selec Primary and Secondary channel
	SetReg 0x441A ,-0x0001	     # Initialize the function in each channel 
	SetReg 0x441E ,-0x0001       # Enable servicing by asigning a Priority 

	SetReg 0x401E ,0x0000	     # Disable Channels
	SetReg 0x4010 ,-0x5555	     # Select NITC funtion
	SetReg 0x4012 ,-0x5556	     # Select NITC funtion

	SetReg	0x4100 ,0x0007	     # Initialize function
	SetReg	0x4104 ,0x0000

	SetReg	0x4110 ,0x0007	     # Initialize function
	SetReg	0x4114 ,0x0000

	SetReg	0x4120 ,0x0007	     # Initialize function
	SetReg	0x4124 ,0x0000

	SetReg	0x4130 ,0x0007	     # Initialize function
	SetReg	0x4134 ,0x0000
	
	SetReg	0x4140 ,0x0007	     # Initialize function
	SetReg	0x4144 ,0x0000

	SetReg	0x4150 ,0x0007	     # Initialize function
	SetReg	0x4154 ,0x0000

	SetReg	0x4160 ,0x0007	     # Initialize function
	SetReg	0x4164 ,0x0000

	SetReg	0x4170 ,0x0007	     # Initialize function
	SetReg	0x4174 ,0x0000


	SetReg 0x4016 ,0x0000	     # Select Single shot, no links
	SetReg 0x401A ,0x5555	     # Initialize the function 
	

	addis	r11,r0,0x0030	     # Disable Interrupt on channel 0 1 2 3 4 5 6 7
	ori	r11,r11,0x400A
	lhz	r12,0(r11)
	andi.	r12,r12,0xff00
	sth	r12,0(r11)
	
	addis	r11,r0,0x0030        # Read CISR
	ori	r11,r11,0x4020 
	lhz	r12,0(r11)

	sth	r0,0(r11)            # Clear interrupt status register
	
	addis	r11,r0,0x0030        # Select IRQ1 for interrupt
	ori	r11,r11,0x4008
	li	r12,0x0200
	sth	r12,0(r11)		

	addis	r11,r0,0x0030        # Enable interrupts on channel 0 1 2 3 4 5 6 7
	ori	r11,r11,0x400A
	lhz	r12,0(r11)
	ori	r12,r12,0xff
	sth	r12,0(r11)
 
	SetReg 0x401E ,-0x0001	     # Enable servicing by asigning a Priority 

')

###############################
#TPUInitUltra
# Initilisation des canaux TPU
################################
define(`TPUInitUltra',`
	.macro SetReg AdRgLo,Value
		addis	r11,r0,0x0030
		ori	r11,r11,\AdRgLo
		li	r12,\Value
		sth	r12,0(r11)
	.endm
	
	.macro SetRegOr AdRgLo,Value
		addis	r11,r0,0x0030
		ori	r11,r11,\AdRgLo
		lhz	r12,0(r11)
		ori	r12,r12,\Value
		sth	r12,0(r11)
	.endm
	
	#configure with TPUMCRX
	SetRegOr 0x442A	, 0x004e	
	SetRegOr 0x4400	, 0x4000	
	SetRegOr 0x4428	, 0x00A2

	SetReg 0x401E ,0x0000		# Disable Channels
	SetReg 0x4010 ,-0x5555		# Select NITC funtion
	SetReg 0x4012 ,-0x5556		# Select NITC funtion

	SetReg	0x4100 ,0x0007		# Initialize function
	SetReg	0x4104 ,0x0000

	SetReg	0x4110 ,0x0007		# Initialize function
	SetReg	0x4114 ,0x0000

	SetReg	0x4120 ,0x0007		# Initialize function
	SetReg	0x4124 ,0x0000

	SetReg	0x4130 ,0x0007		# Initialize function
	SetReg	0x4134 ,0x0000
	
	SetReg	0x4140 ,0x0007		# Initialize function
	SetReg	0x4144 ,0x0000

	SetReg	0x4150 ,0x0007		# Initialize function
	SetReg	0x4154 ,0x0000

	SetReg	0x4160 ,0x0007		# Initialize function
	SetReg	0x4164 ,0x0000

	SetReg	0x4170 ,0x0007		# Initialize function
	SetReg	0x4174 ,0x0000


	SetReg 0x4016 ,0x0000		# Select Single shot, no links
	SetReg 0x401A ,0x5555		# Initialize the function 
	

	addis	r11,r0,0x0030	    	# Disable Interrupt on channel 0 1 2 3 4 5 6 7
	ori	r11,r11,0x400A
	lhz	r12,0(r11)
	andi.	r12,r12,0xff00
	sth	r12,0(r11)
	
	addis	r11,r0,0x0030  		# Read CISR
	ori	r11,r11,0x4020 
	lhz	r12,0(r11)

	sth	r0,0(r11)      		# Clear interrupt status register
	
	addis	r11,r0,0x0030  		# Select IRQ1 for interrupt
	ori	r11,r11,0x4008
	li	r12,0x0200
	sth	r12,0(r11)		

	addis	r11,r0,0x0030  		# Enable interrupts on channel 0 1 2 3 4 5 6 7
	ori	r11,r11,0x400A
	lhz	r12,0(r11)
	ori	r12,r12,0xff
	sth	r12,0(r11)
 
	SetReg 0x401E ,-0x0001	        # Enable servicing by asigning a Priority 


')

###############################
#TPUStop
#  Arret des canaux TPU et remise a zero
#  Equivalent de TPUStop + TPUReset dans le programme sans syndex
################################
define(`TPUStop',`
	# Save context r0,r11,r12,LR,CR,SRR0,SRR1

	li	r31,0		        # Set r31=0x0

	addis	r11,r31,0x0030		# Stop TPU3A
	ori	r11,r11,0x4000
	lhz	r12,0(r11)
	ori	r12,r12,0x8000
	sth	r12,0(r11)
	
	addis	r11,r31,0x0030	# Reset TPU3A
	ori	r11,r11,0x4028
	lhz	r12,0(r11)
	ori	r12,r12,0x80
	sth	r12,0(r11)
	andi.	r12,r12,0xf7f
	sth	r12,0(r11)

')

###################################
#UltraDistInt
# Masque les interruptions liees aux ultra son
###################################
define(`UltraDistInt',`
	li	r31,0        	# Set r31=0x0

	addis	r11,r31,0x0030	# Disable Interrupt on channel 0 1 2 3 4 5 6 7
	ori	r11,r11,0x400A
	lhz	r12,0(r11)
	andi.	r12,r12,0xff00
	sth	r12,0(r11)
	
')

#*********************************************************************************
# UltraRead
# 	Read TPU channel 2 : distance mesuree par le capteur ultrason
#	Pour les tests seul UltraRead 3 est implemente (seul le capteur 4 est lu)
#*********************************************************************************
define(`UltraRead3',`

	li	r30,0
	addis	r30,r30,0x0030	
	ori	r30,r30,0x4138	    # r30 = A_FINAL_TRANS_TIME

	lhz	r27,0x0(r30)
	
	li 	r31,0x0
	addis 	r29,r31,0x0030
	ori	r29,r29,0x609A
	sth	r27,0x0(r29)


')

#*****************************************
# ReadEtatCap (int *value)
# lit l,etat du bus de donne D0 a D15
#******************************************
define(`ReadEtatCap',`

	li	r30,0
	addis	r30,r30,0x0FF0	
	ori	r30,r30,0x0000		# r30=0x0FF00000
	
	lwz	r0,0x0(r30)		# lecture a vide

')

##########################################################################################
#
#	FIN GESTION CAPTEUR ULTRA SON 
#
##########################################################################################

# =============================================================
# Macros for RTAI i386
# --------------------
define(`display',`ifelse(
MGC,`LOOP',`rt_printk("X:%5d\n",*$1);')')

define(`enum_linio_switch',0) dnl PP:control enum_linio_(in/out)put switch test.

define(`enum_linio',`dnl
ifdef(`enum_linio_cnt',,`define(`enum_linio_cnt',0)')
ifelse($1,,`undefine(`enum_linio_cnt')',dnl
`ifelse(eval(enum_linio_cnt >= enum_linio_switch),1,dnl
`*`'$1 = shm_seg`'LINUXIO_SEG_NUM_->`'$1;',dnl
`shm_seg`'LINUXIO_SEG_NUM_->`'$1 = *`'$1;')dnl
define(`enum_linio_cnt',incr(enum_linio_cnt))dnl
enum_linio(shift($@))')')

define(`linio_field',`dnl
ifdef(`linio_field_cnt',,`define(`linio_field_cnt',2)')dnl
ifelse($1,,`undefine(`linio_field_cnt')',
``  '$1_type_` '$1`;'
define(`linio_field_cnt',incr(linio_field_cnt))`'linio_field(shift($@))')')

define(`linio_size',`dnl
ifdef(`linio_size_cnt',,`define(`linio_size_cnt',0)')dnl
ifelse($1,,linio_size_cnt`undefine(`linio_size_cnt')',`dnl
define(`linio_size_cnt',eval(linio_size_cnt + $1_type_()_size_))
linio_size(shift($@))')')

define(`LINUXIO_SHM_BASE_',0x10000)dnl PP: define the rtai shared mem base addr.
define(`LINUXIO_SEG_BASE_',LINUXIO_SHM_BASE_)dnl PP: var for segments base addr.
define(`LINUXIO_SEG_SIZE_',0)dnl PP: var for segments size.

define(`linuxIO_',`ifelse(
MGC,`INIT',`ifdef(`linuxIO_ini_once',
`define(`LINUXIO_SEG_NUM_',incr(LINUXIO_SEG_NUM_)) divert(declarations)dnl',
`define(`LINUXIO_SEG_NUM_',0)dnl PP: var for segments number.
define(`linuxIO_ini_once')dnl
syscmd(`rm -rf linio2.h;'dnl
`printf "'`/* linio2.h: auto-generated C users API frontend.\n'dnl
` * Copyright Robosoft 2001, Pierre Pomiers <pierre@robosoft.fr>. */\n\n'dnl
`/* LINUX IO shared memory data segments. */\n\n'dnl
`" | cat > linio2.h')dnl
divert(declarations)dnl
#include "linio2.h"')
static shm_seg`'LINUXIO_SEG_NUM_`'_def *shm_seg`'LINUXIO_SEG_NUM_; 
divert(main_loop)dnl
/* PP: now create the dedicated shared memory segment used for exchanging 
 *     data with the Linux user space. */
shm_seg`'LINUXIO_SEG_NUM_ = rtai_kmalloc(SEG`'LINUXIO_SEG_NUM_`'_BASE, sizeof(shm_seg`'LINUXIO_SEG_NUM_`'_def)); 
memset(shm_seg`'LINUXIO_SEG_NUM_, 0, sizeof(shm_seg`'LINUXIO_SEG_NUM_`'_def));
divert(main_end)dnl
  rtai_kfree(SEG`'LINUXIO_SEG_NUM_`'_BASE); /* PP: free shared memory segment.*/
divert(normal)',
MGC,`LOOP',`ifdef(`linuxIO_loop_once',
`define(`LINUXIO_SEG_NUM_',incr(LINUXIO_SEG_NUM_))',
`define(`LINUXIO_SEG_NUM_',0)dnl PP: var for segments number.
define(`linuxIO_loop_once')')dnl end of linuxIO_loop_once
define(`LINUXIO_SEG_SIZE_',eval(5 + linio_size(shift(shift($@)))))
//  if (!(shm_seg`'LINUXIO_SEG_NUM_`'->sem)) {`'dnl
define(`enum_linio_switch',$1) enum_linio(shift(shift($@)))dnl
    shm_seg`'LINUXIO_SEG_NUM_`'->time = (int)count2nano(rt_get_time()); 
    shm_seg`'LINUXIO_SEG_NUM_`'->sem = 1; /* Set semaphore, unlock segment access. */
//  } 
  syscmd(`mv linio2.h linio2.h~; printf "'dnl
`#define SEG'LINUXIO_SEG_NUM_`_BASE 0x'eval(LINUXIO_SEG_BASE_,16,4)`\n'dnl
`typedef struct {\n'dnl
linio_field(shift(shift($@)))dnl
`  int time;\n'dnl
`  unsigned char sem;\n'dnl
`} shm_seg'LINUXIO_SEG_NUM_`_def;\n\n'dnl
`" > .tmp; cat linio2.h~ .tmp > linio2.h; rm -rf linio2.h~ .tmp')
define(`LINUXIO_SEG_BASE_',eval(LINUXIO_SEG_BASE_ + LINUXIO_SEG_SIZE_))')')



divert`'dnl