fwp/lib/fwp/fwp_vres.c

   1 /**************************************************************************/
   2 /* ---------------------------------------------------------------------- */
   3 /* Copyright (C) 2006 - 2008 FRESCOR consortium partners:                 */
   4 /*                                                                        */
   5 /*   Universidad de Cantabria,              SPAIN                         */
   6 /*   University of York,                    UK                            */
   7 /*   Scuola Superiore Sant'Anna,            ITALY                         */
   8 /*   Kaiserslautern University,             GERMANY                       */
   9 /*   Univ. Politécnica  Valencia,           SPAIN                        */
  10 /*   Czech Technical University in Prague,  CZECH REPUBLIC                */
  11 /*   ENEA                                   SWEDEN                        */
  12 /*   Thales Communication S.A.              FRANCE                        */
  13 /*   Visual Tools S.A.                      SPAIN                         */
  14 /*   Rapita Systems Ltd                     UK                            */
  15 /*   Evidence                               ITALY                         */
  16 /*                                                                        */
  17 /*   See http://www.frescor.org for a link to partners' websites          */
  18 /*                                                                        */
  19 /*          FRESCOR project (FP6/2005/IST/5-034026) is funded             */
  20 /*       in part by the European Union Sixth Framework Programme          */
  21 /*       The European Union is not liable of any use that may be          */
  22 /*       made of this code.                                               */
  23 /*                                                                        */
  24 /*                                                                        */
  25 /*  This file is part of FWP (Frescor WLAN Protocol)                      */
  26 /*                                                                        */
  27 /* FWP is free software; you can redistribute it and/or modify it         */
  28 /* under terms of the GNU General Public License as published by the      */
  29 /* Free Software Foundation; either version 2, or (at your option) any    */
  30 /* later version.  FWP is distributed in the hope that it will be         */
  31 /* useful, but WITHOUT ANY WARRANTY; without even the implied warranty    */
  32 /* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU    */
  33 /* General Public License for more details. You should have received a    */
  34 /* copy of the GNU General Public License along with FWP; see file        */
  35 /* COPYING. If not, write to the Free Software Foundation, 675 Mass Ave,  */
  36 /* Cambridge, MA 02139, USA.                                              */
  37 /*                                                                        */
  38 /* As a special exception, including FWP header files in a file,          */
  39 /* instantiating FWP generics or templates, or linking other files        */
  40 /* with FWP objects to produce an executable application, does not        */
  41 /* by itself cause the resulting executable application to be covered     */
  42 /* by the GNU General Public License. This exception does not             */
  43 /* however invalidate any other reasons why the executable file might be  */
  44 /* covered by the GNU Public License.                                     */
  45 /**************************************************************************/
  46 #include "fwp_utils.h"
  47 #include "fwp_vres.h"
  48
  49 #include "fwp_msgq.h"
  50 #include "fwp_endpoint.h"
  51
  52 #include <string.h>
  53
  54 static void* fwp_vres_tx_thread(void *_vres);
  55
  56 typedef enum {
  57         FWP_VF_USED             = 1 ,
  58         FWP_VF_BOUND            = 2 ,
  59         FWP_VF_RESCHED          = 4 ,
  60 } fwp_vres_flag_t;
  61
  62 fwp_vres_params_t fwp_vres_params_default = {
  63         .id = 0,
  64         .ac_id = FWP_AC_VO,
  65         .budget = 100,
  66         .period = {.tv_sec = 2 , .tv_nsec = 111111}
  67 };
  68
  69 /**
  70  * Structure of FWP vres.
  71  * Internal representation of vres
  72  *
  73  */
  74 struct fwp_vres{
  75         struct fwp_vres_params          params;
  76         /* consideration: move tx_queue to endpoint */
  77         /**< queue for messages to send */
  78         struct fwp_msgq                 tx_queue;
  79         fwp_vres_flag_t                 flags;
  80         /**< endpoint bounded to this vres */
  81         /*fwp_endpoint_t                *epoint; */
  82         pthread_t                       tx_thread; /**< tx_thread id*/
  83         pthread_attr_t                  tx_thread_attr;
  84         int                             ac_sockd;  /**< ac socket descriptor */
  85 };
  86
  87 typedef
  88 struct fwp_vres_table {
  89         unsigned int                    max_vres;
  90         fwp_vres_t                      *entry;
  91         pthread_mutex_t                 lock;
  92 } fwp_vres_table_t;
  93
  94 /* Global variable - vres table */
  95 static fwp_vres_table_t  fwp_vres_table = {
  96         .max_vres = 0,
  97         .entry = NULL,
  98         .lock = PTHREAD_MUTEX_INITIALIZER,
  99 };
 100
 101 /**< mapping priority to ac*/
 102 static const int prio_to_ac[8] = {2,3,3,2,1,1,0,0};
 103 /**< IP tos for AC_VI, AC_VO, AC_BE, AC_BK */
 104 static const unsigned int ac_to_tos[4] = {224,160,96,64};
 105
 106 /**
 107  * Set access category (AC) to socket
 108  *
 109  * \param[in] sockd Socket descriptor
 110  * \param[in] ac_id AC identifier
 111  *
 112  * \return On success returns zero.
 113  * On error, negative error code is returned.
 114  *
 115  */
 116 static inline int fwp_vres_set_ac(int sockd, fwp_ac_t ac_id)
 117 {
 118         unsigned int tos;
 119
 120         tos = ac_to_tos[ac_id];
 121         if (setsockopt(sockd, SOL_IP, IP_TOS, &tos, sizeof(tos)) == -1) {
 122                 FWP_ERROR("setsockopt: %s", strerror(errno));
 123                 return (-1);
 124         }
 125
 126         return 0;
 127 }
 128
 129 static inline void fwp_vres_set_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
 130 {
 131         vres->flags |= (1 << flag);
 132 }
 133
 134 static inline void fwp_vres_clear_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
 135 {
 136         vres->flags &= ~(1 << flag);
 137 }
 138
 139 static inline int fwp_vres_get_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
 140 {
 141         return !!(vres->flags & (1 << flag));
 142 }
 143
 144 static inline void fwp_vres_clearall_flag(fwp_vres_t *vres)
 145 {
 146         vres->flags = 0;
 147 }
 148
 149 #if 0
 150 /* Deprecated */
 151 static int fwp_vres_ac_open(fwp_ac_t ac_id)
 152 {
 153         int sockd;
 154         unsigned int tos;
 155
 156         if ((ac_id < 0)||(ac_id >= FWP_AC_NUM)) {
 157                 errno = EINVAL;
 158                 return -1;
 159         }
 160
 161         if ((sockd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
 162                 FWP_ERROR("Unable to open socket for AC: %s", strerror(errno));
 163                 return (-1);
 164         }
 165
 166         tos = ac_to_tos[ac_id];
 167
 168         if (setsockopt(sockd, SOL_IP, IP_TOS, &tos, sizeof(tos)) == -1) {
 169                 int e = errno;
 170                 FWP_ERROR("setsockopt(IP_TOS): %s", strerror(errno));
 171                 close(sockfd);
 172                 errno = e;
 173                 return -1;
 174         }
 175
 176         return sockd;
 177 }
 178 #endif
 179
 180 static inline int _fwp_vres_send(unsigned int ac_sockd, struct fwp_msgb* msgb)
 181 {
 182         /*_fwp_sendto(ac_sockd, msgb->data, msgb->len, 0,
 183                         msgb->peer->addr, msgb->peer->addrlen);*/
 184         return _fwp_send(ac_sockd, msgb->data, msgb->len, 0);
 185 }
 186
 187 static inline void fwp_vres_free(fwp_vres_t *vres)
 188 {
 189         fwp_vres_clearall_flag(vres);
 190 }
 191
 192 static inline int fwp_vres_is_valid(fwp_vres_t *vres)
 193 {
 194         int id  = vres - fwp_vres_table.entry;
 195
 196         if ((id < 0) || (id > fwp_vres_table.max_vres - 1) ||
 197                 (!fwp_vres_get_flag(vres, FWP_VF_USED)))
 198                 return 0;
 199
 200         return 1;
 201 }
 202
 203 /*inline int fwp_vres_get(fwp_vres_id_t vres_id, fwp_vres_t **vres )
 204 {
 205         if ((vres_id < 0) || (vres_id > fwp_vres_table.nr_vres - 1))
 206                 return -EINVAL;
 207         *vres = &fwp_vres_table.entry[vres_id];
 208         return 0;
 209 }
 210 */
 211
 212 int fwp_vres_table_init(unsigned int max_vres)
 213 {
 214         unsigned int table_size = max_vres * sizeof(fwp_vres_t);
 215
 216         fwp_vres_table.entry = (fwp_vres_t*) malloc(table_size);
 217         if (!fwp_vres_table.entry)
 218                 return -1;      /* Errno is set by malloc */
 219
 220         memset((void*) fwp_vres_table.entry, 0, table_size);
 221         fwp_vres_table.max_vres = max_vres;
 222         return 0;
 223 }
 224
 225 fwp_vres_id_t fwp_vres_get_id(fwp_vres_d_t vresd)
 226 {
 227         fwp_vres_t *vres = vresd;
 228
 229         return (vres - fwp_vres_table.entry);
 230 }
 231
 232 /**
 233  * Allocate vres
 234  *
 235  * \return On success returns vres descriptor.
 236  */
 237 fwp_vres_d_t fwp_vres_alloc()
 238 {
 239         int i;
 240         unsigned int max_vres;
 241
 242         /* find free vres id */
 243         pthread_mutex_lock(&fwp_vres_table.lock);
 244         i = 0;
 245         max_vres = fwp_vres_table.max_vres;
 246         while ((i < max_vres) &&
 247                 (fwp_vres_get_flag(&fwp_vres_table.entry[i], FWP_VF_USED))) {
 248                 i++;
 249         }
 250
 251         if (i == max_vres) {
 252                 pthread_mutex_unlock(&fwp_vres_table.lock);
 253                 errno = ENOBUFS;
 254                 return NULL;
 255         }
 256
 257         FWP_DEBUG("Allocated vres id = %d\n",i);
 258         fwp_vres_set_flag(&fwp_vres_table.entry[i], FWP_VF_USED);
 259         pthread_mutex_unlock(&fwp_vres_table.lock);
 260         return (&fwp_vres_table.entry[i]);
 261 }
 262
 263 inline int _fwp_vres_set_params(fwp_vres_t *vres, fwp_vres_params_t *params)
 264 {
 265         int rv;
 266
 267         /* copy vres paramters into vres structure */
 268         rv = fwp_vres_set_ac(vres->ac_sockd, params->ac_id);
 269         if (!rv)
 270                 return rv;
 271         memcpy(&vres->params, params, sizeof(struct fwp_vres_params));
 272         fwp_vres_set_flag(vres, FWP_VF_RESCHED);
 273
 274         return 0;
 275 }
 276
 277 /**
 278  * Set vres params
 279  *
 280  * \param[in] vresdp Vres descriptor
 281  * \param[in] params Vres parameters
 282  *
 283  * \return On success returns zero.
 284  * On error, negative error code is returned.
 285  *
 286  */
 287 int fwp_vres_set_params(fwp_vres_d_t vresd, fwp_vres_params_t *params)
 288 {
 289         fwp_vres_t *vres = vresd;
 290
 291         if (!fwp_vres_is_valid(vres)) {
 292                 errno = EINVAL;
 293                 return -1;
 294         }
 295
 296         return _fwp_vres_set_params(vres, params);
 297 }
 298
 299 /**
 300  * Creates new vres
 301  *
 302  * \param[in] params Vres parameters
 303  * \param[out] vresdp Pointer to the descriptor of newly created vres
 304  *
 305  * \return On success returns descriptor of vres.
 306  * On error, negative error code is returned.
 307  *
 308  */
 309 int fwp_vres_create(fwp_vres_params_t *params, fwp_vres_d_t *vresdp)
 310 {
 311         int rv;
 312         fwp_vres_t *vres;
 313
 314         vres = fwp_vres_alloc();
 315         if (!vres) {
 316                 errno = ENOMEM;
 317                 return -1;
 318         }
 319         /* initialize msg queue */
 320         fwp_msgq_init(&vres->tx_queue);
 321
 322         memcpy(&vres->params, params, sizeof(struct fwp_vres_params));
 323         fwp_vres_set_flag(vres, FWP_VF_RESCHED);
 324         pthread_attr_init(&vres->tx_thread_attr);
 325         if ((rv = pthread_create(&vres->tx_thread, &vres->tx_thread_attr,
 326                             fwp_vres_tx_thread, (void*) vres)) != 0){
 327                 goto err;
 328         }
 329
 330         *vresdp = vres;
 331         return 0;
 332 err:
 333         fwp_vres_free(vres);
 334         return -1;
 335 }
 336
 337 /**
 338  * Destroys vres
 339  *
 340  * \param[in] vresd Vres descriptor
 341  *
 342  * \return On success returns 0.
 343  * On error, negative error code is returned.
 344  *
 345  */
 346 int fwp_vres_destroy(fwp_vres_d_t vresd)
 347 {
 348         fwp_vres_t *vres = vresd;
 349
 350         if (!fwp_vres_is_valid(vres)) {
 351                 errno = EINVAL;
 352                 return -1;
 353         }
 354
 355         pthread_cancel(vres->tx_thread);
 356
 357         FWP_DEBUG("Vres vparam_id=%d destroyed.\n", vres->params.id);
 358         return  0;
 359 }
 360
 361 static void fwp_vres_cleanup(void *_vres)
 362 {
 363         fwp_vres_t *vres = (fwp_vres_t*)_vres;
 364
 365         fwp_msgq_dequeue_all(&vres->tx_queue);
 366         fwp_vres_free(vres);
 367 }
 368
 369 static inline void
 370 fwp_vres_sched_update(fwp_vres_t *vres, struct timespec *period,
 371                         fwp_budget_t  *budget)
 372 {
 373         if (fwp_vres_get_flag(vres, FWP_VF_RESCHED)) {
 374                 /*period->tv_nsec = vres->params.period % SEC_TO_USEC;
 375                 period->tv_sec = vres->params.period / SEC_TO_USEC;*/
 376                 *period = vres->params.period;
 377                 *budget = vres->params.budget;
 378                 FWP_DEBUG("Vres tx thread with budget=%ld period_sec=%ld "
 379                                 "period_nsec=%ld.\n",vres->params.budget,
 380                                 period->tv_sec, period->tv_nsec);
 381                 fwp_vres_clear_flag(vres, FWP_VF_RESCHED);
 382         }
 383 }
 384
 385 /**
 386  * Thread that does budgeting
 387  *
 388  */
 389 static void* fwp_vres_tx_thread(void *_vres)
 390 {
 391         struct fwp_vres *vres = (struct fwp_vres*)_vres;
 392         struct fwp_msgq *msgq = &vres->tx_queue;
 393         struct fwp_msgb *msgb = NULL;
 394         unsigned int    ac_id = vres->params.ac_id;
 395         fwp_budget_t    budget = vres->params.budget;
 396         fwp_budget_t    curr_budget;
 397         int             rc;
 398
 399         struct timespec  start_period, end_period, period;
 400         struct timespec  current_time, interval;
 401
 402         fwp_set_rt_prio(90 - ac_id);
 403
 404
 405         pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
 406         pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
 407         pthread_cleanup_push(fwp_vres_cleanup, (void*)vres);
 408
 409         clock_gettime(CLOCK_MONOTONIC, &start_period);
 410
 411         while (1) {
 412                 /* wait for next period and then send */
 413                 fwp_timespec_add(&end_period, &start_period, &period);
 414                 clock_gettime(CLOCK_MONOTONIC, &current_time);
 415                 fwp_timespec_sub(&interval, &end_period, &current_time);
 416                 nanosleep(&interval, NULL);
 417
 418                 sem_wait(&msgq->empty_lock);
 419                 fwp_vres_sched_update(vres, &period, &budget);
 420                 clock_gettime(CLOCK_MONOTONIC, &start_period);
 421
 422                 /*msgb = fwp_msgq_dequeue(msgq);
 423                 *if (msgb){*/
 424                 curr_budget = 0;
 425                 while ((curr_budget < budget)&&
 426                        (msgb = fwp_msgq_dequeue(msgq))) {
 427                         rc = _fwp_vres_send(vres->ac_sockd, msgb);
 428                         if (!(rc < 0)) {
 429                                 FWP_DEBUG("Message sent through AC%d\n",ac_id);
 430                                 /* Switch to this in the future */
 431                                  //curr_budget+= msgb->len;
 432                                  curr_budget++;
 433                         }
 434
 435                         fwp_msgb_free(msgb);
 436                 }
 437
 438                 /*pthread_testcancel(); */
 439                 /*pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
 440
 441                 fwp_timespec_add(&end_period, &start_period, &period);
 442                 clock_gettime(CLOCK_MONOTONIC, &current_time);
 443                 fwp_timespec_sub(&interval, &end_period, &current_time);
 444                 nanosleep(&interval, NULL);
 445                 */
 446         }
 447
 448         /* it should normaly never come here */
 449         pthread_cleanup_pop(0);
 450         fwp_vres_free(vres);
 451
 452         return NULL;
 453 }
 454
 455 int fwp_vres_send(fwp_vres_d_t vresd, struct fwp_msgb* msgb)
 456 {
 457         fwp_vres_t *vres = vresd;
 458
 459         if (fwp_vres_is_valid(vres)) {
 460                 return fwp_msgq_enqueue(&vres->tx_queue, msgb);
 461         } else {
 462                 errno = EPERM;  /* TODO: Use more appropriate error than EPERM. */
 463                 return -1;
 464         }
 465 }
 466
 467 /*int fwp_vres_bind(fwp_vres_d_t vresd, fwp_endpoint_t *epoint)*/
 468 int fwp_vres_bind(fwp_vres_d_t vresd, int sockd)
 469 {
 470         fwp_vres_t *vres = vresd;
 471         int rv = 0;
 472
 473         pthread_mutex_lock(&fwp_vres_table.lock);
 474         if (!fwp_vres_is_valid(vres)) {
 475                 errno = EINVAL;
 476                 rv = -1;
 477                 goto err;
 478         }
 479
 480         if (fwp_vres_get_flag(vres, FWP_VF_BOUND)) { /*if already bounded */
 481                 errno = EPERM;
 482                 rv = -1;
 483                 goto err;
 484         }
 485
 486         vres->ac_sockd = sockd;
 487         rv = fwp_vres_set_ac(vres->ac_sockd,vres->params.ac_id);
 488         /*if (rv)
 489                 goto err;*/
 490         fwp_vres_set_flag(vres, FWP_VF_BOUND);
 491 err:
 492         pthread_mutex_unlock(&fwp_vres_table.lock);
 493         return rv;
 494 }
 495
 496 int fwp_vres_unbind(fwp_vres_d_t vresd)
 497 {
 498         fwp_vres_t *vres = vresd;
 499
 500         if (!fwp_vres_is_valid(vresd)) {
 501                 errno = EINVAL;
 502                 return -1;
 503         }
 504         fwp_vres_clear_flag(vres, FWP_VF_BOUND);
 505         /* TODO: consider what to do with pending messages */
 506         /* fwp_vres_free_msgb(vres->tx_queue); */
 507         return 0;
 508 }