static void loop_handler(void * arg)
{
uint8_t i = (uint16_t)arg;
loop_t l; memcpy_P(&l, &loops[i], sizeof(loop_t));
timer_t prev = loop_next[i];
sch_add(l.func);
timer_t next = prev + l.period;
timer_add_cmp(next, (sch_t){ loop_handler, arg, (typeof(l.func.level))-1 });
assert(in_range(prev, timer_now(), next));
loop_next[i] = next;
}
void timer_int()
{
slot_t c = slot[first];
slot[first].next = MAX_TIMERS;
#ifndef NDEBUG
timer_t now = timer_now();
if (in_range(timer_tracked_get(), c.cmp, now)) {
DBG static timer_t timer_late_max;
timer_late_max = MAX(timer_late_max, now - c.cmp);
}
#endif
if (c.next == first) {
first = MAX_TIMERS;
timer_unset();
} else {
timer_tracked = c.cmp;
first = c.next;
timer_set(slot[first].cmp);
}
sch_add(c.func);
}
void cmd_create_thread(struct pm_msg_create_thread *msg, UINT16 creator_task_id)
{
struct thread mk_thread;
UINT16 new_thread_id;
struct pm_thread *curr_thr;
INT32 stack_slot = -1;
struct pm_task *task = tsk_get(msg->task_id);
struct pm_thread *thread = NULL;
ADDR pg = NULL;
if (task == NULL || task->state != TSK_NORMAL || (task->flags & TSK_SHARED_LIB) // task is not null, it's on normal state and it's not a shared library
|| (task->num_threads > 0 && creator_task_id != task->id) // only the owner task can create it's threads (other than the first one)
|| (task->num_threads == 0 && creator_task_id != task->creator_task) // only the creator task can create the first thread
|| (msg->interrupt != 0 && creator_task_id != task->id)) // only the owner task can create an interrupt thread
{
cmd_inform_result(msg, creator_task_id, PM_THREAD_FAILED, 0, 0);
return;
}
if (new_thread_id != 0xFFFF && thr_get(new_thread_id))
{
cmd_inform_result(msg, creator_task_id, PM_THREAD_ID_INVALID, 0, 0);
return;
}
new_thread_id = thr_get_id(0, MAX_THR);
if(new_thread_id == 0xFFFF)
{
cmd_inform_result(msg, creator_task_id, PM_THREAD_NO_ROOM, 0, 0);
return;
}
/* Ok, task id is fine and we got a free thread id. Let's get through with it. */
thread = thr_create(new_thread_id, task);
if(thread == NULL)
{
cmd_inform_result(msg, creator_task_id, PM_NOT_ENOUGH_MEM, 0, 0);
return;
}
/* save info */
thread->state = THR_WAITING;
/* Create microkernel thread */
mk_thread.task_num = msg->task_id;
mk_thread.invoke_mode = PRIV_LEVEL_ONLY;
mk_thread.invoke_level = 0;
mk_thread.ep = (ADDR)msg->entry_point;
thread->io_event_src.file_id = task->io_event_src.file_id;
thread->io_event_src.fs_service = task->io_event_src.fs_service;
thread->io_event_src.size = task->io_event_src.size;
/* Find a location for the thread stack. */
if(msg->stack_addr != NULL)
{
// see it it's a valid stack address
if(task->num_threads == 1 || (UINT32)msg->stack_addr > task->vmm_info.max_addr)
{
cmd_inform_result(msg, creator_task_id, PM_THREAD_INVALID_STACK, 0, 0);
return;
}
mk_thread.stack = thread->stack_addr = (ADDR)STACK_ADDR(msg->stack_addr);
}
else
{
/* See if a given slot is free, if not increment and test again... */
do
{
stack_slot++;
/* See if slot is taken */
curr_thr = task->first_thread;
while(curr_thr != NULL && (curr_thr->stack_addr != (ADDR)STACK_ADDR(PMAN_THREAD_STACK_BASE - stack_slot * 0x20000)))
{
curr_thr = curr_thr->next_thread;
}
}while(curr_thr != NULL);
mk_thread.stack = thread->stack_addr = (ADDR)STACK_ADDR(PMAN_THREAD_STACK_BASE - stack_slot * 0x20000);
}
/* Set thread entry point from elf file if 0 is specified.
If this is a dynamic task (i.e. uses shared libraries)
we will set the entry point to that of the ld service.
*/
if(msg->interrupt != 0 || task->num_threads > 1)
{
if(msg->entry_point == 0)
mk_thread.ep = (ADDR)task->loader_inf.elf_header.e_entry;
}
else
{
mk_thread.ep = loader_task_ep(task);
}
if(msg->interrupt != 0)
{
/* Only services will be allowed to handle interrupts. */
/* Check interrupt is not already being handled */
if(!(task->flags & TSK_FLAG_SERVICE) || !int_can_attach(thread, msg->interrupt))
{
thread->state = THR_KILLED;
thr_destroy_thread(thread->id);
cmd_inform_result(msg, creator_task_id, PM_THREAD_INT_TAKEN, 0, 0);
return;
}
}
if(create_thread(new_thread_id, &mk_thread))
{
thread->state = THR_KILLED;
thr_destroy_thread(thread->id);
cmd_inform_result(msg, creator_task_id, PM_THREAD_FAILED, 0, 0);
return;
}
/*
- Get a page for the stack if it's the first thread
so we can put init info for the task in there.
- Get a page for the stack if it's an interrupt handler
*/
if((task->num_threads == 1 && !(task->flags & TSK_FLAG_SYS_SERVICE)) || msg->interrupt != 0)
{
/* Lets see if the page table is present on the page directory and if not give it one */
if(task->vmm_info.page_directory->tables[PM_LINEAR_TO_DIR(((UINT32)thread->stack_addr + SARTORIS_PROCBASE_LINEAR))].ia32entry.present == 0)
{
/* Get a Page and set taken */
pg = vmm_get_tblpage(task->id, PG_ADDRESS(((UINT32)thread->stack_addr + SARTORIS_PROCBASE_LINEAR)));
/* Page in the table on task linear space. */
pm_page_in(task->id, (ADDR)PG_ADDRESS(((UINT32)thread->stack_addr + SARTORIS_PROCBASE_LINEAR)), (ADDR)LINEAR2PHYSICAL(pg), 1, PGATT_WRITE_ENA);
task->vmm_info.page_count++;
}
pg = vmm_get_page(thread->task_id, (UINT32)thread->stack_addr + SARTORIS_PROCBASE_LINEAR);
if(pg == NULL)
{
cmd_inform_result(msg, creator_task_id, PM_THREAD_FAILED, 0, 0);
thr_destroy_thread(thread->id);
return;
}
/* If it's a page of a system service, lock the page */
if(task->flags & TSK_FLAG_SYS_SERVICE)
vmm_set_flags(thread->task_id, pg, TRUE, TAKEN_EFLAG_SERVICE, TRUE);
pm_page_in(thread->task_id, (ADDR)((UINT32)thread->stack_addr + SARTORIS_PROCBASE_LINEAR), (ADDR)LINEAR2PHYSICAL(pg), 2, PGATT_WRITE_ENA);
// set init data at the begining of the stack
if(pg != NULL && msg->interrupt == 0)
{
UINT32 stackpad = ((UINT32)thread->stack_addr & 0x00000FFF);
UINT32 *size = (UINT32*)((UINT32)pg + stackpad);
if(task->flags & TSK_DYNAMIC)
{
pman_print_dbg("COMMAND: Dynamic task init creation. Thread EP: %x Task EP: %x\n", ((msg->entry_point == 0)? task->loader_inf.elf_header.e_entry : msg->entry_point), mk_thread.ep);
struct init_data_dl *idatd = (struct init_data_dl *)((UINT32)pg + stackpad - sizeof(struct init_data_dl));
idatd->ldexit = NULL;
idatd->creator_task = creator_task_id;
idatd->param = task->loader_inf.param;
idatd->bss_end = task->tsk_bss_end;
idatd->curr_limit = task->vmm_info.max_addr;
idatd->prg_start = ((msg->entry_point == 0)? (UINT32)task->loader_inf.elf_header.e_entry : (UINT32)msg->entry_point);
idatd->ld_dynamic = (PMAN_MAPPING_BASE + (UINT32)loader_lddynsec_addr());
idatd->ld_start = PMAN_MAPPING_BASE;
idatd->ld_size = ld_size; // comes from loader.h
idatd->phsmo = task->loader_inf.phdrs_smo;
idatd->phsize = task->loader_inf.elf_header.e_phentsize;
idatd->phcount = task->loader_inf.elf_header.e_phnum;
*size = sizeof(struct init_data_dl);
}
else
{
struct init_data *idat = (struct init_data *)((UINT32)pg + stackpad - sizeof(struct init_data));
idat->ldexit = NULL;
idat->creator_task = creator_task_id;
idat->param = task->loader_inf.param;
idat->bss_end = task->tsk_bss_end;
idat->curr_limit = task->vmm_info.max_addr;
*size = sizeof(struct init_data);
}
}
/* Page out from pman address space */
vmm_unmap_page(thread->task_id, (UINT32)thread->stack_addr + SARTORIS_PROCBASE_LINEAR);
task->vmm_info.page_count++;
}
/*
Protocol for PMAN thread creation does not support privileges,
so we assign the lowest value by default.
*/
thread->sch.priority = 0;
if(msg->interrupt != 0)
{
thread->state = THR_INTHNDL; // thread won't be scheduled
thread->interrupt = msg->interrupt;
if(!int_attach(thread, msg->interrupt, (0x000000FF & msg->int_priority)))
{
cmd_inform_result(msg, creator_task_id, PM_THREAD_FAILED, 0, 0);
thr_destroy_thread(thread->id);
return;
}
}
else
{
/* If it's task is being debugged,
init trace and let the debugger know
a new thread was created */
if(task->flags & TSK_DEBUG)
{
struct dbg_message dbg_msg;
dbg_msg.task = thread->task_id;
dbg_msg.thread = thread->id;
dbg_msg.command = DEBUG_THR_CREATED;
if(ttrace_begin(thread->id, task->dbg_task) == FAILURE)
{
dbg_msg.status = DEBUG_STATUS_DBG_FAILED;
send_msg(task->dbg_task, task->dbg_port, &dbg_msg);
}
else
{
dbg_msg.status = DEBUG_STATUS_OK;
send_msg(task->dbg_task, task->dbg_port, &dbg_msg);
}
}
/* Add thread to scheduler list */
sch_add(thread);
/* Begin scheduling! */
sch_activate(thread);
}
cmd_inform_result(msg, creator_task_id, PM_THREAD_OK, new_thread_id, msg->task_id);
}
void loops_start()
{
for (uint8_t i = 0; i < LOOP_NR; i++) sch_add((sch_t){ loop_handler, (void *)(uintptr_t)i, (uint8_t)-1 });
}
// slot_size MUST be a 4k multiple
// Creates a system service, based on the initfs image.
// Returns address of first page assigned to the task.
ADDR create_service(UINT16 task, UINT16 thread, INT32 invoke_level, UINT32 size, BOOL low_mem, BOOL load_all, char *image_name)
{
struct pm_task *ptask = NULL;
struct pm_thread *pthread = NULL;
UINT32 psize = 0, first_page, i = 0;
char *path = NULL;
struct vmm_page_table *ptbl = NULL;
struct thread mk_thread;
BOOL isld = FALSE;
if(strcmp(image_name,"ld"))
{
ld_task = task;
isld = TRUE;
}
while(image_name[psize] != '\0'){ psize++; }
path = kmalloc(psize);
if(path == NULL)
pman_print_and_stop("Could not allocate memory for path task: %s", image_name);
while(image_name[i] != '\0'){ path[i] = image_name[i]; i++; }
path[i] = '\0';
// Create a service task
ptask = tsk_create(task);
if(ptask == NULL)
pman_print_and_stop("Error allocating task for %s", image_name);
if(loader_create_task(ptask, path, psize, 0, 1, LOADER_CTASK_TYPE_SYS) != PM_OK)
pman_print_and_stop("Error creating task for %s", image_name);
/*
Create task gave us a page directory, the first page table, and initialized task structure as a service.
But since sysservice is TRUE, it did not begin fetching from FS.
*/
ptask->flags = 0;
ptask->flags |= TSK_FLAG_SYS_SERVICE;
if(low_mem)
ptask->flags |= TSK_LOW_MEM;
/* Setup the task */
ptask->creator_task = 0xFFFF;
ptask->creator_task_port = 0xFFFF;
ptask->command_inf.command_req_id = 0;
ptask->command_inf.command_sender_id = 0xFFFFFFFF;
/* Parse elf */
if(elf_begin(ptask, pminit_elf_read, pminit_elf_seek) == -1)
pman_print_and_stop("Elf parsing failed for %s", image_name);
/* Put pages for the Service */
UINT32 max_addr = put_pages(ptask, !load_all, low_mem, isld);
/* Get first page */
ptbl = (struct vmm_page_table*)PHYSICAL2LINEAR(PG_ADDRESS(ptask->vmm_info.page_directory->tables[PM_LINEAR_TO_DIR(SARTORIS_PROCBASE_LINEAR)].b));
first_page = PG_ADDRESS(ptbl->pages[PM_LINEAR_TO_TAB(SARTORIS_PROCBASE_LINEAR)].entry.phy_page_addr);
/* Setup first thread */
if(!isld)
{
pthread = thr_create(thread, ptask);
pthread->state = THR_WAITING;
/* Create microkernel thread */
mk_thread.task_num = task;
mk_thread.invoke_mode = PRIV_LEVEL_ONLY;
mk_thread.invoke_level = 0;
mk_thread.ep = (ADDR)ptask->loader_inf.elf_header.e_entry;
mk_thread.stack = pthread->stack_addr = (ADDR)STACK_ADDR(PMAN_THREAD_STACK_BASE);
if(create_thread(thread, &mk_thread))
pman_print_and_stop("Could not create thread for %s", image_name);
/* Schedule and activate thread */
sch_add(pthread);
sch_activate(pthread);
}
else
{
ld_size = max_addr;
ptask->vmm_info.max_addr = max_addr;
}
ptask->state = TSK_NORMAL;
return (ADDR)first_page;
}
