Context Navigation

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

Last change on this file since 521 was 514, checked in by alain, 10 years ago
Remove vobj object from mapping_info.
File size: 31.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	///////////////////////////////////////////////////////////////////////////////////
	9
[442]	10	#include <utils.h>
[455]	11	#include <tty0.h>
[258]	12	#include <giet_config.h>
[324]	13	#include <hard_config.h>
[258]	14	#include <mapping_info.h>
[442]	15	#include <tty_driver.h>
[258]	16	#include <ctx_handler.h>
	17
	18	// This global variable is allocated in the boot.c file or in kernel_init.c file
[430]	19	extern static_scheduler_t* _schedulers[X_SIZE][Y_SIZE][NB_PROCS_MAX];
[258]	20
	21	///////////////////////////////////////////////////////////////////////////////////
[399]	22	// CP0 registers access functions
[258]	23	///////////////////////////////////////////////////////////////////////////////////
	24
[408]	25	/////////////////////////
[378]	26	unsigned int _get_sched()
[258]	27	{
	28	unsigned int ret;
[295]	29	asm volatile( "mfc0 %0, $4,2 \n"
	30	: "=r"(ret) );
[258]	31	return ret;
	32	}
[408]	33	///////////////////////
[378]	34	unsigned int _get_epc()
[258]	35	{
	36	unsigned int ret;
[295]	37	asm volatile( "mfc0 %0, $14 \n"
	38	: "=r"(ret) );
[258]	39	return ret;
	40	}
[408]	41	////////////////////////
[378]	42	unsigned int _get_bvar()
[258]	43	{
	44	unsigned int ret;
[295]	45	asm volatile( "mfc0 %0, $8 \n"
	46	: "=r"(ret));
[258]	47	return ret;
	48	}
[408]	49	//////////////////////
[378]	50	unsigned int _get_cr()
[258]	51	{
	52	unsigned int ret;
[295]	53	asm volatile( "mfc0 %0, $13 \n"
	54	: "=r"(ret));
[258]	55	return ret;
	56	}
[408]	57	//////////////////////
[378]	58	unsigned int _get_sr()
[258]	59	{
	60	unsigned int ret;
[295]	61	asm volatile( "mfc0 %0, $12 \n"
	62	: "=r"(ret));
[258]	63	return ret;
	64	}
[408]	65	//////////////////////////
[378]	66	unsigned int _get_procid()
[258]	67	{
	68	unsigned int ret;
[295]	69	asm volatile ( "mfc0 %0, $15, 1 \n"
	70	:"=r" (ret) );
[430]	71	return (ret & 0xFFF);
[258]	72	}
[408]	73	////////////////////////////
[378]	74	unsigned int _get_proctime()
[258]	75	{
	76	unsigned int ret;
[295]	77	asm volatile ( "mfc0 %0, $9 \n"
	78	:"=r" (ret) );
[258]	79	return ret;
	80	}
[408]	81
	82	/////////////////////////////////////////////
[378]	83	void _it_disable( unsigned int * save_sr_ptr)
[258]	84	{
[345]	85	unsigned int sr = 0;
[295]	86	asm volatile( "li $3, 0xFFFFFFFE \n"
	87	"mfc0 %0, $12 \n"
	88	"and $3, $3, %0 \n"
	89	"mtc0 $3, $12 \n"
[345]	90	: "+r"(sr)
[295]	91	:
[345]	92	: "$3" );
[295]	93	*save_sr_ptr = sr;
[258]	94	}
[408]	95	//////////////////////////////////////////////
[378]	96	void _it_restore( unsigned int * save_sr_ptr )
[295]	97	{
	98	unsigned int sr = *save_sr_ptr;
	99	asm volatile( "mtc0 %0, $12 \n"
	100	:
[301]	101	: "r"(sr)
	102	: "memory" );
[295]	103	}
	104
[408]	105	/////////////////////////////////
[399]	106	void _set_sched(unsigned int val)
	107	{
	108	asm volatile ( "mtc0 %0, $4, 2 \n"
	109	:
	110	:"r" (val) );
	111	}
[408]	112	//////////////////////////////
	113	void _set_sr(unsigned int val)
	114	{
	115	asm volatile ( "mtc0 %0, $12 \n"
	116	:
	117	:"r" (val) );
	118	}
[399]	119
[408]	120
[399]	121	///////////////////////////////////////////////////////////////////////////////////
	122	// CP2 registers access functions
	123	///////////////////////////////////////////////////////////////////////////////////
	124
[408]	125	////////////////////////////
[399]	126	unsigned int _get_mmu_ptpr()
	127	{
	128	unsigned int ret;
	129	asm volatile( "mfc2 %0, $0 \n"
	130	: "=r"(ret) );
	131	return ret;
	132	}
[408]	133	////////////////////////////
[399]	134	unsigned int _get_mmu_mode()
	135	{
	136	unsigned int ret;
	137	asm volatile( "mfc2 %0, $1 \n"
	138	: "=r"(ret) );
	139	return ret;
	140	}
[408]	141	////////////////////////////////////
[378]	142	void _set_mmu_ptpr(unsigned int val)
[258]	143	{
[408]	144	asm volatile ( "mtc2 %0, $0 \n"
[295]	145	:
[345]	146	:"r" (val)
	147	:"memory" );
[258]	148	}
[408]	149	////////////////////////////////////
[378]	150	void _set_mmu_mode(unsigned int val)
[258]	151	{
[408]	152	asm volatile ( "mtc2 %0, $1 \n"
[295]	153	:
[345]	154	:"r" (val)
	155	:"memory" );
[258]	156	}
[408]	157	////////////////////////////////////////////
	158	void _set_mmu_dcache_inval(unsigned int val)
	159	{
	160	asm volatile ( "mtc2 %0, $7 \n"
	161	:
	162	:"r" (val)
	163	:"memory" );
	164	}
[258]	165
[408]	166
[258]	167	////////////////////////////////////////////////////////////////////////////
	168	// Physical addressing related functions
	169	////////////////////////////////////////////////////////////////////////////
	170
[442]	171	///////////////////////////////////////////////////////
[378]	172	unsigned int _physical_read( unsigned long long paddr )
[258]	173	{
	174	unsigned int value;
	175	unsigned int lsb = (unsigned int) paddr;
	176	unsigned int msb = (unsigned int) (paddr >> 32);
[301]	177	unsigned int sr;
[258]	178
[301]	179	_it_disable(&sr);
[258]	180
[344]	181	asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */
	182	"andi $3, $2, 0xb \n"
	183	"mtc2 $3, $1 \n" /* DTLB off */
[258]	184
[344]	185	"mtc2 %2, $24 \n" /* PADDR_EXT <= msb */
	186	"lw %0, 0(%1) \n" /* value <= paddr /
	187	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	188
	189	"mtc2 $2, $1 \n" /* restore MMU_MODE */
	190	: "=r" (value)
	191	: "r" (lsb), "r" (msb)
	192	: "$2", "$3" );
	193
[301]	194	_it_restore(&sr);
[258]	195	return value;
	196	}
[442]	197	////////////////////////////////////////////////
[378]	198	void _physical_write( unsigned long long paddr,
[430]	199	unsigned int value )
[258]	200	{
	201	unsigned int lsb = (unsigned int)paddr;
	202	unsigned int msb = (unsigned int)(paddr >> 32);
[301]	203	unsigned int sr;
[258]	204
[430]	205	_it_disable(&sr);
[258]	206
[344]	207	asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */
	208	"andi $3, $2, 0xb \n"
	209	"mtc2 $3, $1 \n" /* DTLB off */
[258]	210
[344]	211	"mtc2 %2, $24 \n" /* PADDR_EXT <= msb */
	212	"sw %0, 0(%1) \n" /* paddr <= value /
	213	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	214
	215	"mtc2 $2, $1 \n" /* restore MMU_MODE */
[345]	216	"sync \n"
[344]	217	:
	218	: "r" (value), "r" (lsb), "r" (msb)
	219	: "$2", "$3" );
	220
[301]	221	_it_restore(&sr);
[258]	222	}
	223
[442]	224	/////////////////////////////////////////////////////////////////
[378]	225	unsigned long long _physical_read_ull( unsigned long long paddr )
[370]	226	{
	227	unsigned int data_lsb;
	228	unsigned int data_msb;
	229	unsigned int addr_lsb = (unsigned int) paddr;
	230	unsigned int addr_msb = (unsigned int) (paddr >> 32);
	231	unsigned int sr;
	232
	233	_it_disable(&sr);
	234
	235	asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */
	236	"andi $3, $2, 0xb \n"
	237	"mtc2 $3, $1 \n" /* DTLB off */
	238
	239	"mtc2 %3, $24 \n" /* PADDR_EXT <= msb */
	240	"lw %0, 0(%2) \n" /* data_lsb <= paddr /
	241	"lw %1, 4(%2) \n" /* data_msb <= paddr+4 /
	242	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	243
	244	"mtc2 $2, $1 \n" /* restore MMU_MODE */
	245	: "=r" (data_lsb), "=r"(data_msb)
	246	: "r" (addr_lsb), "r" (addr_msb)
	247	: "$2", "$3" );
	248
	249	_it_restore(&sr);
	250
	251	return ( (((unsigned long long)data_msb)<<32) +
	252	(((unsigned long long)data_lsb)) );
	253	}
	254
[442]	255	///////////////////////////////////////////////////
[378]	256	void _physical_write_ull( unsigned long long paddr,
[399]	257	unsigned long long value )
[370]	258	{
	259	unsigned int addr_lsb = (unsigned int)paddr;
	260	unsigned int addr_msb = (unsigned int)(paddr >> 32);
	261	unsigned int data_lsb = (unsigned int)value;
	262	unsigned int data_msb = (unsigned int)(value >> 32);
	263	unsigned int sr;
	264
	265	_it_disable(&sr);
	266
	267	asm volatile( "mfc2 $2, $1 \n" /* $2 <= MMU_MODE */
	268	"andi $3, $2, 0xb \n"
	269	"mtc2 $3, $1 \n" /* DTLB off */
	270
	271	"mtc2 %3, $24 \n" /* PADDR_EXT <= msb */
	272	"sw %0, 0(%2) \n" /* paddr <= value /
	273	"sw %1, 4(%2) \n" /* paddr+4 <= value /
	274	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	275
	276	"mtc2 $2, $1 \n" /* restore MMU_MODE */
	277	"sync \n"
	278	:
	279	: "r" (data_lsb), "r" (data_msb), "r" (addr_lsb), "r" (addr_msb)
	280	: "$2", "$3" );
	281
	282	_it_restore(&sr);
	283	}
	284
[442]	285	////////////////////////////////////////////////////
[430]	286	void _physical_memcpy( unsigned long long dst_paddr, // destination buffer paddr
[399]	287	unsigned long long src_paddr, // source buffer paddr
	288	unsigned int size ) // bytes
[344]	289	{
	290	// check alignment constraints
	291	if ( (dst_paddr & 3) \|\| (src_paddr & 3) \|\| (size & 3) )
	292	{
[442]	293	_puts("\n[GIET ERROR] in _physical_memcpy() : buffer unaligned\n");
[344]	294	_exit();
	295	}
	296
	297	unsigned int src_lsb = (unsigned int)src_paddr;
	298	unsigned int src_msb = (unsigned int)(src_paddr >> 32);
	299	unsigned int dst_lsb = (unsigned int)dst_paddr;
	300	unsigned int dst_msb = (unsigned int)(dst_paddr >> 32);
	301	unsigned int iter = size>>2;
	302	unsigned int data;
	303	unsigned int sr;
	304
	305	_it_disable(&sr);
	306
	307	asm volatile( "mfc2 $2, $1 \n" /* $2 <= current MMU_MODE */
	308	"andi $3, $2, 0xb \n" /* $3 <= new MMU_MODE */
	309	"mtc2 $3, $1 \n" /* DTLB off */
	310
	311	"move $4, %5 \n" /* $4 < iter */
	312	"move $5, %1 \n" /* $5 < src_lsb */
	313	"move $6, %3 \n" /* $6 < src_lsb */
	314
	315	"ph_memcpy_loop: \n"
	316	"mtc2 %2, $24 \n" /* PADDR_EXT <= src_msb */
	317	"lw %0, 0($5) \n" /* data <= src_paddr /
	318	"mtc2 %4, $24 \n" /* PADDR_EXT <= dst_msb */
	319	"sw %0, 0($6) \n" /* dst_paddr <= data /
	320
	321	"addi $4, $4, -1 \n" /* iter = iter - 1 */
	322	"addi $5, $5, 4 \n" /* src_lsb += 4 */
	323	"addi $6, $6, 4 \n" /* dst_lsb += 4 */
	324	"bne $4, $0, ph_memcpy_loop \n"
	325	"nop \n"
	326
	327	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	328	"mtc2 $2, $1 \n" /* restore MMU_MODE */
	329	: "=r" (data)
	330	: "r" (src_lsb), "r" (src_msb), "r" (dst_lsb), "r"(dst_msb), "r"(iter)
	331	: "$2", "$3", "$4", "$5", "$6" );
	332
	333	_it_restore(&sr);
[430]	334	} // end _physical_memcpy()
[344]	335
[442]	336	////////////////////////////////////////////////
[430]	337	void _physical_memset( unsigned long long paddr, // destination buffer paddr
	338	unsigned int size, // bytes
	339	unsigned int data ) // written value
	340	{
	341	// check alignment constraints
[433]	342	if ( (paddr & 3) \|\| (size & 7) )
[430]	343	{
[442]	344	_puts("\n[GIET ERROR] in _physical_memset() : buffer unaligned\n");
[430]	345	_exit();
	346	}
	347
	348	unsigned int lsb = (unsigned int)paddr;
	349	unsigned int msb = (unsigned int)(paddr >> 32);
	350	unsigned int sr;
	351
	352	_it_disable(&sr);
	353
[433]	354	asm volatile( "mfc2 $8, $1 \n" /* $8 <= current MMU_MODE */
	355	"andi $9, $8, 0xb \n" /* $9 <= new MMU_MODE */
	356	"mtc2 $9, $1 \n" /* DTLB off */
	357	"mtc2 %3, $24 \n" /* PADDR_EXT <= msb */
[430]	358
[433]	359	"1: \n" /* set 8 bytes per iter */
	360	"sw %2, 0(%0) \n" /* src_paddr = data /
	361	"sw %2, 4(%0) \n" /* (src_paddr+4) = data /
	362	"addi %1, %1, -8 \n" /* size -= 8 */
	363	"addi %0, %0, 8 \n" /* src_paddr += 8 */
	364	"bnez %1, 1b \n" /* loop while size != 0 */
[430]	365
[433]	366	"mtc2 $0, $24 \n" /* PADDR_EXT <= 0 */
	367	"mtc2 $8, $1 \n" /* restore MMU_MODE */
	368	: "+r"(lsb), "+r"(size)
	369	: "r"(data), "r" (msb)
	370	: "$8", "$9", "memory" );
[430]	371
	372	_it_restore(&sr);
[433]	373	} // _physical_memset()
[430]	374
[442]	375	///////////////////////////////////////////////
[378]	376	void _io_extended_write( unsigned int* vaddr,
[399]	377	unsigned int value )
[295]	378	{
	379	unsigned long long paddr;
	380
	381	if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address
	382	{
	383	*vaddr = value;
	384	}
	385	else // use paddr extension for IO
	386	{
	387	paddr = (unsigned long long)(unsigned int)vaddr +
	388	(((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32);
	389	_physical_write( paddr, value );
	390	}
	391	asm volatile("sync" ::: "memory");
	392	}
	393
[442]	394	//////////////////////////////////////////////////////
[378]	395	unsigned int _io_extended_read( unsigned int* vaddr )
[295]	396	{
	397	unsigned long long paddr;
	398
	399	if ( _get_mmu_mode() & 0x4 ) // MMU activated : use virtual address
	400	{
	401	return (volatile unsigned int)vaddr;
	402	}
	403	else // use paddr extension for IO
	404	{
	405	paddr = (unsigned long long)(unsigned int)vaddr +
	406	(((unsigned long long)((X_IO<<Y_WIDTH) + Y_IO))<<32);
	407	return _physical_read( paddr );
	408	}
	409	}
	410
[490]	411	///////////////////////////////////////////////////////////////////////////////////
	412	// barrier functions
	413	///////////////////////////////////////////////////////////////////////////////////
	414	void _barrier_init( _giet_barrier_t* barrier,
	415	unsigned int ntasks )
	416	{
	417	barrier->ntasks = ntasks;
	418	barrier->count = ntasks;
	419	barrier->sense = 0;
[258]	420
[490]	421	asm volatile ("sync" ::: "memory");
	422	}
[351]	423
[490]	424	////////////////////////////////////////////
	425	void _barrier_wait( _giet_barrier_t* barrier )
	426	{
	427
	428	// compute expected sense value
	429	unsigned int expected;
	430	if ( barrier->sense == 0 ) expected = 1;
	431	else expected = 0;
	432
	433	// parallel decrement barrier counter using atomic instructions LL/SC
	434	// - input : pointer on the barrier counter (pcount)
	435	// - output : counter value (count)
	436	volatile unsigned int* pcount = (unsigned int *)&barrier->count;
	437	volatile unsigned int count = 0; // avoid a warning
	438
	439	asm volatile( "addu $2, %1, $0 \n"
	440	"barrier_llsc: \n"
	441	"ll $8, 0($2) \n"
	442	"addi $9, $8, -1 \n"
	443	"sc $9, 0($2) \n"
	444	"beqz $9, barrier_llsc \n"
	445	"addu %0, $8, $0 \n"
	446	: "=r" (count)
	447	: "r" (pcount)
	448	: "$2", "$8", "$9", "memory" );
	449
	450	// the last task re-initializes count and toggle sense,
	451	// waking up all other waiting tasks
	452	if (count == 1) // last task
	453	{
	454	barrier->count = barrier->ntasks;
	455	barrier->sense = expected;
	456	}
	457	else // other tasks busy waiting the sense flag
	458	{
	459	// polling sense flag
	460	// input: pointer on the sens flag (psense)
	461	// input: expected sense value (expected)
	462	volatile unsigned int* psense = (unsigned int *)&barrier->sense;
	463	asm volatile ( "barrier_sense: \n"
	464	"lw $3, 0(%0) \n"
	465	"bne $3, %1, barrier_sense \n"
	466	:
	467	: "r"(psense), "r"(expected)
	468	: "$3" );
	469	}
	470
	471	asm volatile ("sync" ::: "memory");
	472	}
	473
	474	///////////////////////////////////////////////////////////////////////////////////
	475	// Locks access functions
	476	///////////////////////////////////////////////////////////////////////////////////
	477
	478	///////////////////////////////////
	479	void _get_lock( _giet_lock_t* lock )
	480	{
[498]	481	unsigned int* plock = (unsigned int*)&(lock->value);
[490]	482
	483	#if NO_HARD_CC
	484
	485	register unsigned int delay = (_get_proctime() ^ _get_procid() << 4) & 0xFF;
	486	if (delay == 0) delay = 0x80;
	487
	488	asm volatile (
	489	"_lock_llsc: \n"
	490	" ll $2, 0(%0) \n" /* $2 <= lock current value */
	491	" bnez $2, _lock_delay \n" /* delay if lock already taken */
	492	" li $3, 1 \n" /* $3 <= argument for sc */
	493	" sc $3, 0(%0) \n" /* try to set lock */
	494	" bnez $3, _lock_ok \n" /* exit if atomic */
	495	" _lock_delay: \n"
	496	" move $4, %1 \n" /* $4 <= delay */
	497	" _lock_loop: \n"
	498	" addi $4, $4, -1 \n" /* $4 <= $4 - 1 */
	499	" bnez $4, _lock_loop \n" /* test end delay */
	500	" nop \n"
	501	" j _lock_llsc \n" /* retry */
	502	" nop \n"
	503	" _lock_ok: \n"
	504	:
	505	:"r"(plock), "r"(delay)
	506	:"$2", "$3", "$4", "memory");
	507	#else
	508
	509	asm volatile (
	510	"_lock_llsc: \n"
	511	" lw $2, 0(%0) \n" /* $2 <= lock current value */
	512	" bnez $2, _lock_llsc \n" /* retry if lock already taken */
	513	" nop \n"
	514	" ll $2, 0(%0) \n" /* ll_buffer <= lock current value */
	515	" bnez $2, _lock_llsc \n" /* retry if lock already taken */
	516	" li $3, 1 \n" /* $3 <= argument for sc */
	517	" sc $3, 0(%0) \n" /* try to set lock */
	518	" beqz $3, _lock_llsc \n" /* retry if sc failure */
	519	" nop \n"
	520	:
	521	:"r"(plock)
	522	:"$2", "$3", "memory");
	523	#endif
	524
	525	}
	526
	527	///////////////////////////////////////
	528	void _release_lock( _giet_lock_t* lock )
	529	{
	530	asm volatile ( "sync\n" ::: "memory" );
	531	// sync is necessary because of the TSAR consistency model
	532	lock->value = 0;
	533	}
	534
[430]	535	////////////////////////////////////////////////////////////////////////////////////
[399]	536	// Scheduler and tasks context access functions
[430]	537	////////////////////////////////////////////////////////////////////////////////////
[258]	538
[430]	539
[442]	540	///////////////////////////////////
[399]	541	unsigned int _get_current_task_id()
[258]	542	{
[399]	543	static_scheduler_t * psched = (static_scheduler_t *) _get_sched();
	544	return (unsigned int) (psched->current);
[258]	545	}
[442]	546
	547	////////////////////////////////////////////
[430]	548	unsigned int _get_task_slot( unsigned int x,
	549	unsigned int y,
	550	unsigned int p,
[258]	551	unsigned int ltid,
	552	unsigned int slot )
	553	{
[430]	554	static_scheduler_t* psched = (static_scheduler_t*)_schedulers[x][y][p];
[258]	555	return psched->context[ltid][slot];
	556	}
[442]	557
	558	////////////////////////////////////
[430]	559	void _set_task_slot( unsigned int x,
	560	unsigned int y,
	561	unsigned int p,
[258]	562	unsigned int ltid,
	563	unsigned int slot,
	564	unsigned int value )
	565	{
[430]	566	static_scheduler_t* psched = (static_scheduler_t*)_schedulers[x][y][p];
[258]	567	psched->context[ltid][slot] = value;
	568	}
[442]	569
	570	///////////////////////////////////////////////////
[258]	571	unsigned int _get_context_slot( unsigned int slot )
	572	{
	573	static_scheduler_t* psched = (static_scheduler_t*)_get_sched();
	574	unsigned int task_id = psched->current;
	575	return psched->context[task_id][slot];
	576	}
[442]	577
	578	///////////////////////////////////////////
[258]	579	void _set_context_slot( unsigned int slot,
	580	unsigned int value )
	581	{
	582	static_scheduler_t* psched = (static_scheduler_t*)_get_sched();
	583	unsigned int task_id = psched->current;
	584	psched->context[task_id][slot] = value;
	585	}
	586
	587	/////////////////////////////////////////////////////////////////////////////
	588	// Access functions to mapping_info data structure
	589	/////////////////////////////////////////////////////////////////////////////
[442]	590
	591	////////////////////////////////////////////////////////////////
[378]	592	mapping_cluster_t * _get_cluster_base(mapping_header_t * header)
[258]	593	{
	594	return (mapping_cluster_t ) ((char ) header +
	595	MAPPING_HEADER_SIZE);
	596	}
[442]	597	//////////////////////////////////////////////////////////
[378]	598	mapping_pseg_t * _get_pseg_base(mapping_header_t * header)
[258]	599	{
	600	return (mapping_pseg_t ) ((char ) header +
	601	MAPPING_HEADER_SIZE +
[263]	602	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE);
[258]	603	}
[442]	604	//////////////////////////////////////////////////////////////
[378]	605	mapping_vspace_t * _get_vspace_base(mapping_header_t * header)
[258]	606	{
	607	return (mapping_vspace_t ) ((char ) header +
	608	MAPPING_HEADER_SIZE +
[263]	609	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	610	MAPPING_PSEG_SIZE * header->psegs);
	611	}
[442]	612	//////////////////////////////////////////////////////////
[378]	613	mapping_vseg_t * _get_vseg_base(mapping_header_t * header)
[258]	614	{
	615	return (mapping_vseg_t ) ((char ) header +
	616	MAPPING_HEADER_SIZE +
[263]	617	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	618	MAPPING_PSEG_SIZE * header->psegs +
	619	MAPPING_VSPACE_SIZE * header->vspaces);
	620	}
[442]	621	//////////////////////////////////////////////////////////
[378]	622	mapping_task_t * _get_task_base(mapping_header_t * header)
[258]	623	{
	624	return (mapping_task_t ) ((char ) header +
	625	MAPPING_HEADER_SIZE +
[263]	626	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	627	MAPPING_PSEG_SIZE * header->psegs +
	628	MAPPING_VSPACE_SIZE * header->vspaces +
	629	MAPPING_VSEG_SIZE * header->vsegs);
	630	}
[442]	631	/////////////////////////////////////////////////////////
[378]	632	mapping_proc_t _get_proc_base(mapping_header_t header)
[258]	633	{
	634	return (mapping_proc_t ) ((char ) header +
	635	MAPPING_HEADER_SIZE +
[263]	636	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	637	MAPPING_PSEG_SIZE * header->psegs +
	638	MAPPING_VSPACE_SIZE * header->vspaces +
	639	MAPPING_VSEG_SIZE * header->vsegs +
	640	MAPPING_TASK_SIZE * header->tasks);
	641	}
[442]	642	///////////////////////////////////////////////////////
[378]	643	mapping_irq_t _get_irq_base(mapping_header_t header)
[258]	644	{
	645	return (mapping_irq_t ) ((char ) header +
	646	MAPPING_HEADER_SIZE +
[263]	647	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	648	MAPPING_PSEG_SIZE * header->psegs +
	649	MAPPING_VSPACE_SIZE * header->vspaces +
	650	MAPPING_VSEG_SIZE * header->vsegs +
	651	MAPPING_TASK_SIZE * header->tasks +
	652	MAPPING_PROC_SIZE * header->procs);
	653	}
[442]	654	/////////////////////////////////////////////////////////////
[378]	655	mapping_coproc_t _get_coproc_base(mapping_header_t header)
[258]	656	{
	657	return (mapping_coproc_t ) ((char ) header +
	658	MAPPING_HEADER_SIZE +
[263]	659	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	660	MAPPING_PSEG_SIZE * header->psegs +
	661	MAPPING_VSPACE_SIZE * header->vspaces +
	662	MAPPING_VSEG_SIZE * header->vsegs +
	663	MAPPING_TASK_SIZE * header->tasks +
	664	MAPPING_PROC_SIZE * header->procs +
	665	MAPPING_IRQ_SIZE * header->irqs);
	666	}
[442]	667	///////////////////////////////////////////////////////////////
[378]	668	mapping_cp_port_t _get_cp_port_base(mapping_header_t header)
[258]	669	{
	670	return (mapping_cp_port_t ) ((char ) header +
	671	MAPPING_HEADER_SIZE +
[263]	672	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	673	MAPPING_PSEG_SIZE * header->psegs +
	674	MAPPING_VSPACE_SIZE * header->vspaces +
	675	MAPPING_VSEG_SIZE * header->vsegs +
	676	MAPPING_TASK_SIZE * header->tasks +
	677	MAPPING_PROC_SIZE * header->procs +
	678	MAPPING_IRQ_SIZE * header->irqs +
	679	MAPPING_COPROC_SIZE * header->coprocs);
	680	}
[442]	681	/////////////////////////////////////////////////////////////
[378]	682	mapping_periph_t _get_periph_base(mapping_header_t header)
[258]	683	{
	684	return (mapping_periph_t ) ((char ) header +
	685	MAPPING_HEADER_SIZE +
[263]	686	MAPPING_CLUSTER_SIZE * X_SIZE * Y_SIZE +
[258]	687	MAPPING_PSEG_SIZE * header->psegs +
	688	MAPPING_VSPACE_SIZE * header->vspaces +
	689	MAPPING_VSEG_SIZE * header->vsegs +
	690	MAPPING_TASK_SIZE * header->tasks +
	691	MAPPING_PROC_SIZE * header->procs +
	692	MAPPING_IRQ_SIZE * header->irqs +
	693	MAPPING_COPROC_SIZE * header->coprocs +
	694	MAPPING_CP_PORT_SIZE * header->cp_ports);
	695	}
	696
[399]	697	///////////////////////////////////////////////////////////////////////////////////
[442]	698	// Miscelaneous functions
[399]	699	///////////////////////////////////////////////////////////////////////////////////
	700
[442]	701	//////////////////////////////////////
	702	__attribute__((noreturn)) void _exit()
	703	{
	704	unsigned int procid = _get_procid();
	705	unsigned int x = (procid >> (Y_WIDTH + P_WIDTH)) & ((1<<X_WIDTH)-1);
	706	unsigned int y = (procid >> P_WIDTH) & ((1<<Y_WIDTH)-1);
	707	unsigned int lpid = procid & ((1<<P_WIDTH)-1);
	708
	709
	710	_puts("\n[GIET PANIC] processor[");
	711	_putd( x );
	712	_puts(",");
	713	_putd( y );
	714	_puts(",");
	715	_putd( lpid );
[455]	716	_puts("] exit at cycle ");
[442]	717	_putd( _get_proctime() );
	718	_puts(" ...\n");
	719
	720	while (1) { asm volatile ("nop"); }
	721	}
	722
	723	/////////////////////////////////////
[399]	724	void _random_wait( unsigned int val )
	725	{
	726	unsigned int mask = (1<<(val&0x1F))-1;
	727	unsigned int delay = (_get_proctime() ^ (_get_procid()<<4)) & mask;
	728	asm volatile( "move $3, %0 \n"
	729	"loop_nic_completed: \n"
[466]	730	"nop \n"
[399]	731	"addi $3, $3, -1 \n"
	732	"bnez $3, loop_nic_completed \n"
	733	"nop \n"
	734	:
	735	: "r" (delay)
	736	: "$3" );
	737	}
[442]	738
	739	///////////////////////////
[399]	740	void _break( char* string )
	741	{
	742	char byte;
	743
[442]	744	_puts("\n[GIET DEBUG] break from ");
	745	_puts( string );
	746	_puts(" / stoke any key to continue\n");
[399]	747	_getc( &byte );
	748	}
	749
[442]	750	///////////////////////////////////////
[399]	751	unsigned int _strncmp( const char * s1,
	752	const char * s2,
	753	unsigned int n )
	754	{
	755	unsigned int i;
	756	for (i = 0; i < n; i++)
	757	{
	758	if (s1[i] != s2[i]) return 1;
	759	if (s1[i] == 0) break;
	760	}
	761	return 0;
	762	}
	763
[442]	764	/////////////////////////////////////////
[399]	765	char* _strcpy( char* dest, char* source )
	766	{
	767	if (!dest \|\| !source) return dest;
	768
	769	while (*source)
	770	(dest++) = (source++);
	771
	772	return dest;
	773	}
	774
[442]	775	/////////////////////////////////////////////////////
[408]	776	void _dcache_buf_invalidate( unsigned int buf_vbase,
	777	unsigned int buf_size )
[399]	778	{
[408]	779	unsigned int offset;
[399]	780	unsigned int tmp;
[408]	781	unsigned int line_size; // bytes
[399]	782
	783	// compute data cache line size based on config register (bits 12:10)
	784	asm volatile(
	785	"mfc0 %0, $16, 1"
	786	: "=r" (tmp) );
[408]	787
[399]	788	tmp = ((tmp >> 10) & 0x7);
	789	line_size = 2 << tmp;
	790
	791	// iterate on cache lines
[408]	792	for ( offset = 0; offset < buf_size; offset += line_size)
[399]	793	{
[408]	794	_set_mmu_dcache_inval( buf_vbase + offset );
[399]	795	}
	796	}
	797
	798
	799
[495]	800	/////////////////////////////////////////////
	801	void _get_sqt_footprint( unsigned int* width,
	802	unsigned int* heigth,
	803	unsigned int* levels )
	804	{
	805	mapping_header_t* header = (mapping_header_t *)SEG_BOOT_MAPPING_BASE;
	806	mapping_cluster_t* cluster = _get_cluster_base(header);
	807
	808	unsigned int x;
	809	unsigned int y;
	810	unsigned int cid;
	811	unsigned int w = 0;
	812	unsigned int h = 0;
	813
	814	// scan all clusters to compute SQT footprint (w,h)
	815	for ( x = 0 ; x < X_SIZE ; x++ )
	816	{
	817	for ( y = 0 ; y < Y_SIZE ; y++ )
	818	{
	819	cid = x * Y_SIZE + y;
	820	if ( cluster[cid].procs ) // cluster contains processors
	821	{
	822	if ( x > w ) w = x;
	823	if ( y > h ) h = y;
	824	}
	825	}
	826	}
	827	*width = w + 1;
	828	*heigth = h + 1;
	829
	830	// compute SQT levels
	831	unsigned int z = (h > w) ? h : w;
	832	*levels = (z < 1) ? 1 : (z < 2) ? 2 : (z < 4) ? 3 : (z < 8) ? 4 : 5;
	833	}
	834
	835
	836
[399]	837	///////////////////////////////////////////////////////////////////////////////////
	838	// Required by GCC
	839	///////////////////////////////////////////////////////////////////////////////////
	840
[442]	841	////////////////////////////////
[399]	842	void* memcpy( void* dest, // dest buffer vbase
	843	const void* source, // source buffer vbase
	844	unsigned int size ) // bytes
	845	{
	846	unsigned int* idst = (unsigned int*)dest;
	847	unsigned int* isrc = (unsigned int*)source;
	848
	849	// word-by-word copy
	850	if (!((unsigned int) idst & 3) && !((unsigned int) isrc & 3))
	851	{
	852	while (size > 3)
	853	{
	854	idst++ = isrc++;
	855	size -= 4;
	856	}
	857	}
	858
	859	unsigned char* cdst = (unsigned char*)dest;
	860	unsigned char* csrc = (unsigned char*)source;
	861
	862	/* byte-by-byte copy */
	863	while (size--)
	864	{
	865	cdst++ = csrc++;
	866	}
	867	return dest;
	868	}
[442]	869
	870	/////////////////////////////////
	871	void * memset( void* dst,
	872	int value,
	873	unsigned int count )
[399]	874	{
	875	// word-by-word copy
[442]	876	unsigned int* idst = dst;
[399]	877	unsigned int data = (((unsigned char)value) ) \|
	878	(((unsigned char)value) << 8) \|
	879	(((unsigned char)value) << 16) \|
	880	(((unsigned char)value) << 24) ;
	881
	882	if ( ! ((unsigned int)idst & 3) )
	883	{
	884	while ( count > 3 )
	885	{
	886	*idst++ = data;
	887	count -= 4;
	888	}
	889	}
	890
	891	// byte-by-byte copy
[442]	892	unsigned char* cdst = dst;
[399]	893	while (count--)
	894	{
	895	*cdst++ = (unsigned char)value;
	896	}
[442]	897	return dst;
[399]	898	}
	899
	900
[258]	901	// Local Variables:
	902	// tab-width: 4
	903	// c-basic-offset: 4
	904	// c-file-offsets:((innamespace . 0)(inline-open . 0))
	905	// indent-tabs-mode: nil
	906	// End:
	907	// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4
	908

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

Download in other formats: