chanmon (OpenWrt) C source Code

---

This code is from project: OpenWrt antenna rotator

---

main.h

#ifndef _H_MAIN_H
#define _H_MAIN_H




#endif

main.c

#include <stdio.h>
#include <pcap.h>
#include <errno.h>
#include <signal.h>
#include <getopt.h>
#include <iwlib.h>
#include <pthread.h>

#include "packet.h"
#include "print.h"
#include "utils.h"
#include "track.h"
#include "serial.h"


pcap_t* descr  = 0;
int sock       = 0;


/****************************************************************************/
/* Cleaning function. Closing descriptors, deleting sockets etc.     l      */
/****************************************************************************/
void cleanup(int signum) {

   
   if(signum == 0)
      printf("Quit after track\n");
   else
      printf("Caught signal %d\n",signum);

   if (descr != 0)
      pcap_close(descr);

   if (sock != 0)
      iw_sockets_close(sock);

   // dodac ifa
   closeThread(THREAD_CLOSE);
   closeSerial();

   exit(0);
}


/****************************************************************************/
/* Main                                                                     */
/****************************************************************************/
int main(int argc, char **argv)
{
   char errbuf[PCAP_ERRBUF_SIZE] = "";
   char bssid_filter[BSSID_LEN] = "";
   char dev[8] = "";

   unsigned char *packet;
   unsigned char *packet_free;
   struct pcap_pkthdr hdr;    // pcap.h

   struct packetShortInfo pSI;

   int channel = 0;
   int delay = 1000;
   static int nofollow_flag = 1;
   static int track_signal = 0;

   static struct option long_options[] = {
      {"bssid",  required_argument, 0, 'b'},
      {"interface",  required_argument, 0, 'i'},
      {"channel",  required_argument, 0, 'c'},
      {"delay",  required_argument, 0, 'd'},
      {"nofollow", no_argument,  &nofollow_flag, 0},
      {"track", no_argument,  &track_signal, 1}
   };

   int option_index = 0;
   int c;

   int thrRes = 0;
   pthread_t   stepThread;


   // init serial port
   if (initSerial(SERIAL_DEVICE) != 0) {
      exit(1);
   }


   // parse options
   while ( (c = getopt_long(argc, argv, "i:b:c:",
                           long_options, &option_index) ) != -1) 
      switch (c) {

         case 0:
            if (long_options[option_index].flag != 0)
               break;
            printf("Unknown option\n");
            exit(2);

         // bssid
         case 'b':
               if (strlen(optarg) != BSSID_LEN) {
                  printf("Wrong len of bssid\n");
                  exit(3);
               }
               strcpy(bssid_filter, optarg);
            break;

         // interface name
         case 'i':
               strcpy(dev, optarg);
            break;

         // channel
         case 'c':
               channel = atoi(optarg);
            break;

         // scan delay
         case 'd':
               delay = atoi(optarg);
            break;

         case '?':
            exit(2);
            break;

         default:
            printf("aborting %d \n", c);
            exit(2);
      }

   // if channel was in the command line, set dedicated channel
   if (channel > 0) {
      setChannel(&sock, dev, channel);
   }

   if(strlen(dev) == 0) {
      printf("Interface name is not set\n");
      exit(3);
   }

   // open pcap capture
   if ((descr = pcap_open_live(dev, BUFSIZ, 0, 1000, errbuf)) == NULL) {
      printf("pcap_open_live(): %s\n",errbuf);
   }

   // attach own function on CTRL-C  or kill -2
   signal(SIGINT, cleanup);

   // if track signal is enabled, start thread to control stepper motor
   if(track_signal) { 
      thrRes = pthread_create(&stepThread, NULL, changeAntPosThr,
                             (void *) delay);

      if (thrRes) {
         perror("Thraed Create");
         cleanup(-2);
      }
   }


   while(1) {

      // if there is no signal, then write info on console      
      if ((packet = (unsigned char *) pcap_next(descr,&hdr)) == NULL) {
         printf("No signal\n");
         continue;
      }
      

      // if we had some packet and it's a good one, start to process
      if(packetProc(packet, hdr.len, bssid_filter, &pSI) == 1) {

        // store signal levels
        if (track_signal)
            track(&pSI);

         // print packet info, bssid, channel, power, etc.
         packetStatPrint(&pSI);
      
         /*  We can also change channel dynamicaly but this sometimes
          *  is not working as it suppose to. Somtetimes channel
          *  section of packet, can be malformed (broken packet)
          */
         if(strlen(bssid_filter) > 0 && nofollow_flag) {

            // different channel
            if (*pSI.channel != channel) {
               channel = *pSI.channel;

               printf("\nChanging to channel %d "
                      "from beacon frame.\n\n", channel);

               // set new channel, found in current packet
               setChannel(&sock, dev, channel);
            }
         }
      }

      // at the end we do cleanup
      if(track_signal && getStatus() == MOVE_END)
         cleanup(0);
   }


   pthread_exit(NULL);
   return 0;
}

packet.h

#ifndef _H_PACKET_H
#define _H_PACKET_H


#define FRAME_BEACON             0x80 
#define BEACON_SIZE              24
#define MGT_SIZE_WITH_SID_ONLY   14


#define MAX_SID_LEN              32
#define BSSID_LEN                17


// different manufacture has
// other monitor packet headers

#ifdef REALTEK
   // realtek

   #define RADIOTAP_SIZE   24
   struct radiotap {
      unsigned char dummy_1[10];
      unsigned char channel[2];
      unsigned char dummy_2[2];
      signed char signal;
      unsigned char dummy_3[9];
   };
#else
   // atheros

   #define RADIOTAP_SIZE   26
   struct radiotap {
      unsigned char dummy_1[18];
      unsigned char channel[2];
      unsigned char dummy_2[2];
      signed char signal;
      unsigned char dummy_3[3];
   };
#endif


struct beacon {
   unsigned char dummy_1[4];
   unsigned char dst[6];
   unsigned char src[6];
   unsigned char bssid[6];
   unsigned char dummy_2[2];
};


struct management {
   unsigned char dummy_1[12];
   unsigned char sid_flag;
   unsigned char sid_len;
   unsigned char tag;
   unsigned char tag_len;
   unsigned char channel;
};


struct packetShortInfo {
   char           bssid[BSSID_LEN];
   signed char    *signal;
   unsigned char  *channel;
   unsigned char  *sid_len;
   char           sid[MAX_SID_LEN + 1];
};


int packetProc(unsigned char *, unsigned int,
               char *,
               struct packetShortInfo *);


#endif

packet.c

#include <stdio.h>
#include <string.h>

#include "packet.h"
#include "print.h"


// Frequency channel array, currently useless
int freq[14] = {
   2412,
   2417,
   2422,
   2427,
   2432,
   2437,
   2442,
   2447,
   2452,
   2457,
   2462,
   2467,
   2472,
   -1
};


/****************************************************************************/
/* Here, parsing packets is made.                                           */
/* We look only for BEACON frames.                                          */
/****************************************************************************/
int packetProc(unsigned char *ptr, unsigned int len,
               char *bssid_filter,
               struct packetShortInfo *pSI) {

   // set valid flag to 0 -> packet invalid
   int valid = 0;

   struct radiotap   *rt;
   struct beacon     *bc;
   struct management *mgt;

   // debug
//    packetPrint(ptr, len);
 
   // getting radiotap header
   bzero(pSI, sizeof(struct packetShortInfo));

   rt = (struct radiotap *) ptr;

   // moving forward
   ptr = ptr + RADIOTAP_SIZE;

   // check type of packet
   switch(*ptr) {
      case FRAME_BEACON:
         // getting beacon header 
         bc = (struct beacon *) ptr;

         // moving forward
         ptr = ptr +  BEACON_SIZE;

         setMac(pSI->bssid, bc->bssid);

         if( strlen(bssid_filter) > 0 &&
             (strstr(pSI->bssid, bssid_filter) == NULL)
           ) {
            return valid;
         }


         mgt = (struct management *) ptr;
         ptr = ptr + MGT_SIZE_WITH_SID_ONLY; 

         if (mgt->sid_flag == 0) 
            setSid(pSI->sid, ptr, mgt->sid_len);
        
         // checking for broken packets
         if (mgt->sid_len > MAX_SID_LEN) {
            return valid;
         }

         // moving forward - after whole ESSID name
         ptr = ptr + mgt->sid_len;

         // getting tag
         mgt->tag       =  *ptr;
         
         // moving forward behind tag
         ptr = ptr + 1;

         mgt->tag_len   =  *ptr;
         
         // moving forward behind tag length and 
         // network capabilities (transfer modes)
         ptr = ptr + mgt->tag_len + 3;

         // getting channel
         mgt->channel   =  *ptr;
     
         // chechking for broken packets 
         if (mgt->channel > 13 || mgt->channel < 1) {
            return valid;
         }

         // debug
//          printf("%s;%d;%d;%d;%s\n", pSI->bssid, rt->signal,
//                                     mgt->channel, mgt->sid_len, sid);
//
         // fill packet info structure (by pointer)                                    
         pSI->signal = &rt->signal;
         pSI->channel = &mgt->channel;
         pSI->sid_len = &mgt->sid_len;

         // set packet as valid      
         valid = 1;

         break;
   };

   return valid;
}

print.h

#ifndef _H_PRINT_H
#define _H_PRINT_H

#include "packet.h"


void packetStatPrint(struct packetShortInfo *);
void packetPrint(unsigned char *, unsigned int);


#endif

print.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "print.h"
#include "packet.h"


/****************************************************************************/
/* Prints packet in hexadecimal                                             */
/****************************************************************************/
void packetPrint(unsigned char *ptr, unsigned int len) {
   int i;

   for(i=1; i <= len; i++) {
      printf("%02x ", ptr[i-1]);

      if (i % 8 == 0)
         printf("  ");

      if (i % 16 == 0)
         printf("\n");
   }
   printf("\n");

}


/****************************************************************************/
/* Prints network information                                               */
/****************************************************************************/
void packetStatPrint(struct packetShortInfo *pSI) {
   printf("%s;%d;%d;%d;%s\n", 
         pSI->bssid,
         *pSI->signal,
         *pSI->channel,
         *pSI->sid_len,
         pSI->sid);
}

serial.h

#ifndef _H_SERIAL_H
#define _H_SERIAL_H


#define SERIAL_DEVICE      "/dev/ttyATH0"
#define SERIAL_SPEED       B9600

#define RECV_RST           1
#define RECV_ERR           2
#define RECV_OK            3
#define RECV_TIMEOUT       4


int initSerial(char *);
int writeCmd(char *);
int readAnswer(int);
int closeSerial(void);
int serialCmd(unsigned char, unsigned char);


#endif

serial.c

#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>

#include "serial.h"


static int devHandle = -2;


/****************************************************************************/
/* Init serial port                                                         */
/****************************************************************************/
int initSerial(char *device) {
   
   struct termios opt;
   
   devHandle = open(device, O_RDWR| O_NOCTTY | O_NDELAY | O_SYNC);

   if (devHandle == -1) {
      perror("Unable to open device");
      return 1;
   }
   
   fcntl(devHandle, F_SETFL, FNDELAY);

   if (tcgetattr(devHandle, &opt) == -1) {
      perror("Unable to obtain device info");
      return 1;      
   }
   
   if (cfsetispeed(&opt, SERIAL_SPEED) !=0 ) {
      perror("Unable to set RX speed");
      return 1; 
   }

   if (cfsetospeed(&opt, SERIAL_SPEED) != 0) {
      perror("Unable to set TX speed");
      return 1;       
   }

   opt.c_cflag |= (CLOCAL | CREAD);

   opt.c_cflag &= ~PARENB;
   opt.c_cflag &= ~CSTOPB;
   opt.c_cflag &= ~CSIZE;
   opt.c_cflag |= CS8;
   
   opt.c_lflag &= ~(ICANON | ECHO);
   opt.c_oflag &= ~OPOST;

   if (tcsetattr(devHandle, TCSANOW, &opt) == -1) {
      perror("Unable to setup port");
      return 1;      
   }

   // cleaning rs232 buffer
   printf("Cleaning buffer, timeout is ok.\n");
   readAnswer(2); 

   return 0;
}


/****************************************************************************/
/* Write string to port                                                     */
/****************************************************************************/
int writeCmd(char *s) {
   
    if (write(devHandle, s, strlen(s)) == -1) {
      perror("Write error");
      return 1;
   }

   return 0;
}


/****************************************************************************/
/* Read answers from RS232 port                                             */
/****************************************************************************/
int readAnswer(int readSecWait) {
   
   char recvBuff = ' '; 
   int readBytes= 0;
   
   time_t   startTime;
   time_t   currentTime;
   
   time(&startTime);
   time(&currentTime);
      
   while(1) {
      recvBuff = ' ';      

      readBytes = read(devHandle, &recvBuff, 1);

      if (readBytes > 0) {
         
         switch(recvBuff) {
            
            case 'R': {
               return RECV_RST;
            } break;
            
            case 'O': {
               return RECV_OK;
            } break;
            
            case 'E': {
               printf("Communication error\n");
               return RECV_ERR;
            } break;
            
            default: {
               printf("Unknown answer\n");
               return RECV_ERR;
            }
         };
      }
      
      if (difftime(currentTime, startTime) >= readSecWait)
         break;
      
      time(&currentTime);
   }

   printf("Read timeout\n");
   return RECV_TIMEOUT;
}


/****************************************************************************/
/* Sending command to device connected to RS232                             */
/****************************************************************************/
int serialCmd(unsigned char x, unsigned char readWait) {
   
   char cmdBuff[4];
  
   // send 'R' - start transmission 
   writeCmd("R");

   if(readAnswer(readWait) == RECV_RST) {
      
      // prepare and send position information
      sprintf(cmdBuff, "%02dH", x);   
      writeCmd(cmdBuff);

      if(readAnswer(readWait) == RECV_OK)
         return 0;
      else
         printf("Without position confirmation !\n"); 
   } else
      printf("Without restet confirmation !\n"); 

   return 1;   
}


/****************************************************************************/
/* Cleanup                                                                  */
/****************************************************************************/
int closeSerial() {

   if(devHandle != -2 && close(devHandle) != 0) {
      perror("Closing device handle");
         return 1;
   }
   
   return 0;
}

track.h

#ifndef _H_TRACK_H
#define _H_TRACK_H

#define MAX_X_POS       90

#define MOVE_BUSY       0
#define MOVE_IDLE       1
#define MOVE_END        2

#define MIN_SIGNAL      -100 

#define THREAD_CLOSE    1
#define THREAD_STATUS   0

#define P_FACTOR        3 

#include "packet.h"


struct sigInfo {
   char signal;
   unsigned short cnt;
};

int track(struct packetShortInfo *);
int setX(void);
void *changeAntPosThr(void *);
void setBestXPos(void);
void initSigArray(void);
unsigned char getStatus();
unsigned char closeThread(unsigned char);

#endif

track.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>  // bzero
#include <unistd.h>
#include <pthread.h>

#include "track.h"
#include "packet.h"
#include "serial.h"

static unsigned char xPos;
static unsigned char statusFlag = MOVE_BUSY;
static struct sigInfo si[MAX_X_POS];


/****************************************************************************/
/* Prepare signal array                                                     */
/****************************************************************************/
void initSigArray() {
   for(xPos = 0; xPos < MAX_X_POS; xPos++)
      si[xPos].signal = MIN_SIGNAL;
}


/****************************************************************************/
/* Tracking signal function                                                 */
/****************************************************************************/
int track(struct packetShortInfo *pSI) {

   // if currently antenna is on the move, quit
   if(getStatus() == MOVE_BUSY)
      return 1;

   // fetch signal at ritght possition in signal array.
   if (si[xPos].signal = -100) 
      si[xPos].signal = *pSI->signal;
   else 
      si[xPos].signal = (si[xPos].signal + *pSI->signal) / 3;
   
   // update packet counter
   si[xPos].cnt++;

   return 0;
}


/****************************************************************************/
/* Setting antenna position -> sending data to RS232 port                   */
/****************************************************************************/
int setX() {
   if (serialCmd(xPos, 10) != 0)
      return 1;

   printf("Set X: %d\n", xPos);

   return 0;
}


/****************************************************************************/
/* Thread, which changes antenna position with given delay.                 */
/****************************************************************************/
void *changeAntPosThr(void *dPtr) {
   
   int *delay = (int *) &dPtr;

   initSigArray();

   xPos = 0;
   setX();

   for(xPos = 0; xPos < MAX_X_POS; xPos++) {
      statusFlag = MOVE_BUSY;
         setX();
      statusFlag = MOVE_IDLE;

      usleep(*delay * 1000);
      if(closeThread(THREAD_STATUS) == THREAD_CLOSE) {
         pthread_exit(NULL);
         return;
      }
   }

   setBestXPos();

   pthread_exit(NULL);
}



/****************************************************************************/
/* Thread cleanup                                                           */
/****************************************************************************/
unsigned char closeThread(unsigned char cmd) {
   static unsigned char running;

   if(cmd == THREAD_CLOSE && running == 0) {
      running = THREAD_STATUS;
   }

   return running;
}


/****************************************************************************/
/* Calculating best antenna position function, by data saved in signal      */
/* array.                                                                   */
/****************************************************************************/
void setBestXPos() {

   // for signal and packet count criterium
   char           globSigMax     = -100;
   unsigned char  globSigMaxPos  = 0;
   unsigned short globCntMax     = 0;
   unsigned short globCntAvg     = 0;
   unsigned char  globCntAvgCnt  = 0;
   unsigned int   globCntAvgSum  = 0;
   unsigned char  globCntMaxPos  = 0;

   // for static range criterium
   unsigned char range           = 0;
   unsigned char mRange          = 0;
   unsigned char eRangePos       = 0;
   unsigned char bRangePos       = 0;
   unsigned char cRangePos       = 0;

   // find global maximum and max packiet count
   for(xPos = 0; xPos < MAX_X_POS; xPos++) {

      if (globSigMax < si[xPos].signal) {
         globSigMax = si[xPos].signal;
         globSigMaxPos = xPos;
      }

      if (globCntMax < si[xPos].cnt) {
         globCntMax = si[xPos].cnt;
         globCntMaxPos = xPos;
      }

      globCntAvgSum+=si[xPos].cnt;

      if(si[xPos].cnt > 0) {
         globCntAvgCnt++;
      }

      printf("signal at %d is %d, count %d\n",
             xPos, si[xPos].signal, si[xPos].cnt);
   }


   globCntAvg = globCntAvgSum / globCntAvgCnt;


   printf("Best signal %d at %d\n", globSigMax, globSigMaxPos);

   printf("Packet sum %d, avg %d\n", globCntAvgSum, globCntAvg);


   // once again check global maximum, but focus on cnt over avg
   // find global maximum and max packiet count
   
   globSigMax = -100;
   globSigMaxPos = 0;
   for(xPos = 0; xPos < MAX_X_POS; xPos++) {

      if (si[xPos].cnt >= globCntAvg)
         if (globSigMax < si[xPos].signal) {
            globSigMax = si[xPos].signal;
            globSigMaxPos = xPos;
         }
   }

   printf("After correction, best signal %d at %d\n",
          globSigMax, globSigMaxPos);


   // find local max static signal 
   for(xPos = 0; xPos < MAX_X_POS -1; xPos++) {

      if (si[xPos].signal != -100 && si[xPos].signal == si[xPos +1].signal ) {
         range++;

         if(mRange <= range) {
            mRange = range;
            eRangePos = xPos + 1;
         }
      } else
         range = 0;
   }

   bRangePos = eRangePos - mRange;
   cRangePos = bRangePos + ((eRangePos - bRangePos) / 2);

   printf("Widest range  from %d, to %d, center is %d\n",
          bRangePos, eRangePos, cRangePos);

   // cheking signal strength, not position
   if(si[eRangePos].signal == globSigMax) {
      printf("Maximum is in the widest range\n");
      printf("Setting center from the widest range\n");
      xPos = cRangePos;
   }
   else {

      printf("Maximum is in the outer range\n");
      printf("Setting best pos from global maximum\n");
      xPos = globSigMaxPos;
   }

   setX();

   printf("Sleepping for 5 sec.\n");
   sleep(5);

   statusFlag = MOVE_END;
}


/****************************************************************************/
/* Retrun status                                                            */
/****************************************************************************/
unsigned char getStatus() {
   return statusFlag;
}

utils.h

#ifndef _H_UTILS_H
#define _H_UTILS_H




int setChannel(int *, char *, int);
void setMac(char *, unsigned char *);
void setSid(char *, unsigned char *, unsigned char);
// int parseChannel(unsigned char *ptr) {



#endif

utils.c

#include <stdio.h>
#include <iwlib.h>

#include "utils.h"
#include "packet.h"


/****************************************************************************/
/* Setting channel on given interface                                       */
/****************************************************************************/
int setChannel(int *sock, char *dev, int channel) {
   struct iwreq wrq;

   iw_float2freq(channel, &wrq.u.freq);
   wrq.u.freq.flags = IW_FREQ_FIXED;

   *sock = iw_sockets_open();

   if(iw_set_ext(*sock, dev, SIOCSIWFREQ, &wrq))
      return 1;
   else
      return 0;

   return 1;
}


/****************************************************************************/
/* Setting global bssid                                                     */
/****************************************************************************/
void setMac(char *bssid, unsigned char *ptr) {
   sprintf(bssid, "%02X:%02X:%02X:%02X:%02X:%02X",
                  ptr[0], ptr[1], ptr[2], ptr[3], ptr[4], ptr[5]);
}


/****************************************************************************/
/* Setting global essid name                                                */
/****************************************************************************/
void setSid(char *sid, unsigned char *ptr, unsigned char len) {
   bzero(sid, MAX_SID_LEN + 1);
   strncpy(sid, ptr, len);
   sid[len] = '\0';
}


/****************************************************************************/
/* Parsing radiotap header channel, useless                                 */
/****************************************************************************/
// int parseChannel(unsigned char *ptr) {
//    int chan; 
// 
//    for(chan=0; chan<15; chan++)
//       if(freq[chan] == (int) (ptr[1] * 16 * 16 + ptr[0]))
//          break;
// 
//    if (chan == 14)
//       chan = -1;
//    return chan+1;
// }

Makefile

PROJECT_NAME=chanmon

OBJECTS=main.o packet.o print.o utils.o track.o serial.o

INCLUDES=
LIBS=-lpcap -liw -lpthread
MACROS = -D$(REALTEK)


all: clean cls $(PROJECT_NAME)

$(PROJECT_NAME): $(OBJECTS)
 $(CC) -o $(PROJECT_NAME) $(OBJECTS) $(LDFLAGS) $(LIBS) $(MARCOS) $(INCLUDES)
  
main.o: main.c main.h track.c track.h
 $(CC) $(CFLAGS) $(INCLUDES) $(MACROS) -c main.c

packet.o: packet.c packet.h
 $(CC) $(CFLAGS) $(INCLUDES) $(MACROS) -c packet.c

print.o: print.c print.h
 $(CC) $(CFLAGS) $(INCLUDES) $(MACROS) -c print.c

utils.o: utils.c utils.h
 $(CC) $(CFLAGS) $(INCLUDES) $(MACROS) -c utils.c

track.o: track.c track.h
 $(CC) $(CFLAGS) $(INCLUDES) $(MACROS) -c track.c

serial.o: serial.c serial.h
 $(CC) $(CFLAGS) $(INCLUDES) $(MACROS) -c serial.c

clean:
  rm -f $(PROJECT_NAME) $(OBJECTS)

cls:
  clear