symb_data symb_typeUp(symb_data S, int del,int type)
{
symb_data ans={{NULL},errortp};
if(S.type==errortp || type<S.type) return ans;
if(S.type==type){ if(del) ans=S; else ans=symb_copy(S); return ans; }
switch(S.type)
{ case vectortp: case indextp: case spintp: case etenstp: return ans; }
switch(type)
{
case vectortp :
case indextp : return ans;
case polytp : if(del) ans=S; else ans=symb_copy(S);
ans.type=polytp;
return ans;
case tenstp : ans.expr.t=newtensor1();
multtenspoly(&(ans.expr.t),S.expr.p); break;
case spintp : ans.expr.s=spin1();
if(S.type==tenstp) multSpinTens(&ans.expr.s,S.expr.t);
else multSpinPoly(&ans.expr.s,S.expr.p);
break;
case etenstp : ans.expr.et=etens1();
if(S.type==tenstp) multEtensTens(&ans.expr.et,S.expr.t);
else multEtensPoly(&ans.expr.et,S.expr.p);
break;
}
ans.type=type;
if(del) symb_clean(S);
return ans;
}
void sys_thread_switch(l4_threadid_t tid)
{
TRACEPOINT_1PAR(SYS_THREAD_SWITCH, tid.raw);
#if defined (CONFIG_DEBUG_TRACE_SYSCALLS)
if (tid == L4_NIL_ID)
spin1(75);
else
printf("sys_thread_switch(tid: %x)\n", tid);
#endif
/* Make sure we are in the ready queue to
* find at least ourself and ensure that the thread
* is rescheduled */
thread_enqueue_ready(get_current_tcb());
tcb_t * tcb = tid_to_tcb(tid);
if (!(!l4_is_nil_id(tid) && (tcb->myself == tid ) && (IS_RUNNING(tcb))))
tcb = find_next_thread();
/* do dispatch only if necessary */
if (tcb != get_current_tcb())
dispatch_thread(tcb);
return_thread_switch();
}
dword_t cpu_mailbox_t::send_command(dword_t cpu, dword_t command, dword_t value)
{
int count = MAILBOX_TIMEOUT;
m_status = value;
m_command = command;
/* signal the request */
cpu_mailbox[cpu].set_request(get_cpu_id());
/* send command IPI to the cpu */
send_command_ipi(cpu);
spin1(71);
/* and now wait for the cpu to handle the reuqest */
while (m_status == value) {
spin1(70);
if (count-- == 0)
{
IPI_PRINTF("mailbox::send_command timeout - resend IPI to %d\n", cpu);
enter_kdebug("timeout");
send_command_ipi(cpu);
count = MAILBOX_TIMEOUT;
}
/* allow incoming requests from other cpus */
if (pending_requests)
{
IPI_PRINTF("mailbox::send_command incoming request\n");
handle_pending_requests();
continue;
}
__asm__ __volatile__ (".byte 0xf3; .byte 0x90\n");
}
return m_status;
}
/* this function is called from the hardware dependend
* interrupt service routine for a timer tick
*/
void handle_timer_interrupt()
{
#if (defined(CONFIG_DEBUGGER_KDB) || defined(CONFIG_DEBUGGER_GDB)) && defined(CONFIG_DEBUG_BREAKIN)
kdebug_check_breakin();
#endif
TRACEPOINT(HANDLE_TIMER_INTERRUPT,
printf("timer interrupt (curr=%p)\n", get_current_tcb()));
#if defined(CONFIG_SMP)
/* only cpu 0 maintains the clock */
if (get_cpu_id() == 0)
#endif
kernel_info_page.clock += TIME_QUANTUM;
tcb_t * current = get_current_tcb();
tcb_t * wakeup = parse_wakeup_queue(current->priority, kernel_info_page.clock);
current->current_timeslice -= TIME_QUANTUM;
/* make sure runable threads are in the run queue */
if (current != get_idle_tcb())
thread_enqueue_ready(current);
if (wakeup)
{
dispatch_thread(wakeup);
return;
}
if (current->current_timeslice <= 0)
{
spin1(78);
current->current_timeslice = current->timeslice;
dispatch_thread(find_next_thread());
}
}
/* central inter-cpu command handler */
void smp_handle_request(dword_t from_cpu)
{
dword_t current_cpu = get_cpu_id();
cpu_mailbox_t * mailbox = &cpu_mailbox[from_cpu];
if (mailbox->get_command() > SMP_CMD_IPC_RECEIVE)
IPI_PRINTF("smp-ipi command %d from %d\n", mailbox->get_command(), from_cpu);
spin1(72);
if (current_cpu == from_cpu) {
enter_kdebug("self command ipi???");
return;
}
//IPI_PRINTF("smp-ipi command %x from %d\n", mailbox->get_command(), from_cpu);
switch(mailbox->get_command()) {
case SMP_CMD_IPC_SHORT:
do_xcpu_ipc_short(mailbox);
break;
case SMP_CMD_IPC_START:
do_xcpu_ipc_start(mailbox);
break;
case SMP_CMD_IPC_END:
do_xcpu_ipc_end(mailbox);
break;
case SMP_CMD_IPC_RECEIVE:
do_xcpu_ipc_receive(mailbox);
break;
case SMP_CMD_THREAD_GET:
do_xcpu_thread_get(mailbox);
break;
case SMP_CMD_THREAD_PUT:
do_xcpu_thread_put(mailbox);
break;
case SMP_CMD_THREAD_EX_REGS:
do_xcpu_thread_ex_regs(mailbox);
break;
case SMP_CMD_UNWIND:
do_xcpu_unwind(mailbox);
break;
case SMP_CMD_DELETE_ALL_THREADS:
do_xcpu_delete_all_threads(mailbox);
break;
case SMP_CMD_FLUSH_TLB:
do_xcpu_flush_tlb(mailbox);
break;
default:
printf("cpu%d: unhandled command from cpu %d: cmd: %x, tcb: %p\n",
get_cpu_id(), from_cpu, mailbox->get_command(), mailbox->tcb);
}
}
void autonomous_routine1(void)
{
unsigned int sonar_distance = 0;
static unsigned long elapsed_time,
old_time = 0,
start_time;
controller_begin_autonomous_mode();
elapsed_time = start_time = SYSTEM_TIMER_SECONDS();
/*
* An autonomous loop with sensor input. Loop terminates when
* a button sensor on port 5 is pressed, or the 20 second time
* period runs out.
*/
forward1(MOTOR_POWER1);
while ( (elapsed_time - start_time) < ROUTINE1_DURATION )
{
sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
if ( (sonar_distance < ROUTINE1_SONAR_DISTANCE) ||
(io_read_digital(BUMPER_LEFT_PORT) == 0) ||
(io_read_digital(BUMPER_RIGHT_PORT) == 0) )
{
reverse1(MOTOR_POWER1);
delay_msec(ROUTINE1_BACKUP_MS);
sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
spin1(MOTOR_POWER1, ROUTINE1_SPIN_MS);
forward1(MOTOR_POWER1);
sonar_distance = sonar_read(SONAR_INTERRUPT_PORT);
}
elapsed_time = SYSTEM_TIMER_MS();
/* Adjust sonar direction every 40 ms */
if ( elapsed_time % ROUTINE1_SONAR_SERVO_DELAY == 0 )
sonar_scan(SONAR_SERVO_PORT);
/* Produce debug output once per second */
elapsed_time /= MS_PER_SEC;
if ( elapsed_time > old_time )
{
old_time = elapsed_time;
/*
if ( rc_new_data_available() )
{
DPRINTF("ET: %ld RC: %x A[1,2]: %x %x "
"D[5,6]: %d %d Shaft I[%d,%d]: %d %d Sonar[%d]: %d\n",
elapsed_time - start_time, rc_read_status(),
io_read_analog(1), io_read_analog(2),
io_read_digital(5), io_read_digital(6),
LEFT_ENCODER_INTERRUPT_PORT, RIGHT_ENCODER_INTERRUPT_PORT,
shaft_encoder_read_std(LEFT_ENCODER_INTERRUPT_PORT),
shaft_encoder_read_std(RIGHT_ENCODER_INTERRUPT_PORT),
SONAR_INTERRUPT_PORT, sonar_distance);
}
*/
}
}
pwm_write(RIGHT_DRIVE_PORT, MOTOR_STOP);
pwm_write(LEFT_DRIVE_PORT, MOTOR_STOP);
controller_submit_data(NO_WAIT);
controller_end_autonomous_mode();
}
