536 lines
No EOL
18 KiB
C++
536 lines
No EOL
18 KiB
C++
/**************************************************************/
|
|
/* tcpClient.cpp Date: 2020/08/21 */
|
|
/* */
|
|
/* Copyright (c) 2020 GIS. All rights reserved. */
|
|
/* */
|
|
/**************************************************************/
|
|
|
|
#include <inttypes.h>
|
|
#include "tcpClient.h"
|
|
|
|
// --------------------------------------------------
|
|
// Calculate CRC32 for ethernet frame. Not a fast
|
|
// implementation, but LUT implementations are
|
|
// available in cyberspace.
|
|
// --------------------------------------------------
|
|
|
|
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 for IPV4. Also used to
|
|
// calculate TCP checksum, which includes parts of
|
|
// IPV4 settings in a 'pseudo-header'
|
|
// --------------------------------------------------
|
|
|
|
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. Parameters passed in
|
|
// with cfg, and any data in payload (with payload
|
|
// length). Data stored in frm_buf, which must be
|
|
// sufficiently large to receive data.
|
|
// --------------------------------------------------
|
|
|
|
uint32_t tcpClient::genTcpIpPkt (tcpConfig_t &cfg, uint32_t* frm_buf, uint32_t* payload, uint32_t payload_len)
|
|
{
|
|
// Inetrmediate buffers for TCP and IPV4 data
|
|
uint32_t tcp_payload[2048];
|
|
uint32_t ipv4_payload[2048];
|
|
|
|
// Construct a TCP segment and place in tcp_payoad. Returns total length of 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 add checksum to TCP (which includes pseudo-IP header).
|
|
// Data places in ipv4_payload and method returns total length.
|
|
uint32_t iplen = ipv4Frame (ipv4_payload, tcp_payload, tcplen, cfg.ip_dst_addr);
|
|
|
|
// Wrap IPV4 Frame in an ethernet frame, placing in frm_buf and returning total length of data
|
|
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)
|
|
{
|
|
// Initialise a frame index
|
|
uint32_t fidx = 0;
|
|
|
|
// Add source port
|
|
tcp_seg[fidx++] = (tcp_port >> 8) & 0xff;
|
|
tcp_seg[fidx++] = tcp_port & 0xff;
|
|
|
|
// Add destination port
|
|
tcp_seg[fidx++] = (dst_port >> 8) & 0xff;
|
|
tcp_seg[fidx++] = dst_port & 0xff;
|
|
|
|
// Add source sequence number
|
|
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;
|
|
|
|
// Addr destination acknowledge number
|
|
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;
|
|
|
|
// Add header length
|
|
tcp_seg[fidx++] = TCP_MIN_HDR_LEN << 4; // NS flag = 0;
|
|
|
|
// Add flags
|
|
tcp_seg[fidx++] = 0x10 | // ACK
|
|
(reset_connection ? 0x04 : 0) | // RST
|
|
(sync_seq ? 0x02 : 0) | // SYN
|
|
(last_pkt ? 0x01 : 0); // FIN
|
|
|
|
// Add window size
|
|
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;
|
|
|
|
// Add payload (if any)
|
|
for (int idx = 0; idx < payload_len; idx++)
|
|
{
|
|
tcp_seg[fidx++] = payload[idx] & 0xff;
|
|
}
|
|
|
|
// Return the length of the TCP segment
|
|
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)
|
|
{
|
|
/ Initialise a frame index
|
|
uint32_t fidx = 0;
|
|
|
|
// Add IPV4 type and header length
|
|
ipv4_frame[fidx++] = (0x4 << 4) | IPV4_MIN_HDR_LEN; // IPv4 and IHL = 5 (DWORDS)
|
|
ipv4_frame[fidx++] = 0x00; // DSCP = 0, ECN = 0
|
|
|
|
// Add the total length for header and payload (in bytes)
|
|
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;
|
|
|
|
// Add and ID
|
|
uint32_t id = 0x0002;
|
|
ipv4_frame[fidx++] = id >> 8;
|
|
ipv4_frame[fidx++] = id & 0xff;
|
|
|
|
// Add flags and any fragment offset
|
|
uint32_t flags = 0;
|
|
uint32_t frag_offset = 0x400;
|
|
ipv4_frame[fidx++] = flags | ((frag_offset >> 8) & 0xf) << 4;
|
|
ipv4_frame[fidx++] = frag_offset & 0xff;
|
|
|
|
// Set the time to live value
|
|
ipv4_frame[fidx++] = 0xff; // Time to Live
|
|
|
|
// Set type ID (= TCP)
|
|
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;
|
|
|
|
// IPV4 SRC address (MSB first)
|
|
for(int idx = 0; idx < 4; idx++)
|
|
{
|
|
ipv4_frame[fidx++] = ipv4_addr >> 8*(3-idx);
|
|
}
|
|
|
|
// IPV4 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);
|
|
|
|
// Add the PIV4 checksum
|
|
ipv4_frame[chksum_offset] = chksum >> 8;
|
|
ipv4_frame[chksum_offset+1] = chksum & 0xff;
|
|
|
|
// Remember the offset where the payload begins
|
|
uint32_t payload_offset = fidx;
|
|
|
|
// Add any payload
|
|
for (int idx = 0; idx < payload_len; idx++)
|
|
{
|
|
ipv4_frame[fidx++] = payload[idx] & 0xff;
|
|
}
|
|
|
|
// If requested, calculate and add the TCP checksum, based on TCP packet and IPV4 pseudo header
|
|
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 the length of the frame (in bytes)
|
|
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);
|
|
|
|
// Check that any payload can fit in an ethernet packet
|
|
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;
|
|
|
|
// Add 6 bytes of preamble
|
|
for (int idx = 0; idx < 6; idx++)
|
|
{
|
|
frame[fidx++] = PREAMBLE;
|
|
}
|
|
|
|
// Add start-of-frame delimiter
|
|
frame[fidx++] = SFD;
|
|
|
|
// Add 48 bits of destination address (MSB first)
|
|
for (int idx = 0; idx < 6; idx++)
|
|
{
|
|
frame[fidx++] = (dst_addr >> (8*(5-idx))) & 0xff;
|
|
}
|
|
|
|
// Add 48 bits of source address (MSB first)
|
|
for (int idx = 0; idx < 6; idx++)
|
|
{
|
|
frame[fidx++] = (mac_addr >> (8*(5-idx))) & 0xff;
|
|
}
|
|
|
|
// Add 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 the length of the ethernet data (in bytes), including preamble
|
|
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);
|
|
}
|
|
|
|
} |