src_frescan/frescan_queues.c

   1 /*!
   2  * @file frescan_queues.c
   3  *
   4  * @brief FRESCAN queues to manage the packets by prio and servers
   5  *
   6  * @version 0.01
   7  *
   8  * @date 27-Feb-2008
   9  *
  10  * @author
  11  *      Daniel Sangorrin
  12  *
  13  * @comments
  14  *
  15  * This file contains the FRESCAN queues where frescan packets are stored and
  16  * managed.
  17  *
  18  * TODO: disable interrupts for mutual exclusion!!!
  19  *
  20  * @license
  21  *
  22  * See MaRTE OS license
  23  *
  24  */
  25
  26 #include <stdlib.h>
  27 #include <time.h>
  28
  29 #include "frescan_queues.h"
  30 #include "frescan_packets.h"
  31 #include "frescan_debug.h"
  32 #include "frescan_id.h"
  33
  34 /**
  35  * frescan_pqueue_create() - creates a priority queue
  36  */
  37
  38 static inline frescan_prio_queue_t *frescan_pqueue_create(uint32_t max_prio,
  39                                                           frescan_network_t net)
  40 {
  41         int ret, prio;
  42         frescan_prio_queue_t *pq; // priority queue
  43
  44         pq = (frescan_prio_queue_t *)malloc(sizeof(frescan_prio_queue_t));
  45         if (pq == NULL) {
  46                 ERROR("could not allocate memory for prio queue\n");
  47                 return NULL;
  48         }
  49
  50         pq->max_prio = max_prio;
  51         pq->net      = net;
  52
  53         ret = sem_init (&pq->sem, 0, 0);
  54         if (ret != 0) {
  55                 ERROR("could not init the semaphore\n");
  56                 free(pq);
  57                 return NULL;
  58         }
  59
  60         pq->fifo_queues = (frescan_packet_t *)
  61                         malloc(max_prio * sizeof(frescan_packet_t));
  62
  63         for(prio=0; prio<max_prio; prio++) {
  64                 INIT_LIST_HEAD(&(pq->fifo_queues[prio].fifo_list));
  65         }
  66
  67         return pq;
  68 }
  69
  70 /**
  71  * frescan_queues_init() - initialize the queues
  72  *
  73  * 1.- create the transmission fixed priority queue
  74  * 2.- create the rx channels and its associated priority queues
  75  *
  76  * TODO: when error free memory
  77  */
  78
  79 int frescan_queues_init(frescan_queues_t *queues,
  80                         frescan_init_params_t *params)
  81 {
  82         int i;
  83         uint32_t max_prio;
  84
  85         // create transmission fixed priority queue
  86         queues->tx_fp_queue = frescan_pqueue_create(params->tx_fp_max_prio,
  87                                                     params->net);
  88
  89         if (queues->tx_fp_queue == NULL)  {
  90                 ERROR("could not allocate memory for tx fp queue\n");
  91                 return -1;
  92         }
  93
  94         // create receiving channels
  95         queues->rx_channel_queues = (frescan_prio_queue_t **)
  96                 malloc(params->rx_num_of_channels *
  97                        sizeof(frescan_prio_queue_t *));
  98
  99         if (queues->rx_channel_queues == NULL) {
 100                 ERROR("could not allocate memory for receiving channels\n");
 101                 return -1;
 102         }
 103
 104         queues->num_rx_channels = params->rx_num_of_channels;
 105
 106         // create a priority queue for each channel
 107         for(i=0; i<params->rx_num_of_channels; i++) {
 108
 109                 if (params->rx_channel_max_prio == NULL) {
 110                         max_prio = params->tx_fp_max_prio;
 111                 } else {
 112                         max_prio = params->rx_channel_max_prio[i];
 113                 }
 114
 115                 queues->rx_channel_queues[i] = frescan_pqueue_create
 116                                                        (max_prio, params->net);
 117
 118                 if (queues->rx_channel_queues[i] == NULL)  {
 119                         ERROR("could not allocate memory for rx pq %d\n", i);
 120                         return -1;
 121                 }
 122         }
 123
 124         return 0;
 125 }
 126
 127 /**
 128  * frescan_pqueue_enqueue() - enqueue a packet
 129  *
 130  * check the packet flags and enqueue the packet in the appropiate queue
 131  */
 132
 133 int frescan_pqueue_enqueue(frescan_prio_queue_t *pqueue,
 134                            frescan_packet_t *packet,
 135                            frescan_prio_t prio)
 136 {
 137         int ret;
 138
 139         if (prio >= pqueue->max_prio) {
 140                 ERROR("priority of the packet is too high\n");
 141                 return -1;
 142         }
 143
 144         DEBUG(FRESCAN_QUEUES_ENABLE_DEBUG,
 145               "enqueue packet with prio %u, pending %u\n",
 146               prio, packet->buffer_pending_bytes);
 147
 148         list_add_tail(&(packet->fifo_list),
 149                       &(pqueue->fifo_queues[prio].fifo_list));
 150
 151         ret = sem_post(&pqueue->sem);
 152         if (ret != 0) return ret;
 153
 154         return 0;
 155 }
 156
 157 /**
 158  * frescan_pqueue_requeue() - requeue a packet
 159  */
 160
 161 int frescan_pqueue_requeue(frescan_prio_queue_t *pqueue,
 162                            frescan_packet_t *packet,
 163                            frescan_prio_t prio)
 164 {
 165         int ret;
 166
 167         if (prio >= pqueue->max_prio) {
 168                 ERROR("priority of the packet is too high\n");
 169                 return -1;
 170         }
 171
 172         list_add(&packet->fifo_list, &(pqueue->fifo_queues[prio].fifo_list));
 173
 174         ret = sem_post(&pqueue->sem);
 175         if (ret != 0) return ret;
 176
 177         return 0;
 178 }
 179
 180 /**
 181  * frescan_pqueue_dequeue() - dequeue the packet with highest priority
 182  */
 183
 184 int frescan_pqueue_dequeue(frescan_prio_queue_t *pqueue,
 185                            frescan_packet_t **packet,
 186                            frescan_prio_t *packet_prio,
 187                            bool blocking)
 188 {
 189         int prio;
 190         int ret;
 191         frescan_packet_t *tmp = NULL;
 192         struct list_head *pos;
 193
 194         *packet = NULL;
 195
 196         if (blocking) {
 197                 DEBUG(FRESCAN_QUEUES_ENABLE_DEBUG, "calling sem_wait\n");
 198                 sem_wait(&pqueue->sem);
 199         } else {
 200                 ret = sem_trywait (&pqueue->sem);
 201                 if (ret != 0) {
 202                         DEBUG(FRESCAN_QUEUES_ENABLE_DEBUG,
 203                               "sem_trywait was locked (no packets)\n");
 204                         return 0;
 205                 }
 206         }
 207
 208         DEBUG(FRESCAN_QUEUES_ENABLE_DEBUG,
 209               "check priority fifo queues (max_prio=%u)\n", pqueue->max_prio);
 210
 211         // NOTE: we only acquire the lock if we block because non-blocking
 212         // calls are made from a context with no interrupts (when updating
 213         // the buffer at 'frescan_hw_buffer_update' which is always called
 214         // with interrupts disabled)
 215         if (blocking) FRESCAN_ACQUIRE_LOCK(&the_networks[pqueue->net].lock);
 216
 217         for(prio=pqueue->max_prio-1; prio >= 0; prio--) {
 218                 if (!list_empty(&pqueue->fifo_queues[prio].fifo_list)) {
 219                         list_for_each(pos,
 220                                       &pqueue->fifo_queues[prio].fifo_list) {
 221                                 tmp = list_entry(pos, frescan_packet_t,
 222                                                  fifo_list);
 223                                 break;
 224                         }
 225                         *packet = tmp;
 226                         list_del(&tmp->fifo_list);
 227                         break;
 228                 }
 229         }
 230
 231         if (blocking) FRESCAN_RELEASE_LOCK(&the_networks[pqueue->net].lock);
 232
 233         DEBUG(FRESCAN_QUEUES_ENABLE_DEBUG, "dequeued prio %u\n", prio);
 234         *packet_prio = prio;
 235
 236         return 0;
 237 }
 238
 239 /**
 240  * frescan_pqueue_get_highest_prio() - returns the packet with highest prio
 241  * but not extracting it from the queue.
 242  */
 243
 244 int frescan_pqueue_get_highest_prio(frescan_prio_queue_t *pqueue,
 245                                     frescan_packet_t **packet,
 246                                     frescan_prio_t *packet_prio)
 247 {
 248         int prio;
 249         frescan_packet_t *tmp = NULL;
 250         struct list_head *pos;
 251
 252         *packet = NULL;
 253
 254         for(prio=pqueue->max_prio-1; prio >= 0; prio--) {
 255                 if (!list_empty(&pqueue->fifo_queues[prio].fifo_list)) {
 256                         list_for_each(pos,
 257                                       &pqueue->fifo_queues[prio].fifo_list) {
 258                                 tmp = list_entry(pos, frescan_packet_t,
 259                                                  fifo_list);
 260                                 break;
 261                         }
 262                         *packet = tmp;
 263                         break;
 264                 }
 265         }
 266
 267         DEBUG(FRESCAN_QUEUES_ENABLE_DEBUG, "highest prio %u\n", prio);
 268         *packet_prio = prio;
 269
 270         return 0;
 271 }
 272
 273
 274 /**
 275  * frescan_servers_enqueue() - enqueue a packet through a server
 276  *
 277  * @net: the network instance
 278  * @id: the identificator for the server
 279  * @packet: the packet being enqueued
 280  *
 281  */
 282
 283 int frescan_servers_enqueue(frescan_network_t net,
 284                             frescan_ss_t id,
 285                             frescan_packet_t *packet)
 286 {
 287         frescan_server_data_t *server = &the_servers_pool[net][id];
 288
 289         clock_gettime (CLOCK_MONOTONIC, &packet->timestamp);
 290
 291         DEBUG(FRESCAN_SERVERS_ENABLE_DEBUG, "id:%u timestamp:(%d, %d)\n",
 292               id, packet->timestamp.tv_sec, packet->timestamp.tv_nsec);
 293
 294         // add the packet to the server fifo list
 295         list_add_tail(&packet->fifo_list, &server->packet_list.fifo_list);
 296
 297         // if the server was inactive (no packets) put it in the active list
 298         if (server->pending_packets == 0) {
 299                 DEBUG(FRESCAN_SERVERS_ENABLE_DEBUG,
 300                       "ss becomes active act_time=timestamp\n");
 301                 list_add_tail(&server->servers_list,
 302                               &the_active_servers[net].servers_list);
 303                 server->act_time = packet->timestamp;
 304         }
 305
 306         server->pending_packets++;
 307         return 0;
 308 }
 309
 310 /**
 311  * frescan_servers_requeue() - requeue a packet through a server
 312  *
 313  * @net: the network instance
 314  * @id: the identificator for the server
 315  * @packet: the packet being requeued
 316  *
 317  */
 318
 319 int frescan_servers_requeue(frescan_network_t net,
 320                             frescan_ss_t id,
 321                             frescan_packet_t *packet)
 322 {
 323         DEBUG(FRESCAN_SERVERS_ENABLE_DEBUG,
 324               "requeue packet for id:%u\n", id);
 325
 326         // add the packet to the server fifo list
 327         list_add(&packet->fifo_list,
 328                   &the_servers_pool[net][id].packet_list.fifo_list);
 329
 330         // if the server was inactive (no packets to send) put it active
 331         // (in the active list)
 332         if (the_servers_pool[net][id].pending_packets == 0) {
 333                 DEBUG(FRESCAN_SERVERS_ENABLE_DEBUG,
 334                       "server was inactive, put in the active list\n");
 335                 list_add(&the_servers_pool[net][id].servers_list,
 336                           &the_active_servers[net].servers_list);
 337         }
 338
 339         the_servers_pool[net][id].pending_packets++;
 340         return 0;
 341 }
 342
 343 /**
 344  * frescan_servers_dequeue() - dequeue a packet from a server
 345  *
 346  * @net: the network instance
 347  * @id: the identificator for the server
 348  * @packet: the packet dequeued
 349  * @packet_prio: the priority current of the server
 350  *
 351  */
 352
 353 int frescan_servers_dequeue(frescan_network_t net,
 354                             frescan_ss_t id,
 355                             frescan_packet_t **packet,
 356                             frescan_prio_t *packet_prio)
 357 {
 358         struct list_head *pos;
 359         frescan_server_data_t *server;
 360
 361         server = &the_servers_pool[net][id];
 362
 363         if (list_empty(&server->packet_list.fifo_list)) {
 364                 ERROR("no packet in server %d fifo list\n", id);
 365                 return -1;
 366         }
 367
 368         list_for_each(pos, &server->packet_list.fifo_list) {
 369                 *packet = list_entry(pos, frescan_packet_t, fifo_list);
 370                 break;
 371         }
 372
 373         list_del(&((*packet)->fifo_list));
 374         *packet_prio = server->current_priority;
 375         server->pending_packets--;
 376
 377         if (server->pending_packets == 0) {
 378                 DEBUG(FRESCAN_SERVERS_ENABLE_DEBUG,
 379                       "no more packets, delete from active list\n");
 380                 list_del(&server->servers_list);
 381         }
 382
 383         DEBUG(FRESCAN_SERVERS_ENABLE_DEBUG,
 384               "dequeued packet server:%u cur_prio:%u pending:%u\n",
 385               id, *packet_prio, server->pending_packets);
 386
 387         return 0;
 388 }