fwp/lib/core/fwp_vres.c

   1 #include "fwp_util.h"
   2 #include "fwp_vres.h"
   3
   4 static void* fwp_vres_tx_thread(void *_vres);
   5
   6
   7 typedef enum {
   8         FWP_VRES_FREE           = 0 ,
   9         FWP_VRES_INACTIVE       = 1 ,
  10         FWP_VRES_UNBOUND        = 2 ,
  11         FWP_VRES_BOUND          = 3 ,
  12 } fwp_vres_status_t;
  13
  14 /**
  15  * Structure of FWP vres.
  16  *
  17  *
  18  */
  19 struct fwp_vres{
  20         struct fwp_vres_params          params;
  21         /* consideration: move tx_queue to endpoint */
  22         /**< queue for messages to send */
  23         struct fwp_msgq                 tx_queue;
  24         int                             flags;
  25         /**< endpoint bounded to this vres */
  26         fwp_endpoint_d_t                epointd;
  27         pthread_t                       tx_thread; /**< tx_thread id*/
  28         pthread_attr_t                  tx_thread_attr;
  29         int                             ac_sockd;  /**< ac socket descriptor */
  30         fwp_vres_status_t               status;
  31 };
  32
  33 typedef
  34 struct fwp_vres_table {
  35         unsigned int                    nr_vres;
  36         fwp_vres_t                      *entry;
  37         pthread_mutex_t                 lock;
  38 } fwp_vres_table_t;
  39
  40 /* Global variable - vres table */
  41 static fwp_vres_table_t  fwp_vres_table = {
  42         .nr_vres = 0,
  43         .entry = NULL,
  44         .lock = PTHREAD_MUTEX_INITIALIZER,
  45 };
  46
  47 static const int prio_to_ac[8] = {2,3,3,2,1,1,0,0};
  48 static const unsigned int ac_to_tos[4] = {224,160,96,64};
  49
  50 static int fwp_vres_ac_open(fwp_ac_t ac_id)
  51 {
  52         int sockd;
  53         unsigned int tos;
  54
  55         if ((ac_id < 0)||(ac_id >= FWP_AC_NUM))
  56                 return -EINVAL;
  57
  58         if ((sockd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
  59                 perror("Unable to open socket for AC");
  60                 return (-errno);
  61         }
  62
  63         unsigned int yes = 1;
  64         if (setsockopt(sockd,SOL_SOCKET, SO_BROADCAST/*SO_REUSEADDR*/, &yes,
  65                          sizeof(yes)) == -1) {
  66                 perror("Unable to set options for socket");
  67                 close(sockd);
  68                 return (-errno);
  69         }
  70
  71         tos = ac_to_tos[ac_id];
  72
  73         if (setsockopt(sockd, SOL_IP, IP_TOS, &tos, sizeof(tos)) == -1) {
  74                 perror("Root permission needed to set AC");
  75                 /* let pass
  76                  * close(sockfd);
  77                  *return (-errno);*/
  78         }
  79
  80         return sockd;
  81 }
  82
  83 static inline int __fwp_vres_send(unsigned int ac_sockd, struct fwp_msgb* msgb)
  84 {
  85         /*_fwp_sendto(ac_sockd, msgb->data, msgb->len, 0,
  86                         msgb->peer->addr, msgb->peer->addrlen);*/
  87         _fwp_send(ac_sockd, msgb->data, msgb->len, 0);
  88         return 0;
  89 }
  90
  91 /*inline int fwp_vres_get(fwp_vres_id_t vres_id, fwp_vres_t **vres )
  92 {
  93         if ((vres_id < 0) || (vres_id > fwp_vres_table.nr_vres - 1))
  94                 return -EINVAL;
  95         *vres = &fwp_vres_table.entry[vres_id];
  96         return 0;
  97 }
  98
  99 inline int fwp_vres_getid(fwp_vres_t *vres, fwp_vres_id_t *vres_id)
 100 {
 101         fwp_vres_id_t id;
 102
 103         id = vres - fwp_vres_table.entry;
 104         if ((id < 0) || (id > fwp_vres_table.nr_vres - 1))
 105                 return -EINVAL;
 106         *vres_id = id;
 107         return 0;
 108 }*/
 109
 110 int fwp_vres_table_init(unsigned int nr_vres)
 111 {
 112         unsigned int table_size = nr_vres * sizeof(fwp_vres_t);
 113
 114         fwp_vres_table.entry = (fwp_vres_t*) malloc(table_size);
 115         if (!fwp_vres_table.entry)
 116                 return -ENOMEM;
 117
 118         memset((void*) fwp_vres_table.entry, 0, table_size);
 119         fwp_vres_table.nr_vres = nr_vres;
 120         return 0;
 121 }
 122
 123 fwp_vres_id_t fwp_vres_get_id(fwp_vres_d_t vresd)
 124 {
 125         fwp_vres_t *vres = vresd;
 126
 127         return (vres - fwp_vres_table.entry);
 128 }
 129
 130 fwp_vres_d_t fwp_vres_alloc()
 131 {
 132         int i;
 133         unsigned int nr_vres;
 134
 135         /* find free vres id */
 136         pthread_mutex_lock(&fwp_vres_table.lock);
 137         i = 0;
 138         nr_vres = fwp_vres_table.nr_vres;
 139         while ((i < nr_vres) &&
 140                 (fwp_vres_table.entry[i].status != FWP_VRES_FREE)) {
 141                 i++;
 142         }
 143
 144         if (i == nr_vres) {
 145                 pthread_mutex_unlock(&fwp_vres_table.lock);
 146                 return NULL;
 147         }
 148
 149         fwp_vres_table.entry[i].status = FWP_VRES_INACTIVE;
 150         pthread_mutex_unlock(&fwp_vres_table.lock);
 151         return (&fwp_vres_table.entry[i]);
 152 }
 153
 154 static inline void fwp_vres_free(fwp_vres_t *vres)
 155 {
 156         vres->status = FWP_VRES_FREE;
 157 }
 158
 159 int fwp_vres_set_params(fwp_vres_d_t vresd, fwp_vres_params_t *params)
 160 {
 161         fwp_vres_t *vres = vresd;
 162         int rv;
 163
 164         /* copy vres paramters into vres structure */
 165         memcpy(&vres->params, params, sizeof(struct fwp_vres_params));
 166
 167         if (vres->status != FWP_VRES_INACTIVE) {
 168                 /* Consider: how to change parameters when vres is in running
 169                  * state - restart thread, set vres_resched flag
 170                  */
 171                 pthread_cancel(vres->tx_thread);
 172                 close(vres->ac_sockd);
 173                 /* or set vres_resched flag and return */
 174         } else{
 175                 /* initialize msg queue */
 176                 fwp_msgq_init(&vres->tx_queue);
 177         }
 178
 179         /* open ac socket */
 180         if ((rv = fwp_vres_ac_open(vres->params.ac_id)) < 0) {
 181                 goto err;
 182         }
 183         vres->ac_sockd = rv;
 184
 185         pthread_attr_init(&vres->tx_thread_attr);
 186         if ((rv = pthread_create(&vres->tx_thread, &vres->tx_thread_attr,
 187                             fwp_vres_tx_thread, (void*) vres)) != 0){
 188                 goto err;
 189
 190         }
 191         vres->status = FWP_VRES_UNBOUND;
 192
 193         return 0;
 194 err:
 195         fwp_vres_free(vres);
 196         return rv;
 197 }
 198
 199 int fwp_vres_create(fwp_vres_params_t *params, fwp_vres_d_t *vresdp)
 200 {
 201         int rv;
 202         fwp_vres_t *vres;
 203
 204         /* Check for validity of the contract */
 205
 206         vres = fwp_vres_alloc();
 207         if (!vres) {
 208                 return -ENOMEM;
 209         }
 210         /* copy vres paramters into vres structure */
 211         memcpy(&vres->params, params, sizeof(struct fwp_vres_params));
 212         /* initialize msg queue */
 213         fwp_msgq_init(&vres->tx_queue);
 214         /* open ac socket */
 215         if ((rv = fwp_vres_ac_open(vres->params.ac_id)) < 0) {
 216                 goto err;
 217         }
 218         vres->ac_sockd = rv;
 219
 220         pthread_attr_init(&vres->tx_thread_attr);
 221         if ((rv = pthread_create(&vres->tx_thread, &vres->tx_thread_attr,
 222                             fwp_vres_tx_thread, (void*) vres)) != 0){
 223                 goto err;
 224
 225         }
 226         vres->status = FWP_VRES_UNBOUND;
 227
 228 /*      return vres->params.id; */
 229         *vresdp = vres;
 230         return 0;
 231 err:
 232         fwp_vres_free(vres);
 233         return rv;
 234 }
 235
 236 int fwp_vres_destroy(fwp_vres_d_t vresd)
 237 {
 238         fwp_vres_t *vres;
 239
 240         vres = vresd;
 241         if (vres->status == FWP_VRES_FREE)
 242                 return -EPERM;
 243
 244         vres->status = FWP_VRES_INACTIVE;
 245
 246         /* unbind endpoint */
 247         fwp_send_endpoint_unbind(vres->epointd);
 248
 249         pthread_cancel(vres->tx_thread);
 250         close(vres->ac_sockd);
 251
 252         FWP_DEBUG("Vres vparam_id=%d destroyed.\n", vres->params.id);
 253         return  0;
 254 }
 255
 256 static void fwp_vres_cleanup(void *_vres)
 257 {
 258         fwp_vres_t *vres = (fwp_vres_t*)_vres;
 259
 260         fwp_msgq_dequeue_all(&vres->tx_queue);
 261         fwp_vres_free(vres);
 262 }
 263
 264 static void* fwp_vres_tx_thread(void *_vres)
 265 {/* TODO: make changes that count with changing of params */
 266         struct fwp_vres *vres = (struct fwp_vres*)_vres;
 267         struct fwp_msgq *msgq = &vres->tx_queue;
 268         struct fwp_msgb *msgb = NULL;
 269         unsigned int    ac_id = vres->params.ac_id;
 270         unsigned int    ac_sockd = vres->ac_sockd;
 271         int             budget = vres->params.budget;
 272         int             curr_budget;
 273         int             rc;
 274
 275         struct timespec  start_period, end_period, period;
 276         struct timespec  current_time, interval;
 277
 278         period.tv_nsec = vres->params.period_usec % SEC_TO_USEC;
 279         period.tv_sec = vres->params.period_usec / SEC_TO_USEC;
 280
 281         fwp_set_rt_prio(90 - ac_id);
 282
 283         FWP_DEBUG("Started vres tx thread with budget:%d period_sec=%ld "
 284                         "period_nsec=%ld.\n",
 285                   vres->params.budget, period.tv_sec, period.tv_nsec);
 286
 287         pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
 288         pthread_cleanup_push(fwp_vres_cleanup, (void*)vres);
 289         clock_gettime(CLOCK_MONOTONIC, &start_period);
 290
 291         while (vres->status && (FWP_VRES_UNBOUND || FWP_VRES_BOUND)) {
 292                 /* wait for next period and then send */
 293                 pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
 294
 295                 fwp_timespec_add(&end_period, &start_period, &period);
 296                 clock_gettime(CLOCK_MONOTONIC, &current_time);
 297                 fwp_timespec_sub(&interval, &end_period, &current_time);
 298                 nanosleep(&interval, NULL);
 299
 300                 pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);
 301                 sem_wait(&msgq->empty_lock);
 302                 clock_gettime(CLOCK_MONOTONIC, &start_period);
 303
 304                 /*msgb = fwp_msgq_dequeue(msgq);
 305                 if (msgb){*/
 306                 curr_budget = 0;
 307                 while ((curr_budget < budget)&&
 308                        (msgb = fwp_msgq_dequeue(msgq))) {
 309                         rc = __fwp_vres_send(ac_sockd, msgb);
 310                         if (!(rc < 0)) {
 311                                 FWP_DEBUG("Message sent through AC%d\n",ac_id);
 312                                 /* Switch to this in the future
 313                                  * curr_budget+= msgb->len;
 314                                  */
 315                                 curr_budget++;
 316                         }
 317                         fwp_msgb_free(msgb);
 318                 }
 319
 320                 pthread_testcancel();
 321                 /*pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
 322
 323                 fwp_timespec_add(&end_period, &start_period, &period);
 324                 clock_gettime(CLOCK_MONOTONIC, &current_time);
 325                 fwp_timespec_sub(&interval, &end_period, &current_time);
 326                 nanosleep(&interval, NULL);
 327                 */
 328         }
 329
 330         /* it should normaly never come here */
 331         pthread_cleanup_pop(0);
 332         fwp_vres_free(vres);
 333
 334         return NULL;
 335 }
 336
 337 int _fwp_vres_send(fwp_vres_d_t vresd, struct fwp_msgb* msgb)
 338 {
 339         fwp_vres_t *vres = vresd;
 340
 341         /* test flags to check whether to send reliably*/
 342         if (vres->status != FWP_VRES_INACTIVE) {
 343                 return fwp_msgq_enqueue(&vres->tx_queue, msgb);
 344         } else
 345                 return -EPERM;
 346 }
 347
 348 int _fwp_vres_bind(fwp_vres_d_t vresd, fwp_endpoint_d_t epointd)
 349 {
 350         fwp_vres_t *vres = vresd;
 351         int rv = 0;
 352
 353         pthread_mutex_lock(&fwp_vres_table.lock);
 354         if (vres->status == FWP_VRES_BOUND) /*if other endpoint is assigned to vres*/
 355                 rv = -EPERM;
 356         else {
 357                 vres->epointd = epointd;
 358                 vres->status = FWP_VRES_BOUND;
 359         }
 360         pthread_mutex_unlock(&fwp_vres_table.lock);
 361         return rv;
 362 }
 363
 364 int _fwp_vres_unbind(fwp_vres_d_t vresd)
 365 {
 366         fwp_vres_t *vres = vresd;
 367
 368         vres->status = FWP_VRES_UNBOUND;
 369         /* TODO: consider what to do with pending messages */
 370         /* fwp_vres_free_msgb(vres->tx_queue); */
 371
 372         return 0;
 373 }
 374