Context Navigation

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

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

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

Download in other formats: