1

/*

     2

 * Copyright (c) 2002,2016 Mario de Sousa (msousa@fe.up.pt)

     3

 *

     4

 * This file is part of the Modbus library for Beremiz and matiec.

     5

 *

     6

 * This Modbus library is free software: you can redistribute it and/or modify

     7

 * it under the terms of the GNU Lesser General Public License as published by

     8

 * the Free Software Foundation, either version 3 of the License, or

     9

 * (at your option) any later version.

    10

 *

    11

 * This program is distributed in the hope that it will be useful, but

    12

 * WITHOUT ANY WARRANTY; without even the implied warranty of

    13

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser 

    14

 * General Public License for more details.

    15

 *

    16

 * You should have received a copy of the GNU Lesser General Public License

    17

 * along with this Modbus library.  If not, see <http://www.gnu.org/licenses/>.

    18

 *

    19

 * This code is made available on the understanding that it will not be

    20

 * used in safety-critical situations without a full and competent review.

    21

 */

    22

    23

    24

    25

 /* Time handling functions used by the modbus protocols... */

    26

    27

    28

#ifndef __MODBUS_TIME_UTIL_H

    29

#define __MODBUS_TIME_UTIL_H

    30

    31

    32

/************************************/

    33

/**                                **/

    34

/**     Time format conversion     **/

    35

/**                                **/

    36

/************************************/

    37

    38

/* Function to load a struct timeval correctly from a double. */

    39

static inline struct timeval d_to_timeval(double time) {

    40

  struct timeval tmp;

    41

    42

  tmp.tv_sec  = time;

    43

  tmp.tv_usec = 1e6*(time - tmp.tv_sec);

    44

  return tmp;

    45

}

    46

    47

    48

/* Function to load a struct timespec correctly from a double. */

    49

static inline struct timespec d_to_timespec(double time) {

    50

  struct timespec tmp;

    51

    52

  tmp.tv_sec  = time;

    53

  tmp.tv_nsec = 1e9*(time - tmp.tv_sec);

    54

  return tmp;

    55

}

    56

    57

    58

    59

/* Function to ... */

    60

static inline struct timespec timespec_dif(struct timespec ts1, struct timespec ts2) {

    61

  struct timespec ts;

    62

    63

  ts.tv_sec  = ts1.tv_sec  - ts2.tv_sec;

    64

  if(ts1.tv_nsec > ts2.tv_nsec) {

    65

    ts.tv_nsec = ts1.tv_nsec - ts2.tv_nsec;

    66

  } else {

    67

    ts.tv_nsec = 1000000000 + ts1.tv_nsec - ts2.tv_nsec;

    68

    ts.tv_sec--;

    69

  }

    70

    71

  if (ts.tv_sec < 0)

    72

    ts.tv_sec = ts.tv_nsec = 0;

    73

    74

  return ts;

    75

}

    76

    77

/* Function to ... */

    78

static inline struct timespec timespec_add(struct timespec ts1, struct timespec ts2) {

    79

  struct timespec ts;

    80

    81

  ts.tv_sec  = ts1.tv_sec  + ts2.tv_sec;

    82

  ts.tv_nsec = ts1.tv_nsec + ts2.tv_nsec;

    83

  ts.tv_sec += ts.tv_nsec / 1000000000;

    84

  ts.tv_nsec = ts.tv_nsec % 1000000000;

    85

  return ts;

    86

}

    87

    88

/* Function to convert a struct timespec to a struct timeval. */

    89

static inline struct timeval timespec_to_timeval(struct timespec ts) {

    90

  struct timeval tv;

    91

    92

  tv.tv_sec  = ts.tv_sec;

    93

  tv.tv_usec = ts.tv_nsec/1000;

    94

  return tv;

    95

}

    96

    97

    98

/*

    99

 * NOTE: clock_gettime() is rather expensive, between 7000 and 7500 clock

   100

 *       cycles (measured with rdtsc on an Intel Pentium)

   101

 *       gettimeofday() is half as expensive (3000 to 3500 clock cycles),

   102

 *       but is not POSIX compliant... :-(

   103

 *       Nevertheless this is peanuts (20 us on a 350 MHz cpu) compared to

   104

 *       the timescales required to read a modbus frame over a serial bus

   105

 *       (aprox. 10 ms for a 10 byte frame on a 9600 baud bus!)

   106

 */

   107

static inline struct timespec timespec_add_curtime(struct timespec ts) {

   108

  struct timespec  res     = {.tv_sec = 0, .tv_nsec = 0};

   109

   110

  /* is ts = 0 also return 0 !! */

   111

  if ((ts.tv_sec != 0) || (ts.tv_nsec != 0))

   112

    if (clock_gettime(CLOCK_MONOTONIC, &res) >= 0)

   113

      res = timespec_add(res, ts);

   114

  return res;

   115

}

   116

   117

/************************************/

   118

/**                                **/

   119

/** select() with absolute timeout **/

   120

/**                                **/

   121

/************************************/

   122

   123

   124

   125

   126

/* My private version of select using an absolute timeout, instead of the

   127

 * usual relative timeout.

   128

 *

   129

 * NOTE: Ususal select semantics for (a: end_time == NULL) and

   130

 *       (b: *end_time == 0) also apply.

   131

 *

   132

 *       (a) Indefinite timeout

   133

 *       (b) Try once, and and quit if no data available.

   134

 */

   135

/* Returns: -1 on error

   136

 *           0 on timeout

   137

 *          >0 on success

   138

 */

   139

static int my_select(int fd, fd_set *rfds, fd_set *wfds, const struct timespec *end_time) {

   140

   141

  int res;

   142

  struct timespec cur_time;

   143

  struct timeval timeout, *tv_ptr;

   144

  fd_set tmp_rfds, *tmp_rfds_ptr, tmp_wfds, *tmp_wfds_ptr;

   145

   146

  tmp_rfds_ptr = NULL;

   147

  tmp_wfds_ptr = NULL;

   148

  if (rfds != NULL) tmp_rfds_ptr = &tmp_rfds;

   149

  if (wfds != NULL) tmp_wfds_ptr = &tmp_wfds;

   150

   151

  /*============================*

   152

   * wait for data availability *

   153

   *============================*/

   154

  do {

   155

    if (rfds != NULL) tmp_rfds = *rfds;

   156

    if (wfds != NULL) tmp_wfds = *wfds;

   157

      /* NOTE: To do the timeout correctly we would have to revert to timers

   158

       *       and asociated signals. That is not very thread friendly, and is

   159

       *       probably too much of a hassle trying to figure out which signal

   160

       *       to use. What if we don't have any free signals?

   161

       *

   162

       *       The following solution is not correct, as it includes a race

   163

       *       condition. The following five lines of code should really

   164

       *       be atomic!

   165

       *

   166

       * NOTE: see also the timeout related comment in the

   167

       *       modbus_tcp_read() function!

   168

       */

   169

    if (end_time == NULL) {

   170

      tv_ptr = NULL;

   171

    } else {

   172

      tv_ptr = &timeout;

   173

      if ((end_time->tv_sec == 0) && (end_time->tv_nsec == 0)) {

   174

        timeout.tv_sec = timeout.tv_usec = 0;

   175

      } else {

   176

        /* ATOMIC - start */

   177

        if (clock_gettime(CLOCK_MONOTONIC, &cur_time) < 0)

   178

          return -1;

   179

        timeout = timespec_to_timeval(timespec_dif(*end_time, cur_time));

   180

      }

   181

    }

   182

   183

    res = select(fd, tmp_rfds_ptr, tmp_wfds_ptr, NULL, tv_ptr);

   184

  /* ATOMIC - end */

   185

   186

#ifdef DEBUG

   187

  {int i;

   188

   if (tmp_rfds_ptr != NULL)

   189

     for (i = 0; i < fd; i++)

   190

       if (FD_ISSET(i, tmp_rfds_ptr))

   191

         fprintf(stderr,"fd=%d is ready for reading\n", i);

   192

   if (tmp_wfds_ptr != NULL)

   193

     for (i = 0; i < fd; i++)

   194

       if (FD_ISSET(i, tmp_wfds_ptr))

   195

         fprintf(stderr,"fd=%d is ready for writing\n", i);

   196

  }

   197

#endif

   198

    if (res == 0) {

   199

#ifdef DEBUG

   200

      printf("Comms time out\n");

   201

#endif

   202

      return 0;

   203

    }

   204

    if ((res < 0) && (errno != EINTR)) {

   205

      return -1;

   206

    }

   207

  } while (res <= 0);

   208

   209

  if (rfds != NULL) *rfds = tmp_rfds;

   210

  if (wfds != NULL) *wfds = tmp_wfds;

   211

  return res;

   212

}

   213

   214

   215

   216

   217

   218

   219

#endif  /* __MODBUS_TIME_UTIL_H */