/**
* @brief Free all pbufs in the queue
* @param pQ the queue to be used
* @retval None
*/
static void netQEmpty(BufQueue * pQ)
{
struct pbuf * p;
int cnt = pQ->count;
for (; cnt > 0; cnt--)
{
p = (struct pbuf*)netQGet(pQ);
if (p) pbuf_free(p);
}
}
/* Pop a message off of the event queue and copy it to the passed in buffer. */
size_t netRecvEvent(Octet *buf, TimeInternal *time, NetPath *netPath)
{
int i, j;
int iPacketSize;
struct pbuf *p, *pcopy;
/* Attempt to get a buffer from the Q. If none is available,
* return length of 0. */
p = netQGet(&netPath->eventQ);
if(p == NULL)
return 0;
pcopy = p;
/* Here, p points to a valid PBUF structure. Verify that we have
* enough space to store the contents. */
if(p->tot_len > PACKET_SIZE) {
ERROR("received truncated message\n");
return 0;
}
/* Copy the PBUF payload into the buffer. */
j = 0;
iPacketSize = p->tot_len;
for(i = 0; i < iPacketSize; i++) {
buf[i] = ((u8_t *)pcopy->payload)[j++];
if(j == pcopy->len) {
pcopy = pcopy->next;
j = 0;
}
}
/* Get the timestamp information. */
time->seconds = p->time_s;
time->nanoseconds = p->time_ns;
/* Free up the pbuf (chain). */
pbuf_free(p);
/* Return the length of data copied. */
return iPacketSize;
}
static ssize_t netRecv(Octet *buf, TimeInternal *time, BufQueue * msgQueue)
{
ssize_t length;
int i, j;
uint8_t mes;
uint32_t seq;
TimeInternal const_delay;
/* get actual buffer */
struct pbuf * p, *pcopy;
const_delay.seconds = 0;
const_delay.nanoseconds = CONST_DELAY;
//Integer32 cur_t = current_t.seconds;
p = (struct pbuf*)netQGet(msgQueue);
if (!p)
{
return 0;
}
pcopy = p;
/* Here, p points to a valid PBUF structure. Verify that we have
* enough space to store the contents. */
if (p->tot_len > PACKET_SIZE)
{
ERROR("netRecv: received truncated message\n");
return 0;
}
/* if (NULL != time)
{
#if LWIP_PTP
getReceivTimestamp(DP83848_PHY_ADDRESS, mes, seq, &time->seconds, &time->nanoseconds);
#else
getTime(time);
#endif
}*/
/* Copy the PBUF payload into the buffer. */
j = 0;
length = p->tot_len;
for (i = 0; i < length; i++)
{
buf[i] = ((u8_t *)pcopy->payload)[j++];
if (j == pcopy->len)
{
pcopy = pcopy->next;
j = 0;
}
}
if (NULL != time && length > 20)
{
#if LWIP_PTP
/*time->seconds = buf[5];
time->seconds |= (cur_t & 0xFFFFFF00);
if(time->seconds > cur_t + 60)
time->seconds &= 0xFFFFFEFF;
time->nanoseconds = *((uint32_t*)buf + 4);
time->nanoseconds += 215;*/
mes = (*(Enumeration4*)(buf + 0)) & 0x0F;
seq = flip16(*(UInteger16*)(buf + 30));
__disable_irq ();
getReceivTimestamp(DP83848_PHY_ADDRESS, mes, seq, &time->seconds, &time->nanoseconds);
__enable_irq ();
subtime(time, &const_delay, time);
#else
getTime(time);
#endif
}
/* Free up the pbuf (chain). */
pbuf_free(p);
return length;
}
static ssize_t netRecv(octet_t *buf, TimeInternal *time, BufQueue *msgQueue)
{
int i;
int j;
u16_t length;
struct pbuf *p;
struct pbuf *pcopy;
/* Get the next buffer from the queue. */
if ((p = (struct pbuf*) netQGet(msgQueue)) == NULL)
{
return 0;
}
/* Verify that we have enough space to store the contents. */
if (p->tot_len > PACKET_SIZE)
{
ERROR("netRecv: received truncated message\n");
pbuf_free(p);
return 0;
}
/* Verify there is contents to copy. */
if (p->tot_len == 0)
{
ERROR("netRecv: received empty packet\n");
pbuf_free(p);
return 0;
}
if (time != NULL)
{
#if LWIP_PTP
time->seconds = p->time_sec;
time->nanoseconds = p->time_nsec;
#else
getTime(time);
#endif
}
/* Get the length of the buffer to copy. */
length = p->tot_len;
/* Copy the pbuf payload into the buffer. */
pcopy = p;
j = 0;
for (i = 0; i < length; i++)
{
// Copy the next byte in the payload.
buf[i] = ((u8_t *)pcopy->payload)[j++];
// Skip to the next buffer in the payload?
if (j == pcopy->len)
{
// Move to the next buffer.
pcopy = pcopy->next;
j = 0;
}
}
/* Free up the pbuf (chain). */
pbuf_free(p);
return length;
}
