stage2.c

newos is new operation system

			case PT_LOAD:
				break;
			case PT_DYNAMIC:
				dynamic_section->start = segment->p_vaddr;
				dynamic_section->size = segment->p_memsz;
			default:
				continue;
		}

//		dprintf("segment %d\n", segmentIndex);
//		dprintf("p_vaddr 0x%x p_paddr 0x%x p_filesz 0x%x p_memsz 0x%x\n",
//			segment->p_vaddr, segment->p_paddr, segment->p_filesz, segment->p_memsz);

		/* Map initialized portion */
		for (segmentOffset = 0;
			segmentOffset < ROUNDUP(segment->p_filesz, PAGE_SIZE);
			segmentOffset += PAGE_SIZE) {

			mmu_map_page(segment->p_vaddr + segmentOffset, *next_paddr);
			memcpy((void *)ROUNDOWN(segment->p_vaddr + segmentOffset, PAGE_SIZE),
				(void *)ROUNDOWN((unsigned)data + segment->p_offset + segmentOffset, PAGE_SIZE), PAGE_SIZE);
			(*next_paddr) += PAGE_SIZE;
		}

		/* Clean out the leftover part of the last page */
		if(segment->p_filesz % PAGE_SIZE > 0) {
//			dprintf("memsetting 0 to va 0x%x, size %d\n", (void*)((unsigned)segment->p_vaddr + segment->p_filesz), PAGE_SIZE  - (segment->p_filesz % PAGE_SIZE));
			memset((void*)((unsigned)segment->p_vaddr + segment->p_filesz), 0, PAGE_SIZE
				- (segment->p_filesz % PAGE_SIZE));
		}

		/* Map uninitialized portion */
		for (; segmentOffset < ROUNDUP(segment->p_memsz, PAGE_SIZE); segmentOffset += PAGE_SIZE) {
//			dprintf("mapping zero page at va 0x%x\n", segment->p_vaddr + segmentOffset);
			mmu_map_page(segment->p_vaddr + segmentOffset, *next_paddr);
			memset((void *)(segment->p_vaddr + segmentOffset), 0, PAGE_SIZE);
			(*next_paddr) += PAGE_SIZE;
		}
		switch(foundSegmentIndex) {
			case 0:
				ar0->start = segment->p_vaddr;
				ar0->size = segment->p_memsz;
				break;
			case 1:
				ar1->start = segment->p_vaddr;
				ar1->size = segment->p_memsz;
				break;
			default:
				;
		}
		foundSegmentIndex++;
	}
	*start_addr = imageHeader->e_entry;
}

// allocate a page directory and page table to facilitate mapping
// pages to the 0x80000000 - 0x80400000 region.
// also identity maps the first 8MB of memory
static int mmu_init(kernel_args *ka, unsigned int *next_paddr)
{
	int i;

	// allocate a new pgdir
	pgdir = (unsigned int *)*next_paddr;
	(*next_paddr) += PAGE_SIZE;
	ka->arch_args.phys_pgdir = (unsigned int)pgdir;

	// clear out the pgdir
	for(i = 0; i < 1024; i++)
		pgdir[i] = 0;

	// make a pagetable at this random spot
	pgtable = (unsigned int *)0x11000;

	for (i = 0; i < 1024; i++) {
		pgtable[i] = (i * 0x1000) | DEFAULT_PAGE_FLAGS;
	}

	pgdir[0] = (unsigned int)pgtable | DEFAULT_PAGE_FLAGS;

	// make another pagetable at this random spot
	pgtable = (unsigned int *)0x12000;

	for (i = 0; i < 1024; i++) {
		pgtable[i] = (i * 0x1000 + 0x400000) | DEFAULT_PAGE_FLAGS;
	}

	pgdir[1] = (unsigned int)pgtable | DEFAULT_PAGE_FLAGS;

	// Get new page table and clear it out
	pgtable = (unsigned int *)*next_paddr;
	ka->arch_args.pgtables[0] = (unsigned int)pgtable;
	ka->arch_args.num_pgtables = 1;

	(*next_paddr) += PAGE_SIZE;
	for (i = 0; i < 1024; i++)
		pgtable[i] = 0;

	// put the new page table into the page directory
	// this maps the kernel at KERNEL_BASE
	pgdir[KERNEL_BASE/(4*1024*1024)] = (unsigned int)pgtable | DEFAULT_PAGE_FLAGS;

	// switch to the new pgdir
	asm("movl %0, %%eax;"
		"movl %%eax, %%cr3;" :: "m" (pgdir) : "eax");
	// Important.  Make sure supervisor threads can fault on read only pages...
	asm("movl %%eax, %%cr0" : : "a" ((1 << 31) | (1 << 16) | (1 << 5) | 1));
		// pkx: moved the paging turn-on to here.

	return 0;
}

// can only map the 4 meg region right after KERNEL_BASE, may fix this later
// if need arises.
static void mmu_map_page(unsigned int vaddr, unsigned int paddr)
{
//	dprintf("mmu_map_page: vaddr 0x%x, paddr 0x%x\n", vaddr, paddr);
	if(vaddr < KERNEL_BASE || vaddr >= (KERNEL_BASE + 4096*1024)) {
		dprintf("mmu_map_page: asked to map invalid page!\n");
		for(;;);
	}
	paddr &= ~(PAGE_SIZE-1);
//	dprintf("paddr 0x%x @ index %d\n", paddr, (vaddr % (PAGE_SIZE * 1024)) / PAGE_SIZE);
	pgtable[(vaddr % (PAGE_SIZE * 1024)) / PAGE_SIZE] = paddr | DEFAULT_PAGE_FLAGS;
}

static int check_cpu(kernel_args *ka)
{
	unsigned int data[4];
	char str[17];

	// check the eflags register to see if the cpuid instruction exists
	if((get_eflags() & 1<<21) == 0) {
		set_eflags(get_eflags() | 1<<21);
		if((get_eflags() & 1<<21) == 0) {
			// we couldn't set the ID bit of the eflags register, this cpu is old
			return -1;
		}
	}

	// we can safely call cpuid

	// print some fun data
	cpuid(0, data);

	// build the vendor string
	memset(str, 0, sizeof(str));
	*(unsigned int *)&str[0] = data[1];
	*(unsigned int *)&str[4] = data[3];
	*(unsigned int *)&str[8] = data[2];

	// get the family, model, stepping
	cpuid(1, data);
	dprintf("CPU: family %d model %d stepping %d, string '%s'\n",
		(data[0] >> 8) & 0xf, (data[0] >> 4) & 0xf, data[0] & 0xf, str);

	// check for bits we need
	cpuid(1, data);
	if(data[3] & 1<<4) {
		ka->arch_args.supports_rdtsc = true;
	} else {
		ka->arch_args.supports_rdtsc = false;
		dprintf("CPU: does not support RDTSC, disabling high resolution timer\n");
	}

	return 0;
}

void sleep(uint64 time)
{
	uint64 start = system_time();

	while(system_time() - start <= time)
		;
}

static void sort_addr_range(addr_range *range, int count)
{
	addr_range temp_range;
	int i;
	bool done;

	do {
		done = true;
		for(i = 1; i < count; i++) {
			if(range[i].start < range[i-1].start) {
				done = false;
				memcpy(&temp_range, &range[i], sizeof(temp_range));
				memcpy(&range[i], &range[i-1], sizeof(temp_range));
				memcpy(&range[i-1], &temp_range, sizeof(temp_range));
			}
		}
	} while(!done);
}

#define outb(value,port) \
	asm("outb %%al,%%dx"::"a" (value),"d" (port))


#define inb(port) ({ \
	unsigned char _v; \
	asm volatile("inb %%dx,%%al":"=a" (_v):"d" (port)); \
	_v; \
	})

#define TIMER_CLKNUM_HZ 1193167

static void calculate_cpu_conversion_factor(void)
{
	unsigned       s_low, s_high;
	unsigned       low, high;
	unsigned long  expired;
	uint64         t1, t2;
	uint64         p1, p2, p3;
	double         r1, r2, r3;


	outb(0x34, 0x43);  /* program the timer to count down mode */
	outb(0xff, 0x40);		/* low and then high */
	outb(0xff, 0x40);

	/* quick sample */
quick_sample:
	do {
		outb(0x00, 0x43); /* latch counter value */
		s_low = inb(0x40);
		s_high = inb(0x40);
	} while(s_high!= 255);
	t1 = rdtsc();
	do {
		outb(0x00, 0x43); /* latch counter value */
		low = inb(0x40);
		high = inb(0x40);
	} while(high> 224);
	t2 = rdtsc();
	p1= t2-t1;
	r1= (double)(p1)/(double)(((s_high<<8)|s_low) - ((high<<8)|low));


	/* not so quick sample */
not_so_quick_sample:
	do {
		outb(0x00, 0x43); /* latch counter value */
		s_low = inb(0x40);
		s_high = inb(0x40);
	} while(s_high!= 255);
	t1 = rdtsc();
	do {
		outb(0x00, 0x43); /* latch counter value */
		low = inb(0x40);
		high = inb(0x40);
	} while(high> 192);
	t2 = rdtsc();
	p2= t2-t1;
	r2= (double)(p2)/(double)(((s_high<<8)|s_low) - ((high<<8)|low));
	if((r1/r2)> 1.01) {
		dprintf("Tuning loop(1)\n");
		goto quick_sample;
	}
	if((r1/r2)< 0.99) {
		dprintf("Tuning loop(1)\n");
		goto quick_sample;
	}

	/* slow sample */
	do {
		outb(0x00, 0x43); /* latch counter value */
		s_low = inb(0x40);
		s_high = inb(0x40);
	} while(s_high!= 255);
	t1 = rdtsc();
	do {
		outb(0x00, 0x43); /* latch counter value */
		low = inb(0x40);
		high = inb(0x40);
	} while(high> 128);
	t2 = rdtsc();
	p3= t2-t1;
	r3= (double)(p3)/(double)(((s_high<<8)|s_low) - ((high<<8)|low));
	if((r2/r3)> 1.01) {
		dprintf("Tuning loop(2)\n");
		goto not_so_quick_sample;
	}
	if((r2/r3)< 0.99) {
		dprintf("Tuning loop(2)\n");
		goto not_so_quick_sample;
	}

	expired = ((s_high<<8)|s_low) - ((high<<8)|low);
	p3*= TIMER_CLKNUM_HZ;

	/*
	 * cv_factor contains time in usecs per CPU cycle * 2^32
	 *
	 * The code below is a bit fancy. Originally Michael Noistering
	 * had it like:
	 *
	 *     cv_factor = ((uint64)1000000<<32) * expired / p3;
	 *
	 * whic is perfect, but unfortunately 1000000ULL<<32*expired
	 * may overflow in fast cpus with the long sampling period
	 * i put there for being as accurate as possible under
	 * vmware.
	 *
	 * The below calculation is based in that we are trying
	 * to calculate:
	 *
	 *     (C*expired)/p3 -> (C*(x0<<k + x1))/p3 ->
	 *     (C*(x0<<k))/p3 + (C*x1)/p3
	 *
	 * Now the term (C*(x0<<k))/p3 is rewritten as:
	 *
	 *     (C*(x0<<k))/p3 -> ((C*x0)/p3)<<k + reminder
	 *
	 * where reminder is:
	 *
	 *     floor((1<<k)*decimalPart((C*x0)/p3))
	 *
	 * which is approximated as:
	 *
	 *     floor((1<<k)*decimalPart(((C*x0)%p3)/p3)) ->
	 *     (((C*x0)%p3)<<k)/p3
	 *
	 * So the final expression is:
	 *
	 *     ((C*x0)/p3)<<k + (((C*x0)%p3)<<k)/p3 + (C*x1)/p3
	 *
	 * Just to make things fancier we choose k based on the input
	 * parameters (we use log2(expired)/3.)
	 *
	 * Of course, you are not expected to understand any of this.
	 */
	{
		unsigned i;
		unsigned k;
		uint64 C;
		uint64 x0;
		uint64 x1;
		uint64 a, b, c;

		/* first calculate k*/
		k= 0;
		for(i= 0; i< 32; i++) {
			if(expired & (1<<i)) {
				k= i;
			}
		}
		k/= 3;

		C = 1000000ULL<<32;
		x0= expired>> k;
		x1= expired&((1<<k)-1);

		a= ((C*x0)/p3)<<k;
		b= (((C*x0)%p3)<<k)/p3;
		c= (C*x1)/p3;
#if 0
		dprintf("a=%Ld\n", a);
		dprintf("b=%Ld\n", b);
		dprintf("c=%Ld\n", c);
		dprintf("%d %Ld\n", expired, p3);
#endif
		cv_factor= a + b + c;
#if 0
		dprintf("cvf=%Ld\n", cv_factor);
#endif
	}

	if(p3/expired/1000000000LL) {
		dprintf("CPU at %Ld.%03Ld GHz\n", p3/expired/1000000000LL, ((p3/expired)%1000000000LL)/1000000LL);
	} else {
		dprintf("CPU at %Ld.%03Ld MHz\n", p3/expired/1000000LL, ((p3/expired)%1000000LL)/1000LL);
	}
}

void clearscreen()
{
	int i;

	for(i=0; i< SCREEN_WIDTH*SCREEN_HEIGHT; i++) {
		kScreenBase[i] = 0xf20;
	}
}

static void scrup()
{
	int i;
	memcpy(kScreenBase, kScreenBase + SCREEN_WIDTH,
		SCREEN_WIDTH * SCREEN_HEIGHT * 2 - SCREEN_WIDTH * 2);
	screenOffset = (SCREEN_HEIGHT - 1) * SCREEN_WIDTH;
	for(i=0; i<SCREEN_WIDTH; i++)
		kScreenBase[screenOffset + i] = 0x0720;
}

int puts(const char *str)
{
	while (*str) {
		if (*str == '\n') {
			screenOffset += SCREEN_WIDTH - (screenOffset % 80);
		} else {
			kScreenBase[screenOffset++] = 0xf00 | *str;
		}
		if (screenOffset >= SCREEN_WIDTH * SCREEN_HEIGHT)
			scrup();

		str++;
	}
	return 0;
}

int dprintf(const char *fmt, ...)
{
	int ret;
	va_list args;
	char temp[256];

	va_start(args, fmt);
	ret = vsprintf(temp,fmt,args);
	va_end(args);

	puts(temp);
	return ret;
}