7u83/freewtp
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

The capwap data channel migrated from userspace to kernalspace

Browse Source
This commit is contained in:
vemax78
2014-09-10 21:58:23 +02:00
parent 71006a9121
commit 8d9985fdea
104 changed files with 6967 additions and 4840 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/wtp/kmod/Makefile
View File

@ -2,4 +2,7 @@ obj-m += smartcapwap.o
smartcapwap-y := \
main.o \
netlinkapp.o
netlinkapp.o \
socket.o \
capwap.o \
capwap_private.o

621
src/wtp/kmod/capwap.c Normal file
View File

@ -0,0 +1,621 @@
#include "config.h"
#include <linux/module.h>
#include <linux/ieee80211.h>
#include "socket.h"
#include "capwap.h"
#include "nlsmartcapwap.h"
#include "netlinkapp.h"
/* */
#define TIMEOUT_PACKET 10
/* */
union capwap_addr sc_localaddr;
/* */
static void sc_capwap_fragment_free(struct sc_capwap_fragment* fragment) {
TRACEKMOD("### sc_capwap_fragment_free\n");
/* */
list_del(&fragment->lru_list);
fragment->flags = 0;
/* Free socket buffer */
while (fragment->fragments) {
struct sk_buff* next = fragment->fragments->next;
kfree_skb(fragment->fragments);
fragment->fragments = next;
}
}
/* */
static void sc_capwap_defrag_evictor(struct sc_capwap_session* session, ktime_t now) {
ktime_t delta;
unsigned long flags;
struct sc_capwap_fragment* fragment;
TRACEKMOD("### sc_capwap_defrag_evictor\n");
/* */
if (now.tv64 == 0) {
TRACEKMOD("*** Get time\n");
now = ktime_get();
}
/* Remove last old fragment */
if (!list_empty(&session->fragments.lru_list)) {
spin_lock_irqsave(&session->fragments.lock, flags);
fragment = list_first_entry(&session->fragments.lru_list, struct sc_capwap_fragment, lru_list);
if (fragment) {
delta = ktime_sub(now, fragment->tstamp);
if ((delta.tv64 < 0) || (delta.tv64 > NSEC_PER_SEC)) {
TRACEKMOD("*** Expired fragment %hu\n", fragment->fragmentid);
/* Reset fragment */
sc_capwap_fragment_free(fragment);
}
}
spin_unlock_irqrestore(&session->fragments.lock, flags);
}
}
/* */
static struct sk_buff* sc_capwap_reasm(struct sc_capwap_fragment* fragment) {
int len;
int offset;
struct sk_buff* skb;
struct sk_buff* skbfrag;
struct sc_capwap_header* header;
/* */
skbfrag = fragment->fragments;
len = GET_HLEN_HEADER((struct sc_capwap_header*)skbfrag->data) * 4;
/* Create new packet */
skb = alloc_skb(len + fragment->totallength, GFP_KERNEL);
if (!skb) {
return NULL;
}
/* The first capwap header is header of reassembled packet without fragment field */
header = (struct sc_capwap_header*)skb_put(skb, len);
memcpy(header, skb->data, len);
SET_FLAG_F_HEADER(header, 0);
SET_FLAG_L_HEADER(header, 0);
header->frag_id = (__be16)0;
header->frag_off = (__be16)0;
/* Copy body */
while (skbfrag) {
offset = GET_HLEN_HEADER((struct sc_capwap_header*)skbfrag->data) * 4;
len = skb->len - offset;
/* */
memcpy(skb_put(skb, len), skb->data + offset, len);
skbfrag = skbfrag->next;
}
return skb;
}
/* */
static struct sk_buff* sc_capwap_defrag(struct sc_capwap_session* session, struct sk_buff* skb) {
unsigned long flags;
uint16_t headersize;
uint16_t frag_id;
struct sk_buff* prev;
struct sk_buff* next;
struct sc_capwap_fragment* fragment;
struct sc_skb_capwap_cb* cb;
struct sk_buff* skb_defrag = NULL;
struct sc_capwap_header* header = (struct sc_capwap_header*)skb->data;
TRACEKMOD("### sc_capwap_defrag\n");
/* */
headersize = GET_HLEN_HEADER(header) * 4;
if (skb->len < headersize) {
goto error;
}
/* */
frag_id = be16_to_cpu(header->frag_id);
/* */
cb = CAPWAP_SKB_CB(skb);
cb->flags |= SKB_CAPWAP_FLAG_FRAGMENT;
cb->frag_offset = be16_to_cpu(header->frag_off);
cb->frag_length = skb->len - headersize;
/* */
spin_lock_irqsave(&session->fragments.lock, flags);
/* Get fragment */
fragment = &session->fragments.queues[frag_id % CAPWAP_FRAGMENT_QUEUE];
if ((fragment->flags & CAPWAP_FRAGMENT_ENABLE) && (fragment->fragmentid != frag_id)) {
goto error2; /* Queue fragment busy*/
}
/* Init fragment */
if (!(fragment->flags & CAPWAP_FRAGMENT_ENABLE)) {
fragment->flags = CAPWAP_FRAGMENT_ENABLE;
fragment->fragmentid = frag_id;
fragment->fragments = NULL;
fragment->lastfragment = NULL;
fragment->recvlength = 0;
fragment->totallength = 0;
list_add_tail(&fragment->lru_list, &session->fragments.lru_list);
}
/* Search fragment position */
prev = fragment->lastfragment;
if (!prev) {
next = NULL;
} else if (CAPWAP_SKB_CB(prev)->frag_offset < cb->frag_offset) {
if ((CAPWAP_SKB_CB(prev)->frag_offset + CAPWAP_SKB_CB(prev)->frag_length) < cb->frag_offset) {
next = NULL;
} else {
sc_capwap_fragment_free(fragment);
goto error2; /* Overlap error */
}
} else {
prev = NULL;
for (next = fragment->fragments; next != NULL; next = next->next) {
struct sc_skb_capwap_cb* next_cb = CAPWAP_SKB_CB(next);
if (next_cb->frag_offset < cb->frag_offset) {
if ((next_cb->frag_offset + next_cb->frag_length) < cb->frag_offset) {
break;
} else {
sc_capwap_fragment_free(fragment);
goto error2; /* Overlap error */
}
}
prev = next;
}
}
/* Insert fragment */
skb->prev = NULL;
skb->next = next;
if (!next) {
fragment->lastfragment = skb;
}
if (prev) {
prev->next = skb;
} else {
fragment->fragments = skb;
}
/* Update size */
fragment->recvlength += cb->frag_length;
if (IS_FLAG_L_HEADER(header)) {
fragment->totallength = cb->frag_offset + cb->frag_length;
fragment->flags |= CAPWAP_FRAGMENT_LAST;
}
/* Check if receive all fragment */
if ((fragment->flags & CAPWAP_FRAGMENT_LAST) && (fragment->recvlength == fragment->totallength)) {
skb_defrag = sc_capwap_reasm(fragment);
/* Free fragment complete */
sc_capwap_fragment_free(fragment);
} else {
/* Update timeout */
fragment->tstamp = skb->tstamp;
if (fragment->tstamp.tv64 == 0) {
fragment->tstamp = ktime_get();
}
/* Set LRU timeout */
if (!list_is_last(&fragment->lru_list, &session->fragments.lru_list)) {
list_move_tail(&fragment->lru_list, &session->fragments.lru_list);
}
}
spin_unlock_irqrestore(&session->fragments.lock, flags);
return skb_defrag;
error2:
spin_unlock_irqrestore(&session->fragments.lock, flags);
error:
kfree_skb(skb);
return NULL;
}
/* */
int sc_capwap_bind(union capwap_addr* sockaddr) {
int ret;
TRACEKMOD("### sc_capwap_bind\n");
/* */
ret = sc_socket_bind(sockaddr);
if (ret) {
return ret;
}
memcpy(&sc_localaddr, sockaddr, sizeof(union capwap_addr));
return 0;
}
/* */
void sc_capwap_initsession(struct sc_capwap_session* session) {
TRACEKMOD("### sc_capwap_initsession\n");
INIT_LIST_HEAD(&session->list);
spin_lock_init(&session->fragmentid_lock);
/* Defragment packets */
memset(&session->fragments, 0, sizeof(struct sc_capwap_fragment_queue));
INIT_LIST_HEAD(&session->fragments.lru_list);
spin_lock_init(&session->fragments.lock);
}
/* */
void sc_capwap_freesession(struct sc_capwap_session* session) {
struct sc_capwap_fragment* temp;
struct sc_capwap_fragment* fragment;
TRACEKMOD("### sc_capwap_freesession\n");
/* Free socket buffers */
list_for_each_entry_safe(fragment, temp, &session->fragments.lru_list, lru_list) {
sc_capwap_fragment_free(fragment);
}
}
/* */
uint16_t sc_capwap_newfragmentid(struct sc_capwap_session* session) {
uint16_t fragmentid;
unsigned long flags;
TRACEKMOD("### sc_capwap_newfragmentid\n");
spin_lock_irqsave(&session->fragmentid_lock, flags);
fragmentid = session->fragmentid++;
spin_unlock_irqrestore(&session->fragmentid_lock, flags);
return fragmentid;
}
/* */
int sc_capwap_createkeepalive(struct sc_capwap_sessionid_element* sessionid, uint8_t* buffer, int size) {
int length;
struct sc_capwap_header* header;
struct sc_capwap_data_message* dataheader;
struct sc_capwap_message_element* msgelement;
TRACEKMOD("### sc_capwap_createkeepalive\n");
/* */
if (size < CAPWAP_KEEP_ALIVE_MAX_SIZE) {
return -ENOMEM;
}
/* Preamble CAPWAP header */
header = (struct sc_capwap_header*)buffer;
length = sizeof(struct sc_capwap_header);
buffer += sizeof(struct sc_capwap_header);
memset(header, 0, sizeof(struct sc_capwap_header));
SET_VERSION_HEADER(header, CAPWAP_PROTOCOL_VERSION);
SET_TYPE_HEADER(header, CAPWAP_PREAMBLE_HEADER);
SET_HLEN_HEADER(header, sizeof(struct sc_capwap_header) / 4);
SET_WBID_HEADER(header, CAPWAP_WIRELESS_BINDING_IEEE80211);
SET_FLAG_K_HEADER(header, 1);
/* CAPWAP Data header */
dataheader = (struct sc_capwap_data_message*)buffer;
length += sizeof(struct sc_capwap_data_message);
buffer += sizeof(struct sc_capwap_data_message);
dataheader->length = cpu_to_be16(sizeof(struct sc_capwap_data_message) + sizeof(struct sc_capwap_message_element) + sizeof(struct sc_capwap_sessionid_element));
/* CAPWAP Keep-Alive Message Element */
msgelement = (struct sc_capwap_message_element*)buffer;
length += sizeof(struct sc_capwap_message_element);
buffer += sizeof(struct sc_capwap_message_element);
msgelement->type = cpu_to_be16(CAPWAP_ELEMENT_SESSIONID);
msgelement->length = cpu_to_be16(sizeof(struct sc_capwap_sessionid_element));
/* Session ID */
memcpy(buffer, sessionid, sizeof(struct sc_capwap_sessionid_element));
length += sizeof(struct sc_capwap_sessionid_element);
return length;
}
/* */
int sc_capwap_parsingpacket(struct sc_capwap_session* session, const union capwap_addr* sockaddr, struct sk_buff* skb) {
int length;
uint16_t headersize;
struct sc_capwap_data_message* dataheader;
struct sc_capwap_message_element* message;
struct sc_capwap_header* header = (struct sc_capwap_header*)skb->data;
TRACEKMOD("### sc_capwap_parsingpacket\n");
/* Linearize socket buffer */
if (skb_linearize(skb)) {
TRACEKMOD("*** Unable to linearize packet\n");
return -EINVAL;
}
/* Check header */
if (skb->len < sizeof(struct sc_capwap_header)) {
TRACEKMOD("*** Invalid capwap header length\n");
return -EINVAL;
} else if (GET_VERSION_HEADER(header) != CAPWAP_PROTOCOL_VERSION) {
TRACEKMOD("*** Invalid capwap header version\n");
return -EINVAL;
} else if (GET_TYPE_HEADER(header) != CAPWAP_PREAMBLE_HEADER) {
TRACEKMOD("*** Packet is encrypted\n");
return -EINVAL; /* Accept only plain packet */
}
/* Cleaning old fragments */
if (session) {
sc_capwap_defrag_evictor(session, skb->tstamp);
}
/* */
if (IS_FLAG_K_HEADER(header)) {
/* Keep alive can not fragment */
if (IS_FLAG_F_HEADER(header)) {
TRACEKMOD("*** Keep alive can not fragment\n");
return -EINVAL;
}
/* */
length = skb->len;
headersize = GET_HLEN_HEADER(header) * 4;
if (length < (headersize + sizeof(struct sc_capwap_data_message))) {
TRACEKMOD("*** Invalid capwap data header length\n");
return -EINVAL;
}
/* Data message */
length -= headersize;
dataheader = (struct sc_capwap_data_message*)(((uint8_t*)header) + headersize);
headersize = ntohs(dataheader->length);
if (length < headersize) {
TRACEKMOD("*** Capwap data header length mismatch\n");
return -EINVAL;
}
/* Message elements */
headersize -= sizeof(struct sc_capwap_data_message);
message = (struct sc_capwap_message_element*)(((uint8_t*)dataheader) + sizeof(struct sc_capwap_data_message));
while (headersize > 0) {
uint16_t msglength = ntohs(message->length);
if (headersize < (msglength + sizeof(struct sc_capwap_message_element))) {
TRACEKMOD("*** Invalid capwap message element length\n");
return -EINVAL;
}
/* */
if ((ntohs(message->type) == CAPWAP_ELEMENT_SESSIONID) && (msglength == sizeof(struct sc_capwap_sessionid_element))) {
struct sc_capwap_sessionid_element* sessionid = (struct sc_capwap_sessionid_element*)(((uint8_t*)message) + sizeof(struct sc_capwap_message_element));
if (!session) {
session = sc_capwap_recvunknownkeepalive(sockaddr, sessionid);
if (!session) {
TRACEKMOD("*** Receive unknown keep alive without valid session\n");
return -EINVAL;
}
} else if (memcmp(&session->sessionid, sessionid, sizeof(struct sc_capwap_sessionid_element))) {
TRACEKMOD("*** Session id mismatch\n");
return -EINVAL;
}
/* Session found */
sc_netlink_notify_recv_keepalive(sockaddr, sessionid);
/* Parsing complete */
kfree_skb(skb);
return 0;
}
/* Next message element */
msglength += sizeof(struct sc_capwap_message_element);
message = (struct sc_capwap_message_element*)(((uint8_t*)message) + msglength);
headersize -= msglength;
}
} else if (session) {
if (IS_FLAG_F_HEADER(header)) {
skb = sc_capwap_defrag(session, skb);
if (!skb) {
return 0;
}
/* Get new header info */
header = (struct sc_capwap_header*)skb->data;
}
/* Parsing data/management packet */
if (!IS_FLAG_T_HEADER(header)) {
sc_capwap_parsingdatapacket(session, skb);
} else if (GET_WBID_HEADER(header) == CAPWAP_WIRELESS_BINDING_IEEE80211) {
struct ieee80211_hdr* hdr = (struct ieee80211_hdr*)(skb->data + GET_HLEN_HEADER(header) * 4);
if (ieee80211_is_data_present(hdr->frame_control)) {
sc_capwap_parsingdatapacket(session, skb);
} else if (ieee80211_is_mgmt(hdr->frame_control) || ieee80211_is_ctl(hdr->frame_control)) {
sc_capwap_parsingmgmtpacket(session, skb);
}
}
/* Parsing complete */
kfree_skb(skb);
return 0;
}
return -EINVAL;
}
/* */
int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, uint8_t binding, struct sk_buff* skb, uint32_t flags, struct sc_capwap_radio_addr* radioaddr, int radioaddrlength, struct sc_capwap_wireless_information* winfo, int winfolength) {
int size;
int length;
int reserve;
int headroom;
int requestfragment;
__be16 fragmentid = 0;
int fragmentoffset = 0;
struct sc_capwap_header* header;
struct sk_buff* clone = NULL;
int packetlength = skb->len;
TRACEKMOD("### sc_capwap_forwarddata\n");
/* Check headroom */
headroom = skb_headroom(skb);
reserve = sizeof(struct sc_capwap_header) + radioaddrlength + winfolength;
if (skb_is_nonlinear(skb) || (headroom < reserve)) {
printk("*** Expand socket buffer\n");
clone = skb_copy_expand(skb, max_t(int, headroom, reserve), skb_tailroom(skb), GFP_KERNEL);
if (!clone) {
printk("*** Unable to expand socket buffer\n");
return -ENOMEM;
}
skb = clone;
}
/* Check MTU */
requestfragment = (((packetlength + reserve) > session->mtu) ? 1 : 0);
if (requestfragment) {
fragmentid = cpu_to_be16(sc_capwap_newfragmentid(session));
}
/* */
header = (struct sc_capwap_header*)skb_push(skb, sizeof(struct sc_capwap_header) + radioaddrlength + winfolength);
while (packetlength > 0) {
memset(header, 0, sizeof(struct sc_capwap_header));
SET_VERSION_HEADER(header, CAPWAP_PROTOCOL_VERSION);
SET_TYPE_HEADER(header, CAPWAP_PREAMBLE_HEADER);
SET_WBID_HEADER(header, binding);
SET_RID_HEADER(header, radioid);
SET_FLAG_T_HEADER(header, ((flags & NLSMARTCAPWAP_FLAGS_TUNNEL_8023) ? 0 : 1));
if (!fragmentoffset) {
uint8_t* headeroption = (uint8_t*)header + sizeof(struct sc_capwap_header);
if (radioaddr) {
SET_FLAG_M_HEADER(header, 1);
memcpy(headeroption, radioaddr, radioaddrlength);
headeroption += radioaddrlength;
}
if (winfo) {
SET_FLAG_W_HEADER(header, 1);
memcpy(headeroption, winfo, winfolength);
headeroption += winfolength;
}
size = sizeof(struct sc_capwap_header) + radioaddrlength + winfolength;
SET_HLEN_HEADER(header, size / 4);
} else {
size = sizeof(struct sc_capwap_header);
SET_HLEN_HEADER(header, size / 4);
}
/* Calculate body size */
length = session->mtu - size;
if (packetlength <= length) {
length = packetlength;
} else if (requestfragment) {
length -= length % 8; /* Capwap fragment size is module 8 */
} else {
break;
}
/* Fragment options */
if (requestfragment) {
SET_FLAG_F_HEADER(header, 1);
if (packetlength == length) {
SET_FLAG_L_HEADER(header, 1);
}
header->frag_id = fragmentid;
header->frag_off = cpu_to_be16(fragmentoffset);
}
/* Send packet */
if (sc_socket_send(SOCKET_UDP, (uint8_t*)header, (size + length), &session->peeraddr) < 0) {
break;
}
/* */
header = (struct sc_capwap_header*)((uint8_t*)header + (size + length));
fragmentoffset += length;
packetlength -= length;
}
if (clone) {
kfree_skb(clone);
}
return (!packetlength ? 0 : -EIO);
}
/* */
void sc_capwap_sessionid_printf(const struct sc_capwap_sessionid_element* sessionid, char* string) {
int i;
char* pos = string;
for (i = 0; i < 16; i++) {
snprintf(pos, 3, "%02x", sessionid->id[i]);
pos += 2;
}
*pos = 0;
}
/* */
struct sc_capwap_radio_addr* sc_capwap_setradiomacaddress(uint8_t* buffer, int size, uint8_t* bssid) {
struct sc_capwap_radio_addr* radioaddr;
struct sc_capwap_macaddress_eui48* addr;
TRACEKMOD("### sc_capwap_setwirelessinformation\n");
memset(buffer, 0, size);
radioaddr = (struct sc_capwap_radio_addr*)buffer;
radioaddr->length = MACADDRESS_EUI48_LENGTH;
addr = (struct sc_capwap_macaddress_eui48*)(buffer + sizeof(struct sc_capwap_radio_addr));
memcpy(addr->addr, bssid, MACADDRESS_EUI48_LENGTH);
return radioaddr;
}
/* */
struct sc_capwap_wireless_information* sc_capwap_setwirelessinformation(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate) {
struct sc_capwap_wireless_information* winfo;
struct sc_capwap_ieee80211_frame_info* frameinfo;
TRACEKMOD("### sc_capwap_setwirelessinformation\n");
memset(buffer, 0, size);
winfo = (struct sc_capwap_wireless_information*)buffer;
winfo->length = sizeof(struct sc_capwap_ieee80211_frame_info);
frameinfo = (struct sc_capwap_ieee80211_frame_info*)(buffer + sizeof(struct sc_capwap_wireless_information));
frameinfo->rssi = rssi;
frameinfo->snr = snr;
frameinfo->rate = cpu_to_be16(rate);
return winfo;
}

128
src/wtp/kmod/capwap.h Normal file
View File

@ -0,0 +1,128 @@
#ifndef __KMOD_CAPWAP_HEADER__
#define __KMOD_CAPWAP_HEADER__
#include "capwap_rfc.h"
#include "socket.h"
/* */
#define MAX_MTU 9000
#define DEFAULT_MTU 1450
#define MIN_MTU 500
#define IEEE80211_MTU 7981
/* */
#define CAPWAP_FRAGMENT_QUEUE 16
/* */
#define CAPWAP_FRAGMENT_ENABLE 0x0001
#define CAPWAP_FRAGMENT_LRUQUEUE 0x0002
#define CAPWAP_FRAGMENT_LAST 0x0004
/* */
#define SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL 0x0001
#define SKB_CAPWAP_FLAG_FROM_USER_SPACE 0x0002
#define SKB_CAPWAP_FLAG_FROM_IEEE80211 0x0004
#define SKB_CAPWAP_FLAG_PEERADDRESS 0x0010
#define SKB_CAPWAP_FLAG_RADIOID 0x0020
#define SKB_CAPWAP_FLAG_BINDING 0x0040
#define SKB_CAPWAP_FLAG_RADIOADDRESS 0x0080
#define SKB_CAPWAP_FLAG_WIRELESSINFORMATION 0x0100
#define SKB_CAPWAP_FLAG_FRAGMENT 0x1000
struct sc_skb_capwap_cb {
uint16_t flags;
struct capwap_addr_little peeraddr;
/* Capwap information */
uint8_t radioid;
uint8_t binding;
/* Radio Address */
uint8_t radioaddr_addr[MACADDRESS_EUI48_LENGTH];
/* Wireless Information */
uint8_t winfo_rssi;
uint8_t winfo_snr;
uint16_t winfo_rate;
/* Fragment */
uint16_t frag_offset;
uint16_t frag_length;
};
#define CAPWAP_SKB_CB(skb) ((struct sc_skb_capwap_cb*)((skb)->cb))
/* */
struct sc_capwap_fragment {
struct list_head lru_list;
uint8_t flags;
ktime_t tstamp;
uint16_t fragmentid;
struct sk_buff* fragments;
struct sk_buff* lastfragment;
int recvlength;
int totallength;
};
/* */
struct sc_capwap_fragment_queue {
spinlock_t lock;
struct list_head lru_list;
struct sc_capwap_fragment queues[CAPWAP_FRAGMENT_QUEUE];
};
/* */
struct sc_capwap_session {
struct list_head list;
struct sc_capwap_session* __rcu next;
uint16_t mtu;
union capwap_addr peeraddr;
struct sc_capwap_sessionid_element sessionid;
uint16_t fragmentid;
spinlock_t fragmentid_lock;
struct sc_capwap_fragment_queue fragments;
};
/* */
extern union capwap_addr sc_localaddr;
/* Dipendent implementation function */
void sc_capwap_recvpacket(struct sk_buff* skb);
struct sc_capwap_session* sc_capwap_recvunknownkeepalive(const union capwap_addr* sockaddr, struct sc_capwap_sessionid_element* sessionid);
void sc_capwap_parsingdatapacket(struct sc_capwap_session* session, struct sk_buff* skb);
void sc_capwap_parsingmgmtpacket(struct sc_capwap_session* session, struct sk_buff* skb);
/* Indipendent implementation function */
int sc_capwap_bind(union capwap_addr* sockaddr);
void sc_capwap_initsession(struct sc_capwap_session* session);
void sc_capwap_freesession(struct sc_capwap_session* session);
uint16_t sc_capwap_newfragmentid(struct sc_capwap_session* session);
void sc_capwap_sessionid_printf(const struct sc_capwap_sessionid_element* sessionid, char* string);
struct sc_capwap_packet* sc_capwap_poppacketqueue(struct sc_capwap_session* session);
void sc_capwap_pushpacketqueue(struct sc_capwap_session* session, struct sc_capwap_packet* packet);
int sc_capwap_createkeepalive(struct sc_capwap_sessionid_element* sessionid, uint8_t* buffer, int size);
int sc_capwap_parsingpacket(struct sc_capwap_session* session, const union capwap_addr* sockaddr, struct sk_buff* skb);
struct sc_capwap_radio_addr* sc_capwap_setradiomacaddress(uint8_t* buffer, int size, uint8_t* bssid);
struct sc_capwap_wireless_information* sc_capwap_setwirelessinformation(uint8_t* buffer, int size, uint8_t rssi, uint8_t snr, uint16_t rate);
int sc_capwap_forwarddata(struct sc_capwap_session* session, uint8_t radioid, uint8_t binding, struct sk_buff* skb, uint32_t flags, struct sc_capwap_radio_addr* radioaddr, int radioaddrlength, struct sc_capwap_wireless_information* winfo, int winfolength);
/* Private funciotn */
#include "capwap_private.h"
#endif /* __KMOD_CAPWAP_HEADER__ */

164
src/wtp/kmod/capwap_private.c Normal file
View File

@ -0,0 +1,164 @@
#include "config.h"
#include <linux/module.h>
#include <linux/kthread.h>
#include <net/ipv6.h>
#include "capwap.h"
#include "nlsmartcapwap.h"
#include "netlinkapp.h"
/* */
static struct sc_capwap_session sc_acsession;
/* */
int sc_capwap_init(uint32_t threads) {
TRACEKMOD("### sc_capwap_init\n");
/* Init session */
memset(&sc_localaddr, 0, sizeof(union capwap_addr));
memset(&sc_acsession, 0, sizeof(struct sc_capwap_session));
sc_capwap_initsession(&sc_acsession);
/* Init sockect */
return sc_socket_init();
}
/* */
void sc_capwap_close(void) {
TRACEKMOD("### sc_capwap_close\n");
/* */
sc_socket_close();
sc_capwap_freesession(&sc_acsession);
}
/* */
int sc_capwap_connect(const union capwap_addr* sockaddr, struct sc_capwap_sessionid_element* sessionid, uint16_t mtu) {
TRACEKMOD("### sc_capwap_connect\n");
if ((sc_localaddr.ss.ss_family != AF_INET) && (sc_localaddr.ss.ss_family != AF_INET6)) {
return -ENONET;
}
/* AC address */
if ((sockaddr->ss.ss_family == AF_INET6) && ipv6_addr_v4mapped(&sockaddr->sin6.sin6_addr)) {
return -EINVAL;
} else if ((sc_localaddr.ss.ss_family == AF_INET) && (sockaddr->ss.ss_family == AF_INET6)) {
return -EINVAL;
}
/* */
memcpy(&sc_acsession.peeraddr, sockaddr, sizeof(union capwap_addr));
memcpy(&sc_acsession.sessionid, sessionid, sizeof(struct sc_capwap_sessionid_element));
sc_acsession.mtu = mtu;
return sc_capwap_sendkeepalive();
}
/* */
void sc_capwap_resetsession(void) {
sc_capwap_freesession(&sc_acsession);
/* Reinit session */
memset(&sc_localaddr, 0, sizeof(union capwap_addr));
memset(&sc_acsession, 0, sizeof(struct sc_capwap_session));
sc_capwap_initsession(&sc_acsession);
}
/* */
int sc_capwap_sendkeepalive(void) {
int ret;
int length;
uint8_t buffer[CAPWAP_KEEP_ALIVE_MAX_SIZE];
TRACEKMOD("### sc_capwap_sendkeepalive\n");
/* Build keepalive */
length = sc_capwap_createkeepalive(&sc_acsession.sessionid, buffer, CAPWAP_KEEP_ALIVE_MAX_SIZE);
/* Send packet */
ret = sc_socket_send(SOCKET_UDP, buffer, length, &sc_acsession.peeraddr);
if (ret > 0) {
ret = 0;
}
return ret;
}
/* */
struct sc_capwap_session* sc_capwap_getsession(const union capwap_addr* sockaddr) {
TRACEKMOD("### sc_capwap_getsession\n");
if (!sockaddr) {
return &sc_acsession;
} else if (sc_acsession.peeraddr.ss.ss_family == sockaddr->ss.ss_family) {
if (sc_acsession.peeraddr.ss.ss_family == AF_INET) {
if ((sc_acsession.peeraddr.sin.sin_port == sockaddr->sin.sin_port) && (sc_acsession.peeraddr.sin.sin_addr.s_addr == sockaddr->sin.sin_addr.s_addr)) {
return &sc_acsession;
}
} else if (sc_acsession.peeraddr.ss.ss_family == AF_INET6) {
if ((sc_acsession.peeraddr.sin6.sin6_port == sockaddr->sin6.sin6_port) && !ipv6_addr_cmp(&sc_acsession.peeraddr.sin6.sin6_addr, &sockaddr->sin6.sin6_addr)) {
return &sc_acsession;
}
}
}
return NULL;
}
/* */
void sc_capwap_recvpacket(struct sk_buff* skb) {
union capwap_addr peeraddr;
struct sc_capwap_session* session;
TRACEKMOD("### sc_capwap_recvpacket\n");
/* Get peer address */
if (sc_socket_getpeeraddr(skb, &peeraddr)) {
goto drop;
}
/* Get session */
session = sc_capwap_getsession(&peeraddr);
if (!session) {
TRACEKMOD("*** Session not found\n");
goto drop;
}
/* Remove UDP header */
if (!skb_pull(skb, sizeof(struct udphdr))) {
TRACEKMOD("*** Invalid packet\n");
goto drop;
}
/* Parsing packet */
if (sc_capwap_parsingpacket(session, &peeraddr, skb)) {
TRACEKMOD("*** Parsing error\n");
goto drop;
}
return;
drop:
kfree_skb(skb);
}
/* */
struct sc_capwap_session* sc_capwap_recvunknownkeepalive(const union capwap_addr* sockaddr, struct sc_capwap_sessionid_element* sessionid) {
TRACEKMOD("### sc_capwap_recvunknownkeepalive\n");
return NULL;
}
/* */
void sc_capwap_parsingdatapacket(struct sc_capwap_session* session, struct sk_buff* skb) {
TRACEKMOD("### sc_capwap_parsingdatapacket\n");
}
/* */
void sc_capwap_parsingmgmtpacket(struct sc_capwap_session* session, struct sk_buff* skb) {
TRACEKMOD("### sc_capwap_parsingmgmtpacket\n");
/* Send packet with capwap header into userspace */
sc_netlink_notify_recv_data(skb->data, skb->len);
}

27
src/wtp/kmod/capwap_private.h Normal file
View File

@ -0,0 +1,27 @@
#ifndef __KMOD_CAPWAP_PRIVATE_HEADER__
#define __KMOD_CAPWAP_PRIVATE_HEADER__
/* */
struct sc_capwap_workthread {
struct task_struct* thread;
struct sk_buff_head queue;
wait_queue_head_t waitevent;
};
/* */
int sc_capwap_init(uint32_t threads);
void sc_capwap_close(void);
/* */
int sc_capwap_connect(const union capwap_addr* sockaddr, struct sc_capwap_sessionid_element* sessionid, uint16_t mtu);
void sc_capwap_resetsession(void);
/* */
struct sc_capwap_session* sc_capwap_getsession(const union capwap_addr* sockaddr);
/* */
int sc_capwap_sendkeepalive(void);
#endif /* __KMOD_CAPWAP_PRIVATE_HEADER__ */

172
src/wtp/kmod/capwap_rfc.h Normal file
View File

@ -0,0 +1,172 @@
#ifndef __KMOD_CAPWAP_RFC_HEADER__
#define __KMOD_CAPWAP_RFC_HEADER__
#include <linux/types.h>
#include <asm/byteorder.h>
/* */
#define CAPWAP_RADIOID_MAX_COUNT 31
#define IS_VALID_RADIOID(x) ((x >= 1) && (x <= CAPWAP_RADIOID_MAX_COUNT))
#define CAPWAP_WLANID_MAX_COUNT 16
#define IS_VALID_WLANID(x) ((x >= 1) && (x <= CAPWAP_WLANID_MAX_COUNT))
/* */
#define CAPWAP_WIRELESS_BINDING_NONE 0
#define CAPWAP_WIRELESS_BINDING_IEEE80211 1
/* */
#define CAPWAP_ELEMENT_SESSIONID 35
/* */
#define CAPWAP_KEEPALIVE_SIZE (sizeof(struct sc_capwap_dtls_header) + \
sizeof(struct sc_capwap_header) + \
sizeof(struct sc_capwap_data_message) + \
sizeof(struct sc_capwap_message_element) + \
sizeof(struct sc_capwap_sessionid_element))
/* Preamble */
struct sc_capwap_preamble {
#if defined(__BIG_ENDIAN_BITFIELD)
uint8_t version: 4,
type: 4;
#elif defined(__LITTLE_ENDIAN_BITFIELD)
uint8_t type: 4,
version: 4;
#endif
} __packed;
/* DTLS header */
struct sc_capwap_dtls_header {
struct sc_capwap_preamble preamble;
uint8_t reserved[3];
} __packed;
/* Plain header */
struct sc_capwap_header {
struct sc_capwap_preamble preamble;
#if defined(__BIG_ENDIAN_BITFIELD)
uint16_t hlen: 5,
rid: 5,
wbid: 5,
flag_t: 1;
uint8_t flag_f: 1,
flag_l: 1,
flag_w: 1,
flag_m: 1,
flag_k: 1,
flag_res: 3;
#elif defined(__LITTLE_ENDIAN_BITFIELD)
uint16_t _rid_hi: 3,
hlen: 5,
flag_t: 1,
wbid: 5,
_rid_lo: 2;
uint8_t flag_res: 3,
flag_k: 1,
flag_m: 1,
flag_w: 1,
flag_l: 1,
flag_f: 1;
#endif
__be16 frag_id;
__be16 frag_off;
} __packed;
/* Mac Address */
#define CAPWAP_RADIO_EUI48_LENGTH_PADDED 8
#define CAPWAP_RADIO_EUI64_LENGTH_PADDED 12
#define CAPWAP_RADIO_MAX_LENGTH_PADDED 12
struct sc_capwap_radio_addr {
uint8_t length;
} __packed;
/* Wireless Information */
#define CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED 8
#define CAPWAP_WINFO_DESTWLAN_LENGTH_PADDED 8
#define CAPWAP_WINFO_MAX_LENGTH_PADDED 8
struct sc_capwap_wireless_information {
uint8_t length;
} __packed;
/* IEEE802.11 Wireless Information */
struct sc_capwap_ieee80211_frame_info {
uint8_t rssi;
uint8_t snr;
__be16 rate;
} __packed;
/* */
#define CAPWAP_HEADER_MAX_LENGTH (sizeof(struct sc_capwap_header) + CAPWAP_RADIO_MAX_LENGTH_PADDED + CAPWAP_WINFO_MAX_LENGTH_PADDED)
/* Data channel message */
struct sc_capwap_data_message {
__be16 length;
} __packed;
/* Message element */
struct sc_capwap_message_element {
__be16 type;
__be16 length;
} __packed;
/* Session id message element */
struct sc_capwap_sessionid_element {
uint8_t id[16];
} __packed;
/* */
#define MACADDRESS_EUI48_LENGTH 6
struct sc_capwap_macaddress_eui48 {
uint8_t addr[MACADDRESS_EUI48_LENGTH];
} __packed;
/* */
#define MACADDRESS_EUI64_LENGTH 8
struct sc_capwap_macaddress_eui64 {
uint8_t addr[MACADDRESS_EUI64_LENGTH];
} __packed;
/* Capwap preamble */
#define CAPWAP_PROTOCOL_VERSION 0
#define CAPWAP_PREAMBLE_HEADER 0
#define CAPWAP_PREAMBLE_DTLS_HEADER 1
#define CAPWAP_WIRELESS_BINDING_NONE 0
#define CAPWAP_WIRELESS_BINDING_IEEE80211 1
/* */
#define CAPWAP_KEEP_ALIVE_MAX_SIZE (sizeof(struct sc_capwap_header) + sizeof(struct sc_capwap_data_message) + sizeof(struct sc_capwap_message_element) + sizeof(struct sc_capwap_sessionid_element))
/* */
#define GET_VERSION_HEADER(x) ((x)->preamble.version)
#define SET_VERSION_HEADER(x, y) ((x)->preamble.version = (uint8_t)(y))
#define GET_TYPE_HEADER(x) ((x)->preamble.type)
#define SET_TYPE_HEADER(x, y) ((x)->preamble.type = (uint8_t)(y))
#define GET_HLEN_HEADER(x) ((x)->hlen)
#define SET_HLEN_HEADER(x, y) ((x)->hlen = (uint16_t)(y))
#if defined(__BIG_ENDIAN_BITFIELD)
#define GET_RID_HEADER(x) ((uint8_t)((x)->rid))
#define SET_RID_HEADER(x, y) ((x)->rid = (uint16_t)(y))
#elif defined(__LITTLE_ENDIAN_BITFIELD)
#define GET_RID_HEADER(x) ((uint8_t)((uint16_t)((x)->_rid_hi << 2 | (x)->_rid_lo)))
#define SET_RID_HEADER(x, y) ({ (x)->_rid_hi = (uint16_t)((y) >> 2); (x)->_rid_lo = (uint16_t)((y) & 0x0003); })
#endif
#define GET_WBID_HEADER(x) ((uint8_t)((x)->wbid))
#define SET_WBID_HEADER(x, y) ((x)->wbid = (uint16_t)(y))
#define IS_FLAG_T_HEADER(x) ((x)->flag_t)
#define SET_FLAG_T_HEADER(x, y) ((x)->flag_t = ((y) ? 1 : 0))
#define IS_FLAG_F_HEADER(x) ((x)->flag_f)
#define SET_FLAG_F_HEADER(x, y) ((x)->flag_f = ((y) ? 1 : 0))
#define IS_FLAG_L_HEADER(x) ((x)->flag_l)
#define SET_FLAG_L_HEADER(x, y) ((x)->flag_l = ((y) ? 1 : 0))
#define IS_FLAG_W_HEADER(x) ((x)->flag_w)
#define SET_FLAG_W_HEADER(x, y) ((x)->flag_w = ((y) ? 1 : 0))
#define IS_FLAG_M_HEADER(x) ((x)->flag_m)
#define SET_FLAG_M_HEADER(x, y) ((x)->flag_m = ((y) ? 1 : 0))
#define IS_FLAG_K_HEADER(x) ((x)->flag_k)
#define SET_FLAG_K_HEADER(x, y) ((x)->flag_k = ((y) ? 1 : 0))
#endif /* __KMOD_CAPWAP_RFC_HEADER__ */

13
src/wtp/kmod/config.h Normal file
View File

@ -0,0 +1,13 @@
#ifndef __KMOD_CONFIG_HEADER__
#define __KMOD_CONFIG_HEADER__
#define DEBUGKMOD 1
#ifdef DEBUGKMOD
#define TRACEKMOD(s, args...) printk(s, ##args)
#else
#define TRACEKMOD(s, args...)
#endif
#endif /* __KMOD_CONFIG_HEADER__ */

26
src/wtp/kmod/main.c
View File

@ -1,23 +1,31 @@
#include "config.h"
#include <linux/module.h>
#include <linux/kernel.h>
#include "netlinkapp.h"
/* */
static int __init smartcapwap_init(void) {
int result = 0;
static int __init smartcapwap_wtp_init(void) {
int ret;
/* */
result = nlsmartcapwap_init();
TRACEKMOD("### smartcapwap_wtp_init\n");
return result;
/* Initialize netlink */
ret = sc_netlink_init();
if (ret) {
return ret;
}
return ret;
}
module_init(smartcapwap_init);
module_init(smartcapwap_wtp_init);
/* */
static void __exit smartcapwap_exit(void) {
nlsmartcapwap_exit();
static void __exit smartcapwap_wtp_exit(void) {
TRACEKMOD("### smartcapwap_wtp_exit\n");
sc_netlink_exit();
}
module_exit(smartcapwap_exit);
module_exit(smartcapwap_wtp_exit);
MODULE_LICENSE("GPL");
MODULE_AUTHOR("Massimo Vellucci <vemax78@gmail.com>");

689
src/wtp/kmod/netlinkapp.c
View File

@ -1,5 +1,7 @@
#include "config.h"
#include <linux/module.h>
#include <linux/rtnetlink.h>
#include <linux/netdevice.h>
#include <linux/netlink.h>
#include <net/genetlink.h>
#include <linux/rcupdate.h>
@ -8,154 +10,200 @@
#include <linux/ieee80211.h>
#include "nlsmartcapwap.h"
#include "netlinkapp.h"
#include "capwap.h"
/* */
struct nlsmartcapwap_device {
struct sc_netlink_device {
struct list_head list;
struct ieee80211_pcktunnel pcktunnel_handler;
u32 usermodeid;
u32 ifindex;
u32 flags;
uint32_t ifindex;
uint8_t radioid;
uint8_t wlanid;
uint8_t binding;
struct net_device* dev;
uint32_t flags;
};
/* */
static u32 nlsmartcapwap_usermodeid = 0;
static LIST_HEAD(nlsmartcapwap_dev_list);
static uint32_t sc_netlink_usermodeid;
static LIST_HEAD(sc_netlink_dev_list);
/* */
static int nlsmartcapwap_pre_doit(__genl_const struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) {
static int sc_netlink_pre_doit(__genl_const struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) {
TRACEKMOD("### sc_netlink_pre_doit\n");
rtnl_lock();
return 0;
}
/* */
static void nlsmartcapwap_post_doit(__genl_const struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) {
static void sc_netlink_post_doit(__genl_const struct genl_ops* ops, struct sk_buff* skb, struct genl_info* info) {
TRACEKMOD("### sc_netlink_post_doit\n");
rtnl_unlock();
}
/* Netlink Family */
static struct genl_family nlsmartcapwap_family = {
static struct genl_family sc_netlink_family = {
.id = GENL_ID_GENERATE,
.name = SMARTCAPWAP_GENL_NAME,
.name = NLSMARTCAPWAP_GENL_NAME,
.hdrsize = 0,
.version = 1,
.maxattr = NLSMARTCAPWAP_ATTR_MAX,
.netnsok = true,
.pre_doit = nlsmartcapwap_pre_doit,
.post_doit = nlsmartcapwap_post_doit,
.pre_doit = sc_netlink_pre_doit,
.post_doit = sc_netlink_post_doit,
};
/* */
static int nlsmartcapwap_handler(u32 ifindex, struct sk_buff* skb, int sig_dbm, unsigned char rate, void* data) {
int result = 0;
int frame8023 = 0;
struct nlsmartcapwap_device* nldev = (struct nlsmartcapwap_device*)data;
static int sc_netlink_handler(uint32_t ifindex, struct sk_buff* skb, int sig_dbm, unsigned char rate, void* data) {
int ret = 0;
struct sc_netlink_device* nldev = (struct sc_netlink_device*)data;
struct ieee80211_hdr* hdr = (struct ieee80211_hdr*)skb->data;
TRACEKMOD("### sc_netlink_handler\n");
/* IEEE802.11 Data Packet */
if (ieee80211_is_data(hdr->frame_control)) {
if ((nldev->flags & SMARTCAPWAP_FLAGS_BLOCK_DATA_FRAME) && ieee80211_is_data_present(hdr->frame_control)) {
result = -1;
int err;
struct sc_capwap_session* session;
unsigned char radioaddrbuffer[CAPWAP_RADIO_EUI48_LENGTH_PADDED];
unsigned char winfobuffer[CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED];
struct sc_capwap_radio_addr* radioaddr = NULL;
struct sc_capwap_wireless_information* winfo = NULL;
printk("*** receive packet\n");
/* Drop packet */
ret = -1;
/* */
session = sc_capwap_getsession(NULL);
if (!session) {
goto error;
}
/* Convert IEEE802.11 to IEEE802.3 */
if (nldev->flags & SMARTCAPWAP_FLAGS_TUNNEL_8023) {
frame8023 = 1;
}
}
/* */
if (nldev->flags & SMARTCAPWAP_FLAGS_SEND_USERSPACE) {
void* msg;
struct sk_buff* sk_msg;
/* Alloc message */
sk_msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
if (sk_msg) {
/* Set command */
msg = genlmsg_put(sk_msg, 0, 0, &nlsmartcapwap_family, 0, NLSMARTCAPWAP_CMD_FRAME);
if (msg) {
/* Set params */
if (nla_put_u32(sk_msg, NLSMARTCAPWAP_ATTR_IFINDEX, nldev->ifindex) ||
nla_put(sk_msg, NLSMARTCAPWAP_ATTR_FRAME, skb->len, skb->data) ||
(frame8023 && nla_put_flag(sk_msg, NLSMARTCAPWAP_ATTR_8023_FRAME)) ||
(sig_dbm && nla_put_u32(sk_msg, NLSMARTCAPWAP_ATTR_RX_SIGNAL_DBM, (u32)sig_dbm)) ||
(rate && nla_put_u8(sk_msg, NLSMARTCAPWAP_ATTR_RX_RATE, (u8)rate))) {
/* Abort message */
genlmsg_cancel(sk_msg, msg);
nlmsg_free(sk_msg);
} else {
/* Send message */
genlmsg_end(sk_msg, msg);
genlmsg_unicast(&init_net, sk_msg, nldev->usermodeid);
}
} else {
nlmsg_free(sk_msg);
/* IEEE 802.11 into IEEE 802.3 */
if (nldev->flags & NLSMARTCAPWAP_FLAGS_TUNNEL_8023) {
if (ieee80211_data_to_8023(skb, nldev->dev->dev_addr, NL80211_IFTYPE_AP)) {
printk("*** convertion error\n");
goto error;
}
/* Create Radio Mac Address */
radioaddr = sc_capwap_setradiomacaddress(radioaddrbuffer, CAPWAP_RADIO_EUI48_LENGTH_PADDED, nldev->dev->dev_addr);
}
/* Create Wireless Information */
if (sig_dbm || rate) {
winfo = sc_capwap_setwirelessinformation(winfobuffer, CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED, (uint8_t)sig_dbm, 0, ((uint16_t)rate) * 5);
}
/* */
CAPWAP_SKB_CB(skb)->flags = SKB_CAPWAP_FLAG_FROM_IEEE80211;
/* Forward to AC */
err = sc_capwap_forwarddata(session, nldev->radioid, nldev->binding, skb, nldev->flags, radioaddr, (radioaddr ? CAPWAP_RADIO_EUI48_LENGTH_PADDED : 0), winfo, (winfo ? CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED : 0));
printk("*** send: %d\n", err);
}
return result;
error:
return ret;
}
/* */
static struct nlsmartcapwap_device* nlsmartcapwap_new_device(u32 ifindex) {
struct nlsmartcapwap_device* nldev;
static struct sc_netlink_device* sc_netlink_new_device(uint32_t ifindex, uint8_t radioid, u8 wlanid, uint8_t binding) {
struct net_device* dev;
struct sc_netlink_device* nldev;
TRACEKMOD("### sc_netlink_new_device\n");
/* Retrieve device from ifindex */
dev = dev_get_by_index(&init_net, ifindex);
if (!dev) {
return NULL;
}
/* Check if wireless device */
if (!dev->ieee80211_ptr || !dev->ieee80211_ptr->wiphy) {
dev_put(dev);
return NULL;
}
/* Create device */
nldev = (struct nlsmartcapwap_device*)kzalloc(sizeof(struct nlsmartcapwap_device), GFP_KERNEL);
if (nldev) {
/* Initialize device */
nldev->pcktunnel_handler.handler = nlsmartcapwap_handler;
nldev->pcktunnel_handler.data = (void*)nldev;
nldev->ifindex = ifindex;
nldev = (struct sc_netlink_device*)kzalloc(sizeof(struct sc_netlink_device), GFP_KERNEL);
if (!nldev) {
dev_put(dev);
return NULL;
}
/* Initialize device */
nldev->pcktunnel_handler.handler = sc_netlink_handler;
nldev->pcktunnel_handler.data = (void*)nldev;
nldev->ifindex = ifindex;
nldev->radioid = radioid;
nldev->wlanid = wlanid;
nldev->binding = binding;
nldev->dev = dev;
return nldev;
}
/* */
static void nlsmartcapwap_free_device(struct nlsmartcapwap_device* nldev) {
static void sc_netlink_free_device(struct sc_netlink_device* nldev) {
TRACEKMOD("### sc_netlink_free_device\n");
/* Disconnect device from mac80211 */
ieee80211_pcktunnel_deregister(nldev->ifindex, &nldev->pcktunnel_handler);
ieee80211_pcktunnel_deregister(nldev->dev, &nldev->pcktunnel_handler);
/* */
dev_put(nldev->dev);
/* Free memory */
kfree(nldev);
}
/* */
static struct nlsmartcapwap_device* nlsmartcapwap_register_device(u32 ifindex) {
struct nlsmartcapwap_device* nldev;
static struct sc_netlink_device* sc_netlink_register_device(uint32_t ifindex, uint8_t radioid, uint16_t wlanid, uint8_t binding) {
struct sc_netlink_device* nldev;
TRACEKMOD("### sc_netlink_register_device\n");
ASSERT_RTNL();
/* */
if (!IS_VALID_RADIOID(radioid) || !IS_VALID_WLANID(wlanid)) {
return NULL;
}
/* Search device */
list_for_each_entry(nldev, &nlsmartcapwap_dev_list, list) {
list_for_each_entry(nldev, &sc_netlink_dev_list, list) {
if (nldev->ifindex == ifindex) {
return nldev;
return NULL;
}
}
/* Create device */
nldev = nlsmartcapwap_new_device(ifindex);
nldev = sc_netlink_new_device(ifindex, radioid, wlanid, binding);
if (nldev) {
list_add_rcu(&nldev->list, &nlsmartcapwap_dev_list);
list_add_rcu(&nldev->list, &sc_netlink_dev_list);
}
return nldev;
}
/* */
static int nlsmartcapwap_unregister_device(u32 ifindex) {
static int sc_netlink_unregister_device(uint32_t ifindex) {
int ret = -ENODEV;
struct nlsmartcapwap_device* nldev;
struct sc_netlink_device* nldev;
TRACEKMOD("### sc_netlink_unregister_device\n");
ASSERT_RTNL();
/* Search device */
list_for_each_entry(nldev, &nlsmartcapwap_dev_list, list) {
list_for_each_entry(nldev, &sc_netlink_dev_list, list) {
if (nldev->ifindex == ifindex) {
/* Remove from list */
list_del_rcu(&nldev->list);
@ -163,7 +211,7 @@ static int nlsmartcapwap_unregister_device(u32 ifindex) {
/* Free device */
ret = 0;
nlsmartcapwap_free_device(nldev);
sc_netlink_free_device(nldev);
break;
}
}
@ -172,27 +220,43 @@ static int nlsmartcapwap_unregister_device(u32 ifindex) {
}
/* */
static void nlsmartcapwap_close(void) {
struct nlsmartcapwap_device* nldev;
struct nlsmartcapwap_device* tmp;
static void sc_netlink_unregister_alldevice(void) {
struct sc_netlink_device* tmp;
struct sc_netlink_device* nldev;
list_for_each_entry_safe(nldev, tmp, &nlsmartcapwap_dev_list, list) {
list_del(&nldev->list);
TRACEKMOD("### sc_netlink_unregister_alldevice\n");
ASSERT_RTNL();
/* Close all devices */
list_for_each_entry_safe(nldev, tmp, &sc_netlink_dev_list, list) {
/* Remove from list */
list_del_rcu(&nldev->list);
synchronize_net();
/* Free device */
nlsmartcapwap_free_device(nldev);
sc_netlink_free_device(nldev);
}
}
/* */
static int nlsmartcapwap_link(struct sk_buff* skb, struct genl_info* info) {
int ret = 0;
u32 portid = genl_info_snd_portid(info);
static int sc_netlink_link(struct sk_buff* skb, struct genl_info* info) {
int ret;
uint32_t portid = genl_info_snd_portid(info);
if (!nlsmartcapwap_usermodeid) {
nlsmartcapwap_usermodeid = portid;
} else if (nlsmartcapwap_usermodeid == portid) {
TRACEKMOD("### sc_netlink_link\n");
if (!sc_netlink_usermodeid) {
/* Initialize library */
ret = sc_capwap_init(1);
if (!ret) {
sc_netlink_usermodeid = portid;
/* Deny unload module */
try_module_get(THIS_MODULE);
}
} else if (sc_netlink_usermodeid == portid) {
ret = -EALREADY;
} else {
ret = -EBUSY;
@ -202,19 +266,39 @@ static int nlsmartcapwap_link(struct sk_buff* skb, struct genl_info* info) {
}
/* */
static int nlsmartcapwap_netlink_notify(struct notifier_block* nb, unsigned long state, void* _notify) {
struct netlink_notify* notify = (struct netlink_notify*)_notify;
u32 portid = netlink_notify_portid(notify);
static int sc_netlink_reset(struct sk_buff* skb, struct genl_info* info) {
/* Check Link */
if (sc_netlink_usermodeid != genl_info_snd_portid(info)) {
return -ENOLINK;
}
/* Close all devices */
sc_netlink_unregister_alldevice();
/* Reset session */
sc_capwap_resetsession();
return 0;
}
/* */
static int sc_netlink_notify(struct notifier_block* nb, unsigned long state, void* _notify) {
struct netlink_notify* notify = (struct netlink_notify*)_notify;
/* */
if (state == NETLINK_URELEASE) {
rtnl_lock();
if (nlsmartcapwap_usermodeid == portid) {
nlsmartcapwap_usermodeid = 0;
if (sc_netlink_usermodeid == netlink_notify_portid(notify)) {
sc_netlink_usermodeid = 0;
/* Close all devices */
nlsmartcapwap_close();
sc_netlink_unregister_alldevice();
/* Close capwap engine */
sc_capwap_close();
/* Allow unload module */
module_put(THIS_MODULE);
}
rtnl_unlock();
@ -224,10 +308,194 @@ static int nlsmartcapwap_netlink_notify(struct notifier_block* nb, unsigned long
}
/* */
static int nlsmartcapwap_join_mac80211_device(struct sk_buff* skb, struct genl_info* info) {
u32 ifindex;
struct nlsmartcapwap_device* nldev;
int ret = -EINVAL;
static int sc_netlink_bind(struct sk_buff* skb, struct genl_info* info) {
union capwap_addr sockaddr;
TRACEKMOD("### sc_netlink_bind\n");
/* Check Link */
if (sc_netlink_usermodeid != genl_info_snd_portid(info)) {
return -ENOLINK;
}
/* Get bind address */
if (!info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS] || (nla_len(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]) != sizeof(struct sockaddr_storage))) {
return -EINVAL;
}
memcpy(&sockaddr.ss, nla_data(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]), sizeof(struct sockaddr_storage));
if ((sockaddr.ss.ss_family != AF_INET) && (sockaddr.ss.ss_family != AF_INET6)) {
return -EINVAL;
}
/* Bind socket */
return sc_capwap_bind(&sockaddr);
}
/* */
static int sc_netlink_connect(struct sk_buff* skb, struct genl_info* info) {
int ret;
union capwap_addr sockaddr;
struct sc_capwap_sessionid_element sessionid;
uint16_t mtu = DEFAULT_MTU;
TRACEKMOD("### sc_netlink_connect\n");
/* Check Link */
if (sc_netlink_usermodeid != genl_info_snd_portid(info)) {
return -ENOLINK;
}
/* Get AC address */
if (!info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS] || (nla_len(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]) != sizeof(struct sockaddr_storage))) {
return -EINVAL;
}
memcpy(&sockaddr.ss, nla_data(info->attrs[NLSMARTCAPWAP_ATTR_ADDRESS]), sizeof(struct sockaddr_storage));
if ((sockaddr.ss.ss_family != AF_INET) && (sockaddr.ss.ss_family != AF_INET6)) {
return -EINVAL;
}
/* Get MTU */
if (info->attrs[NLSMARTCAPWAP_ATTR_MTU]) {
mtu = nla_get_u16(info->attrs[NLSMARTCAPWAP_ATTR_MTU]);
if ((mtu < MIN_MTU) || (mtu > MAX_MTU)) {
return -EINVAL;
}
}
/* Get Session ID */
if (info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID] && (nla_len(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]) == sizeof(struct sc_capwap_sessionid_element))) {
memcpy(sessionid.id, nla_data(info->attrs[NLSMARTCAPWAP_ATTR_SESSION_ID]), sizeof(struct sc_capwap_sessionid_element));
} else {
return -EINVAL;
}
/* Send packet */
ret = sc_capwap_connect(&sockaddr, &sessionid, mtu);
if (ret < 0) {
return ret;
}
return 0;
}
/* */
static int sc_netlink_send_keepalive(struct sk_buff* skb, struct genl_info* info) {
int ret;
TRACEKMOD("### sc_netlink_send_keepalive\n");
/* Check Link */
if (sc_netlink_usermodeid != genl_info_snd_portid(info)) {
return -ENOLINK;
}
/* Send packet */
ret = sc_capwap_sendkeepalive();
if (ret < 0) {
return ret;
}
return 0;
}
/* */
static int sc_netlink_send_data(struct sk_buff* skb, struct genl_info* info) {
int ret;
uint8_t radioid;
uint8_t binding;
uint8_t rssi = 0;
uint8_t snr = 0;
uint16_t rate = 0;
int length;
struct sk_buff* skbdata;
struct sc_capwap_session* session;
unsigned char winfobuffer[CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED];
struct sc_capwap_wireless_information* winfo = NULL;
TRACEKMOD("### sc_netlink_send_data\n");
/* Check Link */
if (sc_netlink_usermodeid != genl_info_snd_portid(info)) {
return -ENOLINK;
} else if (!info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] || !info->attrs[NLSMARTCAPWAP_ATTR_DATA_FRAME]) {
return -EINVAL;
}
/* */
session = sc_capwap_getsession(NULL);
if (!session) {
return -ENOLINK;
}
/* Get radioid */
radioid = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RADIOID]);
if (!IS_VALID_RADIOID(radioid) || !info->attrs[NLSMARTCAPWAP_ATTR_BINDING]) {
return -EINVAL;
}
/* Get binding */
binding = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_BINDING]);
/* Get RSSI */
if (info->attrs[NLSMARTCAPWAP_ATTR_RSSI]) {
rssi = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RSSI]);
}
/* Get SNR */
if (info->attrs[NLSMARTCAPWAP_ATTR_SNR]) {
snr = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_SNR]);
}
/* Get RATE */
if (info->attrs[NLSMARTCAPWAP_ATTR_RATE]) {
rate = nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RATE]);
}
/* Create Wireless Information */
if (rssi || snr || rate) {
winfo = sc_capwap_setwirelessinformation(winfobuffer, CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED, rssi, snr, rate);
}
/* Create socket buffer */
length = nla_len(info->attrs[NLSMARTCAPWAP_ATTR_DATA_FRAME]);
skbdata = alloc_skb(length + CAPWAP_HEADER_MAX_LENGTH, GFP_KERNEL);
if (!skbdata) {
return -ENOMEM;
}
/* Reserve space for Capwap Header */
skb_reserve(skbdata, CAPWAP_HEADER_MAX_LENGTH);
/* Copy data into socket buffer */
memcpy(skb_put(skbdata, length), nla_data(info->attrs[NLSMARTCAPWAP_ATTR_DATA_FRAME]), length);
/* */
CAPWAP_SKB_CB(skb)->flags = SKB_CAPWAP_FLAG_FROM_USER_SPACE;
/* Send packet */
ret = sc_capwap_forwarddata(session, radioid, binding, skbdata, 0, NULL, 0, winfo, (winfo ? CAPWAP_WINFO_FRAMEINFO_LENGTH_PADDED : 0));
if (ret) {
TRACEKMOD("*** Unable send packet from sc_netlink_send_data function\n");
}
kfree_skb(skbdata);
return ret;
}
/* */
static int sc_netlink_join_mac80211_device(struct sk_buff* skb, struct genl_info* info) {
int ret;
uint32_t ifindex;
struct sc_netlink_device* nldev;
TRACEKMOD("### sc_netlink_join_mac80211_device\n");
/* Check Link */
if (sc_netlink_usermodeid != genl_info_snd_portid(info)) {
return -ENOLINK;
}
/* Get interface index */
if (!info->attrs[NLSMARTCAPWAP_ATTR_IFINDEX]) {
@ -239,8 +507,13 @@ static int nlsmartcapwap_join_mac80211_device(struct sk_buff* skb, struct genl_i
return -EINVAL;
}
/* Check */
if (!info->attrs[NLSMARTCAPWAP_ATTR_RADIOID] || !info->attrs[NLSMARTCAPWAP_ATTR_WLANID] || !info->attrs[NLSMARTCAPWAP_ATTR_BINDING]) {
return -EINVAL;
}
/* Register device */
nldev = nlsmartcapwap_register_device(ifindex);
nldev = sc_netlink_register_device(ifindex, nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_RADIOID]), nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_WLANID]), nla_get_u8(info->attrs[NLSMARTCAPWAP_ATTR_BINDING]));
if (!nldev) {
return -EINVAL;
}
@ -263,106 +536,240 @@ static int nlsmartcapwap_join_mac80211_device(struct sk_buff* skb, struct genl_i
nldev->pcktunnel_handler.subtype_mask[2] = nla_get_u16(info->attrs[NLSMARTCAPWAP_ATTR_DATA_SUBTYPE_MASK]);
}
/* */
if (nldev->flags & SMARTCAPWAP_FLAGS_SEND_USERSPACE) {
nldev->usermodeid = genl_info_snd_portid(info);
}
/* Connect device to mac80211 */
ret = ieee80211_pcktunnel_register(ifindex, &nldev->pcktunnel_handler);
ret = ieee80211_pcktunnel_register(nldev->dev, &nldev->pcktunnel_handler);
if (ret) {
nlsmartcapwap_unregister_device(ifindex);
sc_netlink_unregister_device(ifindex);
}
return ret;
}
/* */
static int nlsmartcapwap_leave_mac80211_device(struct sk_buff* skb, struct genl_info* info) {
u32 ifindex;
static int sc_netlink_leave_mac80211_device(struct sk_buff* skb, struct genl_info* info) {
TRACEKMOD("### sc_netlink_leave_mac80211_device\n");
/* Check Link */
if (sc_netlink_usermodeid != genl_info_snd_portid(info)) {
return -ENOLINK;
}
/* Get interface index */
if (!info->attrs[NLSMARTCAPWAP_ATTR_IFINDEX]) {
return -EINVAL;
}
ifindex = nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_IFINDEX]);
if (!ifindex) {
return -EINVAL;
}
/* Unregister device */
return nlsmartcapwap_unregister_device(ifindex);
return sc_netlink_unregister_device(nla_get_u32(info->attrs[NLSMARTCAPWAP_ATTR_IFINDEX]));
}
/* */
static const struct nla_policy nlsmartcapwap_policy[NLSMARTCAPWAP_ATTR_MAX + 1] = {
static int sc_device_event(struct notifier_block* unused, unsigned long event, void* ptr) {
struct net_device* dev = netdev_notifier_info_to_dev(ptr);
/* Check event only if connect with WTP userspace */
if (!sc_netlink_usermodeid) {
return NOTIFY_DONE;
}
/* */
switch (event) {
case NETDEV_UNREGISTER: {
/* Try to unregister device */
sc_netlink_unregister_device(dev->ifindex);
break;
}
}
return NOTIFY_DONE;
}
/* */
static const struct nla_policy sc_netlink_policy[NLSMARTCAPWAP_ATTR_MAX + 1] = {
[NLSMARTCAPWAP_ATTR_IFINDEX] = { .type = NLA_U32 },
[NLSMARTCAPWAP_ATTR_RADIOID] = { .type = NLA_U8 },
[NLSMARTCAPWAP_ATTR_WLANID] = { .type = NLA_U8 },
[NLSMARTCAPWAP_ATTR_BINDING] = { .type = NLA_U8 },
[NLSMARTCAPWAP_ATTR_FLAGS] = { .type = NLA_U32 },
[NLSMARTCAPWAP_ATTR_MGMT_SUBTYPE_MASK] = { .type = NLA_U16 },
[NLSMARTCAPWAP_ATTR_CTRL_SUBTYPE_MASK] = { .type = NLA_U16 },
[NLSMARTCAPWAP_ATTR_DATA_SUBTYPE_MASK] = { .type = NLA_U16 },
[NLSMARTCAPWAP_ATTR_FRAME] = { .type = NLA_BINARY, .len = IEEE80211_MAX_DATA_LEN },
[NLSMARTCAPWAP_ATTR_8023_FRAME] = { .type = NLA_FLAG },
[NLSMARTCAPWAP_ATTR_RX_SIGNAL_DBM] = { .type = NLA_U32 },
[NLSMARTCAPWAP_ATTR_RX_RATE] = { .type = NLA_U8 },
[NLSMARTCAPWAP_ATTR_ADDRESS] = { .type = NLA_BINARY, .len = sizeof(struct sockaddr_storage) },
[NLSMARTCAPWAP_ATTR_MTU] = { .type = NLA_U16 },
[NLSMARTCAPWAP_ATTR_SESSION_ID] = { .type = NLA_BINARY, .len = sizeof(struct sc_capwap_sessionid_element) },
[NLSMARTCAPWAP_ATTR_DTLS] = { .type = NLA_U16 },
[NLSMARTCAPWAP_ATTR_DATA_FRAME] = { .type = NLA_BINARY, .len = IEEE80211_MTU },
[NLSMARTCAPWAP_ATTR_RSSI] = { .type = NLA_U8 },
[NLSMARTCAPWAP_ATTR_SNR] = { .type = NLA_U8 },
[NLSMARTCAPWAP_ATTR_RATE] = { .type = NLA_U16 },
};
/* Netlink Ops */
static __genl_const struct genl_ops nlsmartcapwap_ops[] = {
static __genl_const struct genl_ops sc_netlink_ops[] = {
{
.cmd = NLSMARTCAPWAP_CMD_LINK,
.doit = nlsmartcapwap_link,
.policy = nlsmartcapwap_policy,
.doit = sc_netlink_link,
.policy = sc_netlink_policy,
.flags = GENL_ADMIN_PERM,
},
{
.cmd = NLSMARTCAPWAP_CMD_BIND,
.doit = sc_netlink_bind,
.policy = sc_netlink_policy,
.flags = GENL_ADMIN_PERM,
},
{
.cmd = NLSMARTCAPWAP_CMD_CONNECT,
.doit = sc_netlink_connect,
.policy = sc_netlink_policy,
.flags = GENL_ADMIN_PERM,
},
{
.cmd = NLSMARTCAPWAP_CMD_RESET,
.doit = sc_netlink_reset,
.policy = sc_netlink_policy,
.flags = GENL_ADMIN_PERM,
},
{
.cmd = NLSMARTCAPWAP_CMD_SEND_KEEPALIVE,
.doit = sc_netlink_send_keepalive,
.policy = sc_netlink_policy,
.flags = GENL_ADMIN_PERM,
},
{
.cmd = NLSMARTCAPWAP_CMD_SEND_DATA,
.doit = sc_netlink_send_data,
.policy = sc_netlink_policy,
.flags = GENL_ADMIN_PERM,
},
{
.cmd = NLSMARTCAPWAP_CMD_JOIN_MAC80211_DEVICE,
.doit = nlsmartcapwap_join_mac80211_device,
.policy = nlsmartcapwap_policy,
.doit = sc_netlink_join_mac80211_device,
.policy = sc_netlink_policy,
.flags = GENL_ADMIN_PERM,
},
{
.cmd = NLSMARTCAPWAP_CMD_LEAVE_MAC80211_DEVICE,
.doit = nlsmartcapwap_leave_mac80211_device,
.policy = nlsmartcapwap_policy,
.doit = sc_netlink_leave_mac80211_device,
.policy = sc_netlink_policy,
.flags = GENL_ADMIN_PERM,
},
};
/* Netlink notify */
static struct notifier_block nlsmartcapwap_netlink_notifier = {
.notifier_call = nlsmartcapwap_netlink_notify,
static struct notifier_block sc_netlink_notifier = {
.notifier_call = sc_netlink_notify,
};
/* Interface notify */
struct notifier_block sc_device_notifier = {
.notifier_call = sc_device_event
};
/* */
int nlsmartcapwap_init(void) {
int sc_netlink_notify_recv_keepalive(const union capwap_addr* sockaddr, struct sc_capwap_sessionid_element* sessionid) {
void* msg;
struct sk_buff* sk_msg;
TRACEKMOD("### sc_netlink_notify_recv_keepalive\n");
/* Alloc message */
sk_msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
if (!sk_msg) {
return -ENOMEM;
}
/* Set command */
msg = genlmsg_put(sk_msg, 0, 0, &sc_netlink_family, 0, NLSMARTCAPWAP_CMD_RECV_KEEPALIVE);
if (!msg) {
nlmsg_free(sk_msg);
return -ENOMEM;
}
/* Send message */
genlmsg_end(sk_msg, msg);
return genlmsg_unicast(&init_net, sk_msg, sc_netlink_usermodeid);
}
/* */
int sc_netlink_notify_recv_data(uint8_t* packet, int length) {
void* msg;
struct sk_buff* sk_msg;
TRACEKMOD("### sc_netlink_notify_recv_data\n");
/* Alloc message */
sk_msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
if (!sk_msg) {
return -ENOMEM;
}
/* Set command */
msg = genlmsg_put(sk_msg, 0, 0, &sc_netlink_family, 0, NLSMARTCAPWAP_CMD_RECV_DATA);
if (!msg) {
goto error;
}
/* */
if (nla_put(sk_msg, NLSMARTCAPWAP_ATTR_DATA_FRAME, length, packet)) {
goto error2;
}
/* Send message */
genlmsg_end(sk_msg, msg);
return genlmsg_unicast(&init_net, sk_msg, sc_netlink_usermodeid);
error2:
genlmsg_cancel(sk_msg, msg);
error:
nlmsg_free(sk_msg);
return -ENOMEM;
}
/* */
int sc_netlink_init(void) {
int ret;
/* Register netlink family */
ret = genl_register_family_with_ops(&nlsmartcapwap_family, nlsmartcapwap_ops);
TRACEKMOD("### sc_netlink_init\n");
/* */
sc_netlink_usermodeid = 0;
/* Register interface event */
ret = register_netdevice_notifier(&sc_device_notifier);
if (ret) {
return ret;
goto error;
}
/* Register netlink family */
ret = genl_register_family_with_ops(&sc_netlink_family, sc_netlink_ops);
if (ret) {
goto error2;
}
/* Register netlink notifier */
ret = netlink_register_notifier(&nlsmartcapwap_netlink_notifier);
ret = netlink_register_notifier(&sc_netlink_notifier);
if (ret) {
genl_unregister_family(&nlsmartcapwap_family);
return ret;
goto error3;
}
return 0;
error3:
genl_unregister_family(&sc_netlink_family);
error2:
unregister_netdevice_notifier(&sc_device_notifier);
error:
return ret;
}
/* */
void nlsmartcapwap_exit(void) {
/* */
rtnl_lock();
nlsmartcapwap_close();
rtnl_unlock();
void sc_netlink_exit(void) {
TRACEKMOD("### sc_netlink_exit\n");
/* */
netlink_unregister_notifier(&nlsmartcapwap_netlink_notifier);
genl_unregister_family(&nlsmartcapwap_family);
netlink_unregister_notifier(&sc_netlink_notifier);
genl_unregister_family(&sc_netlink_family);
unregister_netdevice_notifier(&sc_device_notifier);
}

17
src/wtp/kmod/netlinkapp.h
View File

@ -1,8 +1,15 @@
#ifndef __KMOD_CAPWAP_NETLINKAPP_HEADER__
#define __KMOD_CAPWAP_NETLINKAPP_HEADER__
#ifndef __KMOD_WTP_NETLINKAPP_HEADER__
#define __KMOD_WTP_NETLINKAPP_HEADER__
#include "capwap_rfc.h"
#include "socket.h"
/* */
int nlsmartcapwap_init(void);
void nlsmartcapwap_exit(void);
int sc_netlink_init(void);
void sc_netlink_exit(void);
#endif /* __KMOD_CAPWAP_NETLINKAPP_HEADER__ */
/* */
int sc_netlink_notify_recv_keepalive(const union capwap_addr* sockaddr, struct sc_capwap_sessionid_element* sessionid);
int sc_netlink_notify_recv_data(uint8_t* packet, int length);
#endif /* __KMOD_WTP_NETLINKAPP_HEADER__ */

37
src/wtp/kmod/nlsmartcapwap.h
View File

@ -1,19 +1,20 @@
#ifndef __CAPWAP_NLSMARTCAPWAP_HEADER__
#define __CAPWAP_NLSMARTCAPWAP_HEADER__
#ifndef __WTP_NLSMARTCAPWAP_HEADER__
#define __WTP_NLSMARTCAPWAP_HEADER__
/* */
#define SMARTCAPWAP_GENL_NAME "smartcapwap"
#define NLSMARTCAPWAP_GENL_NAME "smartcapwap_wtp"
/* */
#define SMARTCAPWAP_FLAGS_SEND_USERSPACE 0x00000001
#define SMARTCAPWAP_FLAGS_BLOCK_DATA_FRAME 0x00000002
#define SMARTCAPWAP_FLAGS_TUNNEL_8023 0x00000004
#define NLSMARTCAPWAP_FLAGS_TUNNEL_8023 0x00000001
/* */
enum nlsmartcapwap_attrs {
NLSMARTCAPWAP_ATTR_UNSPEC,
NLSMARTCAPWAP_ATTR_IFINDEX,
NLSMARTCAPWAP_ATTR_RADIOID,
NLSMARTCAPWAP_ATTR_WLANID,
NLSMARTCAPWAP_ATTR_BINDING,
NLSMARTCAPWAP_ATTR_FLAGS,
@ -21,10 +22,17 @@ enum nlsmartcapwap_attrs {
NLSMARTCAPWAP_ATTR_CTRL_SUBTYPE_MASK,
NLSMARTCAPWAP_ATTR_DATA_SUBTYPE_MASK,
NLSMARTCAPWAP_ATTR_FRAME,
NLSMARTCAPWAP_ATTR_8023_FRAME,
NLSMARTCAPWAP_ATTR_RX_SIGNAL_DBM,
NLSMARTCAPWAP_ATTR_RX_RATE,
NLSMARTCAPWAP_ATTR_ADDRESS,
NLSMARTCAPWAP_ATTR_MTU,
NLSMARTCAPWAP_ATTR_SESSION_ID,
NLSMARTCAPWAP_ATTR_DTLS,
NLSMARTCAPWAP_ATTR_DATA_FRAME,
NLSMARTCAPWAP_ATTR_RSSI,
NLSMARTCAPWAP_ATTR_SNR,
NLSMARTCAPWAP_ATTR_RATE,
/* Last attribute */
__NLSMARTCAPWAP_ATTR_AFTER_LAST,
@ -37,14 +45,15 @@ enum nlsmartcapwap_commands {
NLSMARTCAPWAP_CMD_LINK,
NLSMARTCAPWAP_CMD_SET_AC_ADDRESS,
NLSMARTCAPWAP_CMD_BIND,
NLSMARTCAPWAP_CMD_CONNECT,
NLSMARTCAPWAP_CMD_TEARDOWN,
NLSMARTCAPWAP_CMD_RESET,
NLSMARTCAPWAP_CMD_SEND_KEEPALIVE,
NLSMARTCAPWAP_CMD_RECV_KEEPALIVE,
NLSMARTCAPWAP_CMD_FRAME,
NLSMARTCAPWAP_CMD_SEND_DATA,
NLSMARTCAPWAP_CMD_RECV_DATA,
NLSMARTCAPWAP_CMD_JOIN_MAC80211_DEVICE,
NLSMARTCAPWAP_CMD_LEAVE_MAC80211_DEVICE,
@ -54,4 +63,4 @@ enum nlsmartcapwap_commands {
NLSMARTCAPWAP_CMD_MAX = __NLSMARTCAPWAP_CMD_AFTER_LAST - 1
};
#endif /* __CAPWAP_NLSMARTCAPWAP_HEADER__ */
#endif /* __WTP_NLSMARTCAPWAP_HEADER__ */

257
src/wtp/kmod/socket.c Normal file
View File

@ -0,0 +1,257 @@
#include "config.h"
#include <linux/module.h>
#include <linux/sched.h>
#include <linux/socket.h>
#include <linux/kthread.h>
#include <linux/net.h>
#include <linux/if_ether.h>
#include <linux/udp.h>
#include <net/ipv6.h>
#include <net/sock.h>
#include <net/udp.h>
#include "socket.h"
#include "capwap.h"
/* Socket */
#define SOCKET_COUNT 2
static struct socket* sc_sockets[SOCKET_COUNT];
/* */
int sc_socket_recvpacket(struct sock* sk, struct sk_buff* skb) {
struct sc_skb_capwap_cb* cb = CAPWAP_SKB_CB(skb);
TRACEKMOD("### sc_socket_recvpacket\n");
/* */
cb->flags = SKB_CAPWAP_FLAG_FROM_DATA_CHANNEL;
/* */
sc_capwap_recvpacket(skb);
return 0;
}
/* */
static int sc_socket_create(int type, union capwap_addr* sockaddr, uint16_t protocol) {
int ret;
TRACEKMOD("### sc_socket_create\n");
/* Create socket */
ret = sock_create_kern(sockaddr->ss.ss_family, SOCK_DGRAM, protocol, &sc_sockets[type]);
if (ret) {
goto failure;
}
/* Bind to interface */
ret = kernel_bind(sc_sockets[type], &sockaddr->sa, sizeof(union capwap_addr));
if (ret) {
goto failure2;
}
/* Set callback */
udp_sk(sc_sockets[type]->sk)->encap_type = 1;
udp_sk(sc_sockets[type]->sk)->encap_rcv = sc_socket_recvpacket;
/* */
if (!((sockaddr->ss.ss_family == AF_INET) ? sockaddr->sin.sin_port : sockaddr->sin6.sin6_port)) {
union capwap_addr localaddr;
int localaddrsize = sizeof(union capwap_addr);
/* Retrieve port */
ret = kernel_getsockname(sc_sockets[type], &localaddr.sa, &localaddrsize);
if (ret) {
goto failure2;
}
/* */
if ((sockaddr->ss.ss_family == AF_INET) && (localaddr.ss.ss_family == AF_INET)) {
sockaddr->sin.sin_port = localaddr.sin.sin_port;
} else if ((sockaddr->ss.ss_family == AF_INET6) && (localaddr.ss.ss_family == AF_INET6)) {
sockaddr->sin6.sin6_port = localaddr.sin6.sin6_port;
} else {
ret = -EFAULT;
goto failure2;
}
}
return ret;
failure2:
sock_release(sc_sockets[type]);
sc_sockets[type] = 0;
failure:
return ret;
}
/* */
int sc_socket_getpeeraddr(struct sk_buff* skb, union capwap_addr* peeraddr) {
unsigned char* nethdr;
TRACEKMOD("### sc_socket_getpeeraddr\n");
/* */
nethdr = skb_network_header(skb);
if (!nethdr) {
return -EINVAL;
}
/* */
switch (ntohs(skb->protocol)) {
case ETH_P_IP: {
/* Validate IPv4 header */
if ((nethdr[0] & 0xf0) != 0x40) {
return -EINVAL;
}
/* Retrieve address */
peeraddr->sin.sin_family = AF_INET;
peeraddr->sin.sin_addr.s_addr = ((struct iphdr*)nethdr)->saddr;
peeraddr->sin.sin_port = udp_hdr(skb)->source;
break;
}
case ETH_P_IPV6: {
/* Validate IPv6 header */
if ((nethdr[0] & 0xf0) != 0x60) {
return -EINVAL;
}
/* Retrieve address */
peeraddr->sin6.sin6_family = AF_INET6;
memcpy(&peeraddr->sin6.sin6_addr, &((struct ipv6hdr*)nethdr)->saddr, sizeof(struct in6_addr));
peeraddr->sin6.sin6_port = udp_hdr(skb)->source;
break;
}
default: {
return -EINVAL;
}
}
return 0;
}
/* */
int sc_socket_send(int type, uint8_t* buffer, int length, union capwap_addr* sockaddr) {
struct kvec vec;
struct msghdr msg;
TRACEKMOD("### sc_socket_send\n");
/* */
vec.iov_base = buffer;
vec.iov_len = length;
/* */
memset(&msg, 0, sizeof(struct msghdr));
msg.msg_name = sockaddr;
msg.msg_namelen = sizeof(union capwap_addr);
msg.msg_flags = MSG_NOSIGNAL | MSG_DONTWAIT;
/* */
return kernel_sendmsg(sc_sockets[type], &msg, &vec, 1, length);
}
/* */
int sc_socket_init(void) {
TRACEKMOD("### sc_socket_init\n");
memset(sc_sockets, 0, sizeof(sc_sockets));
return 0;
}
/* */
int sc_socket_bind(union capwap_addr* sockaddr) {
int ret;
TRACEKMOD("### sc_socket_bind\n");
/* */
if (sc_sockets[SOCKET_UDP] || sc_sockets[SOCKET_UDPLITE]) {
return -EBUSY;
}
/* UDP socket */
ret = sc_socket_create(SOCKET_UDP, sockaddr, IPPROTO_UDP);
if (ret) {
goto failure;
}
/* UDPLite socket */
ret = sc_socket_create(SOCKET_UDPLITE, sockaddr, IPPROTO_UDPLITE);
if (ret) {
goto failure;
}
/* */
udp_encap_enable();
if (sockaddr->ss.ss_family == AF_INET6) {
udpv6_encap_enable();
}
return 0;
failure:
sc_socket_close();
return ret;
}
/* */
void sc_socket_close(void) {
TRACEKMOD("### sc_socket_close\n");
/* Close sockets */
if (sc_sockets[SOCKET_UDP]) {
kernel_sock_shutdown(sc_sockets[SOCKET_UDP], SHUT_RDWR);
sock_release(sc_sockets[SOCKET_UDP]);
}
if (sc_sockets[SOCKET_UDPLITE]) {
kernel_sock_shutdown(sc_sockets[SOCKET_UDPLITE], SHUT_RDWR);
sock_release(sc_sockets[SOCKET_UDPLITE]);
}
memset(sc_sockets, 0, sizeof(sc_sockets));
}
/* */
int sc_addr_compare(const union capwap_addr* addr1, const union capwap_addr* addr2) {
TRACEKMOD("### sc_addr_compare\n");
if (addr1->ss.ss_family == addr2->ss.ss_family) {
if (addr1->ss.ss_family == AF_INET) {
return (((addr1->sin.sin_addr.s_addr == addr2->sin.sin_addr.s_addr) && (addr1->sin.sin_port == addr2->sin.sin_port)) ? 0 : -1);
} else if (addr1->ss.ss_family == AF_INET6) {
return ((!memcmp(&addr1->sin6.sin6_addr, &addr2->sin6.sin6_addr, sizeof(struct in6_addr)) && (addr1->sin6.sin6_port == addr2->sin6.sin6_port)) ? 0 : -1);
}
}
return -1;
}
/* */
void sc_addr_tolittle(const union capwap_addr* addr, struct capwap_addr_little* little) {
little->family = (uint8_t)addr->ss.ss_family;
if (little->family == AF_INET) {
memcpy(&little->addr4, &addr->sin.sin_addr, sizeof(struct in_addr));
little->port = addr->sin.sin_port;
} else if (little->family == AF_INET6) {
memcpy(&little->addr6, &addr->sin6.sin6_addr, sizeof(struct in6_addr));
little->port = addr->sin6.sin6_port;
}
}
/* */
void sc_addr_fromlittle(const struct capwap_addr_little* little, union capwap_addr* addr) {
memset(addr, 0, sizeof(union capwap_addr));
addr->ss.ss_family = little->family;
if (little->family == AF_INET) {
memcpy(&addr->sin.sin_addr, &little->addr4, sizeof(struct in_addr));
addr->sin.sin_port = little->port;
} else if (little->family == AF_INET6) {
memcpy(&addr->sin6.sin6_addr, &little->addr6, sizeof(struct in6_addr));
addr->sin6.sin6_port = little->port;
}
}

45
src/wtp/kmod/socket.h Normal file
View File

@ -0,0 +1,45 @@
#ifndef __KMOD_SOCKET_HEADER__
#define __KMOD_SOCKET_HEADER__
#include <linux/socket.h>
#include <linux/in.h>
#include <linux/in6.h>
#include <linux/skbuff.h>
/* */
#define SOCKET_UDP 0
#define SOCKET_UDPLITE 1
/* Little socket address */
struct capwap_addr_little {
uint8_t family;
union {
struct in_addr addr4;
struct in6_addr addr6;
};
uint16_t port;
};
/* Universal socket address */
union capwap_addr {
struct sockaddr sa;
struct sockaddr_in sin;
struct sockaddr_in6 sin6;
struct sockaddr_storage ss;
};
/* */
int sc_socket_init(void);
void sc_socket_close(void);
/* */
int sc_socket_bind(union capwap_addr* sockaddr);
int sc_socket_send(int type, uint8_t* buffer, int length, union capwap_addr* sockaddr);
int sc_socket_getpeeraddr(struct sk_buff* skb, union capwap_addr* peeraddr);
/* */
int sc_addr_compare(const union capwap_addr* addr1, const union capwap_addr* addr2);
void sc_addr_tolittle(const union capwap_addr* addr, struct capwap_addr_little* little);
void sc_addr_fromlittle(const struct capwap_addr_little* little, union capwap_addr* addr);
#endif /* __KMOD_SOCKET_HEADER__ */