143 lines
2.9 KiB
C
143 lines
2.9 KiB
C
/*
|
||
* eth_checksum.c
|
||
*
|
||
* Created on: 13 мар. 2023 г.
|
||
* Author: sychev
|
||
*/
|
||
#include "free_rtos/ethernet_ip/eth_checksum.h"
|
||
#include "free_rtos/base/swap.h"
|
||
|
||
#include <arm_acle.h>
|
||
|
||
uint16_t eth_calcChksum(uint32_t sum, uint8_t * const data, uint16_t len)
|
||
{
|
||
uint16_t i;
|
||
uint16_t size = len - 1;
|
||
|
||
if ( !len )
|
||
return 0;
|
||
|
||
for ( i = 0; i < size; i+=2 )
|
||
sum += ((uint16_t)data[i] << 8) + data[i+1];
|
||
|
||
if (len & 0x01)
|
||
sum += (uint16_t)data[i] << 8;
|
||
|
||
while ( sum >> 16 )
|
||
sum = (sum & 0xFFFF) + (sum >> 16);
|
||
|
||
return ~sum;
|
||
}
|
||
|
||
uint16_t eth_calcChksum2(uint32_t sum, uint8_t * const hdata, uint16_t hlen, uint8_t * const data, uint16_t len)
|
||
{
|
||
uint16_t i;
|
||
uint16_t hsize = hlen - 1;
|
||
uint16_t size = len - 1;
|
||
|
||
if ( (hlen == 0) || (len == 0) )
|
||
return 0;
|
||
|
||
for ( i = 0; i < hsize; i+=2 )
|
||
sum += ((uint16_t)hdata[i] << 8) + hdata[i+1];
|
||
|
||
if (hlen & 0x01)
|
||
sum += (uint16_t)hdata[i] << 8;
|
||
|
||
for ( i = 0; i < size; i+=2 )
|
||
sum += ((uint16_t)data[i] << 8) + data[i+1];
|
||
|
||
if (len & 0x01)
|
||
sum += (uint16_t)data[i] << 8;
|
||
|
||
while ( sum >> 16 )
|
||
sum = (sum & 0xFFFF) + (sum >> 16);
|
||
|
||
return ~sum;
|
||
}
|
||
|
||
union PointerConverter
|
||
{
|
||
void * unknown;
|
||
uint64_t * octet;
|
||
uint32_t * quartet;
|
||
uint16_t * duet;
|
||
uint8_t * solo;
|
||
};
|
||
|
||
struct Array {
|
||
uint16_t duet0;
|
||
uint16_t duet1;
|
||
uint16_t duet2;
|
||
uint16_t duet3;
|
||
};
|
||
|
||
union ValueConverter
|
||
{
|
||
uint64_t octet;
|
||
uint32_t quartet;
|
||
uint16_t duet;
|
||
uint8_t solo;
|
||
struct Array array;
|
||
};
|
||
|
||
uint32_t calcSum(void * const buffer, uint32_t sum, void * const data, uint16_t len)
|
||
{
|
||
union PointerConverter buffer_start;
|
||
union PointerConverter data_start;
|
||
union ValueConverter value;
|
||
|
||
buffer_start.unknown = buffer;
|
||
data_start.unknown = data;
|
||
|
||
while(len >= sizeof(uint32_t)) {
|
||
len -= sizeof(uint32_t);
|
||
|
||
value.quartet = *data_start.quartet++;
|
||
*buffer_start.quartet++ = value.quartet;
|
||
|
||
value.quartet = __rev16(value.quartet);
|
||
sum += value.array.duet0 + value.array.duet1;
|
||
}
|
||
|
||
if(len >= sizeof(uint16_t)) {
|
||
len -= sizeof(uint16_t);
|
||
|
||
value.duet = *data_start.duet++;
|
||
*buffer_start.duet++ = value.duet;
|
||
|
||
value.quartet = __rev16(value.quartet);
|
||
sum += value.duet;
|
||
}
|
||
|
||
if(len >= sizeof(uint8_t)) {
|
||
len -= sizeof(uint8_t);
|
||
|
||
value.solo = *data_start.solo++;
|
||
*buffer_start.solo++ = value.solo;
|
||
|
||
value.quartet = __rev16(value.quartet);
|
||
sum += value.solo;
|
||
}
|
||
|
||
return sum;
|
||
}
|
||
|
||
uint16_t eth_calcChksum3(void * const buffer, uint32_t sum, void * const hdata, uint16_t hlen, void * const data, uint16_t len)
|
||
{
|
||
if(hdata != 0) {
|
||
sum = calcSum(buffer, sum, hdata, hlen);
|
||
|
||
}
|
||
|
||
if(data != 0) {
|
||
sum = calcSum((uint8_t*)buffer + hlen, sum, data, len);
|
||
}
|
||
|
||
while (sum >> 16) {
|
||
sum = (sum & 0xFFFF) + (sum >> 16);
|
||
}
|
||
|
||
return ~sum;
|
||
}
|