00001 #include "rt_wmp.h"
00002
00003 #define WMP_PROTO 0x6969
00004
00005
00006
00007 static struct net_device * net_dev;
00008 struct ieee80211_sub_if_data *sdata;
00009 static int initialized = 0, promisc = 0;
00010
00011 static struct rx_queue {
00012 struct sk_buff *skb;
00013 struct semaphore rx_sem;
00014 spinlock_t spin;
00015 } rx_rtwmp_queue;
00016
00017 static unsigned char ethernet_frame[2500];
00018 static struct ethhdr *ethernet_header;
00019 static unsigned char dst_mac[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
00020 static unsigned char src_mac[ETH_ALEN] = { 0xea, 0x1e, 0x1e, 0xe1, 0x00, 0x00 };
00021
00022
00023
00024
00025 static void init_rx_queue(struct rx_queue *q) {
00026 sema_init(&q->rx_sem, 0);
00027 spin_lock_init(&q->spin);
00028 q->skb=NULL;
00029 }
00030
00031
00032 static void purge_rx_queue(struct rx_queue *q) {
00033 if(q->skb!=NULL)
00034 dev_kfree_skb_any(q->skb);
00035 }
00036
00037 static void enqueue_rx_queue(struct rx_queue *q, struct sk_buff *skb) {
00038 unsigned long flags;
00039 spin_lock_irqsave(&q->spin, flags);
00040 if(q->skb == NULL){
00041 q->skb = skb;
00042 up(&q->rx_sem);
00043 } else {
00044 dev_kfree_skb_any(q->skb);
00045 q->skb = skb;
00046 }
00047 spin_unlock_irqrestore(&q->spin, flags);
00048 }
00049
00050 static struct sk_buff *dequeue_rx_queue(struct rx_queue *q, long timeout) {
00051 unsigned long flags;
00052 struct sk_buff *skb = NULL;
00053 if (timeout == 0) {
00054 if (0==down_interruptible(&q->rx_sem)) {
00055 spin_lock_irqsave(&q->spin, flags);
00056 skb = q->skb;
00057 q->skb = NULL;
00058 while(!down_trylock(&q->rx_sem));
00059 spin_unlock_irqrestore(&q->spin, flags);
00060 }
00061 } else if (0==down_timeout(&q->rx_sem, msecs_to_jiffies(timeout))) {
00062 spin_lock_irqsave(&q->spin, flags);
00063 skb = q->skb;
00064 q->skb = NULL;
00065 while(!down_trylock(&q->rx_sem)){
00066 printk(KERN_ERR "Spinning\n");
00067 }
00068 spin_unlock_irqrestore(&q->spin, flags);
00069 }
00070
00071 return skb;
00072 }
00073
00074 int rt_wmp_is_active(){
00075 return initialized;
00076 }
00077
00078 int rt_wmp_netif_receive_skb(struct sk_buff *skb, int signal){
00079 int proto;
00080 if (!initialized){
00081 #ifdef MAC80211
00082 dev_kfree_skb(skb);
00083 #else
00084 netif_rx(skb);
00085 #endif
00086 }else{
00087
00088 #ifdef MAC80211
00089 unsigned short * protocol;
00090 struct ieee80211_radiotap_header * rth = (struct ieee80211_radiotap_header *) skb->data;
00091
00092 protocol = (unsigned short *) (skb->data + rth->it_len + sizeof(struct ieee80211_hdr));
00093 proto = *protocol;
00094 #else
00095 proto = skb->protocol;
00096 skb->data[0] = 98 + (signed char) signal;
00097 #endif
00098
00099
00100 if (promisc){
00101 enqueue_rx_queue(&rx_rtwmp_queue, skb);
00102 }else{
00103 if (proto == 0x6969) {
00104 enqueue_rx_queue(&rx_rtwmp_queue, skb);
00105 }
00106 }
00107 }
00108
00109 return 1;
00110 }
00111
00112 int ieee80211_rtwmp_timed_receive_raw(struct sk_buff **skb, unsigned int ms){
00113 struct sk_buff * k = dequeue_rx_queue(&rx_rtwmp_queue, ms);
00114 *skb=k;
00115 return (k!=NULL);
00116 }
00117
00118 void ieee80211_rtwmp_finish(void){
00119 purge_rx_queue(&rx_rtwmp_queue);
00120 initialized = 0;
00121 printk(KERN_INFO "MAC80211-RT-WMP closed\n");
00122 }
00123
00124
00125 int ieee80211_rtwmp_send(char * f, int size) {
00126 int ret;
00127 struct sk_buff *skb;
00128
00129 skb = alloc_skb(size + ETH_HLEN, GFP_ATOMIC);
00130
00131 if (skb == NULL) {
00132 printk(KERN_ERR "Error: Unable to allocate skb (llsend) \n");
00133 return -ENOBUFS;
00134 }
00135
00136
00137 memcpy(ethernet_frame + ETH_HLEN, f, size);
00138
00139
00140 memcpy(skb_put(skb, size + ETH_HLEN), ethernet_frame, size + ETH_HLEN);
00141
00142 skb->protocol = WMP_PROTO;
00143 skb->dev = net_dev;
00144
00145 ret = net_dev->netdev_ops->ndo_start_xmit(skb, net_dev);
00146 if (ret != 0) {
00147 kfree_skb(skb);
00148 return 0;
00149 }
00150 return 1;
00151 }
00152
00153 #ifdef MAC80211
00154 static void parse_radiotap(struct ieee80211_radiotap_header * buf, int * rate, char * rssi, char * noise) {
00155 int pkt_rate_100kHz = 0, antenna = 0, pwr = 0;
00156 char rssi_dbm = 0, noise_dbm = 0;
00157 struct ieee80211_radiotap_iterator iterator;
00158 int ret = ieee80211_radiotap_iterator_init(&iterator, buf, buf->it_len,0);
00159
00160 while (!ret) {
00161
00162 ret = ieee80211_radiotap_iterator_next(&iterator);
00163
00164 if (ret)
00165 continue;
00166
00167
00168
00169 switch (iterator.this_arg_index) {
00170
00171
00172
00173
00174
00175
00176 case IEEE80211_RADIOTAP_RATE:
00177
00178
00179
00180 pkt_rate_100kHz = (*iterator.this_arg) * 5;
00181 break;
00182 case IEEE80211_RADIOTAP_DBM_ANTSIGNAL:
00183 rssi_dbm = (*iterator.this_arg);
00184 break;
00185 case IEEE80211_RADIOTAP_ANTENNA:
00186
00187 antenna = (*iterator.this_arg);
00188 break;
00189
00190 case IEEE80211_RADIOTAP_DBM_TX_POWER:
00191 pwr = *iterator.this_arg;
00192 break;
00193 case IEEE80211_RADIOTAP_DB_ANTNOISE:
00194 noise_dbm = *iterator.this_arg;
00195 break;
00196 default:
00197 break;
00198 }
00199 }
00200 *noise = noise_dbm;
00201 *rssi = rssi_dbm;
00202 *rate = pkt_rate_100kHz;
00203 }
00204 #endif
00205
00206 int ieee80211_rtwmp_timed_receive(char * buff, rxInfo * ret, int ms){
00207 unsigned int pad = 0;
00208 struct sk_buff * skb = dequeue_rx_queue(&rx_rtwmp_queue, ms);
00209 if (skb!=NULL){
00210
00211 #ifdef MAC80211
00212 int rate = 0, llc_size = 2;
00213 char rssi, noise;
00214 struct ieee80211_radiotap_header * rth = (struct ieee80211_radiotap_header *) skb->data;
00215 pad = rth->it_len + sizeof(struct ieee80211_hdr) + llc_size;
00216 parse_radiotap(rth, &rate, &rssi, &noise);
00217 ret->proto = (*((unsigned short *) (skb->data + rth->it_len + sizeof(struct ieee80211_hdr))));
00218 ret->rate = rate;
00219 ret->rssi = rssi;
00220 ret->noise = noise;
00221 ret->has_lq = 1;
00222 #else
00223 ret->has_lq = 0 ;
00224 ret->proto = skb->protocol;
00225 #endif
00226
00227
00228 ret->size = skb->len-pad;
00229 ret->error = 0;
00230 memcpy(buff, skb->data + pad, ret->size);
00231
00232
00233 return 1;
00234 }else{
00235 ret->error = 1;
00236 return 0;
00237 }
00238 }
00239
00240 int ieee80211_rtwmp_init(char * net_dev_name){
00241 int found = 0;
00242 net_dev = first_net_device(&init_net);
00243 while (net_dev) {
00244 net_dev = next_net_device(net_dev);
00245 if (strstr(net_dev->name, net_dev_name) != NULL) {
00246 found = 1;
00247 break;
00248 }
00249 }
00250 if (!found){
00251 printk(KERN_INFO "Error: unable to find net_device %s\n",net_dev_name);
00252 return 0;
00253 }
00254
00255
00256 ethernet_header = (struct ethhdr *) ethernet_frame;
00257 memcpy(ethernet_header->h_source, src_mac, ETH_ALEN);
00258 memcpy(ethernet_header->h_dest, dst_mac, ETH_ALEN);
00259 ethernet_header->h_proto = htons(WMP_PROTO);
00260
00261 init_rx_queue(&rx_rtwmp_queue);
00262
00263 printk(KERN_INFO "Found net_device %s",net_dev->name);
00264 printk(KERN_INFO "MAC80211-RT-WMP initialized successfully\n");
00265 initialized = 1;
00266 return 1;
00267
00268 }
00269
00270
00271
00272 void ieee80211_rtwmp_set_promisc(int _promisc){
00273 promisc = _promisc;
00274 }
00275
00276
00277 EXPORT_SYMBOL(ieee80211_rtwmp_init);
00278 EXPORT_SYMBOL(ieee80211_rtwmp_timed_receive);
00279 EXPORT_SYMBOL(ieee80211_rtwmp_timed_receive_raw);
00280 EXPORT_SYMBOL(ieee80211_rtwmp_finish);
00281 EXPORT_SYMBOL(ieee80211_rtwmp_send);
00282 EXPORT_SYMBOL(ieee80211_rtwmp_set_promisc);