pico_ipfilter.c

00001 /*********************************************************************
00002 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
00003 See LICENSE and COPYING for usage.
00004 
00005 Authors: Simon Maes
00006 *********************************************************************/
00007 
00008 #include "pico_ipv4.h"
00009 #include "pico_config.h"
00010 #include "pico_icmp4.h"
00011 #include "pico_stack.h"
00012 #include "pico_eth.h"
00013 #include "pico_socket.h"
00014 #include "pico_device.h"
00015 #include "pico_ipfilter.h"
00016 #include "pico_tcp.h"
00017 #include "pico_udp.h"
00018 
00019 
00020 //#define ipf_dbg dbg
00021 #define ipf_dbg(...) do{}while(0)
00022 
00023 struct filter_node;
00024 typedef int (*func_pntr)(struct filter_node *filter, struct pico_frame *f);
00025 
00026 struct filter_node {
00027   struct pico_device *fdev;
00028   struct filter_node *next_filter;
00029   uint32_t out_addr;
00030   uint32_t out_addr_netmask;
00031   uint32_t in_addr;
00032   uint32_t in_addr_netmask;
00033   uint16_t out_port;
00034   uint16_t in_port;
00035   uint8_t proto;
00036   int8_t priority;
00037   uint8_t tos;
00038   uint8_t filter_id;
00039   func_pntr function_ptr;
00040 };
00041 
00042 static struct filter_node *head = NULL;
00043 static struct filter_node *tail = NULL;
00044 
00045 /*======================== FUNCTION PNTRS ==========================*/
00046 
00047 static int fp_accept(struct filter_node *filter, struct pico_frame *f) {return 0;}
00048 
00049 static int fp_priority(struct filter_node *filter, struct pico_frame *f) {
00050 
00051   //TODO do priority-stuff
00052   return 0;
00053 }
00054 
00055 static int fp_reject(struct filter_node *filter, struct pico_frame *f) {
00056 // TODO check first if sender is pico itself or not
00057   ipf_dbg("ipfilter> #reject\n");
00058   pico_icmp4_packet_filtered(f);
00059   pico_frame_discard(f);
00060   return 1;
00061 }
00062 
00063 static int fp_drop(struct filter_node *filter, struct pico_frame *f) {
00064 
00065   ipf_dbg("ipfilter> # drop\n");
00066   pico_frame_discard(f);
00067   return 1;
00068 }
00069 
00070 /*============================ API CALLS ============================*/
00071 int pico_ipv4_filter_add(struct pico_device *dev, uint8_t proto, struct pico_ip4 *out_addr, struct pico_ip4 *out_addr_netmask, struct pico_ip4 *in_addr, struct pico_ip4 *in_addr_netmask, uint16_t out_port, uint16_t in_port, int8_t priority, uint8_t tos, enum filter_action action)
00072 {
00073   static uint8_t filter_id = 0;
00074   struct filter_node *new_filter;
00075 
00076   if ( !(dev != NULL || proto != 0 || (out_addr != NULL && out_addr->addr != 0U) || (out_addr_netmask != NULL && out_addr_netmask->addr != 0U)|| (in_addr != NULL && in_addr->addr != 0U) || (in_addr_netmask != NULL && in_addr_netmask->addr != 0U)|| out_port != 0 || in_port !=0 || tos != 0 )) {
00077     pico_err = PICO_ERR_EINVAL;
00078     return -1;
00079   }
00080   if ( priority > 10 || priority < -10) {
00081     pico_err = PICO_ERR_EINVAL;
00082     return -1;
00083   }
00084   if (action > 3 || action < 0) {
00085     pico_err = PICO_ERR_EINVAL;
00086     return -1;
00087   }
00088   ipf_dbg("ipfilter> # adding filter\n");
00089 
00090   new_filter = pico_zalloc(sizeof(struct filter_node));
00091   if (!head) {
00092     head = tail = new_filter;
00093   } else {
00094     tail->next_filter = new_filter;
00095     tail = new_filter;
00096   }
00097 
00098   new_filter->fdev = dev;
00099   new_filter->proto = proto;
00100   if (out_addr != NULL)
00101     new_filter->out_addr = out_addr->addr;
00102   else
00103     new_filter->out_addr = 0U;
00104 
00105   if (out_addr_netmask != NULL)
00106     new_filter->out_addr_netmask = out_addr_netmask->addr;
00107   else
00108     new_filter->out_addr_netmask = 0U;
00109 
00110   if (in_addr != NULL)
00111     new_filter->in_addr = in_addr->addr;
00112   else
00113     new_filter->in_addr = 0U;
00114  
00115   if (in_addr_netmask != NULL)
00116     new_filter->in_addr_netmask = in_addr_netmask->addr;
00117   else
00118     new_filter->in_addr_netmask = 0U;
00119 
00120   new_filter->out_port = out_port;
00121   new_filter->in_port = in_port;
00122   new_filter->priority = priority;
00123   new_filter->tos = tos;
00124   new_filter->filter_id = filter_id++;
00125 
00126   /*Define filterType_functionPointer here instead of in ipfilter-function, to prevent running multiple times through switch*/
00127   switch (action) {
00128     case FILTER_ACCEPT:
00129       new_filter->function_ptr = fp_accept;
00130       break;
00131     case FILTER_PRIORITY:
00132       new_filter->function_ptr = fp_priority;
00133       break;
00134     case FILTER_REJECT:
00135       new_filter->function_ptr = fp_reject;
00136       break;
00137     case FILTER_DROP:
00138       new_filter->function_ptr = fp_drop;
00139       break;
00140     default:
00141       ipf_dbg("ipfilter> #unknown filter action\n");
00142       break;
00143   }
00144   return new_filter->filter_id;
00145 }
00146 
00147 int pico_ipv4_filter_del(uint8_t filter_id)
00148 {
00149   struct filter_node *work;
00150   struct filter_node *prev;
00151 
00152   if (!tail || !head) {
00153     pico_err = PICO_ERR_EPERM;
00154     return -1;
00155   }
00156 
00157   work = head;
00158   if (work->filter_id == filter_id) {
00159       /*delete filter_node from linked list*/
00160       head = work->next_filter;
00161       pico_free(work);
00162       return 0;
00163   }
00164   prev = work;
00165   work = work->next_filter;
00166 
00167   while (1) {
00168     if (work->filter_id == filter_id) {
00169         if (work != tail) {
00170         /*delete filter_node from linked list*/
00171         prev->next_filter = work->next_filter;
00172         pico_free(work);
00173         return 0;
00174         } else {
00175           prev->next_filter = NULL;
00176           pico_free(work);
00177           return 0;
00178         }
00179     } else {
00180       /*check next filter_node*/
00181       prev = work;
00182       work = work->next_filter;
00183       if (work == tail) {
00184         pico_err = PICO_ERR_EINVAL;
00185         return -1;
00186       }
00187     }
00188   }
00189 }
00190 
00191 /*================================== CORE FILTER FUNCTIONS ==================================*/
00192 int match_filter(struct filter_node *filter, struct pico_frame *f)
00193 {
00194   struct filter_node temp;
00195   struct pico_ipv4_hdr *ipv4_hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00196   struct pico_tcp_hdr *tcp_hdr;
00197   struct pico_udp_hdr *udp_hdr;
00198 
00199   if (!filter|| !f) {
00200     ipf_dbg("ipfilter> ## nullpointer in match filter \n");
00201     return -1;
00202   }
00203 
00204   temp.fdev = f->dev;
00205   temp.out_addr = ipv4_hdr->dst.addr;
00206   temp.in_addr = ipv4_hdr->src.addr;
00207   if (ipv4_hdr->proto == PICO_PROTO_TCP ) {
00208       tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
00209       temp.out_port = short_be(tcp_hdr->trans.dport);
00210       temp.in_port = short_be(tcp_hdr->trans.sport);
00211   }else if (ipv4_hdr->proto == PICO_PROTO_UDP ) {
00212       udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
00213       temp.out_port = short_be(udp_hdr->trans.dport);
00214       temp.in_port = short_be(udp_hdr->trans.sport);
00215   } else {
00216     temp.out_port = temp.in_port = 0;
00217   }
00218   temp.proto = ipv4_hdr->proto;
00219   temp.priority = f->priority;
00220   temp.tos = ipv4_hdr->tos;
00221 
00222 
00223 
00224   if ( ((filter->fdev == NULL || filter->fdev == temp.fdev) && \
00225         (filter->in_addr == 0 || ((filter->in_addr_netmask == 0) ? (filter->in_addr == temp.in_addr) : 1)) &&\
00226         (filter->in_port == 0 || filter->in_port == temp.in_port) &&\
00227         (filter->out_addr == 0 || ((filter->out_addr_netmask == 0) ? (filter->out_addr == temp.out_addr) : 1)) && \
00228         (filter->out_port == 0 || filter->out_port == temp.out_port)  && \
00229         (filter->proto == 0 || filter->proto == temp.proto ) &&\
00230         (filter->priority == 0 || filter->priority == temp.priority ) &&\
00231         (filter->tos == 0 || filter->tos == temp.tos ) &&\
00232         (filter->out_addr_netmask == 0 || ((filter->out_addr & filter->out_addr_netmask) == (temp.out_addr & filter->out_addr_netmask)) ) &&\
00233         (filter->in_addr_netmask == 0 || ((filter->in_addr & filter->in_addr_netmask) == (temp.in_addr & filter->in_addr_netmask)) )\
00234        ) ) 
00235     return 0;
00236 
00237   //No filter match!
00238   ipf_dbg("ipfilter> #no match\n");
00239   return 1;
00240 }
00241 
00242 int ipfilter(struct pico_frame *f)
00243 {
00244   struct filter_node *work = head;
00245 
00246   /*return 1 if pico_frame is discarded as result of the filtering, 0 for an incomming packet, -1 for faults*/
00247   if (!tail || !head)  {
00248     return 0;
00249   }
00250 
00251   if ( match_filter(work, f) == 0 ) { 
00252     ipf_dbg("ipfilter> # ipfilter match\n");
00253     /*filter match, execute filter!*/
00254     return work->function_ptr(work, f);
00255   } 
00256   while (tail != work) {
00257     ipf_dbg("ipfilter> next filter..\n");
00258     work = work->next_filter;
00259     if ( match_filter(work, f) == 0 ) {
00260       ipf_dbg("ipfilter> # ipfilter match\n");
00261       /*filter match, execute filter!*/
00262       return work->function_ptr(work, f);
00263     }
00264   }
00265   return 0;
00266 }
00267