Daniele Lacamera / PicoTCP-Experimental_CDC_ECM_Branch

Free (GPLv2) TCP/IP stack developed by TASS Belgium

Fork of PicoTCP by Daniele Lacamera

modules/pico_ipfilter.c@51:18637a3d071f, 2013-08-03 (annotated)

Committer:
daniele
Date:
Sat Aug 03 08:50:27 2013 +0000
Revision:
51:18637a3d071f
Parent:
3:b4047e8a0123 
Branch for CDC-ECM: Work in progress

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daniele 3:b4047e8a0123 1 /*********************************************************************
daniele 3:b4047e8a0123 2 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
daniele 3:b4047e8a0123 3 See LICENSE and COPYING for usage.
daniele 3:b4047e8a0123 4
daniele 3:b4047e8a0123 5 Authors: Simon Maes
daniele 3:b4047e8a0123 6 *********************************************************************/
daniele 3:b4047e8a0123 7
daniele 3:b4047e8a0123 8 #include "pico_ipv4.h"
daniele 3:b4047e8a0123 9 #include "pico_config.h"
daniele 3:b4047e8a0123 10 #include "pico_icmp4.h"
daniele 3:b4047e8a0123 11 #include "pico_stack.h"
daniele 3:b4047e8a0123 12 #include "pico_eth.h"
daniele 3:b4047e8a0123 13 #include "pico_socket.h"
daniele 3:b4047e8a0123 14 #include "pico_device.h"
daniele 3:b4047e8a0123 15 #include "pico_ipfilter.h"
daniele 3:b4047e8a0123 16 #include "pico_tcp.h"
daniele 3:b4047e8a0123 17 #include "pico_udp.h"
daniele 3:b4047e8a0123 18
daniele 3:b4047e8a0123 19
daniele 3:b4047e8a0123 20 //#define ipf_dbg dbg
daniele 3:b4047e8a0123 21 #define ipf_dbg(...) do{}while(0)
daniele 3:b4047e8a0123 22
daniele 3:b4047e8a0123 23 struct filter_node;
daniele 3:b4047e8a0123 24 typedef int (*func_pntr)(struct filter_node *filter, struct pico_frame *f);
daniele 3:b4047e8a0123 25
daniele 3:b4047e8a0123 26 struct filter_node {
daniele 3:b4047e8a0123 27 struct pico_device *fdev;
daniele 3:b4047e8a0123 28 struct filter_node *next_filter;
daniele 3:b4047e8a0123 29 uint32_t out_addr;
daniele 3:b4047e8a0123 30 uint32_t out_addr_netmask;
daniele 3:b4047e8a0123 31 uint32_t in_addr;
daniele 3:b4047e8a0123 32 uint32_t in_addr_netmask;
daniele 3:b4047e8a0123 33 uint16_t out_port;
daniele 3:b4047e8a0123 34 uint16_t in_port;
daniele 3:b4047e8a0123 35 uint8_t proto;
daniele 3:b4047e8a0123 36 int8_t priority;
daniele 3:b4047e8a0123 37 uint8_t tos;
daniele 3:b4047e8a0123 38 uint8_t filter_id;
daniele 3:b4047e8a0123 39 func_pntr function_ptr;
daniele 3:b4047e8a0123 40 };
daniele 3:b4047e8a0123 41
daniele 3:b4047e8a0123 42 static struct filter_node *head = NULL;
daniele 3:b4047e8a0123 43 static struct filter_node *tail = NULL;
daniele 3:b4047e8a0123 44
daniele 3:b4047e8a0123 45 /*======================== FUNCTION PNTRS ==========================*/
daniele 3:b4047e8a0123 46
daniele 3:b4047e8a0123 47 static int fp_accept(struct filter_node *filter, struct pico_frame *f) {return 0;}
daniele 3:b4047e8a0123 48
daniele 3:b4047e8a0123 49 static int fp_priority(struct filter_node *filter, struct pico_frame *f) {
daniele 3:b4047e8a0123 50
daniele 3:b4047e8a0123 51 //TODO do priority-stuff
daniele 3:b4047e8a0123 52 return 0;
daniele 3:b4047e8a0123 53 }
daniele 3:b4047e8a0123 54
daniele 3:b4047e8a0123 55 static int fp_reject(struct filter_node *filter, struct pico_frame *f) {
daniele 3:b4047e8a0123 56 // TODO check first if sender is pico itself or not
daniele 3:b4047e8a0123 57 ipf_dbg("ipfilter> #reject\n");
daniele 3:b4047e8a0123 58 pico_icmp4_packet_filtered(f);
daniele 3:b4047e8a0123 59 pico_frame_discard(f);
daniele 3:b4047e8a0123 60 return 1;
daniele 3:b4047e8a0123 61 }
daniele 3:b4047e8a0123 62
daniele 3:b4047e8a0123 63 static int fp_drop(struct filter_node *filter, struct pico_frame *f) {
daniele 3:b4047e8a0123 64
daniele 3:b4047e8a0123 65 ipf_dbg("ipfilter> # drop\n");
daniele 3:b4047e8a0123 66 pico_frame_discard(f);
daniele 3:b4047e8a0123 67 return 1;
daniele 3:b4047e8a0123 68 }
daniele 3:b4047e8a0123 69
daniele 3:b4047e8a0123 70 /*============================ API CALLS ============================*/
daniele 3:b4047e8a0123 71 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)
daniele 3:b4047e8a0123 72 {
daniele 3:b4047e8a0123 73 static uint8_t filter_id = 0;
daniele 3:b4047e8a0123 74 struct filter_node *new_filter;
daniele 3:b4047e8a0123 75
daniele 3:b4047e8a0123 76 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 )) {
daniele 3:b4047e8a0123 77 pico_err = PICO_ERR_EINVAL;
daniele 3:b4047e8a0123 78 return -1;
daniele 3:b4047e8a0123 79 }
daniele 3:b4047e8a0123 80 if ( priority > 10 || priority < -10) {
daniele 3:b4047e8a0123 81 pico_err = PICO_ERR_EINVAL;
daniele 3:b4047e8a0123 82 return -1;
daniele 3:b4047e8a0123 83 }
daniele 3:b4047e8a0123 84 if (action > 3 || action < 0) {
daniele 3:b4047e8a0123 85 pico_err = PICO_ERR_EINVAL;
daniele 3:b4047e8a0123 86 return -1;
daniele 3:b4047e8a0123 87 }
daniele 3:b4047e8a0123 88 ipf_dbg("ipfilter> # adding filter\n");
daniele 3:b4047e8a0123 89
daniele 3:b4047e8a0123 90 new_filter = pico_zalloc(sizeof(struct filter_node));
daniele 3:b4047e8a0123 91 if (!head) {
daniele 3:b4047e8a0123 92 head = tail = new_filter;
daniele 3:b4047e8a0123 93 } else {
daniele 3:b4047e8a0123 94 tail->next_filter = new_filter;
daniele 3:b4047e8a0123 95 tail = new_filter;
daniele 3:b4047e8a0123 96 }
daniele 3:b4047e8a0123 97
daniele 3:b4047e8a0123 98 new_filter->fdev = dev;
daniele 3:b4047e8a0123 99 new_filter->proto = proto;
daniele 3:b4047e8a0123 100 if (out_addr != NULL)
daniele 3:b4047e8a0123 101 new_filter->out_addr = out_addr->addr;
daniele 3:b4047e8a0123 102 else
daniele 3:b4047e8a0123 103 new_filter->out_addr = 0U;
daniele 3:b4047e8a0123 104
daniele 3:b4047e8a0123 105 if (out_addr_netmask != NULL)
daniele 3:b4047e8a0123 106 new_filter->out_addr_netmask = out_addr_netmask->addr;
daniele 3:b4047e8a0123 107 else
daniele 3:b4047e8a0123 108 new_filter->out_addr_netmask = 0U;
daniele 3:b4047e8a0123 109
daniele 3:b4047e8a0123 110 if (in_addr != NULL)
daniele 3:b4047e8a0123 111 new_filter->in_addr = in_addr->addr;
daniele 3:b4047e8a0123 112 else
daniele 3:b4047e8a0123 113 new_filter->in_addr = 0U;
daniele 3:b4047e8a0123 114
daniele 3:b4047e8a0123 115 if (in_addr_netmask != NULL)
daniele 3:b4047e8a0123 116 new_filter->in_addr_netmask = in_addr_netmask->addr;
daniele 3:b4047e8a0123 117 else
daniele 3:b4047e8a0123 118 new_filter->in_addr_netmask = 0U;
daniele 3:b4047e8a0123 119
daniele 3:b4047e8a0123 120 new_filter->out_port = out_port;
daniele 3:b4047e8a0123 121 new_filter->in_port = in_port;
daniele 3:b4047e8a0123 122 new_filter->priority = priority;
daniele 3:b4047e8a0123 123 new_filter->tos = tos;
daniele 3:b4047e8a0123 124 new_filter->filter_id = filter_id++;
daniele 3:b4047e8a0123 125
daniele 3:b4047e8a0123 126 /*Define filterType_functionPointer here instead of in ipfilter-function, to prevent running multiple times through switch*/
daniele 3:b4047e8a0123 127 switch (action) {
daniele 3:b4047e8a0123 128 case FILTER_ACCEPT:
daniele 3:b4047e8a0123 129 new_filter->function_ptr = fp_accept;
daniele 3:b4047e8a0123 130 break;
daniele 3:b4047e8a0123 131 case FILTER_PRIORITY:
daniele 3:b4047e8a0123 132 new_filter->function_ptr = fp_priority;
daniele 3:b4047e8a0123 133 break;
daniele 3:b4047e8a0123 134 case FILTER_REJECT:
daniele 3:b4047e8a0123 135 new_filter->function_ptr = fp_reject;
daniele 3:b4047e8a0123 136 break;
daniele 3:b4047e8a0123 137 case FILTER_DROP:
daniele 3:b4047e8a0123 138 new_filter->function_ptr = fp_drop;
daniele 3:b4047e8a0123 139 break;
daniele 3:b4047e8a0123 140 default:
daniele 3:b4047e8a0123 141 ipf_dbg("ipfilter> #unknown filter action\n");
daniele 3:b4047e8a0123 142 break;
daniele 3:b4047e8a0123 143 }
daniele 3:b4047e8a0123 144 return new_filter->filter_id;
daniele 3:b4047e8a0123 145 }
daniele 3:b4047e8a0123 146
daniele 3:b4047e8a0123 147 int pico_ipv4_filter_del(uint8_t filter_id)
daniele 3:b4047e8a0123 148 {
daniele 3:b4047e8a0123 149 struct filter_node *work;
daniele 3:b4047e8a0123 150 struct filter_node *prev;
daniele 3:b4047e8a0123 151
daniele 3:b4047e8a0123 152 if (!tail || !head) {
daniele 3:b4047e8a0123 153 pico_err = PICO_ERR_EPERM;
daniele 3:b4047e8a0123 154 return -1;
daniele 3:b4047e8a0123 155 }
daniele 3:b4047e8a0123 156
daniele 3:b4047e8a0123 157 work = head;
daniele 3:b4047e8a0123 158 if (work->filter_id == filter_id) {
daniele 3:b4047e8a0123 159 /*delete filter_node from linked list*/
daniele 3:b4047e8a0123 160 head = work->next_filter;
daniele 3:b4047e8a0123 161 pico_free(work);
daniele 3:b4047e8a0123 162 return 0;
daniele 3:b4047e8a0123 163 }
daniele 3:b4047e8a0123 164 prev = work;
daniele 3:b4047e8a0123 165 work = work->next_filter;
daniele 3:b4047e8a0123 166
daniele 3:b4047e8a0123 167 while (1) {
daniele 3:b4047e8a0123 168 if (work->filter_id == filter_id) {
daniele 3:b4047e8a0123 169 if (work != tail) {
daniele 3:b4047e8a0123 170 /*delete filter_node from linked list*/
daniele 3:b4047e8a0123 171 prev->next_filter = work->next_filter;
daniele 3:b4047e8a0123 172 pico_free(work);
daniele 3:b4047e8a0123 173 return 0;
daniele 3:b4047e8a0123 174 } else {
daniele 3:b4047e8a0123 175 prev->next_filter = NULL;
daniele 3:b4047e8a0123 176 pico_free(work);
daniele 3:b4047e8a0123 177 return 0;
daniele 3:b4047e8a0123 178 }
daniele 3:b4047e8a0123 179 } else {
daniele 3:b4047e8a0123 180 /*check next filter_node*/
daniele 3:b4047e8a0123 181 prev = work;
daniele 3:b4047e8a0123 182 work = work->next_filter;
daniele 3:b4047e8a0123 183 if (work == tail) {
daniele 3:b4047e8a0123 184 pico_err = PICO_ERR_EINVAL;
daniele 3:b4047e8a0123 185 return -1;
daniele 3:b4047e8a0123 186 }
daniele 3:b4047e8a0123 187 }
daniele 3:b4047e8a0123 188 }
daniele 3:b4047e8a0123 189 }
daniele 3:b4047e8a0123 190
daniele 3:b4047e8a0123 191 /*================================== CORE FILTER FUNCTIONS ==================================*/
daniele 3:b4047e8a0123 192 int match_filter(struct filter_node *filter, struct pico_frame *f)
daniele 3:b4047e8a0123 193 {
daniele 3:b4047e8a0123 194 struct filter_node temp;
daniele 3:b4047e8a0123 195 struct pico_ipv4_hdr *ipv4_hdr = (struct pico_ipv4_hdr *) f->net_hdr;
daniele 3:b4047e8a0123 196 struct pico_tcp_hdr *tcp_hdr;
daniele 3:b4047e8a0123 197 struct pico_udp_hdr *udp_hdr;
daniele 3:b4047e8a0123 198
daniele 3:b4047e8a0123 199 if (!filter|| !f) {
daniele 3:b4047e8a0123 200 ipf_dbg("ipfilter> ## nullpointer in match filter \n");
daniele 3:b4047e8a0123 201 return -1;
daniele 3:b4047e8a0123 202 }
daniele 3:b4047e8a0123 203
daniele 3:b4047e8a0123 204 temp.fdev = f->dev;
daniele 3:b4047e8a0123 205 temp.out_addr = ipv4_hdr->dst.addr;
daniele 3:b4047e8a0123 206 temp.in_addr = ipv4_hdr->src.addr;
daniele 3:b4047e8a0123 207 if (ipv4_hdr->proto == PICO_PROTO_TCP ) {
daniele 3:b4047e8a0123 208 tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
daniele 3:b4047e8a0123 209 temp.out_port = short_be(tcp_hdr->trans.dport);
daniele 3:b4047e8a0123 210 temp.in_port = short_be(tcp_hdr->trans.sport);
daniele 3:b4047e8a0123 211 }else if (ipv4_hdr->proto == PICO_PROTO_UDP ) {
daniele 3:b4047e8a0123 212 udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
daniele 3:b4047e8a0123 213 temp.out_port = short_be(udp_hdr->trans.dport);
daniele 3:b4047e8a0123 214 temp.in_port = short_be(udp_hdr->trans.sport);
daniele 3:b4047e8a0123 215 } else {
daniele 3:b4047e8a0123 216 temp.out_port = temp.in_port = 0;
daniele 3:b4047e8a0123 217 }
daniele 3:b4047e8a0123 218 temp.proto = ipv4_hdr->proto;
daniele 3:b4047e8a0123 219 temp.priority = f->priority;
daniele 3:b4047e8a0123 220 temp.tos = ipv4_hdr->tos;
daniele 3:b4047e8a0123 221
daniele 3:b4047e8a0123 222
daniele 3:b4047e8a0123 223
daniele 3:b4047e8a0123 224 if ( ((filter->fdev == NULL || filter->fdev == temp.fdev) && \
daniele 3:b4047e8a0123 225 (filter->in_addr == 0 || ((filter->in_addr_netmask == 0) ? (filter->in_addr == temp.in_addr) : 1)) &&\
daniele 3:b4047e8a0123 226 (filter->in_port == 0 || filter->in_port == temp.in_port) &&\
daniele 3:b4047e8a0123 227 (filter->out_addr == 0 || ((filter->out_addr_netmask == 0) ? (filter->out_addr == temp.out_addr) : 1)) && \
daniele 3:b4047e8a0123 228 (filter->out_port == 0 || filter->out_port == temp.out_port) && \
daniele 3:b4047e8a0123 229 (filter->proto == 0 || filter->proto == temp.proto ) &&\
daniele 3:b4047e8a0123 230 (filter->priority == 0 || filter->priority == temp.priority ) &&\
daniele 3:b4047e8a0123 231 (filter->tos == 0 || filter->tos == temp.tos ) &&\
daniele 3:b4047e8a0123 232 (filter->out_addr_netmask == 0 || ((filter->out_addr & filter->out_addr_netmask) == (temp.out_addr & filter->out_addr_netmask)) ) &&\
daniele 3:b4047e8a0123 233 (filter->in_addr_netmask == 0 || ((filter->in_addr & filter->in_addr_netmask) == (temp.in_addr & filter->in_addr_netmask)) )\
daniele 3:b4047e8a0123 234 ) )
daniele 3:b4047e8a0123 235 return 0;
daniele 3:b4047e8a0123 236
daniele 3:b4047e8a0123 237 //No filter match!
daniele 3:b4047e8a0123 238 ipf_dbg("ipfilter> #no match\n");
daniele 3:b4047e8a0123 239 return 1;
daniele 3:b4047e8a0123 240 }
daniele 3:b4047e8a0123 241
daniele 3:b4047e8a0123 242 int ipfilter(struct pico_frame *f)
daniele 3:b4047e8a0123 243 {
daniele 3:b4047e8a0123 244 struct filter_node *work = head;
daniele 3:b4047e8a0123 245
daniele 3:b4047e8a0123 246 /*return 1 if pico_frame is discarded as result of the filtering, 0 for an incomming packet, -1 for faults*/
daniele 3:b4047e8a0123 247 if (!tail || !head) {
daniele 3:b4047e8a0123 248 return 0;
daniele 3:b4047e8a0123 249 }
daniele 3:b4047e8a0123 250
daniele 3:b4047e8a0123 251 if ( match_filter(work, f) == 0 ) {
daniele 3:b4047e8a0123 252 ipf_dbg("ipfilter> # ipfilter match\n");
daniele 3:b4047e8a0123 253 /*filter match, execute filter!*/
daniele 3:b4047e8a0123 254 return work->function_ptr(work, f);
daniele 3:b4047e8a0123 255 }
daniele 3:b4047e8a0123 256 while (tail != work) {
daniele 3:b4047e8a0123 257 ipf_dbg("ipfilter> next filter..\n");
daniele 3:b4047e8a0123 258 work = work->next_filter;
daniele 3:b4047e8a0123 259 if ( match_filter(work, f) == 0 ) {
daniele 3:b4047e8a0123 260 ipf_dbg("ipfilter> # ipfilter match\n");
daniele 3:b4047e8a0123 261 /*filter match, execute filter!*/
daniele 3:b4047e8a0123 262 return work->function_ptr(work, f);
daniele 3:b4047e8a0123 263 }
daniele 3:b4047e8a0123 264 }
daniele 3:b4047e8a0123 265 return 0;
daniele 3:b4047e8a0123 266 }
daniele 3:b4047e8a0123 267