firmware_tools/verification/lib/tests/utils/tcpClient.cpp

504 lines
No EOL
16 KiB
C++

/**************************************************************/
/* tcpClient.cpp Date: 2020/08/21 */
/* */
/* Copyright (c) 2020 GIS. All rights reserved. */
/* */
/**************************************************************/
#include <inttypes.h>
#include "tcpClient.h"
// --------------------------------------------------
// Calculate CRC
// --------------------------------------------------
uint32_t tcpClient::crc32(uint32_t *buf, uint32_t len, uint32_t poly, uint32_t init, bool debug)
{
uint32_t val, crc;
uint8_t i;
crc = init;
while (len--)
{
if (debug)
{
printf("crc32: byte = 0x%02x\n", *buf);
}
val = (crc ^ *buf++) & 0xFF;
for(i = 0; i < 8; i++)
{
val = (val & 1) ? (val >> 1) ^ poly : val >> 1;
}
crc = val ^ crc >> 8;
}
return crc ^ 0xFFFFFFFF;
}
// --------------------------------------------------
// Checksum calculation
// --------------------------------------------------
uint32_t tcpClient::ipv4_chksum (uint32_t* buf, uint32_t len, bool debug)
{
uint32_t sum = 0;
for (int idx = 0; idx < len; idx+=2)
{
sum += ((buf[idx] & 0xff) << 8) | (buf[idx+1] & 0xff);
if (debug)
{
VPrint ("sum=0x%04x word=0x%04x\n", sum, ((buf[idx] & 0xff) << 8) | (buf[idx+1] & 0xff));
}
}
if (len&1)
{
sum += (buf[len-1] & 0xff) << 8 ;
}
return sum;
}
// --------------------------------------------------
// Generate a TCP/IP packet
// --------------------------------------------------
uint32_t tcpClient::genTcpIpPkt (tcpConfig_t &cfg, uint32_t* frm_buf, uint32_t* payload, uint32_t payload_len)
{
uint32_t tcp_payload[2048];
uint32_t ipv4_payload[2048];
// TODO: do parameter checking
// Construct a TCP segment
uint32_t tcplen = tcpSegment(tcp_payload,
payload,
payload_len,
cfg.dst_port,
cfg.seq_num,
cfg.ack_num,
cfg.rst_conn,
cfg.sync_seq,
cfg.last_pkt,
cfg.win_size);
// Wrap TCP segment in an IPV4 frame, and checksum to TCP (which includes pseudo-IP header)
uint32_t iplen = ipv4Frame (ipv4_payload, tcp_payload, tcplen, cfg.ip_dst_addr);
// Wrap IPV4 Frame in an ethernet frame
uint32_t flen = ethFrame (frm_buf, ipv4_payload, iplen, cfg.mac_dst_addr);
// Return length of data in bytes.
return flen;
}
// --------------------------------------------------
// Construct TCP segment
// -------------------------------------------------
uint32_t tcpClient::tcpSegment (uint32_t* tcp_seg,
uint32_t* payload,
uint32_t payload_len,
uint32_t dst_port,
uint32_t seq_num,
uint32_t ack_num,
bool reset_connection,
bool sync_seq,
bool last_pkt,
uint32_t window_size)
{
uint32_t fidx = 0;
tcp_seg[fidx++] = (tcp_port >> 8) & 0xff;
tcp_seg[fidx++] = tcp_port & 0xff;
tcp_seg[fidx++] = (dst_port >> 8) & 0xff;
tcp_seg[fidx++] = dst_port & 0xff;
tcp_seg[fidx++] = (seq_num >> 24) & 0xff;
tcp_seg[fidx++] = (seq_num >> 16) & 0xff;
tcp_seg[fidx++] = (seq_num >> 8) & 0xff;
tcp_seg[fidx++] = (seq_num >> 0) & 0xff;
tcp_seg[fidx++] = (ack_num >> 24) & 0xff;
tcp_seg[fidx++] = (ack_num >> 16) & 0xff;
tcp_seg[fidx++] = (ack_num >> 8) & 0xff;
tcp_seg[fidx++] = (ack_num >> 0) & 0xff;
tcp_seg[fidx++] = TCP_MIN_HDR_LEN << 4; // NS flag = 0;
tcp_seg[fidx++] = 0x10 | // ACK
(reset_connection ? 0x04 : 0) | // RST
(sync_seq ? 0x02 : 0) | // SYN
(last_pkt ? 0x01 : 0); // FIN
tcp_seg[fidx++] = (window_size >> 8) & 0xff;
tcp_seg[fidx++] = window_size & 0xff;
// Blank checksum holder (need IP values for IP pseudo header before we can calculate, so postpone)
uint32_t chksum_offset = fidx;
tcp_seg[fidx++] = 0;
tcp_seg[fidx++] = 0;
// Urgent pointer
tcp_seg[fidx++] = 0;
tcp_seg[fidx++] = 0;
// Payload (if any)
for (int idx = 0; idx < payload_len; idx++)
{
tcp_seg[fidx++] = payload[idx] & 0xff;
}
return fidx;
}
// --------------------------------------------------
// Construct IPv4 frame
// --------------------------------------------------
uint32_t tcpClient::ipv4Frame (uint32_t* ipv4_frame, uint32_t* payload, uint32_t payload_len, uint32_t ipv4_dst_addr, bool add_tcp_chksum)
{
uint32_t fidx = 0;
ipv4_frame[fidx++] = (0x4 << 4) | IPV4_MIN_HDR_LEN; // IPv4 and IHL = 5 (DWORDS)
ipv4_frame[fidx++] = 0x00; // DSCP = 0, ECN = 0
uint32_t total_len = IPV4_MIN_HDR_LEN*4 + payload_len; // Total length in bytes
ipv4_frame[fidx++] = total_len >> 8;
ipv4_frame[fidx++] = total_len & 0xff;
uint32_t id = 0x0002;
ipv4_frame[fidx++] = id >> 8;
ipv4_frame[fidx++] = id & 0xff;
uint32_t flags = 0;
uint32_t frag_offset = 0x400;
ipv4_frame[fidx++] = flags | ((frag_offset >> 8) & 0xf) << 4;
ipv4_frame[fidx++] = frag_offset & 0xff;
ipv4_frame[fidx++] = 0xff; // Time to Live
ipv4_frame[fidx++] = TCP_PROTOCOL_NUM; // Protocol (= TCP)
// Blank checksum and fill in once addresses added and can calculate
uint32_t chksum_offset = fidx;
ipv4_frame[fidx++] = 0;
ipv4_frame[fidx++] = 0;
// SRC address (MSB first)
for(int idx = 0; idx < 4; idx++)
{
ipv4_frame[fidx++] = ipv4_addr >> 8*(3-idx);
}
// DST address (MSB first)
for(int idx = 0; idx < 4; idx++)
{
ipv4_frame[fidx++] = ipv4_dst_addr >> 8*(3-idx);
}
// Calculate basic checksum
uint32_t chksum = ipv4_chksum(ipv4_frame, fidx);
// One's complement checksum
chksum = ~((chksum & 0xffff) + (chksum >> 16)) & 0xffff;
//VPrint("chksum = 0x%04x\n", chksum);
ipv4_frame[chksum_offset] = chksum >> 8;
ipv4_frame[chksum_offset+1] = chksum & 0xff;
uint32_t payload_offset = fidx;
// Payload
for (int idx = 0; idx < payload_len; idx++)
{
ipv4_frame[fidx++] = payload[idx] & 0xff;
}
if (add_tcp_chksum)
{
// Calculate the partial checksum for the TCP segment
uint32_t partial_chksum = ipv4_chksum(&ipv4_frame[payload_offset], payload_len);
// Calculate the rest of the checksum with the IP pseudo-header data
partial_chksum += (ipv4_addr >> 16) & 0xffff;
partial_chksum += (ipv4_addr >> 0) & 0xffff;
partial_chksum += (ipv4_dst_addr >> 16) & 0xffff;
partial_chksum += (ipv4_dst_addr >> 0) & 0xffff;
partial_chksum += (TCP_PROTOCOL_NUM);
partial_chksum += (payload_len);
//printf("Uncomplemented checksum = 0x%04x\n", partial_chksum);
// One's complement checksum
partial_chksum = ~((partial_chksum & 0xffff) + (partial_chksum >> 16)) & 0xffff;
//printf("TCP checksum = 0x%04x\n", partial_chksum);
// Write TCP checksum to buffer
ipv4_frame[payload_offset + TCP_CHKSUM_OFFSET] = partial_chksum >> 8;
ipv4_frame[payload_offset + TCP_CHKSUM_OFFSET+1] = partial_chksum & 0xff;
}
return fidx;
}
// --------------------------------------------------
// Construct ethernet frame
// --------------------------------------------------
uint32_t tcpClient::ethFrame(uint32_t* frame, uint32_t* payload, uint32_t payload_len, uint64_t dst_addr)
{
uint32_t fidx = 0;
//printf ("ethFrame() : payload_len = %d ", payload_len);
if (payload_len > 1500)
{
printf("NODE%d: ethFrame() : ***ERROR. Specified payload length (%d) too big. Must be <= 1500\n");
return 0;
}
// Add a start-of-frame token
frame[fidx++] = SOF;
// 6 bytes of preamble
for (int idx = 0; idx < 6; idx++)
{
frame[fidx++] = PREAMBLE;
}
// Start-of-frame delimiter
frame[fidx++] = SFD;
// 48 bits of destination address (MSB first)
for (int idx = 0; idx < 6; idx++)
{
frame[fidx++] = (dst_addr >> (8*(5-idx))) & 0xff;
}
// 48 bits of source address (MSB first)
for (int idx = 0; idx < 6; idx++)
{
frame[fidx++] = (mac_addr >> (8*(5-idx))) & 0xff;
}
// 2 bytes of Ethernet type (0x800 = IPv4)
frame[fidx++] = 0x08;
frame[fidx++] = 0x00;
// Add the payload
for (int idx = 0; idx < payload_len; idx++)
{
frame[fidx++] = payload[idx] & 0xff;
}
// If the payload runs short of the 64 byte minimum size then pad
if (payload_len < 46)
{
for (int idx = 0; idx < (46 - payload_len); idx++)
{
frame[fidx++] = 0;
}
}
// Calculate the CRC (excluding SOF, SFD and preamble)
uint32_t crc = crc32(&frame[8], fidx-8);
// printf("crc (tx) = 0x%08x\n", crc);
// Add crc
for (int idx = 0; idx < 4; idx++)
{
frame[fidx++] = (crc >> (8*idx)) & 0xff;
}
// Add the EOF delimiter
frame[fidx++] = EoF;
return fidx;
}
// --------------------------------------------------
// Process the received frames
// --------------------------------------------------
uint32_t tcpClient::processFrame (uint32_t* rx_buff, uint32_t rx_len)
{
uint32_t error = 0;
rxInfo_t rxInfo;
// -------------------------
// MAC
// -------------------------
// Check frame's CRC
uint32_t crc = crc32(rx_buff, rx_len-4);
uint32_t pktcrc = rx_buff[rx_len-1] << 24 |
rx_buff[rx_len-2] << 16 |
rx_buff[rx_len-3] << 8 |
rx_buff[rx_len-4] << 0 ;
//printf("CRC = 0x%08x pktcrc = 0x%08x\n", crc, pktcrc);
if (crc != pktcrc)
{
error |= RX_BAD_CRC;
}
// Check that the MAC address is for us
uint32_t ridx = 0;
uint64_t dst_mac_addr = (uint64)rx_buff[ridx++] << 40 |
(uint64)rx_buff[ridx++] << 32 |
(uint64)rx_buff[ridx++] << 24 |
(uint64)rx_buff[ridx++] << 16 |
(uint64)rx_buff[ridx++] << 8 |
(uint64)rx_buff[ridx++];
//printf("dst_mac_addr = 0x%08x%08x mac_addr = 0x%08x%08x\n", (uint32_t)(dst_mac_addr >> 32), (uint32_t)(dst_mac_addr & 0xffffffff),
// (uint32_t)(mac_addr >> 32), (uint32_t)(mac_addr & 0xffffffff));
if (dst_mac_addr != mac_addr)
{
error |= 1; // Incorrect MAC addr
}
// Extract SRC addr
rxInfo.src_mac_addr = (uint64)rx_buff[ridx++] << 40 |
(uint64)rx_buff[ridx++] << 32 |
(uint64)rx_buff[ridx++] << 24 |
(uint64)rx_buff[ridx++] << 16 |
(uint64)rx_buff[ridx++] << 8 |
(uint64)rx_buff[ridx++];
// -------------------------
// IPV4
// -------------------------
// Check IP header for integrity and addressed to us and, if so, save src address
uint32_t chksum = ipv4_chksum(&rx_buff[ETH_HDR_LEN], IPV4_MIN_HDR_LEN*4);
chksum = ~((chksum & 0xffff) + (chksum >> 16)) & 0xffff;
if (chksum)
{
error |= RX_BAD_IPV4_CHECKSUM;
}
//printf("IPV4 header checksum = 0x%04x\n", chksum);
uint32_t total_len = (rx_buff[ETH_HDR_LEN+2] << 8) |
(rx_buff[ETH_HDR_LEN+3]);
uint32_t ipv4_payload_len = total_len - IPV4_MIN_HDR_LEN*4;
ridx = ETH_HDR_LEN + IPV4_SRC_ADDR_OFFSET*4;
rxInfo.ipv4_src_addr = rx_buff[ridx++] << 24 |
rx_buff[ridx++] << 16 |
rx_buff[ridx++] << 8 |
rx_buff[ridx++];
uint32_t ipv4_dst_addr = rx_buff[ridx++] << 24 |
rx_buff[ridx++] << 16 |
rx_buff[ridx++] << 8 |
rx_buff[ridx++];
if (ipv4_dst_addr != ipv4_addr)
{
error |= RX_WRONG_IPV4_ADDR;
}
//printf("IPV4: src = 0x%08x dst = 0x%08x total_len=%d payload_len=%d\n", rxInfo.ipv4_src_addr, ipv4_dst_addr, total_len, ipv4_payload_len);
// -------------------------
// TCP
// -------------------------
// Check TCP segment integrity and correct port. Save src port #, src seq and ack#, winsize
// Calculate the partial checksum for the TCP segment
uint32_t partial_chksum = ipv4_chksum(&rx_buff[ridx], ipv4_payload_len);
// Calculate the rest of the checksum with the IP pseudo-header data
partial_chksum += (rxInfo.ipv4_src_addr >> 16) & 0xffff;
partial_chksum += (rxInfo.ipv4_src_addr >> 0) & 0xffff;
partial_chksum += (ipv4_dst_addr >> 16) & 0xffff;
partial_chksum += (ipv4_dst_addr >> 0) & 0xffff;
partial_chksum += (TCP_PROTOCOL_NUM);
partial_chksum += (ipv4_payload_len);
// One's complement checksum
partial_chksum = ~((partial_chksum & 0xffff) + (partial_chksum >> 16)) & 0xffff;
//printf("TCP checksum = 0x%04x\n", partial_chksum);
if (partial_chksum)
{
error |= RX_BAD_TCP_CHECKSUM;
}
// Extract TCP info
rxInfo.tcp_src_port = rx_buff[ridx++] << 8 |
rx_buff[ridx++];
uint32_t tcp_dst_port = rx_buff[ridx++] << 8 |
rx_buff[ridx++];
//printf("src_port = 0x%04x dst_port = 0x%04x tcp_port = 0x%04x\n", rxInfo.tcp_src_port, tcp_dst_port, tcp_port);
if (tcp_dst_port != tcp_port)
{
error |= RX_WRONG_TCP_PORT;
}
rxInfo.tcp_seq_num = rx_buff[ridx++] << 24 |
rx_buff[ridx++] << 16 |
rx_buff[ridx++] << 8 |
rx_buff[ridx++];
rxInfo.tcp_ack_num = rx_buff[ridx++] << 24 |
rx_buff[ridx++] << 16 |
rx_buff[ridx++] << 8 |
rx_buff[ridx++];
uint32_t data_off_bytes = (rx_buff[ridx] >> 4) * 4;
rxInfo.tcp_flags = (rx_buff[ridx++] & 0x01) << 8 |
rx_buff[ridx++];
rxInfo.tcp_win_size = rx_buff[ridx++] << 8 |
rx_buff[ridx++];
// Skip over next DWORDS (checksum and urgent pointer)
ridx += 4;
// Skip over any extra bytes specified beyond the minimum
ridx += data_off_bytes - IPV4_MIN_HDR_LEN*4;
//printf("errors = 0x%04x\n", error);
if (error)
{
printf("WARNING: received packet with error flags (0x%02x)\n", error);
}
// If all checks out, extract payload and call usr callback, if one registered
if (!error && usrRxCbFunc != null)
{
rxInfo.rx_len = ipv4_payload_len-data_off_bytes;
for(int idx = 0; idx < rxInfo.rx_len; idx++)
{
rxInfo.rx_payload[idx] = rx_buff[ridx++];
}
(*usrRxCbFunc)(rxInfo, hdl);
}
}