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); }