void RunTask(void (*TaskName)(),u16 Time,u8 TaskId)
{
if(TIME_TASK[TaskId]>=Time)
{
TaskName();
TIME_TASK[TaskId]=0;
}
}
void lock_leave(lock_t *lock)
{
if (!lock->init) return;
check_lock();
//bool dbg = (strcmp(lock->name, "&waterfall_hw_lock") == 0);
bool dbg = false;
lock->leave++;
if (dbg) printf("LOCK t%d %s RELEASE %s\n", TaskID(), TaskName(), lock->name);
}
void lock_enter(lock_t *lock)
{
if (!lock->init) return;
check_lock();
int token = lock->enter++;
//bool dbg = (strcmp(lock->name, "&waterfall_hw_lock") == 0);
bool dbg = false;
bool waiting = false;
while (token > lock->leave) {
if (dbg && !waiting) {
printf("LOCK t%d %s WAIT %s\n", TaskID(), TaskName(), lock->name);
waiting = true;
}
NextTaskL("lock enter");
check_lock();
}
if (dbg) printf("LOCK t%d %s ACQUIRE %s\n", TaskID(), TaskName(), lock->name);
}
void ev(int event, const char *s, const char *s2)
{
int i;
ev_t *e = &evs[evc++];
u4_t now = timer_us();
int reset=0;
u64_t freeS2 = (u64_t) s2 & 1;
s2 = (char*) ((u64_t) s2 & ~1);
if (!triggered && (event == EV_TRIGGER)) {
epoch = last_time = now;
for (i=0; i<NEVT; i++) tlast[i] = now;
evc=0;
triggered=1;
if (freeS2) free((void*) s2);
return;
}
if (!triggered) {
if (freeS2) free((void*) s2);
return;
}
if (event < 0) {
event = -event;
tlast[event] = now;
reset=1;
}
e->event = event;
e->reset = reset;
e->s = s;
e->s2 = s2;
e->tseq = now - last_time;
e->tlast = now - tlast[event];
e->tepoch = now - epoch;
e->task = TaskName();
if (evc == NEV) {
for (i=0; i<NEV; i++) {
e = &evs[i];
printf("%4d %8s%c %7.3f %7.3f %9.3f %16s, %16s %s\n", i, evn[e->event], e->reset? '*':' ',
(float) e->tlast / 1000, (float) e->tseq / 1000, (float) e->tepoch / 1000,
e->task, e->s, e->s2);
}
exit(0);
}
last_time = now;
}
//---------------------------------------------------------
//---------------------------------------------------------
void CNPC_Roller::TaskFail( AI_TaskFailureCode_t code )
{
const Task_t *pTask = GetTask();
if( pTask )
{
Msg( "Failed to: %s\n", TaskName( pTask->iTask ) );
switch( pTask->iTask )
{
case TASK_WALK_PATH:
case TASK_RUN_PATH:
case TASK_GET_PATH_TO_ENEMY:
case TASK_GET_PATH_TO_ENEMY_LKP:
case TASK_GET_PATH_TO_HINTNODE:
m_iFail++;
break;
}
}
BaseClass::TaskFail( code );
}
static void spi_scan(SPI_MOSI *mosi, SPI_MISO *miso=&junk, int rbytes=0) {
int i;
assert(rbytes <= NSPI_RX);
int tx_bytes = sizeof(SPI_MOSI);
int tx_xfers = SPI_B2X(tx_bytes);
int rx_bytes = sizeof(miso->status) + rbytes;
int rx_xfers = SPI_B2X(rx_bytes);
int arx_xfers = MAX(tx_xfers, prev->len_xfers);
int arx_bytes = SPI_X2B(arx_xfers);
//jks
#if 0
static u4_t last_st;
static float acc_st, acc2_st;
static int big_rx;
if (arx_bytes > 1024) big_rx = 1;
u4_t st = timer_us();
float inc_st = (float) (st - last_st) / 1000.0;
acc_st += inc_st;
if (mosi->cmd == CmdSetWFFreq) acc2_st = 0;
printf("SCAN +%.3f %6.3f %6.3f %12s %16s tx(%dX=%dB) rx(%dX=%dB) arx(%dX=%dB)\n",
inc_st, acc_st, acc2_st, TaskName(), cmds[mosi->cmd], tx_xfers, tx_bytes, rx_xfers, rx_bytes, arx_xfers, arx_bytes);
if (mosi->cmd == CmdDuplex && big_rx) {
acc_st = 0;
big_rx = 0;
} else {
}
acc2_st += inc_st;
last_st = st;
#endif
//memset(miso, 0xee, sizeof(*miso));
miso->len_xfers = rx_xfers;
miso->cmd = mosi->cmd;
rx_xfers = arx_xfers;
if (mosi->cmd == CmdDuplex) mosi->cmd = CmdDummy;
if (mosi->cmd == CmdNoDuplex) mosi->cmd = CmdDummy;
for (;;) {
peri_spi(SPI_HOST,
mosi->msg, tx_xfers, // MOSI: new request
prev->msg, rx_xfers); // MISO: response to previous caller's request
evSpi(EV_SPILOOP, "spiScan", "peri_spi done");
// fixme: understand why is this needed (hangs w/o it)
if (spi_delay) spin_us(spi_delay); else usleep(10);
evSpi(EV_SPILOOP, "spiScan", "spin_us done");
if (prev->status != BUSY) break; // new request accepted?
evSpi(EV_SPILOOP, "spiScan", "BUSY -> NextTask");
NextTaskL("spi_scan"); // wait and try again
}
//jks
#if 0
printf("RX %d: ", prev->len_xfers);
if (prev == &junk) {
printf("(junk) ");
} else {
printf("%s ", cmds[prev->cmd]);
}
for (i=0; i<(prev->len_xfers+10); i++) {
printf("%d:", i);
#ifdef SPI_8
printf("%02x ", prev->msg[i]);
#endif
#ifdef SPI_16
printf("%04x ", prev->msg[i]);
#endif
#ifdef SPI_32
printf("%08x ", prev->msg[i]);
#endif
}
printf("\n");
#endif
u4_t status = prev->status;
prev = miso; // next caller collects this for us
pcmd=mosi->cmd;
//if (status & 0x0fff)
//printf("st %04x\n", status);
//if (mosi->cmd == CmdGetRXCount) printf("C");
//if (mosi->cmd == CmdGetRX) printf("GRX\n");
static int ff;
if (status & (1<<SPI_SFT)) {
rx0_wakeup = 1;
evSnd(EV_SND, "wakeup", "");
//printf(".");
ff = 0;
} else {
rx0_wakeup = 0;
if (!ff) {
//printf(".");
ff = 1;
}
}
// process rx channel wakeup bits
for (i=0; i<RX_CHANS; i++) {
u4_t ch = 1<<(i+SPI_SFT);
conn_t *c;
if (status & ch) {
c = &conns[i*2];
assert(c->type == STREAM_SOUND);
//if (c->task && !c->stop_data) printf("wakeup %d\n", c->task);
//if (c->task && !c->stop_data) { printf("%d:%d ", i, c->task); fflush(stdout); }
if (c->task && !c->stop_data) TaskWakeup(c->task, FALSE, 0);
}
}
}
CDebug::CDebug()
:CMsgQueueTask(TaskPrio(), TaskName())
{
}
void spi_dev(SPI_SEL sel, SPI_MOSI *mosi, int tx_xfers, SPI_MISO *miso, int rx_xfers)
{
int rx=0, tx=0;
u4_t stat;
SPI_T *txb = mosi->msg;
SPI_T *rxb = miso->msg;
assert(init);
if (sel == SPI_BOOT) {
GPIO_CLR_BIT(SPI0_CS1);
}
evSpiDev(EC_EVENT, EV_SPILOOP, -1, "spi_dev", evprintf("%s(%d) T%dx R%dx", (sel != SPI_HOST)? "BOOT" : cmds[mosi->data.cmd], mosi->data.cmd, tx_xfers, rx_xfers));
if (use_spidev) {
int spi_bytes = SPI_X2B(MAX(tx_xfers, rx_xfers));
struct spi_ioc_transfer spi_tr;
memset(&spi_tr, 0, sizeof spi_tr);
spi_tr.tx_buf = (unsigned long) txb;
spi_tr.rx_buf = (unsigned long) rxb;
spi_tr.len = spi_bytes;
spi_tr.delay_usecs = 0;
spi_tr.speed_hz = speed;
spi_tr.bits_per_word = SPI_BPW; // zero also means 8-bits?
spi_tr.cs_change = 0;
int actual_rxbytes;
if ((actual_rxbytes = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi_tr)) < 0) sys_panic("SPI_IOC_MESSAGE");
check(actual_rxbytes == spi_bytes);
if (actual_rxbytes != spi_bytes) printf("actual_rxbytes %d spi_bytes %d\n", actual_rxbytes, spi_bytes);
} else {
SPI0_CONF = FORCE | SPI_CONF(spi_clkg, spi_speed); // add FORCE
SPI0_CTRL = EN;
static char pbuf[65536];
char *p = pbuf;
int evp_spi;
#if defined(EV_MEAS_SPI_DEV) || defined(SPI_PUMP_CHECK)
evp_spi = (sel == SPI_HOST);
#else
evp_spi = 0;
#endif
int evp = evp_spi;
if (evp) p += sprintf(p, "T %p %p %d R %p %p %d ", mosi, txb, tx_xfers, miso, rxb, rx_xfers);
while (tx < tx_xfers) {
stat = SPI0_STAT;
if (!(stat & TX_FULL)) {
if (evp) p += sprintf(p, "TA%d:%x ", tx, txb[tx]);
SPI0_TX = txb[tx++]; // move data only if FIFOs are able to accept
}
if (!(stat & RX_EMPTY)) {
rxb[rx++] = SPI0_RX;
if (evp) p += sprintf(p, "RA%d:%x ", rx-1, rxb[rx-1]);
}
}
// at this point all tx data in FIFO but rx data may or may not have been moved out of FIFO
// if necessary send dummy tx words to force larger number of rx words to be pulled through
while (tx < rx_xfers) {
stat = SPI0_STAT;
if (!(stat & TX_FULL)) {
if (evp) p += sprintf(p, "TB%d:Z ", tx);
SPI0_TX = 0; tx++;
}
if (!(stat & RX_EMPTY)) {
rxb[rx++] = SPI0_RX;
if (evp) p += sprintf(p, "RB%d:%x ", rx-1, rxb[rx-1]);
}
if (tx == 8 && evp) {
p += sprintf(p, "... ");
evp = 0;
}
if (tx == (rx_xfers-3)) evp = evp_spi;
}
evp = evp_spi;
// handles the case of the FIFOs being slightly out-of-sync given that
// tx/rx words moved must eventually be equal
#ifdef SPI_32XXX
int spin=0, rfull=0;
while (rx < rx_xfers) {
stat = SPI0_STAT;
if (stat & RX_FULL) rfull++;
spin++;
if (!(stat & RX_EMPTY)) {
rxb[rx++] = SPI0_RX;
if (evp) p += sprintf(p, "RC%d:%x ", rx-1, rxb[rx-1]);
spin=0;
}
if ((rx == (rx_xfers-1)) && ((stat & (EOT|RXS)) == (EOT|RXS))) {
rxb[rx++] = SPI0_RX; // last doesn't show in fifo flags
if (evp) p += sprintf(p, "RL%d:%x ", rx-1, rxb[rx-1]);
}
if ((spin > 1024) && ((stat & (EOT|RXS)) == (EOT|RXS))) {
printf("%s STUCK: rfull %d rx %d rx_xfers %d stat %02x\n", TaskName(), rfull, rx, rx_xfers, stat);
while (rx < rx_xfers) {
rxb[rx++] = SPI0_RX;
}
break;
}
}
#else
while (rx < rx_xfers) {
stat = SPI0_STAT;
if (!(stat & RX_EMPTY)) {
rxb[rx++] = SPI0_RX;
if (evp) p += sprintf(p, "RC%d:%x ", rx-1, rxb[rx-1]);
}
if ((rx == (rx_xfers-1)) && ((stat & (EOT|RXS)) == (EOT|RXS))) {
rxb[rx++] = SPI0_RX; // last doesn't show in fifo flags
if (evp) p += sprintf(p, "RL%d:%x ", rx-1, rxb[rx-1]);
}
}
#endif
if (evp) evSpiDev(EC_EVENT, EV_SPILOOP, -1, "spi_dev", evprintf("%s", pbuf));
while ((SPI0_STAT & TX_EMPTY) == 0) // FIXME: needed for 6 MHz case, why?
;
while ((SPI0_STAT & EOT) == 0) // FIXME: really needed?
;
SPI0_CTRL = 0;
SPI0_CONF = SPI_CONF(spi_clkg, spi_speed); // remove FORCE
}
if (sel == SPI_BOOT) {
GPIO_SET_BIT(SPI0_CS1);
}
}
extern void AppBark(TN_ ## NR) (void); \ const TtaskDefinitionBlock PROGMEM tdb ## NR = TDB_DEF(NR,STACKOFFSETPARAMS(NR)) #define TASK_DEF_SHARED(NR) \ const Tbyte tcb ## NR[(sizeof(TtaskControlBlock) + SLOTDEPTHTCBEXTENSION(NR) + QEUETCBEXTENSION(NR)) ]; \ extern void AppInit(TN_ ## NR) (void); \ extern void AppLoop(TN_ ## NR) (void); \ extern void AppBark(TN_ ## NR) (void); \ const TtaskDefinitionBlock PROGMEM tdb ## NR = TDB_DEF(NR,STACKOFFSETPARAMSSHARED) #define QUEU_DEF(NR) \ const Tbyte Queu ## NR[QueuSize(LN_ ## NR)]; #if defNumberOfTasks > 0 #if cfgUseTasknames == cfgTrue const Tchar TaskNameStr00[] PROGMEM = TaskName(TN_00); #endif #if StackIsShared(TN_00) TASK_DEF_SHARED(00) #else TASK_DEF(00) #endif #endif #if defNumberOfTasks > 1 #if cfgUseTasknames == cfgTrue const Tchar TaskNameStr01[] PROGMEM = TaskName(TN_01); #endif #if StackIsShared(TN_01) TASK_DEF_SHARED(01) #else
