53 lines
1.3 KiB
C
53 lines
1.3 KiB
C
#include "ether.h"
|
|
#include "ipv4.h"
|
|
#include "icmp.h"
|
|
#include "memory.h"
|
|
#include "string.h"
|
|
|
|
static inline unsigned short sum_to_checksum(unsigned int sum) {
|
|
while (sum >> 16) {
|
|
sum = (sum >> 16) + (sum & 0xFFFF);
|
|
}
|
|
|
|
return ~sum;
|
|
}
|
|
|
|
void icmp_send(struct ether_t *ether, unsigned char type, unsigned char code, unsigned int other, unsigned int dest, char *buff, int len) {
|
|
struct icmp_header_t *icmp_h = (struct icmp_header_t *)malloc(sizeof(struct icmp_header_t) + len);
|
|
unsigned int l = len;
|
|
unsigned short *ptr = (unsigned short *)buff;
|
|
unsigned int sum = 0;
|
|
|
|
icmp_h->type = type;
|
|
icmp_h->code = code;
|
|
icmp_h->rest = other;
|
|
|
|
for (;l > 1; l-=2) {
|
|
sum+= *ptr++;
|
|
}
|
|
if (l) {
|
|
sum += *((unsigned char *)ptr);
|
|
}
|
|
icmp_h->checksum = sum_to_checksum(sum);
|
|
|
|
memcpy((char *)(icmp_h + 1), buff, len);
|
|
|
|
ipv4_send(ether, 1, dest, (char *)icmp_h, len + sizeof(struct icmp_header_t));
|
|
}
|
|
|
|
void icmp_process_packet(struct ether_t *ether, unsigned int src_addr, char *packet, int len) {
|
|
struct icmp_header_t *icmp_h = (struct icmp_header_t *)packet;
|
|
|
|
switch (icmp_h->type) {
|
|
case 8: // ECHO REQ
|
|
{
|
|
switch (icmp_h->code) {
|
|
case 0: // ECHO REQ
|
|
icmp_send(ether, 0, 0, icmp_h->rest, src_addr, (char *)(icmp_h + 1), len - sizeof(struct icmp_header_t));
|
|
break;
|
|
}
|
|
|
|
}
|
|
break;
|
|
}
|
|
} |