rt_wmp.c
Go to the documentation of this file.
00001 #include "rt_wmp.h"
00002 
00003 #define WMP_PROTO 0x6969
00004 //#define MAC80211
00005 
00006 /* Queue for RT-WMP packet reception */
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 /* Initialization of the reception semaphore and queue for RT-WMP */
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 /* Free the remaining SKBs in the reception queue */
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){  // No hay elemento
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 
00081 //      printk(KERN_ERR "DEV_MINE:%p\n", net_dev);
00082 
00083 
00084         if (!initialized){
00085 #ifdef MAC80211
00086                 dev_kfree_skb(skb);
00087 #else
00088                 netif_rx(skb);
00089 #endif
00090         }else{
00091 
00092 #ifdef MAC80211
00093                 unsigned short * protocol;
00094                 struct ieee80211_radiotap_header * rth = (struct ieee80211_radiotap_header *) skb->data;
00095 //              struct ieee80211_hdr * hdr =  (struct ieee80211_hdr *) (skb->data + rth->it_len);
00096                 protocol = (unsigned short *) (skb->data + rth->it_len + sizeof(struct ieee80211_hdr));
00097                 proto = *protocol;
00098 #else
00099                 proto = skb->protocol;
00100                 skb->data[0] = 98 + (signed char) signal;
00101 #endif
00102 
00103                 if (promisc){
00104                         enqueue_rx_queue(&rx_rtwmp_queue, skb);
00105                 }else{
00106                         if (proto == 0x6969) {
00107                                 enqueue_rx_queue(&rx_rtwmp_queue, skb);
00108                         }
00109                 }
00110         }
00111 
00112         return 1;
00113 }
00114 
00115 int ieee80211_rtwmp_timed_receive_raw(struct sk_buff **skb, unsigned int ms){
00116         struct sk_buff * k = dequeue_rx_queue(&rx_rtwmp_queue, ms);
00117         *skb=k;
00118         return (k!=NULL);
00119 }
00120 
00121 void ieee80211_rtwmp_finish(void){
00122         purge_rx_queue(&rx_rtwmp_queue);
00123         initialized = 0;
00124         printk(KERN_INFO "MAC80211-RT-WMP closed\n");
00125 }
00126 
00127 
00128 int ieee80211_rtwmp_send(char * f, int size) {
00129         int ret;
00130         struct sk_buff *skb;
00131 
00132         skb = alloc_skb(size + ETH_HLEN, GFP_ATOMIC);
00133 
00134         if (skb == NULL) {
00135                 printk(KERN_ERR "Error: Unable to allocate skb (llsend) \n");
00136                 return -ENOBUFS;
00137         }
00138 
00139         /* Copy the message into the ethernet frame */
00140         memcpy(ethernet_frame + ETH_HLEN, f, size);
00141 
00142         /* Copy the ethernet frame into the skb */
00143         memcpy(skb_put(skb, size + ETH_HLEN), ethernet_frame, size + ETH_HLEN);
00144 
00145         skb->protocol = WMP_PROTO;
00146         skb->dev = net_dev;
00147 
00148         ret = net_dev->netdev_ops->ndo_start_xmit(skb, net_dev);
00149         if (ret != 0) {
00150                 kfree_skb(skb);
00151                 return 0;
00152         }
00153         return 1;
00154 }
00155 
00156 #ifdef MAC80211
00157 static void parse_radiotap(struct ieee80211_radiotap_header * buf, int * rate, char * rssi, char * noise) {
00158         int pkt_rate_100kHz = 0, antenna = 0, pwr = 0;
00159         char rssi_dbm = 0, noise_dbm = 0;
00160         struct ieee80211_radiotap_iterator iterator;
00161         int ret = ieee80211_radiotap_iterator_init(&iterator, buf, buf->it_len,0);
00162 
00163         while (!ret) {
00164 
00165                 ret = ieee80211_radiotap_iterator_next(&iterator);
00166 
00167                 if (ret)
00168                         continue;
00169 
00170                 /* see if this argument is something we can use */
00171 
00172                 switch (iterator.this_arg_index) {
00173                 /*
00174                  * You must take care when dereferencing iterator.this_arg
00175                  * for multibyte types... the pointer is not aligned.  Use
00176                  * get_unaligned((type *)iterator.this_arg) to dereference
00177                  * iterator.this_arg for type "type" safely on all arches.
00178                  */
00179                 case IEEE80211_RADIOTAP_RATE:
00180                         /* radiotap "rate" u8 is in
00181                          * 500kbps units, eg, 0x02=1Mbps
00182                          */
00183                         pkt_rate_100kHz = (*iterator.this_arg) * 5;
00184                         break;
00185                 case IEEE80211_RADIOTAP_DBM_ANTSIGNAL:
00186                         rssi_dbm = (*iterator.this_arg);
00187                         break;
00188                 case IEEE80211_RADIOTAP_ANTENNA:
00189                         /* radiotap uses 0 for 1st ant */
00190                         antenna = (*iterator.this_arg);
00191                         break;
00192 
00193                 case IEEE80211_RADIOTAP_DBM_TX_POWER:
00194                         pwr = *iterator.this_arg;
00195                         break;
00196                 case IEEE80211_RADIOTAP_DB_ANTNOISE:
00197                         noise_dbm = *iterator.this_arg;
00198                         break;
00199                 default:
00200                         break;
00201                 }
00202         } /* while more rt headers */
00203         *noise = noise_dbm;
00204         *rssi = rssi_dbm;
00205         *rate = pkt_rate_100kHz;
00206 }
00207 #endif
00208 
00209 int ieee80211_rtwmp_timed_receive(char * buff, rxInfo * ret, int ms){
00210         unsigned int pad = 0;
00211         struct sk_buff * skb = dequeue_rx_queue(&rx_rtwmp_queue, ms);
00212         if (skb!=NULL){
00213 
00214 #ifdef MAC80211
00215                 int rate = 0, llc_size = 2;
00216                 char rssi, noise;
00217                 struct ieee80211_radiotap_header * rth = (struct ieee80211_radiotap_header *) skb->data;
00218                 pad = rth->it_len + sizeof(struct ieee80211_hdr) + llc_size;
00219                 parse_radiotap(rth, &rate, &rssi, &noise);
00220                 ret->proto = (*((unsigned short *) (skb->data + rth->it_len + sizeof(struct ieee80211_hdr))));
00221                 ret->rate = rate;
00222                 ret->rssi = rssi;
00223                 ret->noise = noise;
00224                 ret->has_lq = 1;
00225 #else
00226                 ret->has_lq = 0 ;
00227                 ret->proto = skb->protocol;
00228 #endif
00229 
00230 
00231                 ret->size = skb->len-pad;
00232                 ret->error = 0;
00233                 memcpy(buff, skb->data + pad, ret->size);
00234 //              printk(KERN_INFO "Dequeuo %d size:%d \n",ret->proto, ret->size);
00235 
00236                 return 1;
00237         }else{
00238                 ret->error = 1;
00239                 return 0;
00240         }
00241 }
00242 
00243 int ieee80211_rtwmp_init(char * net_dev_name){
00244         int found = 0;
00245         net_dev = first_net_device(&init_net);
00246         while (net_dev) {
00247                 net_dev = next_net_device(net_dev);
00248                 if (strstr(net_dev->name, net_dev_name) != NULL) {
00249                         found = 1;
00250                         break;
00251                 }
00252         }
00253         if (!found){
00254                 printk(KERN_INFO "Error: unable to find net_device %s\n",net_dev_name);
00255                 return 0;
00256         }
00257 
00258         /* Build the ethernet header used for tx */
00259         ethernet_header = (struct ethhdr *) ethernet_frame;
00260         memcpy(ethernet_header->h_source, src_mac, ETH_ALEN);
00261         memcpy(ethernet_header->h_dest, dst_mac, ETH_ALEN);
00262         ethernet_header->h_proto = htons(WMP_PROTO);
00263 
00264         init_rx_queue(&rx_rtwmp_queue);
00265 
00266         printk(KERN_INFO "Found net_device %s",net_dev->name);
00267         printk(KERN_INFO "MAC80211-RT-WMP initialized successfully\n");
00268         initialized = 1;
00269         return 1;
00270 
00271 }
00272 
00273 
00274 
00275 void ieee80211_rtwmp_set_promisc(int _promisc){
00276         promisc = _promisc;
00277 }
00278 
00279 
00280 EXPORT_SYMBOL(ieee80211_rtwmp_init);
00281 EXPORT_SYMBOL(ieee80211_rtwmp_timed_receive);
00282 EXPORT_SYMBOL(ieee80211_rtwmp_timed_receive_raw);
00283 EXPORT_SYMBOL(ieee80211_rtwmp_finish);
00284 EXPORT_SYMBOL(ieee80211_rtwmp_send);
00285 EXPORT_SYMBOL(ieee80211_rtwmp_set_promisc);


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:11