In this example I am going to change TOS in outgoing http(tcp/80) connections. I will get packets using libipq and send them out using raw socket mechanisim.
modprobe iptable_filter
modprobe ip_queue
iptables -I OUTPUT -p tcp --dport 80 -m tos ! --tos 0x30 -j QUEUE
/*
* This code is GPL.
*
* Compile : gcc change-tos.c -o change-tos -lipq
*
*/
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/tcp.h>
#include <linux/netfilter.h>
#include <libipq.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <sys/socket.h>
#include <sys/types.h>
#define BUFSIZE 4096
struct pseudo_tcphdr
{
unsigned int ip_src;
unsigned int ip_dst;
unsigned char zero;
unsigned char protocol;
unsigned short tcp_len;
};
static void die(struct ipq_handle *h)
{
ipq_perror("passer");
ipq_destroy_handle(h);
exit(1);
}
unsigned short in_checksum(unsigned short *buffer, int size)
{
unsigned long cksum = 0;
while(size >1) {
cksum += *buffer++;
size -= sizeof(unsigned short);
}
if(size)
cksum += *(unsigned char *)buffer;
cksum = (cksum >> 16) + (cksum & 0xffff);
cksum += (cksum >>16);
return (unsigned short)(~cksum);
}
unsigned short tcp_checksum(struct iphdr* iph, struct tcphdr* tcph,
char* data, int size)
{
char buffer[65536];
struct pseudo_tcphdr psd_header;
psd_header.ip_dst = iph->daddr;
psd_header.ip_src = iph->saddr;
psd_header.zero = 0;
psd_header.protocol = iph->protocol;
psd_header.tcp_len = htons(sizeof(struct tcphdr)+size);
tcph->check = 0;
memcpy(buffer, &psd_header, sizeof(struct pseudo_tcphdr));
memcpy(buffer+sizeof(struct pseudo_tcphdr), tcph,
sizeof(struct tcphdr));
memcpy(buffer+sizeof(struct pseudo_tcphdr)+sizeof(struct tcphdr),
data, size);
return tcph->check = in_checksum((unsigned short *)buffer,
sizeof(struct pseudo_tcphdr)+sizeof(struct tcphdr)+size);
}
void show_buffer(unsigned char *buf, int len)
{
int i, j;
char str[17];
str[16] = 0;
printf("\n");
for (i = 0; i < len; i += 16) {
fprintf(stdout, " ");
for (j = 0; j < 16 && i+j < len; j++) {
fprintf(stdout, "%02x ", buf[i+j]);
if (buf[i+j] > 32 && buf[i+j] < 127)
str[j] = buf[i+j];
else
str[j] = '.';
}
for (; j < 16; j++) {
fprintf(stdout, " ", buf[i+j]);
str[j] = ' ';
}
printf(" | %s |\n", str);
}
}
void raw_send(unsigned char *buf, int len)
{
/* Create a raw socket */
int s = socket (PF_INET, SOCK_RAW, IPPROTO_TCP);
if(s == -1)
{
perror("Failed to create socket");
exit(1);
}
/* IP header */
struct iphdr *iph = (struct iphdr *) buf;
/* TCP header */
struct tcphdr *tcph = (struct tcphdr *) (buf + sizeof (struct ip));
/* Destination address resolution */
struct sockaddr_in sin;
sin.sin_family = AF_INET;
sin.sin_port = tcph->dest;
sin.sin_addr.s_addr = iph->daddr;
/* Tell to the kernel that headers are included in the packet */
int one = 1;
const int *val = &one;
if (setsockopt (s, IPPROTO_IP, IP_HDRINCL, val, sizeof (one)) < 0)
{
perror("Error setting IP_HDRINCL");
exit(0);
}
printf ("Ready to send:");
show_buffer(buf,len);
/* Send the packet */
if (sendto (s, buf, len , 0, (struct sockaddr *) &sin, sizeof (sin)) < 0)
perror("sendto failed");
else
printf ("Packet Send. Length : %d \n" , len);
close(s);
}
int main()
{
int status;
unsigned char buf[BUFSIZE];
struct ipq_handle *h;
size_t pack_len;
int i;
struct iphdr *iph;
struct tcphdr *tcph;
unsigned char *pack;
char *src_addr, *dst_addr;
long long count;
/*
* Initialisation
* IPV4 protocol only
*/
h = ipq_create_handle(0, NFPROTO_IPV4);
if (!h)
die(h);
/*
* Setting the Queue Mode
* Get packet metadata and packet payloads
*/
status = ipq_set_mode(h, IPQ_COPY_PACKET, BUFSIZE);
if (status < 0)
die(h);
/* Receiving Packets from the Queue */
count = 1;
do {
status = ipq_read(h, buf, BUFSIZE, 0);
if (status < 0)
die(h);
fprintf(stdout, "\nCount: %lld\n", count++);
switch (ipq_message_type(buf)) {
case NLMSG_ERROR:
fprintf(stderr, "Received error message %d\n",
ipq_get_msgerr(buf));
break;
case IPQM_PACKET: {
ipq_packet_msg_t *m = ipq_get_packet(buf);
/* get and show packet length */
pack_len = m->data_len;
fprintf(stdout, "Length: %d\n", pack_len);
/* Show original packet */
fprintf(stdout, "Payload: ");
show_buffer(m->payload, pack_len);
/* Create new changed packet */
pack = (unsigned char *) malloc(pack_len);
memset((char *)pack, '\0', pack_len);
memcpy(pack, m->payload, pack_len);
iph = (struct iphdr *) pack;
/* change something */
iph->tos = 0x30;
/* Change IP checksum */
iph->check = 0;
iph->check = (unsigned short) in_checksum(
(unsigned short *) iph, sizeof(struct iphdr));
/* Change TCP checksum */
tcph = (struct tcphdr *) &pack[ iph->ihl * 4 ];
tcph->check = 0;
tcph->check = (unsigned short) tcp_checksum(
iph, tcph, (char *)tcph + 20,
pack_len - sizeof(struct iphdr)
- sizeof(struct tcphdr));
/* Show new packet */
fprintf(stdout, "Changed: ");
show_buffer(pack, pack_len);
/* Drop original packet in netfilter */
status = ipq_set_verdict(h, m->packet_id, NF_DROP,
pack_len, (unsigned char *) pack);
if (status < 0)
die(h);
/* Send changed packet */
raw_send(pack, pack_len);
break;
}
default:
fprintf(stderr, "Unknown message type!\n");
break;
}
} while (1);
ipq_destroy_handle(h);
return 0;
}
BY: Pejman Moghadam
TAG: libipq, iptables, raw-socket
DATE: 2012-11-11 00:46:55