wmp_utils.c
Go to the documentation of this file.
00001 /*------------------------------------------------------------------------
00002  *---------------------           RT-WMP              --------------------
00003  *------------------------------------------------------------------------
00004  *                                                         V7.0B  11/05/10
00005  *
00006  *
00007  *  File: ./src/core/wmp_utils.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 
00037 #include "config/compiler.h"
00038 #include "include/wmp_utils.h"
00039 #include "include/wmp_misc.h"
00040 #include "include/lqm.h"
00041 #include "interface/wmp_interface.h"
00042 
00043 
00044 #define NET_DEBUG
00045 
00046 long long wmp_timespec_to_ns(struct timespec *ts){
00047     long long val = (long long) ts->tv_sec * 1000000000 + (long long) ts->tv_nsec;
00048     return val;
00049 }
00050 
00051 long long wmp_elapsed_ms (struct timespec *begin, struct timespec *end){
00052    long long diff = DO_DIV64(wmp_timespec_to_ns(end), 1000000LL) - DO_DIV64(wmp_timespec_to_ns(begin), 1000000LL);
00053         return diff;
00054 }
00055 
00056 void wmp_add_ms(struct timespec *ts, int ms){
00057         long long long_ms, sec, nsec;
00058         long_ms = ((long long)ms * 1000 * 1000) + ts->tv_nsec;
00059    sec = DO_DIV64(long_ms , (1000 * 1000 * 1000));
00060    nsec = long_ms - ((long long)sec*(1000 * 1000 * 1000));
00061         ts->tv_sec  += sec;
00062         ts->tv_nsec = nsec;
00063 }
00064 
00065 
00066 unsigned int wmp_calculate_frame_duration_ms(int rate, int size) {
00067         int res = (242 + (10 * (28 + size * 8) / rate));
00068         res=res/1000;
00069         if (res == 0){
00070                 res++;
00071         }
00072         return res;
00073 }
00074 
00075 unsigned int wmp_calculate_frame_duration_us(int rate, int size) {
00076         int res = (242 + (10 * (28 + size * 8) / rate));
00077         return res;
00078 }
00079 
00080 int wmpGetFrameHash(wmpFrame * p){
00081         return (p->hdr.serial * 10000 + p->hdr.from * 1000 + p->hdr.to
00082                         * 100 + p->hdr.retries * 10 + p->hdr.type);//+sdh.frame_type;
00083 }
00084 
00085 int wmpGetFrameHashNoRetry(wmpFrame * p){
00086         return (p->hdr.serial * 10000 + p->hdr.from * 1000 + p->hdr.to * 10 + p->hdr.type);
00087 }
00088 
00089 void lqmToString(char **lqm, char * s, int nnodes){
00090          int i,j;
00091          char txt[10], s1[10];
00092          int v=0;
00093 
00094          sprintf(s,"%s","    ");
00095          for (i=0;i<nnodes;i++){
00096                 sprintf(txt,"%d   ",i);
00097                 strcat(s,txt);
00098          }
00099 
00100          strcat(s,"\n");
00101      for (i=0;i<nnodes;i++){
00102                 sprintf(txt,"%d ",i);
00103         strcat(s,txt);
00104         for (j=0;j<nnodes;j++){
00105             v=lqm[i][j];
00106             if(i!=j) {
00107                 if (v<10) sprintf(s1,"  %d",v);
00108                 else if (v<100) sprintf(s1," %d",v);
00109                 else  sprintf(s1,"%d",v);
00110             } else{
00111                 sprintf(s1,"  -");
00112             }
00113             strcat(s,s1);
00114             strcat(s," ");
00115         }
00116         strcat(s,"\n");
00117      }
00118 }
00119 
00120 void wmp_print_lqm(char ** lqm, char * s, int nnodes){
00121         char txt[1000];
00122         lqmToString(lqm,txt, nnodes);
00123         WMP_MSG(stderr,"%s\n%s\n",s,txt);
00124 }
00125 
00126 void wmp_print_lqmn(char * s, int d){
00127         char txt[1000];
00128         lqmToString(lqm_get_ptr(),txt,wmpGetNumOfNodes());
00129         WMP_MSG(stderr,"I'm Node %d, Node: %d: %s\n%s\n",wmpGetNodeId(),d ,s,txt);
00130 }
00131 /* wmp_time */
00132 
00133 unsigned long long s_time[10];
00134 
00135 void wmp_time_store(int i){
00136         s_time[i] = getRawActualTimeus();
00137 }
00138 
00139 long long wmp_time_diff_us(int i, int j){
00140         long long diff = (s_time[i] - s_time[j]);
00141         return diff;
00142 }
00143 
00144 int wmp_time_diff_ms(int i, int j){
00145         long long diff = (s_time[i] - s_time[j]);
00146         return (int) diff;
00147 }
00148 
00149 void wmp_time_print_diff_ms(int i, int j, char * txt){
00150         WMP_MSG(stderr,"%s: %d ms\n",txt,wmp_time_diff_ms(i,j));
00151 }
00152 void wmp_time_print_diff_us(int i, int j, char * txt){
00153         WMP_MSG(stderr,"%s: %lld us\n",txt,wmp_time_diff_us(i,j));
00154 }
00155 
00156 void wmp_time_subtract(int i){
00157         long long ts = (getRawActualTimeus() - s_time[i]);
00158         s_time[i] = ts;
00159 }
00160 
00161 void wmp_time_print_us(int i, char * txt){
00162         WMP_MSG(stderr,"%s: %llu us\n",txt,s_time[i]);
00163 }
00164 
00165 void wmp_time_print_ms(int i, char * txt){
00166    unsigned long long ms = DO_DIV64(s_time[i] , 1000);
00167         WMP_MSG(stderr,"%s: %llu ms\n",txt,ms);
00168 }
00169 
00170 void wmp_time_subtract_and_print_us(int i, char * txt){
00171         wmp_time_subtract(i);
00172         wmp_time_print_us(i,txt);
00173 }
00174 
00175 void wmp_time_subtract_and_print_ms(int i, char * txt){
00176         wmp_time_subtract(i);
00177         wmp_time_print_ms(i,txt);
00178 }
00179 
00180 int inline ms_to_us(int n){
00181         return n*1000;
00182 }
00183 int inline us_to_ms(int n){
00184         return n/1000;
00185 }
00186 
00187 
00188 #ifdef NET_DEBUG
00189 
00190         static char buffer[1000];
00191         static int buffer_idx = 0;
00192         void wmp_print(const char *format, ...) {
00193                   va_list args;
00194                   va_start (args, format);
00195                   vsprintf (buffer+buffer_idx,format, args);
00196                   va_end (args);
00197                   buffer_idx=strlen(buffer);
00198         }
00199 
00200         void wmp_print_append(wmpFrame * p) {
00201                 int len;
00202                 char * net_data;
00203                 len =  wmp_get_frame_total_lenght(p);
00204                 net_data = ((char*) p) + len;
00205                 strcpy(buffer, net_data);
00206                 buffer_idx = strlen(buffer);
00207         }
00208 
00209         int wmp_print_get_size(void) {
00210                 return strlen(buffer);
00211         }
00212         void wmp_print_reset(void) {
00213                 buffer[0] = 0;
00214                 buffer_idx = 0;
00215         }
00216 
00217         void wmp_print_clean(wmpFrame * p) {
00218                 int i, len =  wmp_get_frame_total_lenght(p);
00219                 char * net_data = ((char*) p) + len;
00220                 for (i=0;i<50;i++){
00221                         net_data[i] = 0;
00222                 }
00223         }
00224 
00225         int wmp_print_put(wmpFrame * p) {
00226                 int len, tail;
00227                 len = tail = wmp_get_frame_total_lenght(p);
00228                 len = 1500 - len;
00229                 len  = strlen(buffer) > len ? len : strlen(buffer);
00230                 len +=2;
00231                 memcpy((((char *) p) + tail), buffer, len);
00232                 wmp_print_reset();
00233                 return len;
00234         }
00235 
00236 
00237 #else
00238         void wmp_print(const char *format, ...) {
00239         }
00240 
00241         int wmp_print_get_size(void) {
00242                 return 0;
00243         }
00244         void wmp_print_reset(void) {
00245         }
00246 
00247         int wmp_print_put(wmpFrame * p) {
00248                 return 0;
00249         }
00250         void wmp_print_clean(wmpFrame * p) {}
00251 
00252         void wmp_print_append(wmpFrame * p) {}
00253 
00254 #endif


ros_rt_wmp
Author(s): Danilo Tardioli, dantard@unizar.es
autogenerated on Fri Jan 3 2014 12:07:56