wmp_interface.c
Go to the documentation of this file.
00001 /*-------------------------------------------------------------------------
00002  *--------------------------- RT-WMP IP INTERFACE -------------------------
00003  *-------------------------------------------------------------------------
00004  *
00005  * File: wmp_interface.c
00006  * Authors: Rubén Durán
00007  *          Danilo Tardioli
00008  *-------------------------------------------------------------------------
00009  *  Copyright (C) 2011, Universidad de Zaragoza, SPAIN
00010  *
00011  *  This program is free software: you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation, either version 3 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU General Public License for more details.
00020  *
00021  *  You should have received a copy of the GNU General Public License
00022  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00023  *
00024  *-----------------------------------------------------------------------*/
00025 
00026 #include "core/interface/wmp_interface.h"
00027 #include <stdio.h>
00028 #include <netinet/in.h>
00029 #include <sys/ioctl.h>
00030 #include <sys/socket.h>
00031 #include <netinet/in.h>
00032 #include <linux/if.h>
00033 #include <netpacket/packet.h>
00034 #include <net/ethernet.h>
00035 #include <linux/fs.h>
00036 #include "../include/cross_space.h"
00037 #include <stdio.h>
00038 #include <fcntl.h>
00039 #include <stdlib.h>
00040 #include <string.h>
00041 #include <unistd.h>
00042 #include "../include/ioctl_interface.h"
00043 
00044 
00045 static int ioctl_sock, fp;
00046 static struct ifreq ifr;
00047 
00048 int wmpInit(char *ifname) {
00049         strcpy(ifr.ifr_name, ifname);
00050 
00051         if ((ioctl_sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
00052                 perror("Error: Unable to create the ioctl socket");
00053                 return 0;
00054         }
00055 
00056         fp = open("/dev/rt-wmp",O_RDWR);
00057         if (fp < 0){
00058                 fprintf(stderr, "Error: Unable to open char device '/dev/rt-wmp'\nHave you insmod-ed the module?\n");
00059                 return 0;
00060         }
00061         return 1;
00062 }
00063 
00064 void wmpClose() {
00065         close(ioctl_sock);
00066 }
00067 
00068 int wmpPush(Msg *p) {
00069         return wmpPushData(0,p->data,p->len,p->dest,p->priority);
00070 }
00071 
00072 unsigned int wmpPop(Msg * p) {
00073         return wmpPopDataTimeoutCopy(0,p->data,&p->len, &p->src,&p->priority,0);
00074 }
00075 
00076 int wmpTimedPop(Msg * p, int timeout_ms) {
00077         return wmpPopDataTimeoutCopy(0,p->data,&p->len, &p->src,&p->priority,timeout_ms);
00078 }
00079 
00080 int wmpNonBlockingPop(Msg * p) {
00081         return wmpPopDataTimeoutCopy(0,p->data,&p->len, &p->src,&p->priority,-1);
00082 }
00083 
00084 int wmpPopDataCopy(unsigned int port, char  * p, unsigned int * size, unsigned char * src, signed char * priority){
00085         return wmpPopDataTimeoutCopy(port,p,size,src,priority,0);
00086 }
00087 
00088 //int wmpPushData(unsigned int port, char  * p, unsigned int   size, unsigned int   dest, char priority){
00089 //      static int allocated = 0;
00090 //      static char * buff = 0;
00091 //      int len = size + sizeof(cross_space_data_t);
00092 //      if (len > allocated) {
00093 //              if (buff != 0) {
00094 //                      free(buff);
00095 //              }
00096 //              buff = malloc(len);
00097 //              if (buff == 0) {
00098 //                      fprintf(stderr, "Unable to allocate memory (wmpPushData)");
00099 //                      return 1;
00100 //              }else{
00101 //                      allocated = len;
00102 //              }
00103 //      }
00104 //      cross_space_data_t * hdr = (cross_space_data_t *) buff;
00105 //      hdr->dest = dest;
00106 //      hdr->priority = priority;
00107 //      hdr->size = size;
00108 //      memcpy(buff + sizeof(cross_space_data_t), p, size);
00109 //      write(fp, buff, len);
00110 //      return 1;
00111 //}
00112 //
00113 //int wmpPopDataTimeoutCopy (unsigned int port, char  * p, unsigned int * size, unsigned char * src, char * priority, int to){
00114 //      int ret,ssize;
00115 //      cross_space_data_t  * m = (cross_space_data_t *) p;
00116 //      m->port = port;
00117 //      m->timeout = 1000;
00118 //      ret = read(fp, p, 1000);
00119 //      if (ret >= 0){
00120 //              *size = m->size;
00121 //              *priority = m->priority;
00122 //              *src = m->src;
00123 //              ssize = m->size;
00124 //              fprintf(stderr,"size: %d p:%d p+:%d %d\n",m->size,p,p+sizeof(cross_space_data_t),(size_t) ssize);
00125 //              memmove(p,p+sizeof(cross_space_data_t),(size_t) ssize);
00126 //              return 1;
00127 //      }
00128 //      return 0;
00129 //}
00130 
00131 
00132 
00133 int wmpPushData(unsigned int port, char  * p, unsigned int   size, unsigned int   dest, signed char priority){
00134         cross_space_data_t hdr ;
00135         hdr.dest = dest;
00136         hdr.priority = priority;
00137         hdr.size = size;
00138         hdr.port = port;
00139         write(fp, &hdr, 1);
00140         write(fp, p, 2);
00141         return 1;
00142 }
00143 
00144 int wmpPopDataTimeoutCopy (unsigned int port, char  * p, unsigned int * size, unsigned char * src, signed char * priority, int to){
00145         int ret;
00146         cross_space_data_t * hdr =(cross_space_data_t *) p;
00147         hdr->port = port;
00148         hdr->timeout = to;
00149         hdr->step = 0;
00150         fprintf(stderr, "first read port:%d\n",hdr->port);
00151 
00152         ret = read(fp, p, 1);
00153         fprintf(stderr, "first read done port:%d\n",hdr->port);
00154         if (ret>0){
00155                 *size = hdr->size;
00156                 *priority = hdr->priority;
00157                 *src = hdr->src;
00158                 ret = read(fp, p, 2);
00159                 fprintf(stderr, "second read done ret:%d \n", ret);
00160         }
00161         if (ret >= 0){
00162                 fprintf(stderr, "size: %d \n", hdr->size);
00163                 fprintf(stderr, "return 1\n");
00164                 return 1;
00165         }
00166         fprintf(stderr, "return 0\n");
00167         return 0;
00168 }
00169 
00170 int wmpPopData(unsigned int port, char  ** p, unsigned int * size, unsigned char * src, signed char * priority){
00171         return wmpPopDataTimeout(port,p,size,src,priority, 0);
00172 }
00173 
00174 int wmpPopDataTimeout(unsigned int port, char  ** p, unsigned int * size, unsigned char * src, signed char * priority, int to){
00175         int ret;
00176         char * q;
00177         cross_space_data_t hdr;
00178         hdr.port = port;
00179         hdr.timeout = to;
00180         hdr.step = 0;
00181 
00182         ret = read(fp, &hdr, 1);
00183         fprintf(stderr, "first read done port:%d\n",hdr.port);
00184         if (ret>0){
00185                 *size = hdr.size;
00186                 *priority = hdr.priority;
00187                 *src = hdr.src;
00188                 q = (char *) malloc(hdr.size);
00189                 if (q==0){
00190                         fprintf(stderr,"Error: Unable to allocate memory (wmpPopDataTimeout\n");
00191                         return 0;
00192                 }else{
00193                         fprintf(stderr,"Allocate memory (%d bytes)\n",hdr.size);
00194 
00195                 }
00196                 memcpy(q,&hdr,sizeof(cross_space_data_t));
00197                 ret = read(fp, q, 2);
00198                 *p = q;
00199                 return (int) q;
00200         }
00201         return 0;
00202 }
00203 
00204 void wmpPopDataDone(int id){
00205         char * p = (char*) id;
00206         if (p!=0){
00207                 free (p);
00208         }
00209 }
00210 
00211 char wmpGetNodeId(void) {
00212         tpNodeInfo info;
00213         info.type = NODEID;
00214 
00215         ifr.ifr_data = (void *) &info;
00216         if ((ioctl(ioctl_sock, SIO_NODEINFO, &ifr)) < 0) {
00217                 perror("Error in wmpGetNodeId IOCTL");
00218                 exit(1);
00219         }
00220         return info.ret;
00221 }
00222 
00223 char wmpGetNumOfNodes(void) {
00224         tpNodeInfo info;
00225         info.type = NUMOFNODES;
00226 
00227         ifr.ifr_data = (void *) &info;
00228         if ((ioctl(ioctl_sock, SIO_NODEINFO, &ifr)) < 0) {
00229                 perror("Error in wmpGetNumOfNodes IOCTL");
00230                 exit(1);
00231         }
00232         return info.ret;
00233 }
00234 
00235 int wmpGetLatestLQM(char * lqm) {
00236         int size = wmpGetNumOfNodes();
00237         size *= size;
00238 
00239         ifr.ifr_data = (void *) lqm;
00240         if ((ioctl(ioctl_sock, SIO_GETLATESTLQM, &ifr)) < 0) {
00241                 perror("Error in wmpGetLatestLQM IOCTL");
00242                 exit(1);
00243         }
00244         return size;
00245 }
00246 
00247 int wmpGetLatestDistances(char * lqm) {
00248         int size = wmpGetNumOfNodes();
00249         size *= size;
00250 
00251         ifr.ifr_data = (void *) lqm;
00252         if ((ioctl(ioctl_sock, SIO_GETLATESTDISTANCES, &ifr)) < 0) {
00253                 perror("Error in wmpGetLatestDistance IOCTL");
00254                 exit(1);
00255         }
00256         return size;
00257 }
00258 
00259 
00260 int wmpIsNetworkConnected(void) {
00261         tpNetworkConnectedInfo info;
00262         info.type = NONBLOCKING;
00263 
00264         ifr.ifr_data = (void *) &info;
00265         if ((ioctl(ioctl_sock, SIO_NETWORKCONNECTED, &ifr)) < 0) {
00266                 perror("Error in wmpIsNetworkConnected IOCTL");
00267                 exit(1);
00268         }
00269         return info.ret;
00270 }
00271 
00272 int wmpIsNetworkConnectedBlocking(int timeout_ms) {
00273         tpNetworkConnectedInfo info;
00274         info.type = BLOCKING;
00275         info.timeout = timeout_ms;
00276 
00277         ifr.ifr_data = (void *) &info;
00278         if ((ioctl(ioctl_sock, SIO_NETWORKCONNECTED, &ifr)) < 0) {
00279                 perror("Error in wmpIsNetworkConnectedBlocking IOCTL");
00280                 exit(1);
00281         }
00282         return info.ret;
00283 }
00284 
00285 int wmp_queue_tx_remove_head(void) {
00286         tpQueueActionInfo info;
00287         info.queueAction = REMOVETXMSG;
00288 
00289         ifr.ifr_data = (void *) &info;
00290         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00291                 perror("Error in wmp_queue_tx_remove_head IOCTL");
00292                 exit(1);
00293         }
00294         return info.ret;
00295 }
00296 
00297 void wmpSetCpuDelay(int val) {
00298         tpQueueActionInfo info;
00299         info.queueAction = SETCPUDELAY;
00300         info.ival = val;
00301 
00302         ifr.ifr_data = (void *) &info;
00303         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00304                 perror("Error in wmpSetCpuDelay IOCTL");
00305                 exit(1);
00306         }
00307 }
00308 
00309 int wmpGetCpuDelay() {
00310         tpQueueActionInfo info;
00311         info.queueAction = GETCPUDELAY;
00312 
00313         ifr.ifr_data = (void *) &info;
00314         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00315                 perror("Error in wmpGetCpuDelay IOCTL");
00316                 exit(1);
00317         }
00318         return info.ival;
00319 }
00320 
00321 void wmpSetTimeout(int val) {
00322         tpQueueActionInfo info;
00323         info.queueAction = SETTIMEOUT;
00324         info.ival = val;
00325 
00326         ifr.ifr_data = (void *) &info;
00327         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00328                 perror("Error in wmpSetTimeout IOCTL");
00329                 exit(1);
00330         }
00331 }
00332 
00333 int wmpGetTimeout() {
00334         tpQueueActionInfo info;
00335         info.queueAction = GETTIMEOUT;
00336 
00337         ifr.ifr_data = (void *) &info;
00338         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00339                 perror("Error in wmpGetTimeout IOCTL");
00340                 exit(1);
00341         }
00342         return info.ival;
00343 }
00344 
00345 void wmpSetWCMult(int val) {
00346         tpQueueActionInfo info;
00347         info.queueAction = SETWCMULT;
00348         info.fval = val;
00349 
00350         ifr.ifr_data = (void *) &info;
00351         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00352                 perror("Error in wmpSetWCMult IOCTL");
00353                 exit(1);
00354         }
00355 }
00356 
00357 int wmpGetWCMult() {
00358         tpQueueActionInfo info;
00359         info.queueAction = GETWCMULT;
00360 
00361         ifr.ifr_data = (void *) &info;
00362         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00363                 perror("Error in wmpGetWCMult IOCTL");
00364                 exit(1);
00365         }
00366         return info.fval;
00367 }
00368 
00369 void wmpSetRate(int val) {
00370         tpQueueActionInfo info;
00371         info.queueAction = SETRATE;
00372         info.fval = val;
00373 
00374         ifr.ifr_data = (void *) &info;
00375         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00376                 perror("Error in wmpSetRate IOCTL");
00377                 exit(1);
00378         }
00379 }
00380 
00381 int wmpGetRate() {
00382         tpQueueActionInfo info;
00383         info.queueAction = GETRATE;
00384 
00385         ifr.ifr_data = (void *) &info;
00386         if ((ioctl(ioctl_sock, SIO_QUEUEACTIONS, &ifr)) < 0) {
00387                 perror("Error in wmpGetRate IOCTL");
00388                 exit(1);
00389         }
00390         return info.fval;
00391 }
00392 
00393 int wmp_queue_tx_get_room(void) {
00394         tpGetQueueElemsInfo info;
00395         info.type = NUMOFFREEPOSITIONSTX;
00396 
00397         ifr.ifr_data = (void *) &info;
00398         if ((ioctl(ioctl_sock, SIO_QUEUEELEMSINFO, &ifr)) < 0) {
00399                 perror("Error in wmp_queue_tx_get_room IOCTL");
00400                 exit(1);
00401         }
00402         return info.ret;
00403 }
00404 
00405 int wmpGetNumOfElementsInTXQueue(void) {
00406         tpGetQueueElemsInfo info;
00407         info.type = NUMOFELEMSTX;
00408 
00409         ifr.ifr_data = (void *) &info;
00410         if ((ioctl(ioctl_sock, SIO_QUEUEELEMSINFO, &ifr)) < 0) {
00411                 perror("Error in wmpGetNumOfElementsInTXQueue IOCTL");
00412                 exit(1);
00413         }
00414         return info.ret;
00415 }
00416 
00417 int wmpGetNumOfElementsInRXQueue(int port) {
00418         tpGetQueueElemsInfo info;
00419         info.type = NUMOFELEMSRX;
00420 
00421         ifr.ifr_data = (void *) &info;
00422         if ((ioctl(ioctl_sock, SIO_QUEUEELEMSINFO, &ifr)) < 0) {
00423                 perror("Error in wmpGetNumOfElementsInRXQueue IOCTL");
00424                 exit(1);
00425         }
00426         return info.ret;
00427 }
00428 
00429 int wmpGetNetIT(void) {
00430         tpRTWMPSetGetInfo info;
00431         info.type = NETIT;
00432 
00433         ifr.ifr_data = (void *) &info;
00434         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00435                 perror("Error in wmpGetNetIT IOCTL");
00436                 exit(1);
00437         }
00438         return info.netIT;
00439 }
00440 
00441 unsigned int wmpGetMTU(void) {
00442         tpRTWMPSetGetInfo info;
00443         info.type = GETMTU;
00444 
00445         ifr.ifr_data = (void *) &info;
00446         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00447                 perror("Error in wmpGetMTU IOCTL");
00448                 exit(1);
00449         }
00450         return info.mtu;
00451 }
00452 
00453 void wmpSetActiveSearch(int val) {
00454         tpRTWMPSetGetInfo info;
00455         info.type = SETAS;
00456         info.activeSearch = val;
00457 
00458         ifr.ifr_data = (void *) &info;
00459         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00460                 perror("Error in wmpSetActiveSearch IOCTL");
00461                 exit(1);
00462         }
00463 }
00464 
00465 int wmpGetActiveSearch(void) {
00466         tpRTWMPSetGetInfo info;
00467         info.type = GETAS;
00468 
00469         ifr.ifr_data = (void *) &info;
00470         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00471                 perror("Error in wmpGetActiveSearch IOCTL");
00472                 exit(1);
00473         }
00474         return info.activeSearch;
00475 }
00476 
00477 void wmpSetInstanceId(short iid) {
00478         tpRTWMPSetGetInfo info;
00479         info.type = SINSTANCEID;
00480         info.instanceId = iid;
00481 
00482         ifr.ifr_data = (void *) &info;
00483         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00484                 perror("Error in wmpSetInstanceId IOCTL");
00485                 exit(1);
00486         }
00487 }
00488 
00489 short wmpGetInstanceId() {
00490         tpRTWMPSetGetInfo info;
00491         info.type = GINSTANCEID;
00492 
00493         ifr.ifr_data = (void *) &info;
00494         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00495                 perror("Error in wmpGetInstanceId IOCTL");
00496                 exit(1);
00497         }
00498         return info.instanceId;
00499 }
00500 
00501 void wmpSetPrimBasedRouting(int val) {
00502         tpRTWMPSetGetInfo info;
00503         info.type = SPRIMBASEDROUTING;
00504         info.primBasedRouting = val;
00505 
00506         ifr.ifr_data = (void *) &info;
00507         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00508                 perror("Error in wmpSetPrimBasedRouting IOCTL");
00509                 exit(1);
00510         }
00511 }
00512 
00513 int wmpGetPrimBasedRouting() {
00514         tpRTWMPSetGetInfo info;
00515         info.type = GPRIMBASEDROUTING;
00516 
00517         ifr.ifr_data = (void *) &info;
00518         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00519                 perror("Error in wmpGetPrimBasedRouting IOCTL");
00520                 exit(1);
00521         }
00522         return info.primBasedRouting;
00523 }
00524 
00525 void wmpSetMessageReschedule(int val) {
00526         tpRTWMPSetGetInfo info;
00527         info.type = SMESSAGERESCHEDULE;
00528         info.messageReschedule = val;
00529 
00530         ifr.ifr_data = (void *) &info;
00531         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00532                 perror("Error in wmpSetMessageReschedule IOCTL");
00533                 exit(1);
00534         }
00535 }
00536 
00537 int wmpGetMessageReschedule() {
00538         tpRTWMPSetGetInfo info;
00539         info.type = GMESSAGERESCHEDULE;
00540 
00541         ifr.ifr_data = (void *) &info;
00542         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00543                 perror("Error in wmpGetMessageReschedule IOCTL");
00544                 exit(1);
00545         }
00546         return info.messageReschedule;
00547 }
00548 
00549 void wmpSetFlowControl(int val) {
00550         tpRTWMPSetGetInfo info;
00551         info.type = SFLOWCONTROL;
00552         info.flowControl = val;
00553 
00554         ifr.ifr_data = (void *) &info;
00555         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00556                 perror("Error in wmpSetFlowControl IOCTL");
00557                 exit(1);
00558         }
00559 }
00560 
00561 int wmpGetFlowControl() {
00562         tpRTWMPSetGetInfo info;
00563         info.type = GFLOWCONTROL;
00564 
00565         ifr.ifr_data = (void *) &info;
00566         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00567                 perror("Error in wmpGetFlowControl IOCTL");
00568                 exit(1);
00569         }
00570         return info.flowControl;
00571 }
00572 
00573 unsigned int wmpGetSerial(void) {
00574         tpRTWMPSetGetInfo info;
00575         info.type = SERIAL;
00576 
00577         ifr.ifr_data = (void *) &info;
00578         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00579                 perror("Error in wmpGetSerial IOCTL");
00580                 exit(1);
00581         }
00582         return info.serial;
00583 }
00584 
00585 unsigned int wmpGetLoopId(void) {
00586         tpRTWMPSetGetInfo info;
00587         info.type = LOOPID;
00588 
00589         ifr.ifr_data = (void *) &info;
00590         if ((ioctl(ioctl_sock, SIO_RTWMPSETGET, &ifr)) < 0) {
00591                 perror("Error in wmpGetLoopId IOCTL");
00592                 exit(1);
00593         }
00594         return info.loopId;
00595 }
00596 
00597 int wmpSetup(char p1, char p2) {
00598         return wmpInit("wmp0");
00599 }
00600 
00601 void wmpRun() {
00602         while (1) {
00603                 sleep(1);
00604         }
00605 }
00606 
00607 void wmpRunBG() {
00608 
00609 }
00610 
00611 int  wmpIsKernelSpace(){
00612         return 1;
00613 }
00614 
00615 char lqm_get_val(int i, int j){
00616         return 0;
00617 }


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