diff --git a/drivers/net/rckemac.c b/drivers/net/rckemac.c index 97f65226..d3188e66 100644 --- a/drivers/net/rckemac.c +++ b/drivers/net/rckemac.c @@ -108,15 +108,67 @@ #define MAC_HI(_x) ((((_x) >> 32)) & 0xFFFF) #define MAC_LO(_x) (((_x) ) & 0xFFFFFFFF) +#define MIN(a, b) (a) < (b) ? (a) : (b) + static struct netif* mynetif; -#if 0 -inline static void *memcpy_noprefetching(void* dest, const void *src, size_t count) +inline static void* memcpy_from_nc(void *dest, const void *src, size_t count) { - int32_t i, j, k; +#if 0 + size_t i; - if (BUILTIN_EXPECT(!dest || !src, 0)) - return dest; + for(i=0; inext) { - memcpy(((uint8_t*)addr) + 2 + i, q->payload, q->len); + memcpy_to_nc(((uint8_t*)addr) + 2 + i, q->payload, q->len); i += q->len; } @@ -224,12 +276,12 @@ again: if (bytes_left < bytes_to_copy) bytes_to_copy = bytes_left; - LWIP_DEBUGF(NETIF_DEBUG, ("special case: copy last %d bytes\n", bytes_to_copy)); + //LWIP_DEBUGF(NETIF_DEBUG, ("special case: copy last %d bytes\n", bytes_to_copy)); q = p; i = 0; while ((q != 0) && (i < bytes_to_copy)) { sz = q->len > bytes_to_copy-i ? bytes_to_copy-i : q->len; - memcpy(((uint8_t*) addr) + 2 + i, q->payload, sz); + memcpy_to_nc(((uint8_t*) addr) + 2 + i, q->payload, sz); bytes_left -= sz; i += sz; if (i < bytes_to_copy) @@ -239,16 +291,16 @@ again: if (bytes_left != 0) { rckemacif->tx_write_offset = 1; addr = rckemacif->tx_buffer + 32; - LWIP_DEBUGF(NETIF_DEBUG, ("special case: copy remaining %d bytes\n", bytes_left)); + //LWIP_DEBUGF(NETIF_DEBUG, ("special case: copy remaining %d bytes\n", bytes_left)); i = 0; if (sz < q->len) { - memcpy((uint8_t*) addr, q->payload + sz, q->len - sz); + memcpy_to_nc((uint8_t*) addr, q->payload + sz, q->len - sz); bytes_left -= (q->len - sz); i = q->len - sz; } for(q=q->next; (q != 0); q = q->next) { - memcpy(((uint8_t*) addr) + i, q->payload, q->len); + memcpy_to_nc(((uint8_t*) addr) + i, q->payload, q->len); i += q->len; } @@ -256,7 +308,7 @@ again: if (rest != 0) rest = 32 - rest; - LWIP_DEBUGF(NETIF_DEBUG, ("Rest is %d\n", rest)); + //LWIP_DEBUGF(NETIF_DEBUG, ("Rest is %d\n", rest)); rckemacif->tx_write_offset += CLINE_PACKETS(bytes_left + rest) - 1; } } @@ -312,7 +364,7 @@ static void rckemacif_rx_handler(struct netif* netif, unsigned int write_offset) { rckemacif_t* rckemacif = netif->state; unsigned short read_offset = rckemacif->rx_read_offset; - unsigned int counter; + //unsigned int counter; volatile void *addr = NULL; uint16_t i, length = 0; uint32_t packets = 0; @@ -373,7 +425,7 @@ again: 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); + memcpy_from_nc((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]; //} @@ -384,33 +436,52 @@ again: int rest; int bytesLeft = length; int bytesToCopy = length; + int counter = 0; /* 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)); + //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++) { + q = p; + i = /*counter =*/ 0; + while ((q != NULL) && (counter < bytesToCopy)) { + i = MIN(q->len, bytesToCopy - counter); + memcpy_from_nc(q->payload, (uint8_t*) addr + 2 + counter, i); + counter += i; + if (counter >= bytesToCopy) + goto out; + else + 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; 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]; + counter = 0; + //LWIP_DEBUGF(NETIF_DEBUG, ("copying from %p, left: %d (%x)\n", addr, bytesLeft, ((uint8_t*)addr)[0])); + + if (i < q->len) { + counter = MIN(q->len - i, bytesLeft); + memcpy_from_nc((uint8_t*)q->payload + i, (uint8_t*) addr, counter); + } + //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]; - } + i = MIN(q->len, bytesLeft - counter); + memcpy_from_nc((uint8_t*)q->payload, (uint8_t*)addr + counter, i); + counter += i; + //for(i=0; (ilen) && (counter < bytesLeft); i++, counter++) { + // ((uint8_t*) q->payload)[i] = ((uint8_t*)addr)[counter]; + //} } read_offset = CLINE_PACKETS(bytesLeft); } else {