Context Navigation

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

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

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

Download in other formats: