fwp/lib/fwp/fwp_vres.c

   1 /**************************************************************************/
   2 /* ---------------------------------------------------------------------- */
   3 /* Copyright (C) 2006 - 2009 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
  47 #include "fwp_utils.h"
  48 #include "fwp_vres.h"
  49
  50 #include "fwp_msgq.h"
  51 #include "fwp_endpoint.h"
  52 #include "fwp_debug.h"
  53
  54 #include <string.h>
  55 #include <errno.h>
  56 #include <stdlib.h>
  57
  58 static void* fwp_vres_tx_thread(void *_vres);
  59
  60 typedef enum {
  61         USED,
  62         UNTOUCHED,
  63         CHANGED,
  64         QUEUED,
  65 } fwp_vres_flag_t;
  66
  67 fwp_vres_params_t fwp_vres_params_default = {
  68         .ac_id = FWP_AC_VO,
  69         .budget = 100,
  70         .period = {.tv_sec = 2 , .tv_nsec = 111111},
  71         .src = { 0 },
  72 };
  73
  74 /**
  75  * Structure of FWP vres.
  76  * Internal representation of vres
  77  *
  78  */
  79 struct fwp_vres{
  80         struct fwp_vres_params          params;
  81         fwp_vres_flag_t                 flags;
  82         pthread_mutex_t                 mutex;
  83         pthread_cond_t                  cond; /**< Signalizes budget replenishment */
  84         fwp_budget_t                    budget; /**< Current remaining budget */
  85         fwp_period_t                    period; /**< Period for this "activation" */
  86         struct timespec                 replenish_at; /**< Time of next replenishment */
  87         sem_t                           consumed;
  88         /**< endpoint bounded to this vres */
  89         struct fwp_endpoint             *epoint;
  90         pthread_t                       tx_thread; /**< tx_thread id*/
  91         pthread_attr_t                  tx_thread_attr;
  92         /** Copy of bound enpoint's socket - used for future changes
  93          * of vres parameters. */
  94         int                             ac_sockd;
  95         /** Queue for messages to send */
  96         struct fwp_msgq                 msg_queue;
  97 };
  98
  99 typedef
 100 struct fwp_vres_table {
 101         unsigned int                    max_vres;
 102         fwp_vres_t                      *entry;
 103         pthread_mutex_t                 lock;
 104 } fwp_vres_table_t;
 105
 106 /* Global variable - vres table */
 107 static fwp_vres_table_t  fwp_vres_table = {
 108         .max_vres = 0,
 109         .entry = NULL,
 110         .lock = PTHREAD_MUTEX_INITIALIZER,
 111 };
 112
 113 /**< mapping priority to ac*/
 114 static const int prio_to_ac[8] = {2,3,3,2,1,1,0,0};
 115 /**< IP tos for AC_VI, AC_VO, AC_BE, AC_BK */
 116 static const unsigned int ac_to_tos[4] = {224,160,96,64};
 117
 118 /**
 119  * Set access category (AC) to socket
 120  *
 121  * \param[in] sockd Socket descriptor
 122  * \param[in] ac_id AC identifier
 123  *
 124  * \return On success returns zero.
 125  * On error, negative error code is returned.
 126  *
 127  */
 128 static inline int fwp_vres_set_ac(int sockd, fwp_ac_t ac_id)
 129 {
 130         unsigned int tos;
 131
 132         tos = ac_to_tos[ac_id];
 133         if (setsockopt(sockd, SOL_IP, IP_TOS, &tos, sizeof(tos)) == -1) {
 134                 FWP_ERROR("setsockopt: %s", strerror(errno));
 135                 return (-1);
 136         }
 137
 138         return 0;
 139 }
 140
 141 static inline void set_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
 142 {
 143         vres->flags |= (1 << flag);
 144 }
 145
 146 static inline void clear_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
 147 {
 148         vres->flags &= ~(1 << flag);
 149 }
 150
 151 static inline int get_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
 152 {
 153         return !!(vres->flags & (1 << flag));
 154 }
 155
 156 static inline void clear_all_flags(fwp_vres_t *vres)
 157 {
 158         vres->flags = 0;
 159 }
 160
 161 static inline void fwp_vres_free(fwp_vres_t *vres)
 162 {
 163         /* Clear USED flag */
 164         clear_all_flags(vres);
 165 }
 166
 167 static inline int fwp_vres_is_valid(fwp_vres_t *vres)
 168 {
 169         int id  = vres - fwp_vres_table.entry;
 170
 171         if ((id < 0) || (id > fwp_vres_table.max_vres - 1) ||
 172                 (!get_flag(vres, USED)))
 173                 return 0;
 174
 175         return 1;
 176 }
 177
 178 /*inline int fwp_vres_get(fwp_vres_id_t vres_id, fwp_vres_t **vres )
 179 {
 180         if ((vres_id < 0) || (vres_id > fwp_vres_table.nr_vres - 1))
 181                 return -EINVAL;
 182         *vres = &fwp_vres_table.entry[vres_id];
 183         return 0;
 184 }
 185 */
 186
 187 int fwp_vres_table_init(unsigned int max_vres)
 188 {
 189         unsigned int table_size = max_vres * sizeof(fwp_vres_t);
 190
 191         fwp_vres_table.entry = (fwp_vres_t*) malloc(table_size);
 192         if (!fwp_vres_table.entry)
 193                 return -1;      /* Errno is set by malloc */
 194
 195         memset((void*) fwp_vres_table.entry, 0, table_size);
 196         fwp_vres_table.max_vres = max_vres;
 197         return 0;
 198 }
 199
 200 /**
 201  * Allocate vres
 202  *
 203  * \return On success returns vres descriptor.
 204  */
 205 fwp_vres_t *fwp_vres_alloc()
 206 {
 207         int i;
 208         unsigned int max_vres;
 209
 210         /* find free vres id */
 211         pthread_mutex_lock(&fwp_vres_table.lock);
 212         i = 0;
 213         max_vres = fwp_vres_table.max_vres;
 214         while ((i < max_vres) &&
 215                 (get_flag(&fwp_vres_table.entry[i], USED))) {
 216                 i++;
 217         }
 218
 219         if (i == max_vres) {
 220                 pthread_mutex_unlock(&fwp_vres_table.lock);
 221                 errno = ENOBUFS;
 222                 return NULL;
 223         }
 224
 225         FWP_DEBUG("Allocated vres id = %d\n",i);
 226         set_flag(&fwp_vres_table.entry[i], USED);
 227         pthread_mutex_unlock(&fwp_vres_table.lock);
 228         return (&fwp_vres_table.entry[i]);
 229 }
 230
 231 static int apply_params(fwp_vres_t *vres)
 232 {
 233         int rv = 0;
 234         vres->period = vres->params.period;
 235         vres->budget = vres->params.budget;
 236         set_flag(vres, UNTOUCHED);
 237         if (get_flag(vres, CHANGED)) {
 238                 clear_flag(vres, CHANGED);
 239                 rv = fwp_vres_set_ac(vres->ac_sockd, vres->params.ac_id);
 240         }
 241         return rv;
 242 }
 243
 244 /**
 245  * Set vres params
 246  *
 247  * \param[in] vresp Vres descriptor
 248  * \param[in] params Vres parameters
 249  *
 250  * \return On success returns zero.
 251  * On error, negative error code is returned.
 252  *
 253  */
 254 int fwp_vres_set_params(fwp_vres_t *vres, fwp_vres_params_t *params)
 255 {
 256         int rv = 0;
 257
 258         if (!fwp_vres_is_valid(vres)) {
 259                 errno = EINVAL;
 260                 return -1;
 261         }
 262
 263         pthread_mutex_lock(&vres->mutex);
 264
 265         if (vres->epoint &&
 266             params->src.s_addr != vres->params.src.s_addr) {
 267                 errno = EREMCHG;
 268                 rv = -1;
 269                 goto out;
 270         }
 271         vres->params = *params;
 272         if (vres->epoint) {
 273                 set_flag(vres, CHANGED);
 274                 if (get_flag(vres, UNTOUCHED))
 275                         rv = apply_params(vres);
 276         }
 277 out:
 278         pthread_mutex_unlock(&vres->mutex);
 279         return rv;
 280 }
 281
 282 /**
 283  * Creates new vres
 284  *
 285  * \param[in] params Vres parameters
 286  * \param[out] vresp Pointer to the descriptor of newly created vres
 287  *
 288  * \return On success returns descriptor of vres.
 289  * On error, negative error code is returned.
 290  *
 291  */
 292 int fwp_vres_create(fwp_vres_params_t *params, fwp_vres_t **vresp)
 293 {
 294         int rv;
 295         fwp_vres_t *vres;
 296
 297         vres = fwp_vres_alloc();
 298         if (!vres) {
 299                 errno = ENOMEM;
 300                 return -1;
 301         }
 302
 303         pthread_mutexattr_t ma;
 304         rv = pthread_mutexattr_init(&ma);
 305         rv = pthread_mutexattr_setprotocol(&ma, PTHREAD_PRIO_INHERIT);
 306         if (rv) return rv;
 307         pthread_mutex_init(&vres->mutex, &ma);
 308         pthread_cond_init(&vres->cond, NULL);
 309
 310         vres->params = *params;
 311         apply_params(vres);
 312         fwp_msgq_init(&vres->msg_queue);
 313
 314         pthread_attr_init(&vres->tx_thread_attr);
 315         if ((rv = pthread_create(&vres->tx_thread, &vres->tx_thread_attr,
 316                             fwp_vres_tx_thread, (void*) vres)) != 0){
 317                 goto err;
 318         }
 319
 320         *vresp = vres;
 321
 322         return 0;
 323 err:
 324         fwp_msgq_dequeue_all(&vres->msg_queue);
 325         fwp_vres_free(vres);
 326         return -1;
 327 }
 328
 329 /**
 330  * Destroys vres
 331  *
 332  * \param[in] vres Vres descriptor
 333  *
 334  * \return On success returns 0.
 335  * On error, negative error code is returned.
 336  *
 337  */
 338 int fwp_vres_destroy(fwp_vres_t *vres)
 339 {
 340         if (!fwp_vres_is_valid(vres)) {
 341                 errno = EINVAL;
 342                 return -1;
 343         }
 344
 345         pthread_cancel(vres->tx_thread);
 346         pthread_cond_destroy(&vres->cond);
 347         pthread_mutex_destroy(&vres->mutex);
 348
 349         fwp_msgq_dequeue_all(&vres->msg_queue);
 350         fwp_vres_free(vres);
 351
 352         FWP_DEBUG("Vres destroyed.\n");
 353         return  0;
 354 }
 355
 356 static void do_consume_budget(struct fwp_vres *vres, size_t size)
 357 {
 358         if (get_flag(vres, UNTOUCHED)) {
 359                 /* Setup next replenish time */
 360                 struct timespec now;
 361                 clear_flag(vres, UNTOUCHED);
 362                 if (get_flag(vres, QUEUED))
 363                         now = vres->replenish_at;
 364                 else
 365                         clock_gettime(CLOCK_MONOTONIC, &now);
 366                 fwp_timespec_add(&vres->replenish_at, &now, &vres->period);
 367                 sem_post(&vres->consumed);
 368         }
 369         vres->budget -= size;
 370 }
 371
 372 int __consume_budget(struct fwp_vres *vres, size_t size, bool can_block)
 373 {
 374         int ret = 0;
 375         if (vres->params.budget < size) {
 376                 errno = ENOSR;
 377                 return -1;
 378         }
 379         while (can_block && vres->budget < size) {
 380                 ret = pthread_cond_wait(&vres->cond, &vres->mutex);
 381                 /* The budget might have been changed while we were
 382                  * waiting, so check it again. */
 383                 if (vres->params.budget < size) {
 384                         errno = ENOSR;
 385                         return -1;
 386                 }
 387         }
 388         if (ret == 0) {
 389                 if (vres->budget >= size) {
 390                         do_consume_budget(vres, size);
 391                         ret = 0;
 392                 } else {
 393                         set_flag(vres, QUEUED);
 394                         ret = 1;
 395                 }
 396         }
 397         return ret;
 398 }
 399
 400 /**
 401  * Tries to consume (a part of) budget
 402  *
 403  * @param vres VRES whose budget is conumed.
 404  * @param size How much to consume (in bytes).
 405  * @param can_block True, indicates that the function can block to
 406  * wait for budget replenishment. False causes no blocking and if 1 is
 407  * returned, the calles must call fwp_vres_enqueue() to enqueue the
 408  * packet to be sent later after replenishing.
 409  *
 410  * @return Zero if budget was consumed, 1 if there is not enough
 411  * budget available and blocking was not allowed, -1 in case of error.
 412  */
 413 int fwp_vres_consume_budget(struct fwp_vres *vres, size_t size, bool can_block)
 414 {
 415         int ret = 0;
 416         pthread_mutex_lock(&vres->mutex);
 417         ret = __consume_budget(vres, size, can_block);
 418         pthread_mutex_unlock(&vres->mutex);
 419         return ret;
 420 }
 421
 422 int fwp_vres_enqueue(struct fwp_vres *vres, struct fwp_endpoint *ep,
 423                      const void *msg, size_t size)
 424 {
 425         struct fwp_msgb *msgb;
 426         int ret;
 427
 428         if (!(msgb = fwp_msgb_alloc(size)))
 429                 return -1;
 430         memcpy(msgb->data, msg, size);
 431         fwp_msgb_put(msgb, size);
 432         ret = fwp_msgq_enqueue(&vres->msg_queue, msgb);
 433         if (ret) {
 434                 fwp_msgb_free(msgb);
 435                 return ret;
 436         }
 437         return ret;
 438 }
 439
 440 static void wait_for_replenish(struct fwp_vres *vres)
 441 {
 442         sem_wait(&vres->consumed);
 443         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
 444                         &vres->replenish_at, NULL);
 445 }
 446
 447 static void send_queue(struct fwp_vres *vres)
 448 {
 449         struct fwp_msgb *msgb;
 450         bool can_send;
 451         msgb = fwp_msgq_dequeue(&vres->msg_queue);
 452         can_send = (0 == __consume_budget(vres, msgb->len, false));
 453         if (!can_send) {
 454                 fwp_msgb_free(msgb);
 455                 FWP_ERROR("Cannot send queued packet (budget decreased?)\n");
 456                 return;
 457         }
 458         /* If we cannot send the whole queue, the flag will be set
 459          * later by __consume_budget(). */
 460         clear_flag(vres, QUEUED);
 461
 462         while (msgb) {
 463                 fwp_endpoint_do_send(vres->epoint, msgb->data, msgb->len);
 464                 fwp_msgb_free(msgb);
 465                 msgb = fwp_msgq_peek(&vres->msg_queue);
 466                 if (msgb) {
 467                         can_send = (0 == __consume_budget(vres, msgb->len, false));
 468                         if (can_send) {
 469                                 msgb = fwp_msgq_dequeue(&vres->msg_queue);
 470                         } else {
 471                                 msgb = NULL;
 472                                 return;
 473                         }
 474                 }
 475         }
 476 }
 477
 478 static void replenish(struct fwp_vres *vres)
 479 {
 480         pthread_mutex_lock(&vres->mutex);
 481         apply_params(vres);
 482         if (get_flag(vres, QUEUED))
 483                 send_queue(vres);
 484         pthread_cond_broadcast(&vres->cond);
 485         pthread_mutex_unlock(&vres->mutex);
 486 }
 487
 488 /**
 489  * Thread that does budgeting
 490  *
 491  */
 492 static void* fwp_vres_tx_thread(void *_vres)
 493 {
 494         struct fwp_vres *vres = (struct fwp_vres*)_vres;
 495         unsigned int    ac_id = vres->params.ac_id;
 496
 497         fwp_set_rt_prio(90 - ac_id);
 498         pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
 499         pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);
 500
 501         while (1) {
 502                 wait_for_replenish(vres);
 503                 pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL);
 504                 replenish(vres);
 505                 pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
 506         }
 507
 508         return NULL;
 509 }
 510
 511 /*int fwp_vres_bind(fwp_vres_t *vres, struct fwp_endpoint *epoint)*/
 512 int fwp_vres_bind(fwp_vres_t *vres, struct fwp_endpoint *ep, int sockd, struct in_addr  *src)
 513 {
 514         int rv = 0;
 515
 516         if (!fwp_vres_is_valid(vres)) {
 517                 errno = EINVAL;
 518                 rv = -1;
 519                 goto err;
 520         }
 521
 522         if (vres->epoint) { /*if already bounded */
 523                 errno = EBUSY;
 524                 rv = -1;
 525                 goto err;
 526         }
 527
 528         vres->ac_sockd = sockd;
 529         *src = vres->params.src;
 530         rv = fwp_vres_set_ac(vres->ac_sockd, vres->params.ac_id);
 531         if (rv)
 532                 goto err;
 533         vres->epoint = ep;
 534 err:
 535         return rv;
 536 }
 537
 538 int fwp_vres_unbind(fwp_vres_t *vres)
 539 {
 540         if (!fwp_vres_is_valid(vres)) {
 541                 errno = EINVAL;
 542                 return -1;
 543         }
 544         pthread_mutex_lock(&vres->mutex);
 545         vres->epoint = NULL;
 546         pthread_mutex_unlock(&vres->mutex);
 547         /* TODO: consider what to do with pending messages */
 548         fwp_msgq_dequeue_all(&vres->msg_queue);
 549         return 0;
 550 }