1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177
| /*
* File: sendCalmarData.c
* Author: cyrill Gremaud
*
* Created on June 14, 2012, 9:54 AM
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <arpa/inet.h>
#include <net/if.h>
#include <netinet/ether.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#include <sys/ioctl.h>
#include <arpa/inet.h>
#include <linux/if_packet.h>
#define SIZE_ETHERNET_HEADER 14
#define SIZE_IP_HEADER 20
#define SIZE_UDP_HEADER 8
#define SIZE_CALMAR_HEADER
#define SIZE_ETHERNET_ADDRESS 6
#define DATA_SIZE 8192
#define DEFAULT_IF "eth0"
/*Definition of the structure of the calmar header */
typedef struct calmar_header{
unsigned int first_field; //32 bits
unsigned int second_field; //32 bits
unsigned int sequence; //32 bits
} calmar_h;
typedef unsigned short u16;
typedef unsigned long u32;
u16 ip_sum_calc(u16 len_ip_header, u16 buff[]){
u16 word16;
u32 sum=0;
u16 i;
// make 16 bit words out of every two adjacent 8 bit words in the packet and add them up
for (i=0;i<len_ip_header;i=i+2){
word16 =((buff[i]<<8)&0xFF00)+(buff[i+1]&0xFF);
sum = sum + (u32) word16;
}
// take only 16 bits out of the 32 bit sum and add up the carries
while (sum>>16)
sum = (sum & 0xFFFF)+(sum >> 16);
// one's complement the result
sum = ~sum;
return ((u16) sum);
}
/*
*
*/
int main(int argc, char** argv) {
char data[DATA_SIZE];
char headerBuffer[sizeof(struct calmar_header)];
char ifName[IFNAMSIZ];
int tx_length = 0;
int sockfd;
struct sockaddr_ll socket_address;
struct ifreq if_idx;
struct ifreq if_mac;
struct ether_header *ethernet_header = (struct ether_header *) data;
struct iphdr *ip_header = (struct iphdr *) (sizeof(struct ether_header) + data);
struct udphdr *udp_header = (struct udphdr *) (sizeof(struct iphdr) + sizeof(struct ether_header) + data);
calmar_h calmar_header;
printf("data size = %lu bytes\n",sizeof(data));
printf("ethernet header size = %lu bytes\n",sizeof(struct ether_header));
printf("ip header size = %lu bytes\n",sizeof(struct iphdr));
printf("udp header size = %lu bytes\n",sizeof(struct udphdr));
printf("Calmar header size = %lu bytes\n",sizeof(struct calmar_header));
/* Set interface name. The first parameter could be the interface name. eg. eth0 */
if (argc > 1){
strcpy(ifName, argv[1]);
}else{
strcpy(ifName, DEFAULT_IF);
}
/*Create UDP socket with RAW IP protocol*/
if ((sockfd = socket(AF_PACKET, SOCK_RAW, IPPROTO_RAW)) == -1) {
perror("socket error");
}
/* Get the index of the interface to send on */
memset(&if_idx, 0, sizeof(struct ifreq));
strncpy(if_idx.ifr_name, ifName, IFNAMSIZ-1);
if (ioctl(sockfd, SIOCGIFINDEX, &if_idx) < 0)
perror("SIOCGIFINDEX");
/* Get the MAC address of the interface to send on */
memset(&if_mac, 0, sizeof(struct ifreq));
strncpy(if_mac.ifr_name, ifName, IFNAMSIZ-1);
if (ioctl(sockfd, SIOCGIFHWADDR, &if_mac) < 0)
perror("SIOCGIFHWADDR");
/*Allocate memory for the data buffer */
memset(data, 0, sizeof(data));
/*Construct the Ethernet header */
memcpy(ethernet_header->ether_shost,"\xc8\x60\x00\x03\xad\x20",6); //Source MAC
memcpy(ethernet_header->ether_dhost,"\x00\x27\x0e\x13\xb8\x0c",6); //Destination MAC
ethernet_header->ether_type = htons(ETH_P_IP); //Protocol IP for the upper layer
tx_length += sizeof(struct ether_header);
printf("Ethernet Header build\n");
/*Construct the IP header */
ip_header->check = 0; //No checksum now
ip_header->daddr = inet_addr ("192.168.1.2"); //Destination IP
ip_header->saddr = inet_addr ("192.168.1.1"); //Source IP
ip_header->frag_off = 0; //No offset
ip_header->id = 1; //Identification
ip_header->ihl = (sizeof(struct iphdr))/4; //IP Header Length
ip_header->protocol = 17; //UDP for the upper layer
ip_header->tos = 0; //Type of Service
ip_header->version = 4; //IPv4
ip_header->ttl = 16; //Time to Live of 16
ip_header->tot_len = htons(sizeof(data));
ip_header->check = ip_sum_calc(ip_header->ihl,data);//0x9a06;
tx_length += sizeof(struct iphdr);
printf("IP Header build\n");
/*Construct the UDP header */
udp_header->check = 0; //No Checksum now
udp_header->source = 12358; //Source port
udp_header->dest = 11234;
udp_header->len = 0; //0 for the moment
tx_length += sizeof(struct udphdr);
printf("UDP Header build\n");
//Build Calmar header
calmar_header.first_field = 0xFFFFFFFF;
calmar_header.second_field = 0xAAAAAAAA;
calmar_header.sequence = 0xBBBBBBBB;
printf("Calmar Header build\n");
/* Packet data (STORE datas in the buffer) */
*((int *)(data+tx_length)) = calmar_header.first_field;
tx_length += sizeof(calmar_header.first_field);
*((int *)(data+tx_length)) = calmar_header.second_field;
tx_length += sizeof(calmar_header.second_field);
*((int *)(data+tx_length)) = calmar_header.sequence;
tx_length += sizeof(calmar_header.sequence);
//we must set len of udp data after to have set calmar header
//because the datas of udp is the calmar header...
udp_header->len = htons((sizeof(udp_header) + sizeof(struct calmar_header))); //Total Length (header+data)
socket_address.sll_ifindex = if_idx.ifr_ifindex;
socket_address.sll_halen = ETH_ALEN;
memcpy(socket_address.sll_addr,"\x00\x27\x0e\x13\xb8\x0c",6);
/* Send packet */
int res = sendto(sockfd, data, tx_length, 0, (struct sockaddr*)&socket_address, sizeof(struct sockaddr_ll));
if (res< 0){
printf("Send failed : Res = %i\n",res);
}else{
printf("UDP datagramm send\n");
}
return (EXIT_SUCCESS);
} |
Partager