queue_core.c
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           RT-WMP              --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V7.0B  11/05/10
00005  *
00006  *
00007  *  File: ./src/plugins/long_messages/long_messages.c
00008  *  Authors: Danilo Tardioli
00009  *  ----------------------------------------------------------------------
00010  *  Copyright (C) 2000-2010, Universidad de Zaragoza, SPAIN
00011  *
00012  *  Contact Addresses: Danilo Tardioli                   dantard@unizar.es
00013  *
00014  *  RT-WMP is free software; you can  redistribute it and/or  modify it
00015  *  under the terms of the GNU General Public License  as published by the
00016  *  Free Software Foundation;  either  version 2, or (at  your option) any
00017  *  later version.
00018  *
00019  *  RT-WMP  is distributed  in the  hope  that  it will be   useful, but
00020  *  WITHOUT  ANY  WARRANTY;     without  even the   implied   warranty  of
00021  *  MERCHANTABILITY  or  FITNESS FOR A  PARTICULAR PURPOSE.    See the GNU
00022  *  General Public License for more details.
00023  *
00024  *  You should have received  a  copy of  the  GNU General Public  License
00025  *  distributed with RT-WMP;  see file COPYING.   If not,  write to the
00026  *  Free Software  Foundation,  59 Temple Place  -  Suite 330,  Boston, MA
00027  *  02111-1307, USA.
00028  *
00029  *  As a  special exception, if you  link this  unit  with other  files to
00030  *  produce an   executable,   this unit  does  not  by  itself cause  the
00031  *  resulting executable to be covered by the  GNU General Public License.
00032  *  This exception does  not however invalidate  any other reasons why the
00033  *  executable file might be covered by the GNU Public License.
00034  *
00035  *----------------------------------------------------------------------*/
00036 #include <stdarg.h>
00037 #include "config/compiler.h"
00038 #include "core/interface/wmp_interface.h"
00039 #include "core/include/global.h"
00040 #include "core/include/frames.h"
00041 #include "include/queue_core.h"
00042 #include "core/include/queues.h"
00043 #include "core/include/wmp_misc.h"
00044 
00045 #ifdef USE_LONG_MESSAGE_COMPRESS
00046 #include <zlib.h>
00047 static Bytef * zd;
00048 static uLongf zlen;
00049 #endif
00050 
00051 /*  ****  LONG MESSAGES */
00052 
00053 void clear(queue_t * q, int idx){
00054         if (idx >= 0 && idx < q->max_msg_num) {
00055                 q->longMsg[idx]->done = 0;
00056                 q->longMsg[idx]->received_parts = 0;
00057                 q->longMsg[idx]->num_parts = 0;
00058                 q->longMsg[idx]->done = 0;
00059                 q->longMsg[idx]->size = 0;
00060                 q->longMsg[idx]->burst_hash = 0;
00061                 q->longMsg[idx]->rescheduled = 0;
00062                 memset(q->longMsg[idx]->received_part,0,q->longMsg[idx]->max_message_parts);
00063                 q->longMsg[idx]->hash = 0;
00064         }
00065 }
00066 
00067 /* Interface */
00068 void inline exclusive_on(queue_t * q){
00069         MUTEX_WAIT(q->uniq_mtx);
00070 }
00071 
00072 void inline exclusive_off(queue_t * q){
00073         MUTEX_SIGNAL(q->uniq_mtx);
00074 }
00075 
00076 int queue_get_size(queue_t * q){
00077         return q->max_msg_num;
00078 }
00079 unsigned int queue_get_elem_port(queue_t * q, int id){
00080         if (id >=0 && id < q->max_msg_size){
00081                 return q->longMsg[id]->port;
00082         }else{
00083                 return -1;
00084         }
00085 }
00086 int queue_get_elem_age(queue_t * q, int id){
00087         if (id >=0 && id < q->max_msg_size){
00088                 return ((int) (getRawActualTimeus() - q->longMsg[id]->ts))/1000;
00089         }else{
00090                 return -1;
00091         }
00092 }
00093 int queue_get_elem_dest(queue_t * q, int id){
00094         if (id >=0 && id < q->max_msg_size){
00095                 return (int) q->longMsg[id]->dest;
00096         }else{
00097                 return -1;
00098         }
00099 }
00100 
00101 int queue_get_elem_burst(queue_t * q, int id) {
00102         int i, count = 0;
00103         unsigned short hash = q->longMsg[id]->burst_hash;
00104         for (i = 0; i < q->max_msg_num; i++) {
00105                 if (hash == q->longMsg[i]->burst_hash) {
00106                         count+=q->longMsg[i]->num_parts - q->longMsg[i]->parts_pointer;
00107                         //printk(KERN_ERR "id:%d hash:%d num_parts%d parts_point:%d", id, hash,q->longMsg[i]->num_parts, q->longMsg[i]->parts_pointer);
00108                 }
00109         }
00110         return count;
00111 }
00112 
00113 int queue_get_elem_source(queue_t * q, int id){
00114         if (id >=0 && id < q->max_msg_size){
00115                 return (int) q->longMsg[id]->src;
00116         }else{
00117                 return -1;
00118         }
00119 }
00120 char *  queue_get_elem_data(queue_t * q, int id){
00121         if (id >=0 && id < q->max_msg_size){
00122                 return q->longMsg[id]->data;
00123         }else{
00124                 return 0;
00125         }
00126 }
00127 
00128 int queue_get_elem_priority(queue_t * q, int id){
00129         if (id >=0 && id < q->max_msg_size){
00130                 return (int) q->longMsg[id]->priority;
00131         }else{
00132                 return -1;
00133         }
00134 }
00135 int queue_get_elem_size(queue_t * q, int id){
00136         if (id >=0 && id < q->max_msg_size){
00137                 return (int) q->longMsg[id]->size;
00138         }else{
00139                 return -1;
00140         }
00141 }
00142 
00143 int queue_get_elem_rescheduled(queue_t * q, int id){
00144         if (id >=0 && id < q->max_msg_size){
00145                 return q->longMsg[id]->rescheduled;
00146         }else{
00147                 return -1;
00148         }
00149 }


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Mon Oct 6 2014 08:27:10