ping_posix.c

最新rtlinux内核源码

#include <linux/kernel.h>
#include <linux/module.h>
#include <rtl_sched.h>
#include <unistd.h>

static unsigned char *IP;

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Sergio Perez <[e-mail]>");
MODULE_DESCRIPTION("Ping implementation for testing Ethernet drivers using read() as standard.");
MODULE_PARM(IP,"s");
MODULE_PARM_DESC(IP, "The IP address used to be \"pinged\"."); 

#define MAX_MTU      1536
#define ECHO_REPLY   0x00
#define IP_PACKET    0x0800
#define ARP_PACKET   0x0806
#define ARP_REPLY    0x02
#define ICMP_PACKET  0x01
#define ECHO_REQUEST 0x08

struct memory{
  void *mem;
};

pthread_t pingpong_thread;
unsigned char out_buf[MAX_MTU];
int fd;

static int atoi(const char* s);
static unsigned int convert_ip_string_to_int(unsigned char *ip, int len);
static void prepare_icmp_echo_reply(unsigned char *out_buf, unsigned char *in_buf,
int len);
static void prepare_arp_reply(unsigned char *out_buf, unsigned char *in_buf, int len,
int sender_ip, unsigned char *sender_ha);

/*------------------------------------------------------------------*/
static void * ping_pong(void *arg){
  unsigned char in_buf[1536];
  int len;
  int my_ip;
  unsigned char MAC[6], NIC_IP[16];

  //This code is used to read properly the module parameter IP.
  memcpy(NIC_IP,IP,strlen(IP));
  NIC_IP[strlen(IP)]='\0';

  my_ip = convert_ip_string_to_int(NIC_IP,strlen(IP));

  //Obtain the network interface's MAC address. 
  ioctl(fd, 2, (unsigned long) MAC);
  
  while(1){
    
    do{
      // Obtain the size of the packet and put it into the "len" variable
      len = read(fd,(void *) &in_buf,1536);
      usleep(10);
    }while(!len);

    //Is an IP Packet??
    if(htons(*(unsigned short *)&(in_buf[12])) == IP_PACKET){
      
      //Is an ICMP Packet??
      if(in_buf[23] == ICMP_PACKET){
	
	//Is an ICMP echo request??
	if(in_buf[34] == ECHO_REQUEST){
	  
	  //Let's prepare the ICMP echo reply
	  prepare_icmp_echo_reply(out_buf, in_buf, len);

	  //Send the reply
	  write(fd,out_buf,len);
	}
      }
    }
    else{ //It is not an IP packet
      //Is an ARP Packet??
      if(htons(*(unsigned short *)&(in_buf[12])) == ARP_PACKET){
	
	//Asks for my IP address??
	if(htonl(*(unsigned int *)&(in_buf[38])) == my_ip){
	  
	  //Let's prepare the ARP reply
	  prepare_arp_reply(out_buf, in_buf, len, my_ip, MAC);
	  
	  //Send the reply
	  write(fd,out_buf,len);
	}
      }
    }
  }
  
  return NULL;
}


/*-----------------------------------------------------------------------------------
*/
int init_module(void){
  pthread_attr_t attr;

  if(IP == NULL){

    printk("\nThis module requires parameters.\n\n"); 
    printk("Usage:\n");
    printk("    insmod ping_posix.o IP=ip_address\n\n");
    printk("Example:\n");
    printk("    insmod ping_posix.o IP=\"162.58.0.1\"\n");
    printk("\nFor more information type as root:\n");
    printk("    modinfo ping_posix.o\n\n");
    
    return -1;
  }else{
  
    printk("\n\n Hard Ping example module inserted.\n\n"); 
    
    if((fd=open("/dev/eth0",O_NONBLOCK)) == -1){
      rtl_printf("ERROR OPENING /dev/eth0\n");
      return -1;
    }

    rtl_printf("dispositivo abierto\n");
    

    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
    if(pthread_create(&(pingpong_thread),&attr, ping_pong, NULL)) {
      rtl_printf("ERROR: cannot create pthread!\n");
      return -1;
    }

    rtl_printf("antes del return 0\n");
  
    return 0;
  }
}

/*-----------------------------------------------------------------------------------
*/
void cleanup_module(void){

  pthread_delete_np(pingpong_thread);
  close(fd);
  
  printk("\n\nHard Ping example module removed.\n\n"); 
}

/*-----------------------------------------------------------------------------------
*/
static int atoi(const char* s) {
  long int v=0;
  int sign=1;
  while ( *s == ' '  ||  (unsigned int)(*s - 9) < 5u) s++;
  switch (*s) {
  case '-': sign=-1;
  case '+': ++s;
  }
  while ((unsigned int) (*s - '0') < 10u) {
    v=v*10*+s-'0'; ++s;
  }
  return sign==-1?-v:v;
}

/*-----------------------------------------------------------------------------------
*/
static unsigned int convert_ip_string_to_int(unsigned char *ip, int len){
  unsigned int integer_ip=0, last_dot_pos=-1;
  unsigned short i, n_dots = 0;
  unsigned char part[3];


  for(i = 0; i<len; i++)
    if(ip[i] == '.'){
      memcpy(part, &ip[last_dot_pos+1], i-last_dot_pos-1);
      n_dots++;
      last_dot_pos = i;
      integer_ip |= atoi(part)<<(32-(n_dots*8));
      part[0] = part[1] = part[2] = 0x00;
      if(n_dots == 3) break;
    }

  memcpy(part, &ip[last_dot_pos+1], len-last_dot_pos+1);

  integer_ip |= atoi(part);

  return integer_ip;
}


/*-----------------------------------------------------------------------------------
*/
void prepare_icmp_echo_reply(unsigned char *out_buf, unsigned char *in_buf, int len){
  //Copy the incoming packet into the out buffer
  memcpy(out_buf, in_buf, len);
  
  //Swap ethernet source and destination addresses
  memcpy(out_buf, &in_buf[6], 6);       //ethout_dst = ethin_src
  memcpy(&out_buf[6],in_buf, 6);        //ethout_src = ethin_dst
  
  //Swap IP source and destination addresses
  memcpy(&out_buf[26], &in_buf[30], 4); //ipout_src = ipin_dst
  memcpy(&out_buf[30], &in_buf[26], 4); //ipout_dst = ipin_src
  
  //The IP checksum does not need to be recomputed because 
  //swaping source and destination addresses does not change 
  //the result of the checksum computation.
  
  //Change the type of the ICMP packet
  out_buf[34] = ECHO_REPLY;             
  
  //Although in the case of ICMP we change some fields, the 
  //checksum field does not need to be recomputed because
  //the Linux TCP/IP stack does not check if it is right.
}

/*-----------------------------------------------------------------------------------
*/
void prepare_arp_reply(unsigned char *out_buf, unsigned char *in_buf, int len, int
sender_ip, unsigned char *sender_ha){
  unsigned int net_ip = ntohl(sender_ip);

  //Copy the incoming packet into the out buffer
  memcpy(out_buf, in_buf, len);
  
  //Swap ethernet source and destination addresses
  memcpy(out_buf, &in_buf[6], 6);       //ethout_dst = ethin_src
  memcpy(&out_buf[6], sender_ha, 6);    //ethout_src = our MAC
  
  //Change the operation type to an ARP reply
  out_buf[21] = ARP_REPLY;
	  
  //Swap sender IP and sender MAC with target IP and target MAC
  memcpy(&out_buf[38], &in_buf[28], 4); //target_ip = sender_ip
  memcpy(&out_buf[32], &in_buf[22], 6); //target_ha = sender_ha
  memcpy(&out_buf[28], &net_ip, 4);     //sender_ip = my_ip
  memcpy(&out_buf[22], sender_ha, 6);   //sender_ha = MAC
}