Context Navigation

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

Last change on this file since 433 was 433, checked in by cfuguet, 10 years ago

boot: two modifications in the GietVM bootloader

Verifying that all vsegs sharing a big page have the same flags.

Bugfixes in the _physical_memset() function

File size: 41.1 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
[433]	350	if ( (paddr & 3) \|\| (size & 7) )
[430]	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
[433]	362	asm volatile( "mfc2 $8, $1 \n" /* $8 <= current MMU_MODE */
	363	"andi $9, $8, 0xb \n" /* $9 <= new MMU_MODE */
	364	"mtc2 $9, $1 \n" /* DTLB off */
	365	"mtc2 %3, $24 \n" /* PADDR_EXT <= msb */
[430]	366
[433]	367	"1: \n" /* set 8 bytes per iter */
	368	"sw %2, 0(%0) \n" /* src_paddr = data /
	369	"sw %2, 4(%0) \n" /* (src_paddr+4) = data /
	370	"addi %1, %1, -8 \n" /* size -= 8 */
	371	"addi %0, %0, 8 \n" /* src_paddr += 8 */
	372	"bnez %1, 1b \n" /* loop while size != 0 */
[430]	373
[433]	374	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	375	"mtc2 $8, $1 \n" /* restore MMU_MODE */
	376	: "+r"(lsb), "+r"(size)
	377	: "r"(data), "r" (msb)
	378	: "$8", "$9", "memory" );
[430]	379
	380	_it_restore(&sr);
[433]	381	} // _physical_memset()
[430]	382
[344]	383	///////////////////////////////////////////////////////////////////////////////////
[378]	384	void _io_extended_write( unsigned int* vaddr,
[399]	385	unsigned int value )
[295]	386	{
	387	unsigned long long paddr;
	388
	389	if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address
	390	{
	391	*vaddr = value;
	392	}
	393	else // use paddr extension for IO
	394	{
	395	paddr = (unsigned long long)(unsigned int)vaddr +
	396	(((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32);
	397	_physical_write( paddr, value );
	398	}
	399	asm volatile("sync" ::: "memory");
	400	}
	401
	402	///////////////////////////////////////////////////////////////////////////////////
[378]	403	unsigned int _io_extended_read( unsigned int* vaddr )
[295]	404	{
	405	unsigned long long paddr;
	406
	407	if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address
	408	{
	409	return (volatile unsigned int)vaddr;
	410	}
	411	else // use paddr extension for IO
	412	{
	413	paddr = (unsigned long long)(unsigned int)vaddr +
	414	(((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32);
	415	return _physical_read( paddr );
	416	}
	417	}
	418
	419	///////////////////////////////////////////////////////////////////////////////////
[258]	420	// Locks access functions
	421	///////////////////////////////////////////////////////////////////////////////////
	422
	423	///////////////////////////////////////////////////////////////////////////////////
[378]	424	void _get_lock(giet_lock_t* lock)
[258]	425	{
[351]	426	register unsigned int* plock = &(lock->value);
	427
[314]	428	#if NO_HARD_CC
	429
[351]	430	register unsigned int delay = (_get_proctime() ^ _get_procid() << 4) & 0xFF;
	431	if (delay == 0) delay = 0x80;
[258]	432
	433	asm volatile (
	434	"_lock_llsc: \n"
[351]	435	" ll $2, 0(%0) \n" /* $2 <= lock current value */
	436	" bnez $2, _lock_delay \n" /* delay if lock already taken */
	437	" li $3, 1 \n" /* $3 <= argument for sc */
	438	" sc $3, 0(%0) \n" /* try to set lock */
	439	" bnez $3, _lock_ok \n" /* exit if atomic */
	440	" _lock_delay: \n"
	441	" move $4, %1 \n" /* $4 <= delay */
	442	" _lock_loop: \n"
	443	" addi $4, $4, -1 \n" /* $4 <= $4 - 1 */
	444	" bnez $4, _lock_loop \n" /* test end delay */
	445	" nop \n"
	446	" j _lock_llsc \n" /* retry */
	447	" nop \n"
	448	" _lock_ok: \n"
[258]	449	:
	450	:"r"(plock), "r"(delay)
[345]	451	:"$2", "$3", "$4", "memory");
[314]	452	#else
	453
	454	asm volatile (
	455	"_lock_llsc: \n"
[351]	456	" lw $2, 0(%0) \n" /* $2 <= lock current value */
	457	" bnez $2, _lock_llsc \n" /* retry if lock already taken */
[314]	458	" nop \n"
[351]	459	" ll $2, 0(%0) \n" /* ll_buffer <= lock current value */
	460	" bnez $2, _lock_llsc \n" /* retry if lock already taken */
	461	" li $3, 1 \n" /* $3 <= argument for sc */
	462	" sc $3, 0(%0) \n" /* try to set lock */
	463	" beqz $3, _lock_llsc \n" /* retry if sc failure */
[314]	464	" nop \n"
	465	:
	466	:"r"(plock)
[345]	467	:"$2", "$3", "memory");
[314]	468	#endif
	469
[258]	470	}
[295]	471
[258]	472	///////////////////////////////////////////////////////////////////////////////////
[378]	473	void _release_lock(giet_lock_t* lock)
[258]	474	{
[295]	475	asm volatile ( "sync\n" ::: "memory" );
	476	// sync is necessary because of the TSAR consistency model
[351]	477	lock->value = 0;
[258]	478	}
	479
	480	///////////////////////////////////////////////////////////////////////////////////
[295]	481	// Access functions to system terminal TTY0
[258]	482	///////////////////////////////////////////////////////////////////////////////////
[295]	483
	484	///////////////////////////////////////////////////////////////////////////////////
	485	// Display "string" argument on TTY0.
	486	// It uses the low level access functions from TTY driver, using a busy waiting
	487	// policy if TTY buffer is full.
	488	// The exclusive access lock should be taken by the caller.
	489	///////////////////////////////////////////////////////////////////////////////////
	490	void _puts( char* string )
[258]	491	{
[295]	492	unsigned int n = 0;
	493
	494	while ( string[n] > 0 )
[258]	495	{
[295]	496	// test status register
	497	while ( (_tty_get_register( 0, TTY_STATUS ) & 0x2) );
	498
	499	// write one byte
[315]	500	if ( string[n] == '\n') {
	501	_tty_set_register( 0, TTY_WRITE, (unsigned int)'\r' );
	502	}
[295]	503	_tty_set_register( 0, TTY_WRITE, (unsigned int)string[n] );
	504	n++;
[258]	505	}
	506	}
	507
	508	///////////////////////////////////////////////////////////////////////////////////
[295]	509	// Display a 32 bits unsigned int as an hexadecimal string on TTY0.
[258]	510	///////////////////////////////////////////////////////////////////////////////////
[295]	511	void _putx( unsigned int val )
[258]	512	{
	513	static const char HexaTab[] = "0123456789ABCDEF";
	514	char buf[11];
	515	unsigned int c;
	516
	517	buf[0] = '0';
	518	buf[1] = 'x';
	519	buf[10] = 0;
	520
	521	for (c = 0; c < 8; c++)
	522	{
	523	buf[9 - c] = HexaTab[val & 0xF];
	524	val = val >> 4;
	525	}
[295]	526	_puts( buf );
[258]	527	}
	528
	529	///////////////////////////////////////////////////////////////////////////////////
[295]	530	// Display a 64 bits unsigned long as an hexadecimal string on TTY0.
[258]	531	///////////////////////////////////////////////////////////////////////////////////
[295]	532	void _putl( unsigned long long val )
[258]	533	{
	534	static const char HexaTab[] = "0123456789ABCDEF";
	535	char buf[19];
	536	unsigned int c;
	537
	538	buf[0] = '0';
	539	buf[1] = 'x';
	540	buf[18] = 0;
	541
	542	for (c = 0; c < 16; c++)
	543	{
	544	buf[17 - c] = HexaTab[(unsigned int)val & 0xF];
	545	val = val >> 4;
	546	}
[295]	547	_puts( buf );
[258]	548	}
	549
	550	///////////////////////////////////////////////////////////////////////////////////
[295]	551	// Display a 32 bits unsigned int as a decimal string on TTY0.
[258]	552	///////////////////////////////////////////////////////////////////////////////////
[295]	553	void _putd( unsigned int val )
[258]	554	{
	555	static const char DecTab[] = "0123456789";
	556	char buf[11];
	557	unsigned int i;
[351]	558	unsigned int first = 0;
[258]	559
	560	buf[10] = 0;
	561
[351]	562	for (i = 0; i < 10; i++)
	563	{
	564	if ((val != 0) \|\| (i == 0))
	565	{
[258]	566	buf[9 - i] = DecTab[val % 10];
	567	first = 9 - i;
	568	}
[351]	569	else
	570	{
[258]	571	break;
	572	}
	573	val /= 10;
	574	}
[295]	575	_puts( &buf[first] );
[258]	576	}
	577
	578	///////////////////////////////////////////////////////////////////////////////////
[295]	579	// Display a format on TTY0.
	580	// To provide an atomic display, this function takes the lock protecting
	581	// exclusive access to TTY0, entering a critical section until the lock
	582	// is released.
	583	// Only a limited number of formats are supported:
	584	// - %d : 32 bits signed decimal
	585	// - %u : 32 bits unsigned decimal
	586	// - %x : 32 bits unsigned hexa
	587	// - %l : 64 bits unsigned hexa
	588	// - %c : char
	589	// - %s : string
	590	///////////////////////////////////////////////////////////////////////////////////
	591	void _printf( char * format, ... )
	592	{
	593	va_list ap;
	594	va_start(ap, format);
[332]	595	unsigned int save_sr; // to save SR value in critical section
[295]	596
	597	// get TTY0 lock
	598	_tty_get_lock( 0, &save_sr );
	599
	600	printf_text:
	601
	602	while (*format)
	603	{
	604	unsigned int i;
	605	for (i = 0 ; format[i] && (format[i] != '%') ; i++);
	606	if (i)
	607	{
	608	if ( _tty_write( format, i, 0 ) != i ) goto return_error;
	609	format += i;
	610	}
	611	if (*format == '%')
	612	{
	613	format++;
	614	goto printf_arguments;
	615	}
	616	}
	617
	618	// release TTY0 lock
	619	_tty_release_lock( 0, &save_sr );
	620
	621	va_end(ap);
	622	return;
	623
	624	printf_arguments:
	625
	626	{
	627	char buf[20];
	628	char * pbuf;
	629	unsigned int len = 0;
	630	static const char HexaTab[] = "0123456789ABCDEF";
	631	unsigned int i;
	632
	633	switch (*format++)
	634	{
	635	case ('c'): /* char conversion */
	636	{
	637	int val = va_arg( ap, int );
	638	len = 1;
	639	buf[0] = val;
	640	pbuf = &buf[0];
	641	break;
	642	}
	643	case ('d'): /* 32 bits decimal signed */
	644	{
	645	int val = va_arg( ap, int );
	646	if (val < 0)
	647	{
	648	val = -val;
	649	if ( _tty_write( "-" , 1, 0 ) != 1 ) goto return_error;
	650	}
	651	for(i = 0; i < 10; i++)
	652	{
	653	buf[9 - i] = HexaTab[val % 10];
	654	if (!(val /= 10)) break;
	655	}
	656	len = i + 1;
	657	pbuf = &buf[9 - i];
	658	break;
	659	}
	660	case ('u'): /* 32 bits decimal unsigned */
	661	{
	662	unsigned int val = va_arg( ap, unsigned int );
	663	for(i = 0; i < 10; i++)
	664	{
	665	buf[9 - i] = HexaTab[val % 10];
	666	if (!(val /= 10)) break;
	667	}
	668	len = i + 1;
	669	pbuf = &buf[9 - i];
	670	break;
	671	}
	672	case ('x'): /* 32 bits hexadecimal unsigned */
	673	{
	674	unsigned int val = va_arg( ap, unsigned int );
	675	if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
	676	for(i = 0; i < 8; i++)
	677	{
	678	buf[7 - i] = HexaTab[val % 16];
	679	if (!(val /= 16)) break;
	680	}
	681	len = i + 1;
	682	pbuf = &buf[7 - i];
	683	break;
	684	}
	685	case ('l'): /* 64 bits hexadecimal unsigned */
	686	{
	687	unsigned long long val = va_arg( ap, unsigned long long );
	688	if ( _tty_write( "0x" , 2, 0 ) != 2 ) goto return_error;
	689	for(i = 0; i < 16; i++)
	690	{
	691	buf[15 - i] = HexaTab[val % 16];
	692	if (!(val /= 16)) break;
	693	}
	694	len = i + 1;
	695	pbuf = &buf[15 - i];
	696	break;
	697	}
	698	case ('s'): /* string */
	699	{
	700	char* str = va_arg( ap, char* );
	701	while (str[len])
	702	{
	703	len++;
	704	}
	705	pbuf = str;
	706	break;
	707	}
	708	default:
	709	goto return_error;
	710	}
	711
	712	if ( _tty_write( pbuf, len, 0 ) != len ) goto return_error;
	713
	714	goto printf_text;
	715	}
	716
	717	return_error:
	718
	719	{
	720	unsigned int procid = _get_procid();
[430]	721	unsigned int x = (procid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
	722	unsigned int y = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
	723	unsigned int lpid = procid & ((1<<P_WIDTH)-1);
[295]	724
	725	_puts("\n\n[GIET ERROR] in _printf() for processor[");
	726	_putd( x );
	727	_puts(",");
	728	_putd( y );
	729	_puts(",");
	730	_putd( lpid );
	731	_puts("]\n");
	732
	733	// release TTY0 lock
	734	_tty_release_lock( 0, &save_sr );
	735
	736	_exit();
	737	}
	738	}
	739
	740	///////////////////////////////////////////////////////////////////////////////////
	741	// Get a character from TTY0.
	742	///////////////////////////////////////////////////////////////////////////////////
	743	void _getc( char* byte )
	744	{
	745	// test status register
	746	while ( _tty_get_register( 0, TTY_STATUS ) == 0 );
	747
	748	// read one byte
	749	*byte = (char)_tty_get_register( 0, TTY_READ );
	750	}
	751
[258]	752
[430]	753	////////////////////////////////////////////////////////////////////////////////////
[399]	754	// Scheduler and tasks context access functions
[430]	755	////////////////////////////////////////////////////////////////////////////////////
[258]	756
[430]	757
	758	////////////////////////////////////////////////////////////////////////////////////
[399]	759	unsigned int _get_current_task_id()
[258]	760	{
[399]	761	static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
	762	return (unsigned int) (psched->current);
[258]	763	}
	764	////////////////////////////////////////////////////////////////////////////////////
[430]	765	unsigned int _get_task_slot( unsigned int x,
	766	unsigned int y,
	767	unsigned int p,
[258]	768	unsigned int ltid,
	769	unsigned int slot )
	770	{
[430]	771	static_scheduler_t* psched = (static_scheduler_t*)_schedulers[x][y][p];
[258]	772	return psched->context[ltid][slot];
	773	}
	774	////////////////////////////////////////////////////////////////////////////////////
[430]	775	void _set_task_slot( unsigned int x,
	776	unsigned int y,
	777	unsigned int p,
[258]	778	unsigned int ltid,
	779	unsigned int slot,
	780	unsigned int value )
	781	{
[430]	782	static_scheduler_t* psched = (static_scheduler_t*)_schedulers[x][y][p];
[258]	783	psched->context[ltid][slot] = value;
	784	}
	785	////////////////////////////////////////////////////////////////////////////////////
	786	unsigned int _get_context_slot( unsigned int slot )
	787	{
	788	static_scheduler_t* psched = (static_scheduler_t*)_get_sched();
	789	unsigned int task_id = psched->current;
	790	return psched->context[task_id][slot];
	791	}
	792	////////////////////////////////////////////////////////////////////////////////////
	793	void _set_context_slot( unsigned int slot,
	794	unsigned int value )
	795	{
	796	static_scheduler_t* psched = (static_scheduler_t*)_get_sched();
	797	unsigned int task_id = psched->current;
	798	psched->context[task_id][slot] = value;
	799	}
	800
	801	/////////////////////////////////////////////////////////////////////////////
	802	// Access functions to mapping_info data structure
	803	/////////////////////////////////////////////////////////////////////////////
[378]	804	mapping_cluster_t * _get_cluster_base(mapping_header_t * header)
[258]	805	{
	806	return (mapping_cluster_t ) ((char ) header +
	807	MAPPING_HEADER_SIZE);
	808	}
	809	/////////////////////////////////////////////////////////////////////////////
[378]	810	mapping_pseg_t * _get_pseg_base(mapping_header_t * header)
[258]	811	{
	812	return (mapping_pseg_t ) ((char ) header +
	813	MAPPING_HEADER_SIZE +
[263]	814	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
[258]	815	}
	816	/////////////////////////////////////////////////////////////////////////////
[378]	817	mapping_vspace_t * _get_vspace_base(mapping_header_t * header)
[258]	818	{
	819	return (mapping_vspace_t ) ((char ) header +
	820	MAPPING_HEADER_SIZE +
[263]	821	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	822	MAPPING_PSEG_SIZE * header->psegs);
	823	}
	824	/////////////////////////////////////////////////////////////////////////////
[378]	825	mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
[258]	826	{
	827	return (mapping_vseg_t ) ((char ) header +
	828	MAPPING_HEADER_SIZE +
[263]	829	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	830	MAPPING_PSEG_SIZE * header->psegs +
	831	MAPPING_VSPACE_SIZE * header->vspaces);
	832	}
	833	/////////////////////////////////////////////////////////////////////////////
[378]	834	mapping_vobj_t * _get_vobj_base(mapping_header_t * header)
[258]	835	{
	836	return (mapping_vobj_t ) ((char ) header +
	837	MAPPING_HEADER_SIZE +
[263]	838	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	839	MAPPING_PSEG_SIZE * header->psegs +
	840	MAPPING_VSPACE_SIZE * header->vspaces +
	841	MAPPING_VSEG_SIZE * header->vsegs );
	842	}
	843	/////////////////////////////////////////////////////////////////////////////
[378]	844	mapping_task_t * _get_task_base(mapping_header_t * header)
[258]	845	{
	846	return (mapping_task_t ) ((char ) header +
	847	MAPPING_HEADER_SIZE +
[263]	848	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	849	MAPPING_PSEG_SIZE * header->psegs +
	850	MAPPING_VSPACE_SIZE * header->vspaces +
	851	MAPPING_VOBJ_SIZE * header->vobjs +
	852	MAPPING_VSEG_SIZE * header->vsegs);
	853	}
	854	/////////////////////////////////////////////////////////////////////////////
[378]	855	mapping_proc_t _get_proc_base(mapping_header_t header)
[258]	856	{
	857	return (mapping_proc_t ) ((char ) header +
	858	MAPPING_HEADER_SIZE +
[263]	859	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	860	MAPPING_PSEG_SIZE * header->psegs +
	861	MAPPING_VSPACE_SIZE * header->vspaces +
	862	MAPPING_VSEG_SIZE * header->vsegs +
	863	MAPPING_VOBJ_SIZE * header->vobjs +
	864	MAPPING_TASK_SIZE * header->tasks);
	865	}
	866	/////////////////////////////////////////////////////////////////////////////
[378]	867	mapping_irq_t _get_irq_base(mapping_header_t header)
[258]	868	{
	869	return (mapping_irq_t ) ((char ) header +
	870	MAPPING_HEADER_SIZE +
[263]	871	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	872	MAPPING_PSEG_SIZE * header->psegs +
	873	MAPPING_VSPACE_SIZE * header->vspaces +
	874	MAPPING_VSEG_SIZE * header->vsegs +
	875	MAPPING_VOBJ_SIZE * header->vobjs +
	876	MAPPING_TASK_SIZE * header->tasks +
	877	MAPPING_PROC_SIZE * header->procs);
	878	}
	879	/////////////////////////////////////////////////////////////////////////////
[378]	880	mapping_coproc_t _get_coproc_base(mapping_header_t header)
[258]	881	{
	882	return (mapping_coproc_t ) ((char ) header +
	883	MAPPING_HEADER_SIZE +
[263]	884	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	885	MAPPING_PSEG_SIZE * header->psegs +
	886	MAPPING_VSPACE_SIZE * header->vspaces +
	887	MAPPING_VOBJ_SIZE * header->vobjs +
	888	MAPPING_VSEG_SIZE * header->vsegs +
	889	MAPPING_TASK_SIZE * header->tasks +
	890	MAPPING_PROC_SIZE * header->procs +
	891	MAPPING_IRQ_SIZE * header->irqs);
	892	}
	893	///////////////////////////////////////////////////////////////////////////////////
[378]	894	mapping_cp_port_t _get_cp_port_base(mapping_header_t header)
[258]	895	{
	896	return (mapping_cp_port_t ) ((char ) header +
	897	MAPPING_HEADER_SIZE +
[263]	898	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	899	MAPPING_PSEG_SIZE * header->psegs +
	900	MAPPING_VSPACE_SIZE * header->vspaces +
	901	MAPPING_VOBJ_SIZE * header->vobjs +
	902	MAPPING_VSEG_SIZE * header->vsegs +
	903	MAPPING_TASK_SIZE * header->tasks +
	904	MAPPING_PROC_SIZE * header->procs +
	905	MAPPING_IRQ_SIZE * header->irqs +
	906	MAPPING_COPROC_SIZE * header->coprocs);
	907	}
	908	///////////////////////////////////////////////////////////////////////////////////
[378]	909	mapping_periph_t _get_periph_base(mapping_header_t header)
[258]	910	{
	911	return (mapping_periph_t ) ((char ) header +
	912	MAPPING_HEADER_SIZE +
[263]	913	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	914	MAPPING_PSEG_SIZE * header->psegs +
	915	MAPPING_VSPACE_SIZE * header->vspaces +
	916	MAPPING_VOBJ_SIZE * header->vobjs +
	917	MAPPING_VSEG_SIZE * header->vsegs +
	918	MAPPING_TASK_SIZE * header->tasks +
	919	MAPPING_PROC_SIZE * header->procs +
	920	MAPPING_IRQ_SIZE * header->irqs +
	921	MAPPING_COPROC_SIZE * header->coprocs +
	922	MAPPING_CP_PORT_SIZE * header->cp_ports);
	923	}
	924
[399]	925	///////////////////////////////////////////////////////////////////////////////////
	926	// Miscelaneous functions
	927	///////////////////////////////////////////////////////////////////////////////////
	928
	929	///////////////////////////////////////////////////////////////////////////////////
	930	// This function implements a pseudo-random delay.
	931	// The val argument define approximately an exponentially increasing mean delay,
	932	// and should not be larger than 32.
	933	///////////////////////////////////////////////////////////////////////////////////
	934	void _random_wait( unsigned int val )
	935	{
	936	unsigned int mask = (1<<(val&0x1F))-1;
	937	unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask;
	938	asm volatile( "move $3, %0 \n"
	939	"loop_nic_completed: \n"
	940	"addi $3, $3, -1 \n"
	941	"bnez $3, loop_nic_completed \n"
	942	"nop \n"
	943	:
	944	: "r" (delay)
	945	: "$3" );
	946	}
	947	//////////////////////////////////////////////////////////////////////////////////
	948	// This function implements an interactive break for debug.
	949	// Execution continue when typing any character on TTY0.
	950	// The "str" argument is supposed to indicate the break location.
	951	//////////////////////////////////////////////////////////////////////////////////
	952	void _break( char* string )
	953	{
	954	char byte;
	955
	956	_printf("\n[GIET DEBUG] break from %s / continue ?\n", string );
	957	_getc( &byte );
	958	}
	959
	960	//////////////////////////////////////////////////////////////////////////////////
	961	// Processor suicide: infinite loop
	962	//////////////////////////////////////////////////////////////////////////////////
	963	__attribute__((noreturn))
	964	void _exit()
	965	{
	966	unsigned int procid = _get_procid();
[430]	967	unsigned int x = (procid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
	968	unsigned int y = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
	969	unsigned int lpid = procid & ((1<<P_WIDTH)-1);
[399]	970
	971
	972	_printf("\n[GIET PANIC] processor[%d,%d,%d] suicide...\n", x, y, lpid );
	973
	974	while (1) { asm volatile ("nop"); }
	975	}
	976	///////////////////////////////////////////////////////////////////////////////////
	977	// Compare two strings s1 & s2 (no more than n characters)
	978	///////////////////////////////////////////////////////////////////////////////////
	979	unsigned int _strncmp( const char * s1,
	980	const char * s2,
	981	unsigned int n )
	982	{
	983	unsigned int i;
	984	for (i = 0; i < n; i++)
	985	{
	986	if (s1[i] != s2[i]) return 1;
	987	if (s1[i] == 0) break;
	988	}
	989	return 0;
	990	}
	991
	992	///////////////////////////////////////////////////////////////////////////////////
	993	// Copy source string to dest string
	994	///////////////////////////////////////////////////////////////////////////////////
	995	char* _strcpy( char* dest, char* source )
	996	{
	997	if (!dest \|\| !source) return dest;
	998
	999	while (*source)
	1000	(dest++) = (source++);
	1001
	1002	return dest;
	1003	}
	1004
	1005	///////////////////////////////////////////////////////////////////////////////////
	1006	// Invalidate all data cache lines corresponding to a memory
[408]	1007	// buffer (identified by virtual base address and size).
[399]	1008	///////////////////////////////////////////////////////////////////////////////////
[408]	1009	void _dcache_buf_invalidate( unsigned int buf_vbase,
	1010	unsigned int buf_size )
[399]	1011	{
[408]	1012	unsigned int offset;
[399]	1013	unsigned int tmp;
[408]	1014	unsigned int line_size; // bytes
[399]	1015
	1016	// compute data cache line size based on config register (bits 12:10)
	1017	asm volatile(
	1018	"mfc0 %0, $16, 1"
	1019	: "=r" (tmp) );
[408]	1020
[399]	1021	tmp = ((tmp >> 10) & 0x7);
	1022	line_size = 2 << tmp;
	1023
	1024	// iterate on cache lines
[408]	1025	for ( offset = 0; offset < buf_size; offset += line_size)
[399]	1026	{
[408]	1027	_set_mmu_dcache_inval( buf_vbase + offset );
[399]	1028	}
	1029	}
	1030	///////////////////////////////////////////////////////////////////////////////////
	1031	// This function returns the information associated to a heap : vaddr and length.
	1032	// - If (x < X_SIZE) and (y < Y_SIZE), it return the heap associated to any task
	1033	// running in cluster(x,y).
	1034	// - Else, it return the heap associated to the calling task.
	1035	// It uses the global task index (CTX_GTID_ID, unique for each giet task) and the
	1036	// vspace index (CTX_VSID_ID), that are defined in the calling task context
	1037	// to find the vobj_id containing the heap.
	1038	// Return 0 if success. Return non zero if not found.
	1039	///////////////////////////////////////////////////////////////////////////////////
	1040	unsigned int _heap_info( unsigned int* vaddr,
	1041	unsigned int* length,
	1042	unsigned int x,
	1043	unsigned int y )
	1044	{
	1045	mapping_header_t * header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
	1046	mapping_task_t * tasks = _get_task_base(header);
	1047	mapping_vobj_t * vobjs = _get_vobj_base(header);
	1048	mapping_vspace_t * vspaces = _get_vspace_base(header);
	1049
	1050	unsigned int task_id;
	1051	unsigned int vspace_id;
	1052	unsigned int vobj_id = 0xFFFFFFFF;
	1053
	1054	// searching the heap vobj_id
	1055	if ( (x < X_SIZE) && (y < Y_SIZE) ) // searching a task in cluster(x,y)
	1056	{
	1057	// get vspace global index
	1058	vspace_id = _get_context_slot(CTX_VSID_ID);
	1059
	1060	// scan all tasks in vspace
	1061	unsigned int min = vspaces[vspace_id].task_offset ;
	1062	unsigned int max = min + vspaces[vspace_id].tasks ;
	1063	for ( task_id = min ; task_id < max ; task_id++ )
	1064	{
	1065	if ( tasks[task_id].clusterid == (x * Y_SIZE + y) )
	1066	{
	1067	vobj_id = tasks[task_id].heap_vobj_id;
	1068	if ( vobj_id != 0xFFFFFFFF ) break;
	1069	}
	1070	}
	1071	}
	1072	else // searching in the calling task
	1073	{
	1074	task_id = _get_context_slot(CTX_GTID_ID);
	1075	vobj_id = tasks[task_id].heap_vobj_id;
	1076	}
	1077
	1078	// analysing the vobj_id
	1079	if ( vobj_id != 0xFFFFFFFF )
	1080	{
[408]	1081	*vaddr = vobjs[vobj_id].vbase;
[399]	1082	*length = vobjs[vobj_id].length;
	1083	return 0;
	1084	}
	1085	else
	1086	{
	1087	*vaddr = 0;
	1088	*length = 0;
	1089	return 1;
	1090	}
	1091	} // end _heap_info()
	1092
	1093
	1094	///////////////////////////////////////////////////////////////////////////////////
	1095	// Required by GCC
	1096	///////////////////////////////////////////////////////////////////////////////////
	1097
	1098	///////////////////////////////////////////////////////////////////////////////////
	1099	// Copy a source memory buffer content to a dest memory buffer (size bytes)
	1100	// Code taken from MutekH.
	1101	///////////////////////////////////////////////////////////////////////////////////
	1102	void* memcpy( void* dest, // dest buffer vbase
	1103	const void* source, // source buffer vbase
	1104	unsigned int size ) // bytes
	1105	{
	1106	unsigned int* idst = (unsigned int*)dest;
	1107	unsigned int* isrc = (unsigned int*)source;
	1108
	1109	// word-by-word copy
	1110	if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3))
	1111	{
	1112	while (size > 3)
	1113	{
	1114	idst++ = isrc++;
	1115	size -= 4;
	1116	}
	1117	}
	1118
	1119	unsigned char* cdst = (unsigned char*)dest;
	1120	unsigned char* csrc = (unsigned char*)source;
	1121
	1122	/* byte-by-byte copy */
	1123	while (size--)
	1124	{
	1125	cdst++ = csrc++;
	1126	}
	1127	return dest;
	1128	}
	1129	//////////////////////////////////////////////////////////////////////////////////
	1130	// Fill a byte string with a byte value.
	1131	//////////////////////////////////////////////////////////////////////////////////
	1132	void * memset( void* dest,
	1133	int value,
	1134	unsigned int count )
	1135	{
	1136	// word-by-word copy
	1137	unsigned int* idst = dest;
	1138	unsigned int data = (((unsigned char)value) ) \|
	1139	(((unsigned char)value) << 8) \|
	1140	(((unsigned char)value) << 16) \|
	1141	(((unsigned char)value) << 24) ;
	1142
	1143	if ( ! ((unsigned int)idst & 3) )
	1144	{
	1145	while ( count > 3 )
	1146	{
	1147	*idst++ = data;
	1148	count -= 4;
	1149	}
	1150	}
	1151
	1152	// byte-by-byte copy
	1153	unsigned char* cdst = dest;
	1154	while (count--)
	1155	{
	1156	*cdst++ = (unsigned char)value;
	1157	}
	1158	return dest;
	1159	}
	1160
	1161
[258]	1162	// Local Variables:
	1163	// tab-width: 4
	1164	// c-basic-offset: 4
	1165	// c-file-offsets:((innamespace . 0)(inline-open . 0))
	1166	// indent-tabs-mode: nil
	1167	// End:
	1168	// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
	1169

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

Download in other formats: