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
00081
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
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
00140 memcpy(ethernet_frame + ETH_HLEN, f, size);
00141
00142
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
00171
00172 switch (iterator.this_arg_index) {
00173
00174
00175
00176
00177
00178
00179 case IEEE80211_RADIOTAP_RATE:
00180
00181
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
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 }
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
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
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);