fwp/lib/core/fwp_vres.c

   1 #include "fwp_util.h"
   2 #include "fwp_vres.h"
   3
   4 #include "fwp_msgq.h"
   5 #include "fwp_endpoint.h"
   6
   7 #include <string.h>
   8
   9 static void* fwp_vres_tx_thread(void *_vres);
  10
  11 typedef enum {
  12         FWP_VF_USED             = 1 ,
  13         FWP_VF_BOUND            = 2 ,
  14         FWP_VF_RESCHED          = 4 ,
  15 } fwp_vres_flag_t;
  16
  17 fwp_vres_params_t fwp_vres_params_default = {
  18         .id = 0,
  19         .ac_id = FWP_AC_VO,
  20         .budget = 100,
  21         .period_usec = 2111111,
  22 };
  23
  24 /**
  25  * Structure of FWP vres.
  26  * Internal representation of vres
  27  *
  28  */
  29 struct fwp_vres{
  30         struct fwp_vres_params          params;
  31         /* consideration: move tx_queue to endpoint */
  32         /**< queue for messages to send */
  33         struct fwp_msgq                 tx_queue;
  34         int                             flags;
  35         /**< endpoint bounded to this vres */
  36         /*fwp_endpoint_t                *epoint; */
  37         pthread_t                       tx_thread; /**< tx_thread id*/
  38         pthread_attr_t                  tx_thread_attr;
  39         int                             ac_sockd;  /**< ac socket descriptor */
  40         fwp_sockaddr_t                  addr;   /**< dest addr,for effectivness*/
  41 };
  42
  43 typedef
  44 struct fwp_vres_table {
  45         unsigned int                    max_vres;
  46         fwp_vres_t                      *entry;
  47         pthread_mutex_t                 lock;
  48 } fwp_vres_table_t;
  49
  50 /* Global variable - vres table */
  51 static fwp_vres_table_t  fwp_vres_table = {
  52         .max_vres = 0,
  53         .entry = NULL,
  54         .lock = PTHREAD_MUTEX_INITIALIZER,
  55 };
  56
  57 /**< mapping priority to ac*/
  58 static const int prio_to_ac[8] = {2,3,3,2,1,1,0,0};
  59 /**< IP tos for AC_VI, AC_VO, AC_BE, AC_BK */
  60 static const unsigned int ac_to_tos[4] = {224,160,96,64};
  61
  62 /**
  63  * Set access category (AC) to socket
  64  *
  65  * \param[in] sockd Socket descriptor
  66  * \param[in] ac_id AC identifier
  67  *
  68  * \return On success returns zero.
  69  * On error, negative error code is returned.
  70  *
  71  */
  72 static inline int fwp_vres_set_ac(int sockd, fwp_ac_t ac_id)
  73 {
  74         unsigned int tos;
  75
  76         tos = ac_to_tos[ac_id];
  77         if (setsockopt(sockd, SOL_IP, IP_TOS, &tos, sizeof(tos)) == -1) {
  78                 FWP_ERROR("setsockopt: %s", strerror(errno));
  79                 return (-1);
  80         }
  81
  82         return 0;
  83 }
  84
  85 static inline void fwp_vres_set_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
  86 {
  87         vres->flags |= (1 << flag);
  88 }
  89
  90 static inline void fwp_vres_clear_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
  91 {
  92         vres->flags &= ~(1 << flag);
  93 }
  94
  95 static inline int fwp_vres_get_flag(fwp_vres_t *vres, fwp_vres_flag_t flag)
  96 {
  97         return !!(vres->flags & (1 << flag));
  98 }
  99
 100 static inline void fwp_vres_clearall_flag(fwp_vres_t *vres)
 101 {
 102         vres->flags = 0;
 103 }
 104
 105 #if 0
 106 /* Deprecated */
 107 static int fwp_vres_ac_open(fwp_ac_t ac_id)
 108 {
 109         int sockd;
 110         unsigned int tos;
 111
 112         if ((ac_id < 0)||(ac_id >= FWP_AC_NUM)) {
 113                 errno = EINVAL;
 114                 return -1;
 115         }
 116
 117         if ((sockd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
 118                 FWP_ERROR("Unable to open socket for AC: %s", strerror(errno));
 119                 return (-1);
 120         }
 121
 122         tos = ac_to_tos[ac_id];
 123
 124         if (setsockopt(sockd, SOL_IP, IP_TOS, &tos, sizeof(tos)) == -1) {
 125                 int e = errno;
 126                 FWP_ERROR("setsockopt(IP_TOS): %s", strerror(errno));
 127                 close(sockfd);
 128                 errno = e;
 129                 return -1;
 130         }
 131
 132         return sockd;
 133 }
 134 #endif
 135
 136 static inline int _fwp_vres_send(unsigned int ac_sockd, struct fwp_msgb* msgb)
 137 {
 138         /*_fwp_sendto(ac_sockd, msgb->data, msgb->len, 0,
 139                         msgb->peer->addr, msgb->peer->addrlen);*/
 140         return _fwp_send(ac_sockd, msgb->data, msgb->len, 0);
 141 }
 142
 143 static inline void fwp_vres_free(fwp_vres_t *vres)
 144 {
 145         fwp_vres_clearall_flag(vres);
 146 }
 147
 148 static inline int fwp_vres_is_valid(fwp_vres_t *vres)
 149 {
 150         int id  = vres - fwp_vres_table.entry;
 151
 152         if ((id < 0) || (id > fwp_vres_table.max_vres - 1) ||
 153                 (!fwp_vres_get_flag(vres, FWP_VF_USED)))
 154                 return 0;
 155
 156         return 1;
 157 }
 158
 159 /*inline int fwp_vres_get(fwp_vres_id_t vres_id, fwp_vres_t **vres )
 160 {
 161         3if ((vres_id < 0) || (vres_id > fwp_vres_table.nr_vres - 1))
 162                 return -EINVAL;
 163         *vres = &fwp_vres_table.entry[vres_id];
 164         return 0;
 165 }
 166 */
 167
 168 int fwp_vres_table_init(unsigned int max_vres)
 169 {
 170         unsigned int table_size = max_vres * sizeof(fwp_vres_t);
 171
 172         fwp_vres_table.entry = (fwp_vres_t*) malloc(table_size);
 173         if (!fwp_vres_table.entry)
 174                 return -1;      /* Errno is set by malloc */
 175
 176         memset((void*) fwp_vres_table.entry, 0, table_size);
 177         fwp_vres_table.max_vres = max_vres;
 178         return 0;
 179 }
 180
 181 fwp_vres_id_t fwp_vres_get_id(fwp_vres_d_t vresd)
 182 {
 183         fwp_vres_t *vres = vresd;
 184
 185         return (vres - fwp_vres_table.entry);
 186 }
 187
 188 /**
 189  * Allocate vres
 190  *
 191  * \return On success returns vres descriptor.
 192  */
 193 fwp_vres_d_t fwp_vres_alloc()
 194 {
 195         int i;
 196         unsigned int max_vres;
 197
 198         /* find free vres id */
 199         pthread_mutex_lock(&fwp_vres_table.lock);
 200         i = 0;
 201         max_vres = fwp_vres_table.max_vres;
 202         while ((i < max_vres) &&
 203                 (fwp_vres_get_flag(&fwp_vres_table.entry[i], FWP_VF_USED))) {
 204                 i++;
 205         }
 206
 207         if (i == max_vres) {
 208                 pthread_mutex_unlock(&fwp_vres_table.lock);
 209                 errno = ENOBUFS;
 210                 return NULL;
 211         }
 212
 213         FWP_DEBUG("Allocated vres id = %d\n",i);
 214         fwp_vres_set_flag(&fwp_vres_table.entry[i], FWP_VF_USED);
 215         pthread_mutex_unlock(&fwp_vres_table.lock);
 216         return (&fwp_vres_table.entry[i]);
 217 }
 218
 219 inline int _fwp_vres_set_params(fwp_vres_t *vres, fwp_vres_params_t *params)
 220 {
 221         int rv;
 222
 223         /* copy vres paramters into vres structure */
 224         rv = fwp_vres_set_ac(vres->ac_sockd, params->ac_id);
 225         if (!rv)
 226                 return rv;
 227         memcpy(&vres->params, params, sizeof(struct fwp_vres_params));
 228         fwp_vres_set_flag(vres, FWP_VF_RESCHED);
 229
 230         return 0;
 231 }
 232
 233 /**
 234  * Set vres params
 235  *
 236  * \param[in] vresdp Vres descriptor
 237  * \param[in] params Vres parameters
 238  *
 239  * \return On success returns zero.
 240  * On error, negative error code is returned.
 241  *
 242  */
 243 int fwp_vres_set_params(fwp_vres_d_t vresd, fwp_vres_params_t *params)
 244 {
 245         fwp_vres_t *vres = vresd;
 246
 247         if (!fwp_vres_is_valid(vres)) {
 248                 errno = EINVAL;
 249                 return -1;
 250         }
 251
 252         return _fwp_vres_set_params(vres, params);
 253 }
 254
 255 /**
 256  * Creates new vres
 257  *
 258  * \param[in] params Vres parameters
 259  * \param[out] vresdp Pointer to the descriptor of newly created vres
 260  *
 261  * \return On success returns descriptor of vres.
 262  * On error, negative error code is returned.
 263  *
 264  */
 265 int fwp_vres_create(fwp_vres_params_t *params, fwp_vres_d_t *vresdp)
 266 {
 267         int rv;
 268         fwp_vres_t *vres;
 269
 270         vres = fwp_vres_alloc();
 271         if (!vres) {
 272                 return -1;
 273         }
 274         /* initialize msg queue */
 275         fwp_msgq_init(&vres->tx_queue);
 276
 277         memcpy(&vres->params, params, sizeof(struct fwp_vres_params));
 278         fwp_vres_set_flag(vres, FWP_VF_RESCHED);
 279         pthread_attr_init(&vres->tx_thread_attr);
 280         if ((rv = pthread_create(&vres->tx_thread, &vres->tx_thread_attr,
 281                             fwp_vres_tx_thread, (void*) vres)) != 0){
 282                 goto err;
 283         }
 284
 285         *vresdp = vres;
 286         return 0;
 287 err:
 288         fwp_vres_free(vres);
 289         return -1;
 290 }
 291
 292 /**
 293  * Destroys vres
 294  *
 295  * \param[in] vresd Vres descriptor
 296  *
 297  * \return On success returns 0.
 298  * On error, negative error code is returned.
 299  *
 300  */
 301 int fwp_vres_destroy(fwp_vres_d_t vresd)
 302 {
 303         fwp_vres_t *vres = vresd;
 304
 305         if (!fwp_vres_is_valid(vres)) {
 306                 errno = EINVAL;
 307                 return -1;
 308         }
 309
 310         pthread_cancel(vres->tx_thread);
 311
 312         FWP_DEBUG("Vres vparam_id=%d destroyed.\n", vres->params.id);
 313         return  0;
 314 }
 315
 316 static void fwp_vres_cleanup(void *_vres)
 317 {
 318         fwp_vres_t *vres = (fwp_vres_t*)_vres;
 319
 320         fwp_msgq_dequeue_all(&vres->tx_queue);
 321         fwp_vres_free(vres);
 322 }
 323
 324 static inline void
 325 fwp_vres_sched_update(fwp_vres_t *vres, struct timespec *period,
 326                         fwp_budget_t  *budget)
 327 {
 328         if (fwp_vres_get_flag(vres, FWP_VF_RESCHED)) {
 329                 period->tv_nsec = vres->params.period_usec % SEC_TO_USEC;
 330                 period->tv_sec = vres->params.period_usec / SEC_TO_USEC;
 331                 *budget = vres->params.budget;
 332                 FWP_DEBUG("Vres tx thread with budget:%d period_sec=%ld "
 333                                 "period_nsec=%ld.\n",vres->params.budget,
 334                                 period->tv_sec, period->tv_nsec);
 335                 fwp_vres_clear_flag(vres, FWP_VF_RESCHED);
 336         }
 337 }
 338
 339 /**
 340  * Thread that does budgeting
 341  *
 342  */
 343 static void* fwp_vres_tx_thread(void *_vres)
 344 {
 345         struct fwp_vres *vres = (struct fwp_vres*)_vres;
 346         struct fwp_msgq *msgq = &vres->tx_queue;
 347         struct fwp_msgb *msgb = NULL;
 348         unsigned int    ac_id = vres->params.ac_id;
 349         fwp_budget_t    budget = vres->params.budget;
 350         fwp_budget_t    curr_budget;
 351         int             rc;
 352
 353         struct timespec  start_period, end_period, period;
 354         struct timespec  current_time, interval;
 355
 356         fwp_set_rt_prio(90 - ac_id);
 357
 358
 359         pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
 360         pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
 361         pthread_cleanup_push(fwp_vres_cleanup, (void*)vres);
 362
 363         clock_gettime(CLOCK_MONOTONIC, &start_period);
 364
 365         while (1) {
 366                 /* wait for next period and then send */
 367                 fwp_timespec_add(&end_period, &start_period, &period);
 368                 clock_gettime(CLOCK_MONOTONIC, &current_time);
 369                 fwp_timespec_sub(&interval, &end_period, &current_time);
 370                 nanosleep(&interval, NULL);
 371
 372                 sem_wait(&msgq->empty_lock);
 373                 fwp_vres_sched_update(vres, &period, &budget);
 374                 clock_gettime(CLOCK_MONOTONIC, &start_period);
 375
 376                 /*msgb = fwp_msgq_dequeue(msgq);
 377                 *if (msgb){*/
 378                 curr_budget = 0;
 379                 while ((curr_budget < budget)&&
 380                        (msgb = fwp_msgq_dequeue(msgq))) {
 381                         rc = _fwp_vres_send(vres->ac_sockd, msgb);
 382                         if (!(rc < 0)) {
 383                                 FWP_DEBUG("Message sent through AC%d\n",ac_id);
 384                                 /* Switch to this in the future
 385                                  * curr_budget+= msgb->len;
 386                                  */
 387                                 curr_budget++;
 388                         }
 389                         fwp_msgb_free(msgb);
 390                 }
 391
 392                 /*pthread_testcancel(); */
 393                 /*pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
 394
 395                 fwp_timespec_add(&end_period, &start_period, &period);
 396                 clock_gettime(CLOCK_MONOTONIC, &current_time);
 397                 fwp_timespec_sub(&interval, &end_period, &current_time);
 398                 nanosleep(&interval, NULL);
 399                 */
 400         }
 401
 402         /* it should normaly never come here */
 403         pthread_cleanup_pop(0);
 404         fwp_vres_free(vres);
 405
 406         return NULL;
 407 }
 408
 409 int fwp_vres_send(fwp_vres_d_t vresd, struct fwp_msgb* msgb)
 410 {
 411         fwp_vres_t *vres = vresd;
 412
 413         if (fwp_vres_is_valid(vres)) {
 414                 return fwp_msgq_enqueue(&vres->tx_queue, msgb);
 415         } else {
 416                 errno = EPERM;  /* TODO: Use more appropriate error than EPERM. */
 417                 return -1;
 418         }
 419 }
 420
 421 /*int fwp_vres_bind(fwp_vres_d_t vresd, fwp_endpoint_t *epoint)*/
 422 int fwp_vres_bind(fwp_vres_d_t vresd, int sockd)
 423 {
 424         fwp_vres_t *vres = vresd;
 425         int rv = 0;
 426
 427         pthread_mutex_lock(&fwp_vres_table.lock);
 428         if (!fwp_vres_is_valid(vres)) {
 429                 errno = EINVAL;
 430                 rv = -1;
 431                 goto err;
 432         }
 433
 434         if (fwp_vres_get_flag(vres, FWP_VF_BOUND)) { /*if already bounded */
 435                 errno = EPERM;
 436                 rv = -1;
 437                 goto err;
 438         }
 439
 440         vres->ac_sockd = sockd;
 441         rv = fwp_vres_set_ac(vres->ac_sockd,vres->params.ac_id);
 442         /*if (rv)
 443                 goto err;*/
 444         fwp_vres_set_flag(vres, FWP_VF_BOUND);
 445         FWP_DEBUG("Bind is ok\n");
 446 err:
 447         pthread_mutex_unlock(&fwp_vres_table.lock);
 448         return rv;
 449 }
 450
 451 int fwp_vres_unbind(fwp_vres_d_t vresd)
 452 {
 453         fwp_vres_t *vres = vresd;
 454
 455         if (!fwp_vres_is_valid(vresd)) {
 456                 errno = EINVAL;
 457                 return -1;
 458         }
 459         fwp_vres_clear_flag(vres, FWP_VF_BOUND);
 460         /* TODO: consider what to do with pending messages */
 461         /* fwp_vres_free_msgb(vres->tx_queue); */
 462         return 0;
 463 }