void
tk_sleep(u_long ticks)
{
tk_cur->tk_flags &= ~TF_AWAKE; /* put task to sleep */
tk_cur->tk_flags |= TF_TIMER;
tk_cur->tk_waketick = TIME_ADD(CTICKS, ticks); /* set wake time */
TK_BLOCK();
tk_cur->tk_flags &= ~TF_TIMEOUT; /* clear timeout flag */
}
void
ping6_timer()
{
struct ping6 *ping;
struct ping6 *next;
LOCK_NET_RESOURCE(NET_RESID); /* protect ping6q list */
ping = ping6q;
while (ping)
{
next = ping->next;
/* see if session should be killed */
if ((ping->sent - ping->replies) > MAX_MISSING_PINGS)
{
ping6_done(ping);
ping = next;
continue;
}
/* see if it's time to ping again or time to give up*/
if (TIME_EXP(ping->nextping, CTICKS))
{
if (TIME_EXP(TIME_ADD(ping->last, ping->wait_time), CTICKS))
{
ping6_done(ping); /* Too long since last received ping */
ping = next;
continue;
}
UNLOCK_NET_RESOURCE(NET_RESID);
if (ping->sent < ping->count)
ping6_send(ping);
LOCK_NET_RESOURCE(NET_RESID); /* protect ping6q list */
}
ping = next;
}
UNLOCK_NET_RESOURCE(NET_RESID);
return;
}
int main(void) //(int argc, char *argv[])
{
printf("Bingo, enter 1st simulator DSP APP~!\n");
#if 0
u8 i, _i;
u32 cnt, next;
u32 msk, setmsk, clrmsk;
u32 delta, deltamin, tnext, hi, lo;
u32 *nextp;
const u32 *hilop;
u32 period;
u32 enmask; /* enable mask */
u32 stmask; /* state mask */
static u32 next_hi_lo[MAX_PWMS][3];
#endif
/*
* init Obj objects of each Chn.
*/
ChanelObj chnObj[MAX_PWMS];
u32 i = 0;
u32 currTime = 0;
//u32 sartRiseTime = 0;
//u32 startFallTime = 0;
for (i = 0; i < MAX_PWMS; i++)
{
chnObj[i].chid = i + 1;
chnObj[i].enmask = 0;
}
u32 prevTime = 0;
time64 currTs64;
while (1)
{
u32 index = 0;
//step 1 : update current time.
currTime = read_PIEP_COUNT();
if(prevTime > currTime)
{
// reverse
currTs64.time_p2++;
}
currTs64.time_p1 = currTime;
prevTime = currTime;
update_flag();
//step 2: judge current if it is arrive at rising edge time
for (index = 0; index < MAX_PWMS; index++)
{
//it is time that arriving rising edge........
if(TIME_GREATER(currTs64, chnObj[index].time_of_hi))
{
chnObj[index].enmask = PWM_CMD ->enmask;
chnObj[index].period_time.time_p1 = PWM_CMD ->periodhi[index][0];
// update time stamp
//chnObj[index].time_of_lo.time_p2 = 0;
//chnObj[index].time_of_lo.time_p1 = PWM_CMD ->periodhi[index][1];
// chnObj[index].time_of_lo = time_add(chnObj[index].time_of_lo, currTs64);
TIME_ADD(chnObj[index].time_of_lo, currTs64, PWM_CMD ->periodhi[index][1]);
//rising_edge_time = current + period
// chnObj[index].time_of_hi = time_add(chnObj[index].period_time, currTs64);
TIME_ADD(chnObj[index].time_of_hi, currTs64, PWM_CMD ->periodhi[index][0]);
if (chnObj[index].enmask & (1U << index))
{
// sartRiseTime = read_PIEP_COUNT();
//fall_edge_time = current + hi_duty
__R30 |= (1U << index); //pull up
printf("<high> chn: %d cur [%08x-%08x] r30 %x hi [%08x-%08x] lo [%08x-%08x] perid %d(high: %d) \n" ,chnObj[index].chid,
currTs64.time_p2, currTs64.time_p1, __R30,
chnObj[index].time_of_hi.time_p2, chnObj[index].time_of_hi.time_p1,
chnObj[index].time_of_lo.time_p2, chnObj[index].time_of_lo.time_p1,
time_sub(currTs64, ts[index][0]), PWM_CMD ->periodhi[index][1]);
ts[index][0] = currTs64;
}
continue;
}
//it is time that arriving falling edge........
if(TIME_GREATER(currTs64, chnObj[index].time_of_lo))
{
TIME_ADD(chnObj[index].time_of_lo, currTs64, PWM_CMD ->periodhi[index][0]);
if (chnObj[index].enmask & (1U << index))
{
__R30 &= ~(1U << index); //pull down
printf("<low> chn: %d cur [%08x-%08x] r30 %x hi [%08x-%08x] lo [%08x-%08x] high-len %d \n" ,chnObj[index].chid,
currTs64.time_p2, currTs64.time_p1, __R30,
chnObj[index].time_of_hi.time_p2, chnObj[index].time_of_hi.time_p1,
chnObj[index].time_of_lo.time_p2, chnObj[index].time_of_lo.time_p1,
time_sub(currTs64, ts[index][0]));
ts[index][1] = currTs64;
}
}
}
}
#if 0
//static struct cxt cxt;
#if 0
/* enable OCP master port */
PRUCFG_SYSCFG &= ~SYSCFG_STANDBY_INIT;
PRUCFG_SYSCFG = (PRUCFG_SYSCFG &
~(SYSCFG_IDLE_MODE_M | SYSCFG_STANDBY_MODE_M)) |
SYSCFG_IDLE_MODE_NO | SYSCFG_STANDBY_MODE_NO;
/* our PRU wins arbitration */
PRUCFG_SPP |= SPP_PRU1_PAD_HP_EN;
pwm_setup();
/* configure timer */
PIEP_GLOBAL_CFG = GLOBAL_CFG_DEFAULT_INC(1) |
GLOBAL_CFG_CMP_INC(1);
PIEP_CMP_STATUS = CMD_STATUS_CMP_HIT(1); /* clear the interrupt */
PIEP_CMP_CMP1 = 0x0;
PIEP_CMP_CFG |= CMP_CFG_CMP_EN(1);
PIEP_GLOBAL_CFG |= GLOBAL_CFG_CNT_ENABLE;
#endif
/* initialize */
cnt = read_PIEP_COUNT();
enmask = cfg.enmask;
stmask = 0; /* starting all low */
clrmsk = 0;
for (i = 0, msk = 1, nextp = &next_hi_lo[0][0], hilop = &cfg.hilo[0][0];
i < MAX_PWMS; i++, msk <<= 1, nextp += 3, hilop += 2)
{
if ((enmask & msk) == 0)
{
nextp[1] = PRU_us(100); /* default */
nextp[2] = PRU_us(100);
continue;
}
nextp[0] = cnt; /* next */
nextp[1] = 200000; /* hi */
nextp[2] = 208000; /* lo */
PWM_CMD ->periodhi[i][0] = 408000;
PWM_CMD ->periodhi[i][1] = 180000;
}
PWM_CMD ->enmask = 0;
clrmsk = enmask;
setmsk = 0;
/* guaranteed to be immediate */
deltamin = 0;
next = cnt + deltamin;
PWM_CMD ->magic = PWM_REPLY_MAGIC;
while (1)
{
update_flag();
if (PWM_CMD ->magic == PWM_CMD_MAGIC)
{
msk = PWM_CMD ->enmask;
for (i = 0, nextp = &next_hi_lo[0][0]; i < MAX_PWMS;
i++, nextp += 3)
{
//Enable
if ((PWM_EN_MASK & (msk & (1U << i)))
&& (enmask & (msk & (1U << i))) == 0)
{
enmask |= (msk & (1U << i));
__R30 |= (msk & (1U << i));
// first enable
if (enmask == (msk & (1U << i)))
cnt = read_PIEP_COUNT();
nextp[0] = cnt; //since we start high, wait this amount
deltamin = 0;
next = cnt;
}
//Disable
if ((PWM_EN_MASK & (msk & (1U << i)))
&& ((msk & ~(1U << i)) == 0))
{
enmask &= ~(1U << i);
__R30 &= ~(1U << i);
}
//get and set pwm_vals
if (PWM_EN_MASK & (msk & (1U << i)))
{
if (b_true == full_period)
{
//nextp = &next_hi_lo[i * 3];
nextp[1] = PWM_CMD ->periodhi[i][1];
period = PWM_CMD ->periodhi[i][0];
nextp[2] = period - nextp[1];
}
}
PWM_CMD ->hilo_read[i][0] = nextp[0];
PWM_CMD ->hilo_read[i][1] = nextp[1];
}
// guaranteed to be immediate
deltamin = 0;
PWM_CMD ->magic = PWM_REPLY_MAGIC;
}
PWM_CMD ->enmask_read = enmask;
/* if nothing is enabled just skip it all */
if (enmask == 0)
continue;
setmsk = 0;
clrmsk = (u32) -1;
deltamin = PRU_ms(100); /* (1U << 31) - 1; */
next = cnt + deltamin;
for (_i = 0; _i < MAX_PWMS; _i++)
{
if (enmask & (1U << (_i)))
{ //
nextp = &next_hi_lo[(_i)][0]; //
tnext = nextp[0]; //
hi = nextp[1]; //
lo = nextp[2]; //
/* avoid signed arithmetic */ //
while (((delta = (tnext - cnt)) & (1U << 31)) != 0)
{ //
/* toggle the state */ //
if (stmask & (1U << (_i)))
{ //
stmask &= ~(1U << (_i)); //
clrmsk &= ~(1U << (_i)); //
tnext += lo; //
printf("CH %d: next hi: %08x\n", _i, tnext);
// flag when all motor channel finish full period
ch_num_period++;
if (!((ch_num_period) %= MAX_PWMS))
{
full_period = b_true;
printf(
"----------------full period--------------\n");
}
}
else
{ //
stmask |= (1U << (_i)); //
setmsk |= (1U << (_i)); //
tnext += hi; //
full_period = b_false;
//if(flag == Period_change)
} //
} //
if (delta <= deltamin)
{ //
deltamin = delta; //
next = tnext; //
} //
nextp[0] = tnext; //
} //
}
#if 0
#define SINGLE_PWM(_i) \
do { \
if (enmask & (1U << (_i))) { \
nextp = &next_hi_lo[(_i)][0]; \
tnext = nextp[0]; \
hi = nextp[1]; \
lo = nextp[2]; \
/* avoid signed arithmetic */ \
while (((delta = (tnext - cnt)) & (1U << 31)) != 0) { \
/* toggle the state */ \
if (stmask & (1U << (_i))) { \
stmask &= ~(1U << (_i)); \
clrmsk &= ~(1U << (_i)); \
tnext += lo; \
} else { \
stmask |= (1U << (_i)); \
setmsk |= (1U << (_i)); \
tnext += hi; \
} \
} \
if (delta <= deltamin) { \
deltamin = delta; \
next = tnext; \
} \
nextp[0] = tnext; \
} \
} while (0)
#if MAX_PWMS > 0 && (PWM_EN_MASK & BIT(0))
SINGLE_PWM(0);
#endif
#if MAX_PWMS > 1 && (PWM_EN_MASK & BIT(1))
SINGLE_PWM(1);
#endif
#if MAX_PWMS > 2 && (PWM_EN_MASK & BIT(2))
SINGLE_PWM(2);
#endif
#if MAX_PWMS > 3 && (PWM_EN_MASK & BIT(3))
SINGLE_PWM(3);
#endif
#if MAX_PWMS > 4 && (PWM_EN_MASK & BIT(4))
SINGLE_PWM(4);
#endif
#if MAX_PWMS > 5 && (PWM_EN_MASK & BIT(5))
SINGLE_PWM(5);
#endif
#if MAX_PWMS > 6 && (PWM_EN_MASK & BIT(6))
SINGLE_PWM(6);
#endif
#if MAX_PWMS > 7 && (PWM_EN_MASK & BIT(7))
SINGLE_PWM(7);
#endif
#if MAX_PWMS > 8 && (PWM_EN_MASK & BIT(8))
SINGLE_PWM(8);
#endif
#if MAX_PWMS > 9 && (PWM_EN_MASK & BIT(9))
SINGLE_PWM(9);
#endif
#if MAX_PWMS > 10 && (PWM_EN_MASK & BIT(10))
SINGLE_PWM(10);
#endif
#if MAX_PWMS > 11 && (PWM_EN_MASK & BIT(11))
SINGLE_PWM(11);
#endif
#if MAX_PWMS > 12 && (PWM_EN_MASK & BIT(12))
SINGLE_PWM(12);
#endif
#endif
/* results in set bits where there are changes */
__R30 = (__R30 & (clrmsk & 0xfff)) | (setmsk & 0xfff);
/* loop while nothing changes */
do
{
cnt = read_PIEP_COUNT();
//if(PWM_CMD->magic == PWM_CMD_MAGIC){
// break;
//}
} while (((next - cnt) & (1U << 31)) == 0);
}
#endif
printf("Bingo, leave 1st simulator DSP APP~!\n");
}
/* FUNCTION: tk_mutex_pend()
*
* Wait for the availability of a mutex.
*
* PARAM1: IN_MUTEX * mutex
* PARAM2: int32_t > 0 == wait time (ticks)
* 0 == wait forever
* < 0 == don't wait
*
* RETURN: int SUCCESS or error code
*/
int
tk_mutex_pend(IN_MUTEX *mutex, int32_t timeout)
{
int err = SUCCESS;
if (!mutex)
{
dtrap();
}
else
{
ENTER_CRIT_SECTION();
#ifdef DEBUG_TASK
if (mutex->tk_tag != MUTEX_TAG)
dtrap();
#endif
if (mutex->tk_owner == (TASK *)NULL) /* mutex is available */
{
mutex->tk_owner = tk_cur;
mutex->tk_nesting = 1;
}
else if (mutex->tk_owner == tk_cur) /* already own it */
{
mutex->tk_nesting++;
}
else if (timeout > 0) /* wait until mutex is available */
{
/* append to the end of the wait queue */
if (mutex->tk_waitq == (TASK *)NULL)
mutex->tk_waitq = tk_cur;
else
{
TASK *task = mutex->tk_waitq;
#ifdef DEBUG_TASK
if ((task == tk_cur) || (tk_cur->tk_waitq != (TASK *)NULL))
dtrap();
#endif
while (task->tk_waitq)
{
#ifdef DEBUG_TASK
if (task == tk_cur)
dtrap();
#endif
task = task->tk_waitq;
}
task->tk_waitq = tk_cur;
}
tk_cur->tk_event = (void *)mutex;
tk_cur->tk_waitq = (TASK *)NULL;
tk_cur->tk_flags |= TF_MUTEX;
if (timeout != INFINITE_DELAY)
{
tk_cur->tk_waketick = TIME_ADD(CTICKS, timeout);
tk_cur->tk_flags |= TF_TIMER;
}
/* wait for something to happen */
EXIT_CRIT_SECTION();
TK_SUSPEND(TK_THIS);
ENTER_CRIT_SECTION();
/* clean up and continue */
err = (tk_cur->tk_flags & TF_TIMEOUT) ? TK_TIMEOUT : 0;
tk_cur->tk_flags &= ~(TF_MUTEX | TF_TIMER | TF_TIMEOUT);
tk_cur->tk_waitq = (TASK *)NULL;
tk_cur->tk_event = NULL;
tk_cur->tk_waketick = 0;
}
else /* not availiable and not waiting */
{
err = TK_TIMEOUT;
}
EXIT_CRIT_SECTION();
}
return (err);
}
/* FUNCTION: tk_sem_pend()
*
* If the semaphore count is greater than 0, decrement the semaphore
* count and continue. Otherwise, add the task to the end of the
* semaphore wait queue, and mark the task "waiting".
*
* PARAM1: TASK * task (NULL == current task)
* PARAM1: IN_SEM * semaphore (NULL == task's semaphore)
* PARAM3: int32_t > 0 == wait time (ticks)
* 0 == wait forever
* < 0 == don't wait
*
* RETURN: int SUCCESS or error code
*/
int
tk_sem_pend(TASK *task, IN_SEM *sem, int32_t timeout)
{
int err = SUCCESS;
ENTER_CRIT_SECTION();
if (task == (TASK *)NULL)
task = tk_cur; /* default to current task */
if (sem == (IN_SEM *)NULL)
sem = task->tk_semaphore;
TK_VALIDATE(task);
if (!sem)
{
dtrap(); /* no semaphore! */
}
else
{
#ifdef DEBUG_TASK
if ((sem->tk_tag != SEMA_TAG) || (task->tk_waitq))
dtrap();
#endif
if (sem->tk_count > 0) /* semaphore is available */
{
sem->tk_count--;
}
else if (timeout > 0) /* wait until semaphore is available */
{
TASK *tk;
/* append task onto the end of the wait queue */
if ((tk = sem->tk_waitq) == (TASK *)NULL)
{
sem->tk_waitq = task;
}
else
{
#ifdef DEBUG_TASK
if (tk == task)
dtrap();
#endif
while (tk->tk_waitq)
{
#ifdef DEBUG_TASK
if (tk == task)
dtrap();
#endif
tk = task->tk_waitq;
}
tk->tk_waitq = task;
}
task->tk_event = (void *)sem;
task->tk_waitq = (TASK *)NULL;
task->tk_flags |= TF_SEMAPHORE;
if (timeout != INFINITE_DELAY)
{
task->tk_waketick = TIME_ADD(CTICKS, timeout);
task->tk_flags |= TF_TIMER;
}
/* wait for something to happen */
EXIT_CRIT_SECTION();
TK_SUSPEND(TK_THIS);
ENTER_CRIT_SECTION();
/* clean up and continue */
err = (task->tk_flags & TF_TIMEOUT) ? TK_TIMEOUT : 0;
task->tk_flags &= ~(TF_SEMAPHORE | TF_TIMER | TF_TIMEOUT);
task->tk_waitq = (TASK *)NULL;
task->tk_event = NULL;
task->tk_waketick = 0;
}
else /* not availiable and not waiting */
{
err = TK_TIMEOUT;
}
}
EXIT_CRIT_SECTION();
return (err);
}
int
ping6_send(struct ping6 *ping)
{
PACKET pkt;
struct icmp6req *pinghdr;
char addrbuf[40]; /* for printf()ing */
int plen = sizeof(struct icmp6req) + ping->length;
int err = 0;
int sendflags;
int bytesleft;
int offset;
/* try for a pkt chain */
LOCK_NET_RESOURCE(FREEQ_RESID);
PK_ALLOC(pkt, plen + MaxLnh + sizeof(struct ipv6));
UNLOCK_NET_RESOURCE(FREEQ_RESID);
if (pkt == NULL)
{
ping->count = 0; /* mark session for deletion */
return (ENP_NOBUFFER);
}
pkt->flags = 0;
/* prepare for cb_read */
pkt->nb_prot = pkt->nb_buff + MaxLnh + sizeof(struct ipv6);
/* got chain? */
if (pkt->pk_next == NULL)
pkt->nb_plen = plen; /* no */
else
pkt->nb_plen = pkt->nb_blen - MaxLnh - sizeof(struct ipv6); /* yes */
/* Advance to point where we write the data */
offset = sizeof(struct icmp6req);
pkt->nb_tlen = offset;
bytesleft = ping->length;
while (bytesleft > PINGSTRSIZE)
{
err = cb_read(pkt, offset, (uint8_t *)pingdata6, PINGSTRSIZE);
if (err < 0)
break;
offset += err;
bytesleft -= err;
}
if (bytesleft && (err >= 0))
err = cb_read(pkt, offset, (uint8_t *)pingdata6, bytesleft);
/* read in data - user or standard? */
/* got err? */
if (err <= 0)
{
LOCK_NET_RESOURCE(FREEQ_RESID);
PK_FREE(pkt);
UNLOCK_NET_RESOURCE(FREEQ_RESID);
ping->count = 0; /* mark session for deletion */
return (ENP_NOBUFFER);
}
#ifdef IP6_ROUTING
/* Put scopeID in pkt */
pkt->soxopts = npalloc(sizeof(struct ip_socopts));
if (pkt->soxopts == NULL)
{
LOCK_NET_RESOURCE(FREEQ_RESID);
pk_free(pkt);
UNLOCK_NET_RESOURCE(FREEQ_RESID);
ping->count = 0; /* mark session for deletion */
return (ENP_NOBUFFER);
}
pkt->soxopts->ip_scopeid = ping->scopeID;
#endif
pinghdr = (struct icmp6req *)pkt->nb_prot;
pinghdr->code = 0;
pinghdr->type = ICMP6_ECHOREQ;
pinghdr->id = ping->sess_id;
pinghdr->sequence = (htons((unshort)ping->sent));
ping->sent++;
pkt->net = ping->net;
pkt->type = htons(0x86dd);
/* multicast ping? */
if (ping->fhost.addr[0] == 0xFF)
{
pkt->flags |= PKF_MCAST; /* send mac multicast */
sendflags = 0; /* no routing */
}
else
{
pkt->flags &= ~PKF_MCAST; /* send mac unicast */
/* see if we can skip the routing step */
if(pkt->net && (!IP6EQ(&ping->nexthop, &ip6unspecified)))
{
pkt->nexthop = &ping->nexthop; /* set next hop */
sendflags = IP6F_NOROUTE;
}
else
sendflags = IP6F_ALL;
}
/* is the scope global? */
if ( (ping->fhost.addr[0] == 0xFF) && ((ping->fhost.addr[1] & 0xF) == 0xE) )
{
/* yup - it's a global ping */
pkt->flags |= PKF_IPV6_GBL_PING; /* global ping */
}
/* loopback? */
if (IN6_IS_ADDR_LOOPBACK((struct in6_addr *)&(ping->fhost.addr)))
IP6CPY((struct in6_addr *)&(ping->lhost.addr),
(struct in6_addr *)&(ping->fhost.addr)); /* both are loopback */
/* put prot at IPv6 hdr */
pkt->nb_prot -= sizeof(struct ipv6);
pkt->nb_plen += sizeof(struct ipv6);
pkt->nb_tlen += sizeof(struct ipv6);
/* set time for next ping */
ping->nextping = TIME_ADD(CTICKS, ping->ping6_interval);
err = icmp6_send(pkt, &ping->lhost, &ping->fhost, sendflags);
pkt->net->icmp6_ifmib.OutEchos++;
if (err < 0)
{
/* Don't record gio error, since we're going to kill this
* ping session anyway.
*/
gio_printf(ping->gio, "error %d sending ping; sess %d, seq:%d\n",
err, ntohs(ping->sess_id), ping->sent);
ping->count = 0; /* mark session for deletion by timer */
}
else if ((err == 1) || (err == 0))
{
err = gio_printf(ping->gio, "Sent ping; sess: %d, Seq: %d to %s\n",
ntohs(ping->sess_id), ping->sent,
print_ip6(&ping->fhost, addrbuf));
}
return (0);
}
