Context Navigation

source: soft/giet_vm/giet_common/utils.c @ 361

Last change on this file since 361 was 351, checked in by alain, 11 years ago
Using the giet_lock_t structure in _get_lock() / _release_lock() functions, to have only one lock per cache line.
File size: 40.5 KB

Rev	Line
[258]	1	///////////////////////////////////////////////////////////////////////////////////
	2	// File : utils.c
	3	// Date : 18/10/2013
	4	// Author : alain greiner
	5	// Copyright (c) UPMC-LIP6
	6	///////////////////////////////////////////////////////////////////////////////////
	7	// The utils.c and utils.h files are part of the GIET-VM nano-kernel.
	8	// They define more or less the GIET-VM HAL (Hardware Abstraction Layer),
	9	// and contains various utility functions, that can be used by both the
	10	// boot code and the kernel code.
	11	///////////////////////////////////////////////////////////////////////////////////
	12
	13	#include <giet_config.h>
[324]	14	#include <hard_config.h>
[258]	15	#include <mapping_info.h>
	16	#include <utils.h>
	17	#include <ctx_handler.h>
	18	#include <tty_driver.h>
	19	#include <stdarg.h>
	20
	21	// This global variable is allocated in the boot.c file or in kernel_init.c file
[293]	22	extern static_scheduler_t* _schedulers[NB_PROCS_MAX<<(X_WIDTH+Y_WIDTH)];
[258]	23
	24	///////////////////////////////////////////////////////////////////////////////////
	25	// This function implements a pseudo-random delay.
	26	// The val argument define approximately an exponentially increasing mean delay,
	27	// and should not be larger than 32.
	28	///////////////////////////////////////////////////////////////////////////////////
	29	inline void _random_wait( unsigned int val )
	30	{
	31	unsigned int mask = (1<<(val&0x1F))-1;
	32	unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask;
	33	asm volatile( "move $3, %0 \n"
	34	"loop_nic_completed: \n"
	35	"addi $3, $3, -1 \n"
	36	"bnez $3, loop_nic_completed \n"
	37	"nop \n"
	38	:
	39	: "r" (delay)
	40	: "$3" );
	41	}
	42	///////////////////////////////////////////////////////////////////////////////////
	43	// Copy a source memory buffer content to a dest memory buffer (size bytes)
	44	// Code taken from MutekH.
	45	///////////////////////////////////////////////////////////////////////////////////
[345]	46	inline void* memcpy( void* dest, // dest buffer vbase
	47	const void* source, // source buffer vbase
	48	unsigned int size ) // bytes
[258]	49	{
[295]	50	unsigned int* idst = (unsigned int*)dest;
	51	unsigned int* isrc = (unsigned int*)source;
[258]	52
	53	// word-by-word copy
[295]	54	if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3))
[258]	55	{
	56	while (size > 3)
	57	{
[295]	58	idst++ = isrc++;
[258]	59	size -= 4;
	60	}
	61	}
	62
[295]	63	unsigned char* cdst = (unsigned char*)dest;
	64	unsigned char* csrc = (unsigned char*)source;
[258]	65
	66	/* byte-by-byte copy */
	67	while (size--)
	68	{
	69	cdst++ = csrc++;
	70	}
	71	return dest;
	72	}
	73	//////////////////////////////////////////////////////////////////////////////////
	74	// Fill a byte string with a byte value.
	75	//////////////////////////////////////////////////////////////////////////////////
[345]	76	inline void * memset( void* dest,
	77	int value,
	78	unsigned int count )
[258]	79	{
[295]	80	// word-by-word copy
	81	unsigned int* idst = dest;
	82	unsigned int data = (((unsigned char)value) ) \|
	83	(((unsigned char)value) << 8) \|
	84	(((unsigned char)value) << 16) \|
	85	(((unsigned char)value) << 24) ;
	86
	87	if ( ! ((unsigned int)idst & 3) )
	88	{
	89	while ( count > 3 )
	90	{
	91	*idst++ = data;
	92	count -= 4;
	93	}
	94	}
	95
	96	// byte-by-byte copy
	97	unsigned char* cdst = dest;
[258]	98	while (count--)
	99	{
[295]	100	*cdst++ = (unsigned char)value;
[258]	101	}
[295]	102	return dest;
[258]	103	}
	104
	105	//////////////////////////////////////////////////////////////////////////////////
[295]	106	// This function implements an interactive break for debug.
	107	// Execution continue when typing any character on TTY0.
	108	// The "str" argument is supposed to indicate the break location.
	109	//////////////////////////////////////////////////////////////////////////////////
	110	inline void _break( char* string )
	111	{
	112	char byte;
	113
	114	_printf("\n[GIET DEBUG] break from %s / continue ?\n", string );
	115	_getc( &byte );
	116	}
	117
	118	//////////////////////////////////////////////////////////////////////////////////
[258]	119	// Processor suicide: infinite loop
	120	//////////////////////////////////////////////////////////////////////////////////
[345]	121	__attribute__((noreturn))
[258]	122	inline void _exit()
	123	{
[295]	124	unsigned int procid = _get_procid();
	125	unsigned int lpid = procid % NB_PROCS_MAX;
	126	unsigned int cluster_xy = procid / NB_PROCS_MAX;
	127	unsigned int x = cluster_xy >> Y_WIDTH;
	128	unsigned int y = cluster_xy & ((1<<Y_WIDTH)-1);
	129
	130
	131	_printf("\n[GIET PANIC] processor[%d,%d,%d] suicide...\n", x, y, lpid );
	132
[258]	133	while (1) { asm volatile ("nop"); }
	134	}
	135	///////////////////////////////////////////////////////////////////////////////////
	136	// CP0 and CP2 registers access functions
	137	///////////////////////////////////////////////////////////////////////////////////
	138
	139	///////////////////////////////////////////////////////////////////////////////////
	140	// Returns the value contained in CP0 SCHED register
	141	// (virtual base address of the processor scheduler).
	142	///////////////////////////////////////////////////////////////////////////////////
	143	inline unsigned int _get_sched()
	144	{
	145	unsigned int ret;
[295]	146	asm volatile( "mfc0 %0, $4,2 \n"
	147	: "=r"(ret) );
[258]	148	return ret;
	149	}
	150	///////////////////////////////////////////////////////////////////////////////////
	151	// Returns PTPR register content.
	152	///////////////////////////////////////////////////////////////////////////////////
	153	inline unsigned int _get_mmu_ptpr()
	154	{
	155	unsigned int ret;
[295]	156	asm volatile( "mfc2 %0, $0 \n"
	157	: "=r"(ret) );
[258]	158	return ret;
	159	}
	160	///////////////////////////////////////////////////////////////////////////////////
[295]	161	// Returns MODE register content.
	162	///////////////////////////////////////////////////////////////////////////////////
	163	inline unsigned int _get_mmu_mode()
	164	{
	165	unsigned int ret;
	166	asm volatile( "mfc2 %0, $1 \n"
	167	: "=r"(ret) );
	168	return ret;
	169	}
	170	///////////////////////////////////////////////////////////////////////////////////
[258]	171	// Returns EPC register content.
	172	///////////////////////////////////////////////////////////////////////////////////
	173	inline unsigned int _get_epc()
	174	{
	175	unsigned int ret;
[295]	176	asm volatile( "mfc0 %0, $14 \n"
	177	: "=r"(ret) );
[258]	178	return ret;
	179	}
	180	///////////////////////////////////////////////////////////////////////////////////
	181	// Returns BVAR register content.
	182	///////////////////////////////////////////////////////////////////////////////////
	183	inline unsigned int _get_bvar()
	184	{
	185	unsigned int ret;
[295]	186	asm volatile( "mfc0 %0, $8 \n"
	187	: "=r"(ret));
[258]	188	return ret;
	189	}
	190	///////////////////////////////////////////////////////////////////////////////////
	191	// Returns CR register content.
	192	///////////////////////////////////////////////////////////////////////////////////
	193	inline unsigned int _get_cr()
	194	{
	195	unsigned int ret;
[295]	196	asm volatile( "mfc0 %0, $13 \n"
	197	: "=r"(ret));
[258]	198	return ret;
	199	}
	200	///////////////////////////////////////////////////////////////////////////////////
	201	// Returns SR register content
	202	///////////////////////////////////////////////////////////////////////////////////
	203	inline unsigned int _get_sr()
	204	{
	205	unsigned int ret;
[295]	206	asm volatile( "mfc0 %0, $12 \n"
	207	: "=r"(ret));
[258]	208	return ret;
	209	}
[262]	210	//////////////////////////////////////////////////////////////////////////////
	211	// This function set a new value for the CP0 status register.
	212	//////////////////////////////////////////////////////////////////////////////
	213	inline void _set_sr(unsigned int val)
	214	{
[295]	215	asm volatile( "mtc0 %0, $12 \n"
	216	:
	217	:"r" (val) );
[262]	218	}
[295]	219	//////////////////////////////////////////////////////////////////////////////
[258]	220	// Returns processor index
[295]	221	//////////////////////////////////////////////////////////////////////////////
[258]	222	inline unsigned int _get_procid()
	223	{
	224	unsigned int ret;
[295]	225	asm volatile ( "mfc0 %0, $15, 1 \n"
	226	:"=r" (ret) );
[258]	227	return (ret & 0x3FF);
	228	}
[295]	229	//////////////////////////////////////////////////////////////////////////////
[258]	230	// Returns local time (32 bits value)
	231	// boot_proctime()
[295]	232	//////////////////////////////////////////////////////////////////////////////
[258]	233	inline unsigned int _get_proctime()
	234	{
	235	unsigned int ret;
[295]	236	asm volatile ( "mfc0 %0, $9 \n"
	237	:"=r" (ret) );
[258]	238	return ret;
	239	}
[295]	240	//////////////////////////////////////////////////////////////////////////////
	241	// Returns index of the currently running task from the processor scheduler.
	242	//////////////////////////////////////////////////////////////////////////////
	243	unsigned int _get_current_task_id()
[258]	244	{
	245	static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
	246	return (unsigned int) (psched->current);
	247	}
	248
[295]	249	//////////////////////////////////////////////////////////////////////////////
	250	// Save SR value into save_sr_ptr variable and disable IRQs.
	251	//////////////////////////////////////////////////////////////////////////////
	252	inline void _it_disable( unsigned int * save_sr_ptr)
[258]	253	{
[345]	254	unsigned int sr = 0;
[295]	255	asm volatile( "li $3, 0xFFFFFFFE \n"
	256	"mfc0 %0, $12 \n"
	257	"and $3, $3, %0 \n"
	258	"mtc0 $3, $12 \n"
[345]	259	: "+r"(sr)
[295]	260	:
[345]	261	: "$3" );
[295]	262	*save_sr_ptr = sr;
[258]	263	}
[295]	264	//////////////////////////////////////////////////////////////////////////////
[258]	265	// Enables IRQs
[295]	266	//////////////////////////////////////////////////////////////////////////////
[258]	267	inline void _it_enable()
	268	{
[295]	269	asm volatile( "li $3, 0x00000001 \n"
	270	"mfc0 $4, $12 \n"
	271	"or $3, $3, $4 \n"
	272	"mtc0 $3, $12 \n"
[301]	273	::: "$3", "$4", "memory" );
[258]	274	}
	275
	276	//////////////////////////////////////////////////////////////////////////////
[295]	277	// Restores previous SR value.
	278	//////////////////////////////////////////////////////////////////////////////
	279	inline void _it_restore( unsigned int * save_sr_ptr )
	280	{
	281	unsigned int sr = *save_sr_ptr;
	282	asm volatile( "mtc0 %0, $12 \n"
	283	:
[301]	284	: "r"(sr)
	285	: "memory" );
[295]	286	}
	287
	288	//////////////////////////////////////////////////////////////////////////////
[258]	289	// This function set a new value for the MMU PTPR register.
	290	//////////////////////////////////////////////////////////////////////////////
	291	inline void _set_mmu_ptpr(unsigned int val)
	292	{
[295]	293	asm volatile ( "mtc2 %0, $0 \n"
	294	:
[345]	295	:"r" (val)
	296	:"memory" );
[258]	297	}
	298	//////////////////////////////////////////////////////////////////////////////
	299	// This function set a new value for the MMU MODE register.
	300	//////////////////////////////////////////////////////////////////////////////
	301	inline void _set_mmu_mode(unsigned int val)
	302	{
[295]	303	asm volatile ( "mtc2 %0, $1 \n"
	304	:
[345]	305	:"r" (val)
	306	:"memory" );
[258]	307	}
	308	//////////////////////////////////////////////////////////////////////////////
	309	// This function set a new value in CP0 SCHED register.
	310	// (virtual base address of the processor scheduler).
	311	//////////////////////////////////////////////////////////////////////////////
	312	inline void _set_sched(unsigned int val)
	313	{
[295]	314	asm volatile ( "mtc0 %0, $4, 2 \n"
	315	:
	316	:"r" (val) );
[258]	317	}
	318
	319	////////////////////////////////////////////////////////////////////////////
	320	// Physical addressing related functions
	321	////////////////////////////////////////////////////////////////////////////
	322
	323	////////////////////////////////////////////////////////////////////////////
	324	// This function makes a physical read access to a 32 bits word in memory,
	325	// after a temporary DTLB de-activation and paddr extension.
	326	////////////////////////////////////////////////////////////////////////////
	327	inline unsigned int _physical_read( unsigned long long paddr )
	328	{
	329	unsigned int value;
	330	unsigned int lsb = (unsigned int) paddr;
	331	unsigned int msb = (unsigned int) (paddr >> 32);
[301]	332	unsigned int sr;
[258]	333
[301]	334	_it_disable(&sr);
[258]	335
[344]	336	asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */
	337	"andi $3, $2, 0xb \n"
	338	"mtc2 $3, $1 \n" /* DTLB off */
[258]	339
[344]	340	"mtc2 %2, $24 \n" /* PADDR_EXT <= msb */
	341	"lw %0, 0(%1) \n" /* value <= paddr /
	342	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	343
	344	"mtc2 $2, $1 \n" /* restore MMU_MODE */
	345	: "=r" (value)
	346	: "r" (lsb), "r" (msb)
	347	: "$2", "$3" );
	348
[301]	349	_it_restore(&sr);
[258]	350	return value;
	351	}
	352	////////////////////////////////////////////////////////////////////////////
	353	// This function makes a physical write access to a 32 bits word in memory,
	354	// after a temporary DTLB de-activation and paddr extension.
	355	////////////////////////////////////////////////////////////////////////////
	356	inline void _physical_write( unsigned long long paddr,
[295]	357	unsigned int value )
[258]	358	{
	359	unsigned int lsb = (unsigned int)paddr;
	360	unsigned int msb = (unsigned int)(paddr >> 32);
[301]	361	unsigned int sr;
[258]	362
[301]	363	_it_disable(&sr);
[258]	364
[344]	365	asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */
	366	"andi $3, $2, 0xb \n"
	367	"mtc2 $3, $1 \n" /* DTLB off */
[258]	368
[344]	369	"mtc2 %2, $24 \n" /* PADDR_EXT <= msb */
	370	"sw %0, 0(%1) \n" /* paddr <= value /
	371	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	372
	373	"mtc2 $2, $1 \n" /* restore MMU_MODE */
[345]	374	"sync \n"
[344]	375	:
	376	: "r" (value), "r" (lsb), "r" (msb)
	377	: "$2", "$3" );
	378
[301]	379	_it_restore(&sr);
[258]	380	}
	381
	382	///////////////////////////////////////////////////////////////////////////////////
[344]	383	// This function makes a memcpy from a source buffer to a destination buffer
	384	// using physical addresses, after a temporary DTLB de-activation.
	385	// source and destination buffers must be aligned, and size must be
	386	// multiple of 4 bytes.
	387	///////////////////////////////////////////////////////////////////////////////////
	388	inline void _physical_memcpy( unsigned long long dst_paddr, // dest buffer paddr
	389	unsigned long long src_paddr, // source buffer paddr
	390	unsigned int size ) // bytes
	391	{
	392	// check alignment constraints
	393	if ( (dst_paddr & 3) \|\| (src_paddr & 3) \|\| (size & 3) )
	394	{
	395	_printf("\n[GIET ERROR] in _physical_memcpy() : buffer unaligned\n");
	396	_exit();
	397	}
	398
	399	unsigned int src_lsb = (unsigned int)src_paddr;
	400	unsigned int src_msb = (unsigned int)(src_paddr >> 32);
	401	unsigned int dst_lsb = (unsigned int)dst_paddr;
	402	unsigned int dst_msb = (unsigned int)(dst_paddr >> 32);
	403	unsigned int iter = size>>2;
	404	unsigned int data;
	405	unsigned int sr;
	406
	407	_it_disable(&sr);
	408
	409	asm volatile( "mfc2 $2, $1 \n" /* $2 <= current MMU_MODE */
	410	"andi $3, $2, 0xb \n" /* $3 <= new MMU_MODE */
	411	"mtc2 $3, $1 \n" /* DTLB off */
	412
	413	"move $4, %5 \n" /* $4 < iter */
	414	"move $5, %1 \n" /* $5 < src_lsb */
	415	"move $6, %3 \n" /* $6 < src_lsb */
	416
	417	"ph_memcpy_loop: \n"
	418	"mtc2 %2, $24 \n" /* PADDR_EXT <= src_msb */
	419	"lw %0, 0($5) \n" /* data <= src_paddr /
	420	"mtc2 %4, $24 \n" /* PADDR_EXT <= dst_msb */
	421	"sw %0, 0($6) \n" /* dst_paddr <= data /
	422
	423	"addi $4, $4, -1 \n" /* iter = iter - 1 */
	424	"addi $5, $5, 4 \n" /* src_lsb += 4 */
	425	"addi $6, $6, 4 \n" /* dst_lsb += 4 */
	426	"bne $4, $0, ph_memcpy_loop \n"
	427	"nop \n"
	428
	429	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	430	"mtc2 $2, $1 \n" /* restore MMU_MODE */
	431	: "=r" (data)
	432	: "r" (src_lsb), "r" (src_msb), "r" (dst_lsb), "r"(dst_msb), "r"(iter)
	433	: "$2", "$3", "$4", "$5", "$6" );
	434
	435	_it_restore(&sr);
	436	}
	437
	438	///////////////////////////////////////////////////////////////////////////////////
[314]	439	// This function is used by several drivers (_xxx_set_register() function)
[295]	440	// If the MMU is not activated, the virtual address is extended using
	441	// X_IO and Y_IO to reach the cluster_io.
	442	///////////////////////////////////////////////////////////////////////////////////
	443	inline void _io_extended_write( unsigned int* vaddr,
	444	unsigned int value )
	445	{
	446	unsigned long long paddr;
	447
	448	if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address
	449	{
	450	*vaddr = value;
	451	}
	452	else // use paddr extension for IO
	453	{
	454	paddr = (unsigned long long)(unsigned int)vaddr +
	455	(((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32);
	456	_physical_write( paddr, value );
	457	}
	458	asm volatile("sync" ::: "memory");
	459	}
	460
	461	///////////////////////////////////////////////////////////////////////////////////
	462	// This function is used by all drivers (_xxx_get_register() function)
	463	// If the MMU is not activated, the virtual address is extended using
	464	// X_IO and Y_IO to reach the cluster_io.
	465	///////////////////////////////////////////////////////////////////////////////////
	466	inline unsigned int _io_extended_read( unsigned int* vaddr )
	467	{
	468	unsigned long long paddr;
	469
	470	if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address
	471	{
	472	return (volatile unsigned int)vaddr;
	473	}
	474	else // use paddr extension for IO
	475	{
	476	paddr = (unsigned long long)(unsigned int)vaddr +
	477	(((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32);
	478	return _physical_read( paddr );
	479	}
	480	}
	481
	482	///////////////////////////////////////////////////////////////////////////////////
[258]	483	// Locks access functions
	484	///////////////////////////////////////////////////////////////////////////////////
	485
	486	///////////////////////////////////////////////////////////////////////////////////
[314]	487	// Takes a lock with a blocking ll/sc atomic access.
	488	// When the cache coherence is granted by the hardware,
	489	// the first read is a standard (cacheable) lw, as the local copy
	490	// can be polled when the lock is already taken by another task, reducing
	491	// trafic on the interconnect. When the lock is released by the owner task,
	492	// the local copy is updated or invalidated by the coherence protocol.
	493	// If there is no hardware cache coherence a random delay is introduced
	494	// betwween two successive retry.
[258]	495	///////////////////////////////////////////////////////////////////////////////////
[351]	496	inline void _get_lock(giet_lock_t* lock)
[258]	497	{
[351]	498	register unsigned int* plock = &(lock->value);
	499
[314]	500	#if NO_HARD_CC
	501
[351]	502	register unsigned int delay = (_get_proctime() ^ _get_procid() << 4) & 0xFF;
	503	if (delay == 0) delay = 0x80;
[258]	504
	505	asm volatile (
	506	"_lock_llsc: \n"
[351]	507	" ll $2, 0(%0) \n" /* $2 <= lock current value */
	508	" bnez $2, _lock_delay \n" /* delay if lock already taken */
	509	" li $3, 1 \n" /* $3 <= argument for sc */
	510	" sc $3, 0(%0) \n" /* try to set lock */
	511	" bnez $3, _lock_ok \n" /* exit if atomic */
	512	" _lock_delay: \n"
	513	" move $4, %1 \n" /* $4 <= delay */
	514	" _lock_loop: \n"
	515	" addi $4, $4, -1 \n" /* $4 <= $4 - 1 */
	516	" bnez $4, _lock_loop \n" /* test end delay */
	517	" nop \n"
	518	" j _lock_llsc \n" /* retry */
	519	" nop \n"
	520	" _lock_ok: \n"
[258]	521	:
	522	:"r"(plock), "r"(delay)
[345]	523	:"$2", "$3", "$4", "memory");
[314]	524	#else
	525
	526	asm volatile (
	527	"_lock_llsc: \n"
[351]	528	" lw $2, 0(%0) \n" /* $2 <= lock current value */
	529	" bnez $2, _lock_llsc \n" /* retry if lock already taken */
[314]	530	" nop \n"
[351]	531	" ll $2, 0(%0) \n" /* ll_buffer <= lock current value */
	532	" bnez $2, _lock_llsc \n" /* retry if lock already taken */
	533	" li $3, 1 \n" /* $3 <= argument for sc */
	534	" sc $3, 0(%0) \n" /* try to set lock */
	535	" beqz $3, _lock_llsc \n" /* retry if sc failure */
[314]	536	" nop \n"
	537	:
	538	:"r"(plock)
[345]	539	:"$2", "$3", "memory");
[314]	540	#endif
	541
[258]	542	}
[295]	543
[258]	544	///////////////////////////////////////////////////////////////////////////////////
	545	// Release a previouly taken lock.
	546	///////////////////////////////////////////////////////////////////////////////////
[351]	547	inline void _release_lock(giet_lock_t* lock)
[258]	548	{
[295]	549	asm volatile ( "sync\n" ::: "memory" );
	550	// sync is necessary because of the TSAR consistency model
[351]	551	lock->value = 0;
[258]	552	}
	553
	554	///////////////////////////////////////////////////////////////////////////////////
[295]	555	// Access functions to system terminal TTY0
[258]	556	///////////////////////////////////////////////////////////////////////////////////
[295]	557
	558	///////////////////////////////////////////////////////////////////////////////////
	559	// Display "string" argument on TTY0.
	560	// It uses the low level access functions from TTY driver, using a busy waiting
	561	// policy if TTY buffer is full.
	562	// The exclusive access lock should be taken by the caller.
	563	///////////////////////////////////////////////////////////////////////////////////
	564	void _puts( char* string )
[258]	565	{
[295]	566	unsigned int n = 0;
	567
	568	while ( string[n] > 0 )
[258]	569	{
[295]	570	// test status register
	571	while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );
	572
	573	// write one byte
[315]	574	if ( string[n] == '\n') {
	575	_tty_set_register( 0, TTY_WRITE, (unsigned int)'\r' );
	576	}
[295]	577	_tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
	578	n++;
[258]	579	}
	580	}
	581
	582	///////////////////////////////////////////////////////////////////////////////////
[295]	583	// Display a 32 bits unsigned int as an hexadecimal string on TTY0.
[258]	584	///////////////////////////////////////////////////////////////////////////////////
[295]	585	void _putx( unsigned int val )
[258]	586	{
	587	static const char HexaTab[] = "0123456789ABCDEF";
	588	char buf[11];
	589	unsigned int c;
	590
	591	buf[0] = '0';
	592	buf[1] = 'x';
	593	buf[10] = 0;
	594
	595	for (c = 0; c < 8; c++)
	596	{
	597	buf[9 - c] = HexaTab[val & 0xF];
	598	val = val >> 4;
	599	}
[295]	600	_puts( buf );
[258]	601	}
	602
	603	///////////////////////////////////////////////////////////////////////////////////
[295]	604	// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
[258]	605	///////////////////////////////////////////////////////////////////////////////////
[295]	606	void _putl( unsigned long long val )
[258]	607	{
	608	static const char HexaTab[] = "0123456789ABCDEF";
	609	char buf[19];
	610	unsigned int c;
	611
	612	buf[0] = '0';
	613	buf[1] = 'x';
	614	buf[18] = 0;
	615
	616	for (c = 0; c < 16; c++)
	617	{
	618	buf[17 - c] = HexaTab[(unsigned int)val & 0xF];
	619	val = val >> 4;
	620	}
[295]	621	_puts( buf );
[258]	622	}
	623
	624	///////////////////////////////////////////////////////////////////////////////////
[295]	625	// Display a 32 bits unsigned int as a decimal string on TTY0.
[258]	626	///////////////////////////////////////////////////////////////////////////////////
[295]	627	void _putd( unsigned int val )
[258]	628	{
	629	static const char DecTab[] = "0123456789";
	630	char buf[11];
	631	unsigned int i;
[351]	632	unsigned int first = 0;
[258]	633
	634	buf[10] = 0;
	635
[351]	636	for (i = 0; i < 10; i++)
	637	{
	638	if ((val != 0) \|\| (i == 0))
	639	{
[258]	640	buf[9 - i] = DecTab[val % 10];
	641	first = 9 - i;
	642	}
[351]	643	else
	644	{
[258]	645	break;
	646	}
	647	val /= 10;
	648	}
[295]	649	_puts( &buf[first] );
[258]	650	}
	651
	652	///////////////////////////////////////////////////////////////////////////////////
[295]	653	// Display a format on TTY0.
	654	// To provide an atomic display, this function takes the lock protecting
	655	// exclusive access to TTY0, entering a critical section until the lock
	656	// is released.
	657	// Only a limited number of formats are supported:
	658	// - %d : 32 bits signed decimal
	659	// - %u : 32 bits unsigned decimal
	660	// - %x : 32 bits unsigned hexa
	661	// - %l : 64 bits unsigned hexa
	662	// - %c : char
	663	// - %s : string
	664	///////////////////////////////////////////////////////////////////////////////////
	665	void _printf( char * format, ... )
	666	{
	667	va_list ap;
	668	va_start(ap, format);
[332]	669	unsigned int save_sr; // to save SR value in critical section
[295]	670
	671	// get TTY0 lock
	672	_tty_get_lock( 0, &save_sr );
	673
	674	printf_text:
	675
	676	while (*format)
	677	{
	678	unsigned int i;
	679	for (i = 0 ; format[i] && (format[i] != '%') ; i++);
	680	if (i)
	681	{
	682	if ( _tty_write( format, i, 0 ) != i ) goto return_error;
	683	format += i;
	684	}
	685	if (*format == '%')
	686	{
	687	format++;
	688	goto printf_arguments;
	689	}
	690	}
	691
	692	// release TTY0 lock
	693	_tty_release_lock( 0, &save_sr );
	694
	695	va_end(ap);
	696	return;
	697
	698	printf_arguments:
	699
	700	{
	701	char buf[20];
	702	char * pbuf;
	703	unsigned int len = 0;
	704	static const char HexaTab[] = "0123456789ABCDEF";
	705	unsigned int i;
	706
	707	switch (*format++)
	708	{
	709	case ('c'): /* char conversion */
	710	{
	711	int val = va_arg( ap, int );
	712	len = 1;
	713	buf[0] = val;
	714	pbuf = &buf[0];
	715	break;
	716	}
	717	case ('d'): /* 32 bits decimal signed */
	718	{
	719	int val = va_arg( ap, int );
	720	if (val < 0)
	721	{
	722	val = -val;
	723	if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
	724	}
	725	for(i = 0; i < 10; i++)
	726	{
	727	buf[9 - i] = HexaTab[val % 10];
	728	if (!(val /= 10)) break;
	729	}
	730	len = i + 1;
	731	pbuf = &buf[9 - i];
	732	break;
	733	}
	734	case ('u'): /* 32 bits decimal unsigned */
	735	{
	736	unsigned int val = va_arg( ap, unsigned int );
	737	for(i = 0; i < 10; i++)
	738	{
	739	buf[9 - i] = HexaTab[val % 10];
	740	if (!(val /= 10)) break;
	741	}
	742	len = i + 1;
	743	pbuf = &buf[9 - i];
	744	break;
	745	}
	746	case ('x'): /* 32 bits hexadecimal unsigned */
	747	{
	748	unsigned int val = va_arg( ap, unsigned int );
	749	if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
	750	for(i = 0; i < 8; i++)
	751	{
	752	buf[7 - i] = HexaTab[val % 16];
	753	if (!(val /= 16)) break;
	754	}
	755	len = i + 1;
	756	pbuf = &buf[7 - i];
	757	break;
	758	}
	759	case ('l'): /* 64 bits hexadecimal unsigned */
	760	{
	761	unsigned long long val = va_arg( ap, unsigned long long );
	762	if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
	763	for(i = 0; i < 16; i++)
	764	{
	765	buf[15 - i] = HexaTab[val % 16];
	766	if (!(val /= 16)) break;
	767	}
	768	len = i + 1;
	769	pbuf = &buf[15 - i];
	770	break;
	771	}
	772	case ('s'): /* string */
	773	{
	774	char* str = va_arg( ap, char* );
	775	while (str[len])
	776	{
	777	len++;
	778	}
	779	pbuf = str;
	780	break;
	781	}
	782	default:
	783	goto return_error;
	784	}
	785
	786	if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
	787
	788	goto printf_text;
	789	}
	790
	791	return_error:
	792
	793	{
	794	unsigned int procid = _get_procid();
	795	unsigned int lpid = procid % NB_PROCS_MAX;
	796	unsigned int cluster_xy = procid / NB_PROCS_MAX;
	797	unsigned int x = cluster_xy >> Y_WIDTH;
	798	unsigned int y = cluster_xy & ((1<<Y_WIDTH)-1);
	799
	800	_puts("\n\n[GIET ERROR] in _printf() for processor[");
	801	_putd( x );
	802	_puts(",");
	803	_putd( y );
	804	_puts(",");
	805	_putd( lpid );
	806	_puts("]\n");
	807
	808	// release TTY0 lock
	809	_tty_release_lock( 0, &save_sr );
	810
	811	_exit();
	812	}
	813	}
	814
	815	///////////////////////////////////////////////////////////////////////////////////
	816	// Get a character from TTY0.
	817	///////////////////////////////////////////////////////////////////////////////////
	818	void _getc( char* byte )
	819	{
	820	// test status register
	821	while ( _tty_get_register( 0, TTY_STATUS ) == 0 );
	822
	823	// read one byte
	824	*byte = (char)_tty_get_register( 0, TTY_READ );
	825	}
	826
	827	///////////////////////////////////////////////////////////////////////////////////
[258]	828	// Compare two strings s1 & s2 (no more than n characters)
	829	///////////////////////////////////////////////////////////////////////////////////
	830	unsigned int _strncmp( const char * s1,
	831	const char * s2,
	832	unsigned int n )
	833	{
	834	unsigned int i;
	835	for (i = 0; i < n; i++)
	836	{
	837	if (s1[i] != s2[i]) return 1;
	838	if (s1[i] == 0) break;
	839	}
	840	return 0;
	841	}
	842
	843	///////////////////////////////////////////////////////////////////////////////////
	844	// Copy source string to dest string
	845	///////////////////////////////////////////////////////////////////////////////////
	846	char* _strcpy( char* dest, char* source )
	847	{
	848	if (!dest \|\| !source) return dest;
	849
	850	while (*source)
	851	(dest++) = (source++);
	852
	853	return dest;
	854	}
	855
	856	///////////////////////////////////////////////////////////////////////////////////
	857	// Invalidate all data cache lines corresponding to a memory
	858	// buffer (identified by an address and a size).
	859	// TODO This should be replaced by a write to the CP2 MMU_DCACHE_INVAL
	860	// register, to be more processor independant.
	861	///////////////////////////////////////////////////////////////////////////////////
[295]	862	void _dcache_buf_invalidate( void * buffer,
[258]	863	unsigned int size)
	864	{
	865	unsigned int i;
	866	unsigned int tmp;
	867	unsigned int line_size;
	868
	869	// compute data cache line size based on config register (bits 12:10)
	870	asm volatile(
	871	"mfc0 %0, $16, 1"
	872	: "=r" (tmp) );
	873	tmp = ((tmp >> 10) & 0x7);
	874	line_size = 2 << tmp;
	875
	876	// iterate on cache lines
	877	for (i = 0; i < size; i += line_size)
	878	{
	879	asm volatile(
	880	" cache %0, %1"
	881	: :"i" (0x11), "R" (((unsigned char ) buffer + i)) );
	882	}
	883	}
	884
	885	////////////////////////////////////////////////////////////////////////////////////
	886	// This function returns the content of a context slot
	887	// for any task identified by the ltid argument (local task index),
	888	// and the gpid argument (global processor index)
	889	////////////////////////////////////////////////////////////////////////////////////
	890	unsigned int _get_task_slot( unsigned int gpid,
	891	unsigned int ltid,
	892	unsigned int slot )
	893	{
	894	static_scheduler_t* psched = (static_scheduler_t*)_schedulers[gpid];
	895	return psched->context[ltid][slot];
	896	}
	897
	898	////////////////////////////////////////////////////////////////////////////////////
	899	// This function updates the content of a context slot
	900	// for any task identified by the ltid argument (local task index),
	901	// and the gpid argument (global processor index)
	902	////////////////////////////////////////////////////////////////////////////////////
	903	void _set_task_slot( unsigned int gpid,
	904	unsigned int ltid,
	905	unsigned int slot,
	906	unsigned int value )
	907	{
	908	static_scheduler_t* psched = (static_scheduler_t*)_schedulers[gpid];
	909	psched->context[ltid][slot] = value;
	910	}
	911
	912	////////////////////////////////////////////////////////////////////////////////////
	913	// This function returns the content of a context slot
	914	// for the running task (defined by the scheduler current field).
	915	////////////////////////////////////////////////////////////////////////////////////
	916	unsigned int _get_context_slot( unsigned int slot )
	917	{
	918	static_scheduler_t* psched = (static_scheduler_t*)_get_sched();
	919	unsigned int task_id = psched->current;
	920	return psched->context[task_id][slot];
	921	}
	922
	923	////////////////////////////////////////////////////////////////////////////////////
	924	// This function updates the content of a context slot for the running task.
	925	////////////////////////////////////////////////////////////////////////////////////
	926	void _set_context_slot( unsigned int slot,
	927	unsigned int value )
	928	{
	929	static_scheduler_t* psched = (static_scheduler_t*)_get_sched();
	930	unsigned int task_id = psched->current;
	931	psched->context[task_id][slot] = value;
	932	}
	933
	934	///////////////////////////////////////////////////////////////////////////////////
[324]	935	// This function returns the information associated to a heap : size and vaddr.
[258]	936	// It uses the global task index (CTX_GTID_ID, unique for each giet task) and the
	937	// vspace index (CTX_VSID_ID) defined in the task context.
	938	///////////////////////////////////////////////////////////////////////////////////
	939	unsigned int _heap_info( unsigned int* vaddr,
	940	unsigned int* size )
	941	{
[324]	942	mapping_header_t * header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
[258]	943	mapping_task_t * tasks = _get_task_base(header);
	944	mapping_vobj_t * vobjs = _get_vobj_base(header);
	945
	946	unsigned int taskid = _get_context_slot(CTX_GTID_ID);
	947
[324]	948	int vobj_id = tasks[taskid].heap_vobj_id;
	949	if (vobj_id != -1)
[258]	950	{
[324]	951	*vaddr = vobjs[vobj_id].vaddr;
	952	*size = vobjs[vobj_id].length;
[258]	953	}
	954	else
	955	{
	956	*vaddr = 0;
	957	*size = 0;
	958	}
[324]	959	return 0;
[258]	960	}
	961
	962	/////////////////////////////////////////////////////////////////////////////
	963	// Access functions to mapping_info data structure
	964	/////////////////////////////////////////////////////////////////////////////
	965	inline mapping_cluster_t * _get_cluster_base(mapping_header_t * header)
	966	{
	967	return (mapping_cluster_t ) ((char ) header +
	968	MAPPING_HEADER_SIZE);
	969	}
	970	/////////////////////////////////////////////////////////////////////////////
	971	inline mapping_pseg_t * _get_pseg_base(mapping_header_t * header)
	972	{
	973	return (mapping_pseg_t ) ((char ) header +
	974	MAPPING_HEADER_SIZE +
[263]	975	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
[258]	976	}
	977	/////////////////////////////////////////////////////////////////////////////
	978	inline mapping_vspace_t * _get_vspace_base(mapping_header_t * header)
	979	{
	980	return (mapping_vspace_t ) ((char ) header +
	981	MAPPING_HEADER_SIZE +
[263]	982	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	983	MAPPING_PSEG_SIZE * header->psegs);
	984	}
	985	/////////////////////////////////////////////////////////////////////////////
	986	inline mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
	987	{
	988	return (mapping_vseg_t ) ((char ) header +
	989	MAPPING_HEADER_SIZE +
[263]	990	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	991	MAPPING_PSEG_SIZE * header->psegs +
	992	MAPPING_VSPACE_SIZE * header->vspaces);
	993	}
	994	/////////////////////////////////////////////////////////////////////////////
	995	inline mapping_vobj_t * _get_vobj_base(mapping_header_t * header)
	996	{
	997	return (mapping_vobj_t ) ((char ) header +
	998	MAPPING_HEADER_SIZE +
[263]	999	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	1000	MAPPING_PSEG_SIZE * header->psegs +
	1001	MAPPING_VSPACE_SIZE * header->vspaces +
	1002	MAPPING_VSEG_SIZE * header->vsegs );
	1003	}
	1004	/////////////////////////////////////////////////////////////////////////////
	1005	inline mapping_task_t * _get_task_base(mapping_header_t * header)
	1006	{
	1007	return (mapping_task_t ) ((char ) header +
	1008	MAPPING_HEADER_SIZE +
[263]	1009	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	1010	MAPPING_PSEG_SIZE * header->psegs +
	1011	MAPPING_VSPACE_SIZE * header->vspaces +
	1012	MAPPING_VOBJ_SIZE * header->vobjs +
	1013	MAPPING_VSEG_SIZE * header->vsegs);
	1014	}
	1015	/////////////////////////////////////////////////////////////////////////////
	1016	inline mapping_proc_t _get_proc_base(mapping_header_t header)
	1017	{
	1018	return (mapping_proc_t ) ((char ) header +
	1019	MAPPING_HEADER_SIZE +
[263]	1020	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	1021	MAPPING_PSEG_SIZE * header->psegs +
	1022	MAPPING_VSPACE_SIZE * header->vspaces +
	1023	MAPPING_VSEG_SIZE * header->vsegs +
	1024	MAPPING_VOBJ_SIZE * header->vobjs +
	1025	MAPPING_TASK_SIZE * header->tasks);
	1026	}
	1027	/////////////////////////////////////////////////////////////////////////////
	1028	inline mapping_irq_t _get_irq_base(mapping_header_t header)
	1029	{
	1030	return (mapping_irq_t ) ((char ) header +
	1031	MAPPING_HEADER_SIZE +
[263]	1032	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	1033	MAPPING_PSEG_SIZE * header->psegs +
	1034	MAPPING_VSPACE_SIZE * header->vspaces +
	1035	MAPPING_VSEG_SIZE * header->vsegs +
	1036	MAPPING_VOBJ_SIZE * header->vobjs +
	1037	MAPPING_TASK_SIZE * header->tasks +
	1038	MAPPING_PROC_SIZE * header->procs);
	1039	}
	1040	/////////////////////////////////////////////////////////////////////////////
	1041	inline mapping_coproc_t _get_coproc_base(mapping_header_t header)
	1042	{
	1043	return (mapping_coproc_t ) ((char ) header +
	1044	MAPPING_HEADER_SIZE +
[263]	1045	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	1046	MAPPING_PSEG_SIZE * header->psegs +
	1047	MAPPING_VSPACE_SIZE * header->vspaces +
	1048	MAPPING_VOBJ_SIZE * header->vobjs +
	1049	MAPPING_VSEG_SIZE * header->vsegs +
	1050	MAPPING_TASK_SIZE * header->tasks +
	1051	MAPPING_PROC_SIZE * header->procs +
	1052	MAPPING_IRQ_SIZE * header->irqs);
	1053	}
	1054	///////////////////////////////////////////////////////////////////////////////////
	1055	inline mapping_cp_port_t _get_cp_port_base(mapping_header_t header)
	1056	{
	1057	return (mapping_cp_port_t ) ((char ) header +
	1058	MAPPING_HEADER_SIZE +
[263]	1059	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	1060	MAPPING_PSEG_SIZE * header->psegs +
	1061	MAPPING_VSPACE_SIZE * header->vspaces +
	1062	MAPPING_VOBJ_SIZE * header->vobjs +
	1063	MAPPING_VSEG_SIZE * header->vsegs +
	1064	MAPPING_TASK_SIZE * header->tasks +
	1065	MAPPING_PROC_SIZE * header->procs +
	1066	MAPPING_IRQ_SIZE * header->irqs +
	1067	MAPPING_COPROC_SIZE * header->coprocs);
	1068	}
	1069	///////////////////////////////////////////////////////////////////////////////////
	1070	inline mapping_periph_t _get_periph_base(mapping_header_t header)
	1071	{
	1072	return (mapping_periph_t ) ((char ) header +
	1073	MAPPING_HEADER_SIZE +
[263]	1074	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	1075	MAPPING_PSEG_SIZE * header->psegs +
	1076	MAPPING_VSPACE_SIZE * header->vspaces +
	1077	MAPPING_VOBJ_SIZE * header->vobjs +
	1078	MAPPING_VSEG_SIZE * header->vsegs +
	1079	MAPPING_TASK_SIZE * header->tasks +
	1080	MAPPING_PROC_SIZE * header->procs +
	1081	MAPPING_IRQ_SIZE * header->irqs +
	1082	MAPPING_COPROC_SIZE * header->coprocs +
	1083	MAPPING_CP_PORT_SIZE * header->cp_ports);
	1084	}
	1085
	1086	// Local Variables:
	1087	// tab-width: 4
	1088	// c-basic-offset: 4
	1089	// c-file-offsets:((innamespace . 0)(inline-open . 0))
	1090	// indent-tabs-mode: nil
	1091	// End:
	1092	// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
	1093

Note: See TracBrowser for help on using the repository browser.

Download in other formats: