Commit 2bfa15c4 authored by Martin Vítek's avatar Martin Vítek

Working PINGgit status!

parent 982af81d
#include "ICMP_protocol.h"
#include <cstdint>
#include <cstring>
#include <machine/endian.h>
#include "USART.h"
......@@ -10,15 +11,24 @@ ICMP_protocol::ICMP_protocol()
{
}
void ICMP_protocol::decode(const uint8_t* data, const uint16_t length)
bool ICMP_protocol::decode(const uint8_t* rx_data, const uint16_t length, uint8_t* tx_data)
{
ICMP_packet* packet = (ICMP_packet*) data;
ICMP_packet* packet = (ICMP_packet*) rx_data;
debug.icmp_packet(packet);
if (packet->type == ECHO_REQUEST)
{
ICMP_echo_packet* echo_packet = (ICMP_echo_packet*) data;
ICMP_echo_packet* echo_request = (ICMP_echo_packet*) rx_data;
ICMP_echo_packet* echo_reply = (ICMP_echo_packet*) tx_data;
memcpy(echo_reply, echo_request, length);
echo_reply->icmp_header.type = ECHO_REPLY;
echo_reply->icmp_header.checksum = 0;
echo_reply->icmp_header.checksum = checksum(tx_data, length);
return true;
/*
debug.info("Got IPCM echo request");
......@@ -26,4 +36,61 @@ void ICMP_protocol::decode(const uint8_t* data, const uint16_t length)
uart.send("\t\tSequence number: ");uart.send_dec(__ntohs(echo_packet->sequence_number));uart.send_new_line();
*/
}
return false;
}
uint16_t ICMP_protocol::checksum(const uint8_t* vdata, uint16_t length)
{
//Adapted from: http://www.microhowto.info/howto/calculate_an_internet_protocol_checksum_in_c.html
// Cast the data pointer to one that can be indexed.
char* data=(char*)vdata;
// Initialize the accumulator.
uint64_t acc=0xffff;
// Handle any partial block at the start of the data.
unsigned int offset=((uintptr_t)data)&3;
if (offset) {
size_t count=4-offset;
if (count>length) count=length;
uint32_t word=0;
memcpy(offset+(char*)&word,data,count);
acc+=__ntohl(word);
data+=count;
length-=count;
}
// Handle any complete 32-bit blocks.
char* data_end=data+(length&~3);
while (data!=data_end) {
uint32_t word;
memcpy(&word,data,4);
acc+=__ntohl(word);
data+=4;
}
length&=3;
// Handle any partial block at the end of the data.
if (length) {
uint32_t word=0;
memcpy(&word,data,length);
acc+=__ntohl(word);
}
// Handle deferred carries.
acc=(acc&0xffffffff)+(acc>>32);
while (acc>>16) {
acc=(acc&0xffff)+(acc>>16);
}
// If the data began at an odd byte address
// then reverse the byte order to compensate.
if (offset&1) {
acc=((acc&0xff00)>>8)|((acc&0x00ff)<<8);
}
// Return the checksum in network byte order.
return __htons(~acc);
}
......@@ -49,6 +49,7 @@ class ICMP_protocol
public:
ICMP_protocol();
void decode(const uint8_t* data, const uint16_t length);
bool decode(const uint8_t* rx_data, const uint16_t length, uint8_t* tx_data);
uint16_t checksum(const uint8_t* vdata, uint16_t length);
};
#include "IP_protocol.h"
#include <cstdint>
#include <cstring>
#include "machine/endian.h"
#include "ethernet_frame.h"
#include "NetworkTypes.h"
......@@ -15,27 +17,114 @@ IP_protocol::IP_protocol(const IP& local_ip): icmp(),
{
}
void IP_protocol::decode(const ethernet_payload& payload)
bool IP_protocol::decode(const ethernet_payload& rx_payload, ethernet_payload& tx_payload)
{
IP_packet* packet = (IP_packet*) payload.data;
debug.ip_packet(packet);
IP_packet* rx_packet = (IP_packet*) rx_payload.data;
//debug.ip_packet(rx_packet);
IP_packet* tx_packet = (IP_packet*) tx_payload.data;
//Trash not IPv4 packets
if (IP(packet->version) != 4) return;
if (IP(rx_packet->version) != 4) return false;
//Trash not mine IP destinations
if (IP(packet->dst_address) != local_ip) return;
if (IP(rx_packet->dst_address) != local_ip) return false;
switch (packet->protocol)
uint16_t data_length = get_data_length(rx_packet->ihl, rx_packet->tl);
switch (rx_packet->protocol)
{
case ICMP: icmp.decode(packet->data, packet->tl - (packet->ihl*4));
break;
case ICMP: if (icmp.decode(rx_packet->data, data_length, tx_packet->data))
{
tx_packet->ihl = 5;
tx_packet->version = 4;
tx_packet->tos = 0;
tx_packet->tl = __htons((4*tx_packet->ihl) + data_length);
tx_packet->identification = rx_packet->identification;
tx_packet->flags_fragment_offset = rx_packet->flags_fragment_offset;
tx_packet->ttl = rx_packet->ttl;
tx_packet->protocol = ICMP;
tx_packet->checksum = 0;
tx_packet->src_address = local_ip.ip.value;
tx_packet->dst_address = rx_packet->src_address;
tx_packet->checksum = checksum((uint8_t*)tx_packet, __ntohs(tx_packet->tl));
tx_payload.length = __ntohs(tx_packet->tl);
return true;
}
case TCP:
break;
case UDP: udp.decode(packet->data, packet->tl - (packet->ihl*4));
case UDP: udp.decode(rx_packet->data, data_length);
break;
}
return false;
}
uint16_t IP_protocol::get_data_length(uint8_t ihl, uint16_t tl)
{
return __ntohs(tl) - (4*ihl);
}
uint16_t IP_protocol::checksum(const uint8_t* vdata, uint16_t length)
{
//Adapted from: http://www.microhowto.info/howto/calculate_an_internet_protocol_checksum_in_c.html
// Cast the data pointer to one that can be indexed.
char* data=(char*)vdata;
// Initialize the accumulator.
uint64_t acc=0xffff;
// Handle any partial block at the start of the data.
unsigned int offset=((uintptr_t)data)&3;
if (offset) {
size_t count=4-offset;
if (count>length) count=length;
uint32_t word=0;
memcpy(offset+(char*)&word,data,count);
acc+=__ntohl(word);
data+=count;
length-=count;
}
// Handle any complete 32-bit blocks.
char* data_end=data+(length&~3);
while (data!=data_end) {
uint32_t word;
memcpy(&word,data,4);
acc+=__ntohl(word);
data+=4;
}
length&=3;
// Handle any partial block at the end of the data.
if (length) {
uint32_t word=0;
memcpy(&word,data,length);
acc+=__ntohl(word);
}
// Handle deferred carries.
acc=(acc&0xffffffff)+(acc>>32);
while (acc>>16) {
acc=(acc&0xffff)+(acc>>16);
}
// If the data began at an odd byte address
// then reverse the byte order to compensate.
if (offset&1) {
acc=((acc&0xff00)>>8)|((acc&0x00ff)<<8);
}
// Return the checksum in network byte order.
return __htons(~acc);
}
......@@ -67,7 +67,9 @@ class IP_protocol
public:
IP_protocol(const IP& local_ip);
void decode(const ethernet_payload& payload);
bool decode(const ethernet_payload& rx_payload, ethernet_payload& tx_payload);
uint16_t get_data_length(uint8_t ihl, uint16_t tl);
uint16_t checksum(const uint8_t* vdata, uint16_t length);
};
......@@ -129,18 +129,27 @@ void NixieStack::packet_handler()
{
ethernet_frame* frame = rx_buffer.read();
//debug.frame(frame);
ethernet_frame* tx_frame = tx_buffer.write();
switch (__htons(frame->type_length))
{
case IPV4:
{
ipv4.decode(frame->payload);
//In tx_frame->payload is packet to send
if (ipv4.decode(frame->payload, tx_frame->payload))
{
memcpy(tx_frame->dest_address, frame->src_address, 6);
memcpy(tx_frame->src_address, mac, 6);
tx_frame->type_length = __htons(IPV4);
tx_buffer.write_advance();
}
}
break;
case ARP:
{
ethernet_frame* tx_frame = tx_buffer.write();
if (arp.decode(frame->payload, tx_frame->payload))
{
//Send ARP reply
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment