diff --git a/drivers/net/rckemac.c b/drivers/net/rckemac.c index 00c0437d..13a29bd7 100644 --- a/drivers/net/rckemac.c +++ b/drivers/net/rckemac.c @@ -290,131 +290,135 @@ static void rckemacif_input(struct netif* netif, struct pbuf* p) static void rckemacif_rx_handler(struct netif* netif, unsigned int write_offset) { rckemacif_t* rckemacif = netif->state; - unsigned short read_offset; + unsigned short read_offset = rckemacif->rx_read_offset; unsigned int counter; - volatile void *addr; - uint16_t i, length; + volatile void *addr = NULL; + uint16_t i, length = 0; + uint32_t packets = 0; struct pbuf *p; struct pbuf* q; if (write_offset > rckemacif->rx_buffer_max) { LWIP_DEBUGF(NETIF_DEBUG, ("Warning, write offset > buffer max!! (%d > %d)\n", write_offset, rckemacif->rx_buffer_max)); read_offset = 1; - write_emac(rckemacif->num_emac, EMAC_RX_CONTROL + EMAC_RX_BUFFER_READ_OFFSET, rckemacif->core, read_offset); - rckemacif->rx_read_offset = read_offset; - return; + goto rxDone; } - while(1) { - if ((write_offset != 0) && (rckemacif->rx_read_offset != write_offset)) { - read_offset = rckemacif->rx_read_offset; - read_offset++; - if (read_offset < 1 || read_offset > rckemacif->rx_buffer_max) { - read_offset = 1; - } - addr = rckemacif->rx_buffer + read_offset * 32; +again: + read_offset++; + if (read_offset < 1 || read_offset > rckemacif->rx_buffer_max) { + read_offset = 1; + } + addr = rckemacif->rx_buffer + read_offset * 32; - length = U16(addr); + length = U16(addr); - // Check for over/underflow - if ((length < 20) || (length > 1536)) { - LWIP_DEBUGF(NETIF_DEBUG, ("rckemacif_rx_handler(): illegal packet length %d => drop\n", length)); - LWIP_DEBUGF(NETIF_DEBUG, ("start read at %d; write_offset at %d; addr: %p, packet len: %d\n", read_offset, write_offset, addr, length)); + // Check for over/underflow + if ((length < 20) || (length > 1536)) { + LWIP_DEBUGF(NETIF_DEBUG, ("rckemacif_rx_handler(): illegal packet length %d => drop\n", length)); + LWIP_DEBUGF(NETIF_DEBUG, ("start read at %d; write_offset at %d; addr: %p, packet len: %d\n", read_offset, write_offset, addr, length)); - read_offset = write_offset; + read_offset = write_offset; #if 1 - kprintf("Buffer:\n"); - for (i = 0; i < 32; i++) { - kprintf("%2.2x ", ((char*)addr)[i] & 0xFF); - } - kprintf("\n"); + kprintf("Buffer:\n"); + for (i = 0; i < 32; i++) { + kprintf("%2.2x ", ((char*)addr)[i] & 0xFF); + } + kprintf("\n"); - kprintf("Buffer0:\n"); - for (i = 0; i < 32; i++) { - kprintf("%2.2x ", ((char*)rckemacif->rx_buffer)[i] & 0xFF); - } - kprintf("\n"); + kprintf("Buffer0:\n"); + for (i = 0; i < 32; i++) { + kprintf("%2.2x ", ((char*)rckemacif->rx_buffer)[i] & 0xFF); + } + kprintf("\n"); #endif - LINK_STATS_INC(link.memerr); - LINK_STATS_INC(link.drop); + LINK_STATS_INC(link.memerr); + LINK_STATS_INC(link.drop); - goto rxDone; + goto rxDone; + } + +#if ETH_PAD_SIZE + length += ETH_PAD_SIZE; /* allow room for Ethernet padding */ +#endif + //LWIP_DEBUGF(NETIF_DEBUG, ("length %u, read_offset %u, write_offset %u\n", length, read_offset, write_offset)); + + p = pbuf_alloc(PBUF_RAW, length, PBUF_POOL); + if (p) { +#if ETH_PAD_SIZE + pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ +#endif + + if (read_offset < write_offset) { + for (q=p, i/*counter=0*/; q!=NULL; q=q->next) { + memcpy((uint8_t*) q->payload, (uint8_t*)addr + 2, q->len); + //for(i=0; ilen; i++, counter++) { + // ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[2 + counter]; + //} } -#if ETH_PAD_SIZE - length += ETH_PAD_SIZE; /* allow room for Ethernet padding */ -#endif - //LWIP_DEBUGF(NETIF_DEBUG, ("length %u, read_offset %u, write_offset %u\n", length, read_offset, write_offset)); + read_offset += CLINE_PACKETS(p->len + 2) - 1; + } else { + int rest; + int bytesLeft = length; + int bytesToCopy = length; - p = pbuf_alloc(PBUF_RAW, length, PBUF_POOL); - if (p) { -#if ETH_PAD_SIZE - pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ -#endif - if (read_offset < write_offset) { - for (q=p, counter=0; q!=NULL; q=q->next) { - for(i=0; ilen; i++, counter++) { - ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[2 + counter]; - } - } + /* rest to the end of buffer - 2 bytes length information */ + rest = (rckemacif->rx_buffer_max - read_offset + 1) * 32 - 2; + if (length > rest) + bytesToCopy = rest; + LWIP_DEBUGF(NETIF_DEBUG, ("bytes to copy: %d, bytesLeft: %d\n", bytesToCopy, bytesLeft)); - read_offset += CLINE_PACKETS(p->len + 2) - 1; - } else { - int rest; - int bytesLeft = length; - int bytesToCopy = length; - - /* rest to the end of buffer - 2 bytes length information */ - rest = (rckemacif->rx_buffer_max - read_offset + 1) * 32 - 2; - if (length > rest) - bytesToCopy = rest; - LWIP_DEBUGF(NETIF_DEBUG, ("bytes to copy: %d, bytesLeft: %d\n", bytesToCopy, bytesLeft)); - - for (q=p, counter=0; q!=NULL; q=q->next) { - for(i=0; ilen; i++, counter++) { - if (counter < bytesToCopy) - ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[2 + counter]; - else - goto out; - } - } + for (q=p, counter=0; q!=NULL; q=q->next) { + for(i=0; ilen; i++, counter++) { + if (counter < bytesToCopy) + ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[2 + counter]; + else + goto out; + } + } out: - bytesLeft -= bytesToCopy; + bytesLeft -= bytesToCopy; - if (bytesLeft != 0) { - addr = rckemacif->rx_buffer + 0x20; - LWIP_DEBUGF(NETIF_DEBUG, ("copying from %p, left: %d (%x)\n", addr, bytesLeft, ((uint8_t*)addr)[0])); - for(counter=0; (ilen) && (counter < bytesLeft); i++, counter++) - ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[counter]; - for(q=q->next; (q!=NULL) && (counter < bytesLeft); q=q->next) { - for(i=0; (ilen) && (counter < bytesLeft); i++, counter++) { - ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[counter]; - } - } - read_offset = CLINE_PACKETS(bytesLeft); - } else { - read_offset += CLINE_PACKETS(p->len + 2) - 1; + if (bytesLeft != 0) { + addr = rckemacif->rx_buffer + 0x20; + LWIP_DEBUGF(NETIF_DEBUG, ("copying from %p, left: %d (%x)\n", addr, bytesLeft, ((uint8_t*)addr)[0])); + for(counter=0; (ilen) && (counter < bytesLeft); i++, counter++) + ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[counter]; + for(q=q->next; (q!=NULL) && (counter < bytesLeft); q=q->next) { + for(i=0; (ilen) && (counter < bytesLeft); i++, counter++) { + ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[counter]; } - } + } + read_offset = CLINE_PACKETS(bytesLeft); + } else { + read_offset += CLINE_PACKETS(p->len + 2) - 1; + } + } #if ETH_PAD_SIZE - pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ + pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ #endif - rckemacif_input(netif, p); - LINK_STATS_INC(link.recv); - } else { - LWIP_DEBUGF(NETIF_DEBUG, ("rckemacif_rx_inthandler: not enough memory!\n")); - LINK_STATS_INC(link.memerr); - LINK_STATS_INC(link.drop); - } + rckemacif_input(netif, p); + LINK_STATS_INC(link.recv); + } else { + LWIP_DEBUGF(NETIF_DEBUG, ("rckemacif_rx_inthandler: not enough memory!\n")); + LINK_STATS_INC(link.memerr); + LINK_STATS_INC(link.drop); + } + + packets++; rxDone: - /* set new read pointer */ - //LWIP_DEBUGF(NETIF_DEBUG, ("Update rx read offset: %d\n", read_offset)); - write_emac(rckemacif->num_emac, EMAC_RX_CONTROL + EMAC_RX_BUFFER_READ_OFFSET, rckemacif->core, read_offset); - rckemacif->rx_read_offset = read_offset; - } else break; + /* set new read pointer */ + //LWIP_DEBUGF(NETIF_DEBUG, ("Update rx read offset: %d\n", read_offset)); + write_emac(rckemacif->num_emac, EMAC_RX_CONTROL + EMAC_RX_BUFFER_READ_OFFSET, rckemacif->core, read_offset); + rckemacif->rx_read_offset = read_offset; + + if (read_offset != write_offset) { + if (packets < 5 /*max_num*/) + goto again; } }