mb_master.c
author bmakuc <blaz.makuc@smarteh.si>
Tue, 10 Dec 2019 09:30:59 +0100
changeset 5 6e94a1dddc5f
parent 1 59783e8ee3d2
permissions -rw-r--r--
Mario's fix for the pointer alignment problem. See changeset 4: 99009b24d401.
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     1
/*
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     2
 * Copyright (c) 2001-2003,2016 Mario de Sousa (msousa@fe.up.pt)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     3
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     4
 * This file is part of the Modbus library for Beremiz and matiec.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     5
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     6
 * This Modbus library is free software: you can redistribute it and/or modify
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     7
 * it under the terms of the GNU Lesser General Public License as published by
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     8
 * the Free Software Foundation, either version 3 of the License, or
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
     9
 * (at your option) any later version.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    10
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    11
 * This program is distributed in the hope that it will be useful, but
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    12
 * WITHOUT ANY WARRANTY; without even the implied warranty of
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    13
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    14
 * General Public License for more details.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    15
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    16
 * You should have received a copy of the GNU Lesser General Public License
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    17
 * along with this Modbus library.  If not, see <http://www.gnu.org/licenses/>.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    18
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    19
 * This code is made available on the understanding that it will not be
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    20
 * used in safety-critical situations without a full and competent review.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    21
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    22
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    23
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    24
/* mb_master.c */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    25
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    26
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    27
#include <fcntl.h>	/* File control definitions */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    28
#include <stdio.h>	/* Standard input/output */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    29
#include <string.h>
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    30
#include <stdlib.h>
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    31
#include <termio.h>	/* POSIX terminal control definitions */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    32
#include <sys/time.h>	/* Time structures for select() */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    33
#include <unistd.h>	/* POSIX Symbolic Constants */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    34
#include <errno.h>	/* Error definitions */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    35
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    36
#include <pthread.h>    /* pthread_mutex_[un]lock() */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    37
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    38
#include <netinet/in.h> /* required for htons() and ntohs() */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    39
#include "mb_layer1.h"
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    40
#include "mb_master.h"
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    41
#include "mb_master_private.h"
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    42
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    43
/* #define DEBUG */		/* uncomment to see the data sent and received */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    44
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    45
#define modbus_write             fptr_[layer1_fin].modbus_write           
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    46
#define modbus_read              fptr_[layer1_fin].modbus_read            
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    47
#define modbus_init              fptr_[layer1_fin].modbus_init            
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    48
#define modbus_done              fptr_[layer1_fin].modbus_done            
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    49
#define modbus_connect           fptr_[layer1_fin].modbus_connect         
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    50
#define modbus_listen            fptr_[layer1_fin].modbus_listen          
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    51
#define modbus_close             fptr_[layer1_fin].modbus_close           
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    52
#define modbus_silence_init      fptr_[layer1_fin].modbus_silence_init    
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    53
#define modbus_get_min_timeout   fptr_[layer1_fin].modbus_get_min_timeout 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    54
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    55
/* the lower two bits of ttyfd are used to store the index to layer1 function pointers */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    56
/* layer1_fin index to fptr_[] is in lowest 2 bits of fd */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    57
#define get_ttyfd()     int layer1_fin = fd & 3; int ttyfd = fd / 4;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    58
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    59
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    60
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    61
/******************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    62
/******************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    63
/**                                      **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    64
/**         Global Variables...          **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    65
/**                                      **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    66
/******************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    67
/******************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    68
   /* The layer 1 (RTU, ASCII, TCP) implementation will be adding some headers and CRC (at the end)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    69
    *  of the packet we build here (actually currently it is only at the end). Since we want to 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    70
    *  re-use the same buffer so as not to continuosly copy the same info from buffer to buffer,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    71
    *  we need tp allocate more bytes than the ones we need for this layer. Therefore, the
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    72
    *  extra_bytes parameter.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    73
    *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    74
    *  Note that we add one more extra byte. This is because some packets will not be 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    75
    *  starting off at byte 0, but rather at byte 1 of the buffer. This is in order to guarantee
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    76
    *  that the data that is sent on the buffer is aligned on even bytes (the 16 bit words!).
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    77
    *  This will allow us to reference this memory as an u16 *, without producing 'bus error'
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    78
    *  messages in some embedded devices that do not allow acessing u16 on odd numbered addresses.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    79
    */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    80
static int buff_extra_bytes_;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    81
#define QUERY_BUFFER_SIZE       (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    82
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    83
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    84
/******************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    85
/******************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    86
/**                                      **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    87
/**       Local Utility functions...     **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    88
/**                                      **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    89
/******************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    90
/******************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    91
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    92
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    93
/*
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    94
 * Function to determine next transaction id.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    95
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    96
 * We use a library wide transaction id, which means that we
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    97
 * use a new transaction id no matter what slave to which we will
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    98
 * be sending the request...
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    99
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   100
static inline u16 next_transaction_id(void) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   101
  static u16 next_id = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   102
  return next_id++;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   103
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   104
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   105
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   106
/*
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   107
 * Functions to convert u16 variables
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   108
 * between network and host byte order
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   109
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   110
 * NOTE: Modbus uses MSByte first, just like
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   111
 *       tcp/ip, so we use the htons() and
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   112
 *       ntoh() functions to guarantee
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   113
 *       code portability.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   114
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   115
static inline u16 mb_hton(u16 h_value) {return htons(h_value);}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   116
static inline u16 mb_ntoh(u16 m_value) {return ntohs(m_value);}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   117
static inline u8  msb    (u16   value) {return (value >> 8) & 0xFF;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   118
static inline u8  lsb    (u16   value) {return  value & 0xFF;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   119
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   120
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   121
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   122
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   123
/*************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   124
/*************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   125
/**                                             **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   126
/**   Common functions for Modbus Protocol.     **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   127
/**                                             **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   128
/*************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   129
/*************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   130
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   131
/* build the common elements of a query frame */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   132
static inline int build_packet(u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   133
                               u8  function,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   134
                               u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   135
                               u16 count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   136
                               u8 *packet) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   137
  union {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   138
      u16 u16;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   139
      u8  u8[2];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   140
  } tmp;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   141
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   142
  packet[0] = slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   143
  packet[1] = function;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   144
    /* NOTE:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   145
     *  Modbus uses high level addressing starting off from 1, but
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   146
     *  this is sent as 0 on the wire!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   147
     *  We could expect the user to specify high level addressing 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   148
     *   starting at 1, and do the conversion to start off at 0 here.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   149
     *   However, to do this we would then need to use an u32 data type
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   150
     *   to correctly hold the address supplied by the user (which could
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   151
     *   correctly be 65536, which does not fit in an u16), which would
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   152
     *   in turn require us to check whether the address supplied by the user
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   153
     *   is correct (i.e. <= 65536). 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   154
     *  I decided to go with the other option of using an u16, and 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   155
     *   requiring the user to use addressing starting off at 0! 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   156
     */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   157
  /* NOTE: we do not use up casting - i.e. the following
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   158
   *       *((u16 *)(packet+2)) = mb_hton(start_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   159
   *       because packet+2 is NOT aligned with an even address, and would
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   160
   *       therefore result in 'bus error' when using compilers that do not 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   161
   *       automatically do the required decomposing of this supposedly 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   162
   *       single bus access into two distinct bus accesses.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   163
   *       (Note that some compilers do do this decomposing automatically
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   164
   *       in which case the following is not necessary).
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   165
   *       At the moment, I (Mario de Sousa) know of at least one cross-compiler
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   166
   *       that does not do the decomposing automatically, i.e. the 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   167
   *       AVR32 cross-compiler.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   168
   */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   169
  tmp.u16 = mb_hton(start_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   170
  packet[2] = tmp.u8[0];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   171
  packet[3] = tmp.u8[1];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   172
  tmp.u16 = mb_hton(count);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   173
  packet[4] = tmp.u8[0];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   174
  packet[5] = tmp.u8[1];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   175
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   176
  return 6;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   177
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   178
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   179
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   180
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   181
/* Execute a Query/Response transaction between client and server */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   182
/* returns: <0    -> ERROR: error codes
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   183
 *          >2    -> SUCCESS: frame length
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   184
 *           0..2 -> will never be returned!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   185
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   186
static int mb_transaction(u8  *packet,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   187
                          int query_length,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   188
                          u8  **data,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   189
                          int fd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   190
                          int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   191
                          u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   192
                          const struct timespec *response_timeout) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   193
  int error = TIMEOUT;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   194
  int response_length = INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   195
  u16 send_transaction_id, recv_transaction_id;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   196
  get_ttyfd(); /* declare the ttyfd variable, ... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   197
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   198
    /* We must also initialize the recv_transaction_id with the same value,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   199
     * since some layer 1 protocols do not support transaction id's, so
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   200
     * simply return the recv_transaction_id variable without any changes...
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   201
     */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   202
  /* NOTE: we re-use the same transaction id for all send re-tries., since, in truth, 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   203
   * it is still the same transaction. This will also simplify re-synchronising with
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   204
   * some slaves that keep a buffer of outstanding requests, and will reply to all of
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   205
   * them, in FIFO order. In this case, once an error occurs we will be swamping the
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   206
   * slave with requests. By using the same transaction id, we may correctly consider
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   207
   * the reply to the first request sent as the reply to the third request! This means
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   208
   * we stop re-trying the sending of further requests, and no longer swamp the slave...
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   209
   */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   210
  send_transaction_id = recv_transaction_id = next_transaction_id();
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   211
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   212
  for (send_retries++; send_retries > 0; send_retries--) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   213
    error = TIMEOUT;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   214
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   215
    if (modbus_write(ttyfd, packet, query_length, send_transaction_id, response_timeout) < 0)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   216
      {error = PORT_FAILURE; continue;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   217
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   218
      /* if we receive a correct response but with a wrong transaction id or wrong modbus function, we try to 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   219
       * receive another frame instead of returning an error or re-sending the request! This first frame could 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   220
       * have been a response to a previous request of ours that timed out waiting for a response, and the 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   221
       * response we are waiting for could be coming 'any minute now'.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   222
       */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   223
    do {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   224
      response_length = modbus_read(&ttyfd, data, &recv_transaction_id,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   225
                                    packet, query_length, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   226
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   227
      /* TIMEOUT condition */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   228
      /* However, if we had previously received an invalid frame, or some other error,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   229
       * we return that error instead!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   230
       * Note that the 'error' variable was initialised with the TIMEOUT error
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   231
       * condition, so if no previous error ocurred, we will be returning the
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   232
       * TIMEOUT error condition here!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   233
       */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   234
      if(response_length == -2)  return error;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   235
      /* NOTE we want to break out of this while loop without even running the while()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   236
       * condition, as that condition is only valid if response_length > 3 !!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   237
       */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   238
      if(response_length  <  0)  {error = PORT_FAILURE; break;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   239
      /* This should never occur! Modbus_read() should only return valid frames! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   240
      if(response_length  <  3)  return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   241
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   242
    } while (/* we have the wrong transaction id */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   243
             (send_transaction_id != recv_transaction_id)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   244
             /* not a response frame to _our_ query */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   245
            ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   246
             (((*data)[1] & ~0x80) != packet[1])
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   247
            /* NOTE: no need to check whether (*data)[0] = slave!              */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   248
            /*       This has already been done by the modbus_read() function! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   249
            );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   250
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   251
    if(response_length < 0)  {error = PORT_FAILURE; continue;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   252
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   253
    /* Now check whether we received a Modbus Exception frame */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   254
    if (((*data)[1] & 0x80) != 0) {       /* we have an exception frame! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   255
      /* NOTE: we have already checked above that data[2] exists! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   256
      if (error_code != NULL)  *error_code = (*data)[2];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   257
      return MODBUS_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   258
    }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   259
    /* success! Let's get out of the send retry loop... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   260
    return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   261
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   262
  /* reached the end of the retries... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   263
  return error;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   264
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   265
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   266
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   267
/**************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   268
/**************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   269
/**                                  **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   270
/**   Modbus Protocol Functions.     **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   271
/**                                  **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   272
/**************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   273
/**************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   274
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   275
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   276
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   277
/* Execute a transaction for functions that READ BITS.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   278
 * Bits are stored on an int array, one bit per int.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   279
 * Called by:  read_input_bits()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   280
 *             read_output_bits()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   281
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   282
static int read_bits(u8  function,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   283
                     u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   284
                     u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   285
                     u16 count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   286
                     u16 *dest,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   287
                     int dest_size,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   288
                     int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   289
                     int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   290
                     u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   291
                     const struct timespec *response_timeout,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   292
                     pthread_mutex_t *data_access_mutex) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   293
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   294
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   295
  u8 *data;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   296
  int response_length, query_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   297
  int temp, i, bit, dest_pos = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   298
  int coils_processed = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   299
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   300
  query_length = build_packet(slave, function, start_addr, count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   301
  if (query_length < 0)  return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   302
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   303
  response_length = mb_transaction(packet, query_length, &data, ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   304
                                   send_retries, error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   305
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   306
  if (response_length  < 0)                  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   307
  /* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   308
  if (response_length != 3 + (count+7)/8)    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   309
  if (data[2]         !=     (count+7)/8)    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   310
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   311
  if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   312
  for( i = 0; (i < data[2]) && (i < dest_size); i++ ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   313
    temp = data[3 + i];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   314
    for( bit = 0x01; (bit & 0xff) && (coils_processed < count); ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   315
      dest[dest_pos] = (temp & bit)?1:0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   316
      coils_processed++;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   317
      dest_pos++;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   318
      bit = bit << 1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   319
    }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   320
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   321
  if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   322
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   323
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   324
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   325
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   326
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   327
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   328
/* Execute a transaction for functions that READ BITS.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   329
 * Bits are stored on an u32 array, 32 bits per u32.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   330
 * Unused bits in last u32 are set to 0.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   331
 * Called by:  read_input_bits_u32()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   332
 *             read_output_bits_u32()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   333
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   334
static int read_bits_u32(u8  function,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   335
                         u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   336
                         u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   337
                         u16 count, /* number of bits !! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   338
                         u32 *dest,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   339
                         int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   340
                         int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   341
                         u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   342
                         const struct timespec *response_timeout) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   343
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   344
  u8 *data;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   345
  int response_length, query_length;                         
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   346
  int byte_count, i, dest_pos = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   347
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   348
  query_length = build_packet(slave, function, start_addr, count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   349
  if (query_length < 0)  return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   350
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   351
  response_length = mb_transaction(packet, query_length, &data, ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   352
                                   send_retries, error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   353
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   354
  if (response_length < 0)                   return response_length;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   355
  /* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   356
  if (response_length != 3 + (count+7)/8)    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   357
  if (data[2]         !=     (count+7)/8)    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   358
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   359
  byte_count = data[2];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   360
  data += 3;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   361
  /* handle groups of 4 bytes... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   362
  for(i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   363
    dest[dest_pos] = data[i] + data[i+1]*0x100 + data[i+2]*0x10000 + data[i+3]*0x1000000;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   364
  /* handle any remaining bytes... begining with the last! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   365
  if (i < byte_count) dest[dest_pos] = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   366
  for(byte_count--; i <= byte_count; byte_count--)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   367
    dest[dest_pos] = dest[dest_pos]*0x100 + data[byte_count];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   368
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   369
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   370
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   371
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   372
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   373
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   374
/* FUNCTION 0x01   - Read Coils
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   375
 * Bits are stored on an int array, one bit per int.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   376
 */
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   377
int read_output_bits(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   378
                     u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   379
                     u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   380
                     u16 *dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   381
                     int dest_size,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   382
                     int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   383
                     int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   384
                     u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   385
                     const struct timespec *response_timeout,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   386
                     pthread_mutex_t *data_access_mutex) {
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   387
  if( count > MAX_READ_BITS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   388
    count = MAX_READ_BITS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   389
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   390
    fprintf( stderr, "Too many coils requested.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   391
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   392
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   393
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   394
  return read_bits(0x01 /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   395
                   slave, start_addr, count, dest, dest_size, ttyfd, 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   396
                   send_retries, error_code, response_timeout, data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   397
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   398
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   399
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   400
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   401
/* FUNCTION 0x01   - Read Coils
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   402
 * Bits are stored on an u32 array, 32 bits per u32.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   403
 * Unused bits in last u32 are set to 0.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   404
 */
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   405
int read_output_bits_u32(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   406
                         u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   407
                         u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   408
                         u32 *dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   409
                         int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   410
                         int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   411
                         u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   412
                         const struct timespec *response_timeout) {
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   413
  if( count > MAX_READ_BITS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   414
    count = MAX_READ_BITS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   415
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   416
    fprintf( stderr, "Too many coils requested.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   417
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   418
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   419
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   420
  return read_bits_u32(0x01 /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   421
                       slave, start_addr, count, dest, ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   422
                       send_retries, error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   423
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   424
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   425
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   426
/* FUNCTION 0x02   - Read Discrete Inputs
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   427
 * Bits are stored on an int array, one bit per int.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   428
 */
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   429
int read_input_bits(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   430
                    u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   431
                    u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   432
                    u16 *dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   433
                    int dest_size,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   434
                    int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   435
                    int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   436
                    u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   437
                    const struct timespec *response_timeout,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   438
                    pthread_mutex_t *data_access_mutex) {
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   439
  if( count > MAX_READ_BITS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   440
    count = MAX_READ_BITS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   441
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   442
    fprintf( stderr, "Too many coils requested.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   443
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   444
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   445
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   446
  return read_bits(0x02 /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   447
                   slave, start_addr, count, dest, dest_size, ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   448
                   send_retries, error_code, response_timeout, data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   449
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   450
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   451
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   452
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   453
/* FUNCTION 0x02   - Read Discrete Inputs
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   454
 * Bits are stored on an u32 array, 32 bits per u32.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   455
 * Unused bits in last u32 are set to 0.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   456
 */
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   457
int read_input_bits_u32(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   458
                        u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   459
                        u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   460
                        u32 *dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   461
                        int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   462
                        int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   463
                        u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   464
                        const struct timespec *response_timeout) {
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   465
  if( count > MAX_READ_BITS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   466
    count = MAX_READ_BITS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   467
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   468
    fprintf( stderr, "Too many coils requested.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   469
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   470
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   471
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   472
  return read_bits_u32(0x02 /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   473
                       slave, start_addr, count, dest, ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   474
                       send_retries, error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   475
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   476
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   477
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   478
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   479
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   480
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   481
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   482
/* Execute a transaction for functions that READ REGISTERS.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   483
 * Called by:  read_input_words()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   484
 *             read_output_words()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   485
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   486
static int read_registers(u8  function,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   487
                          u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   488
                          u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   489
                          u16 count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   490
                          u16 *dest,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   491
                          int dest_size,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   492
                          int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   493
                          int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   494
                          u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   495
                          const struct timespec *response_timeout,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   496
                          pthread_mutex_t *data_access_mutex) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   497
  u8 *data;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   498
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   499
  int response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   500
  int query_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   501
  int temp,i;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   502
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   503
  query_length = build_packet(slave, function, start_addr, count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   504
  if (query_length < 0)    return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   505
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   506
  response_length = mb_transaction(packet, query_length, &data, ttyfd, 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   507
                                   send_retries, error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   508
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   509
  if (response_length  < 0)              return response_length;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   510
  if (response_length != 3 + 2*count)    return INVALID_FRAME;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   511
  if (data[2]         !=     2*count)    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   512
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   513
  if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   514
  for(i = 0; (i < (data[2]*2)) && (i < dest_size); i++ ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   515
    temp = data[3 + i *2] << 8;    /* copy reg hi byte to temp hi byte*/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   516
    temp = temp | data[4 + i * 2]; /* copy reg lo byte to temp lo byte*/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   517
    dest[i] = temp;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   518
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   519
  if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   520
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   521
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   522
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   523
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   524
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   525
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   526
/* Execute a transaction for functions that READ REGISTERS.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   527
 * return the array with the data to the calling function
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   528
 * Called by:  read_input_words_u16_ref()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   529
 *             read_output_words_u16_ref()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   530
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   531
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   532
static int read_registers_u16_ref(u8  function,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   533
                                  u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   534
                                  u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   535
                                  u16 count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   536
                                  u16 **dest,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   537
                                  int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   538
                                  int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   539
                                  u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   540
                                  const struct timespec *response_timeout) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   541
  u8 *data;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   542
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   543
  int response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   544
  int query_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   545
  int i, byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   546
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   547
  query_length = build_packet(slave, function, start_addr, count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   548
  if (query_length < 0)    return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   549
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   550
  response_length = mb_transaction(packet, query_length, &data, ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   551
                                   send_retries, error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   552
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   553
  if (response_length < 0)               return response_length;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   554
  if (response_length != 3 + 2*count)    return INVALID_FRAME;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   555
  if (data[2]         !=     2*count)    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   556
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   557
  byte_count = data[2];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   558
  data = data + 3; /* & data[3] */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   559
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   560
  if (ntohs(0x0102) != 0x0102) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   561
   /* little endian host... => we need to swap the bytes! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   562
    for(i = 0; i < byte_count; i++ ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   563
      /* the following 3 lines result in the two values being exchanged! */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   564
      data[i  ] = data[i] ^ data[i+1];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   565
      data[i+1] = data[i] ^ data[i+1];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   566
      data[i  ] = data[i] ^ data[i+1];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   567
    }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   568
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   569
  *dest = (u16 *)data;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   570
  return byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   571
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   572
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   573
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   574
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   575
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   576
/* Execute a transaction for functions that READ REGISTERS.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   577
 * u16 registers are stored in array of u32, two registers per u32.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   578
 * Unused bits of last u32 element are set to 0.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   579
 * Called by:  read_input_words_u32()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   580
 *             read_output_words_u32()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   581
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   582
static int read_registers_u32(u8  function,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   583
                              u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   584
                              u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   585
                              u16 count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   586
                              u32 *dest,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   587
                              int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   588
                              int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   589
                              u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   590
                              const struct timespec *response_timeout) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   591
  u8 *data;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   592
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   593
  int response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   594
  int query_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   595
  int i, byte_count, dest_pos;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   596
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   597
  query_length = build_packet(slave, function, start_addr, count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   598
  if (query_length < 0)    return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   599
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   600
  response_length = mb_transaction(packet, query_length, &data, ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   601
                                   send_retries, error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   602
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   603
  if (response_length  < 0)              return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   604
  if (response_length != 3 + 2*count)    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   605
  if (data[2]         !=     2*count)    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   606
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   607
  byte_count = data[2];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   608
  data += 3;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   609
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   610
  if (ntohs(0x0102) == 0x0102) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   611
   /* big endian host... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   612
    /* handle groups of 4 bytes... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   613
    for(i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   614
      *(((u8 *)(dest + dest_pos))+ 0) = *(data+i+3);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   615
      *(((u8 *)(dest + dest_pos))+ 1) = *(data+i+4);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   616
      *(((u8 *)(dest + dest_pos))+ 2) = *(data+i+0);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   617
      *(((u8 *)(dest + dest_pos))+ 3) = *(data+i+1);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   618
    }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   619
    /* handle any remaining bytes...
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   620
     * since byte_count is supposed to be multiple of 2,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   621
     * (and has already been verified above 'if (data[2] != 2*count)')
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   622
     * this will be either 2, or none at all!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   623
     */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   624
    if (i + 1 < byte_count)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   625
      *(((u8 *)(dest + dest_pos))+ 0) = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   626
      *(((u8 *)(dest + dest_pos))+ 1) = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   627
      *(((u8 *)(dest + dest_pos))+ 2) = *(data+i+0);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   628
      *(((u8 *)(dest + dest_pos))+ 3) = *(data+i+1);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   629
  } else {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   630
   /* little endian host... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   631
    /* handle groups of 4 bytes... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   632
    for(i = 0, dest_pos = 0; i + 3 < byte_count; i += 4, dest_pos++) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   633
      *(((u8 *)(dest + dest_pos))+ 0) = *(data+i+1);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   634
      *(((u8 *)(dest + dest_pos))+ 1) = *(data+i+0);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   635
      *(((u8 *)(dest + dest_pos))+ 2) = *(data+i+3);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   636
      *(((u8 *)(dest + dest_pos))+ 3) = *(data+i+2);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   637
    }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   638
    /* handle any remaining bytes...
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   639
     * since byte_count is supposed to be multiple of 2,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   640
     * (and has already been verified above 'if (data[2] != 2*count)')
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   641
     * this will be either 2, or none at all!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   642
     */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   643
    if (i + 1 < byte_count)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   644
      *(((u8 *)(dest + dest_pos))+ 0) = *(data+i+1);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   645
      *(((u8 *)(dest + dest_pos))+ 1) = *(data+i+0);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   646
      *(((u8 *)(dest + dest_pos))+ 2) = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   647
      *(((u8 *)(dest + dest_pos))+ 3) = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   648
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   649
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   650
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   651
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   652
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   653
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   654
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   655
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   656
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   657
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   658
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   659
/* FUNCTION 0x03   - Read Holding Registers */
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   660
int read_output_words(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   661
                      u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   662
                      u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   663
                      u16 *dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   664
                      int dest_size,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   665
                      int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   666
                      int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   667
                      u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   668
                      const struct timespec *response_timeout,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   669
                      pthread_mutex_t *data_access_mutex) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   670
  if( count > MAX_READ_REGS ) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   671
    count = MAX_READ_REGS;
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   672
    #ifdef DEBUG
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   673
    fprintf( stderr, "Too many registers requested.\n" );
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   674
    #endif
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   675
  }
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   676
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   677
  return read_registers(0x03 /* function */,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   678
                        slave, start_addr, count, dest, dest_size, ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   679
                        send_retries, error_code, response_timeout, data_access_mutex);
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   680
}
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   681
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   682
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   683
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   684
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   685
/* FUNCTION 0x03   - Read Holding Registers
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   686
 * u16 registers are stored in array of u32, two registers per u32.
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   687
 * Unused bits of last u32 element are set to 0.
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   688
 */
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   689
int read_output_words_u32(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   690
                          u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   691
                          u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   692
                          u32 *dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   693
                          int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   694
                          int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   695
                          u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   696
                          const struct timespec *response_timeout) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   697
  if( count > MAX_READ_REGS ) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   698
    count = MAX_READ_REGS;
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   699
    #ifdef DEBUG
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   700
    fprintf( stderr, "Too many registers requested.\n" );
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   701
    #endif
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   702
  }
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   703
  
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   704
  return read_registers_u32(0x03 /* function */,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   705
                            slave, start_addr, count, dest, ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   706
                            send_retries, error_code, response_timeout);
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   707
}
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   708
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   709
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   710
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   711
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   712
/* FUNCTION 0x03   - Read Holding Registers
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   713
 * return the array with the data to the calling function
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   714
 */
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   715
int read_output_words_u16_ref(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   716
                              u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   717
                              u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   718
                              u16 **dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   719
                              int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   720
                              int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   721
                              u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   722
                              const struct timespec *response_timeout) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   723
  if( count > MAX_READ_REGS ) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   724
    count = MAX_READ_REGS;
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   725
    #ifdef DEBUG
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   726
    fprintf( stderr, "Too many registers requested.\n" );
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   727
    #endif
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   728
  }
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   729
  
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   730
  return read_registers_u16_ref(0x03 /* function */,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   731
                                slave, start_addr, count, dest, ttyfd, send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   732
                                error_code, response_timeout);
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   733
}
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   734
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   735
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   736
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   737
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   738
/* FUNCTION 0x04   - Read Input Registers */
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   739
int read_input_words(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   740
                     u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   741
                     u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   742
                     u16 *dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   743
                     int dest_size,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   744
                     int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   745
                     int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   746
                     u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   747
                     const struct timespec *response_timeout,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   748
                     pthread_mutex_t *data_access_mutex) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   749
  if( count > MAX_READ_REGS ) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   750
    count = MAX_READ_REGS;
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   751
    #ifdef DEBUG
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   752
    fprintf( stderr, "Too many input registers requested.\n" );
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   753
    #endif
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   754
  }
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   755
  
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   756
  return read_registers(0x04 /* function */,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   757
                        slave, start_addr, count, dest, dest_size, ttyfd, send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   758
                        error_code, response_timeout, data_access_mutex);
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   759
}
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   760
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   761
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   762
/* FUNCTION 0x04   - Read Input Registers
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   763
 * u16 registers are stored in array of u32, two registers per u32.
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   764
 * Unused bits of last u32 element are set to 0.
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   765
 */
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   766
int read_input_words_u32(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   767
                         u16 start_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   768
                         u16 count,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   769
                         u32 *dest,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   770
                         int ttyfd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   771
                         int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   772
                         u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   773
                         const struct timespec *response_timeout) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   774
  if( count > MAX_READ_REGS ) {
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   775
    count = MAX_READ_REGS;
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   776
    #ifdef DEBUG
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   777
    fprintf( stderr, "Too many input registers requested.\n" );
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   778
    #endif
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   779
  }
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   780
  
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   781
  return read_registers_u32(0x04 /* function */,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   782
                            slave, start_addr, count, dest, ttyfd, send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   783
                            error_code, response_timeout);
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   784
}
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   785
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   786
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   787
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   788
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   789
/* FUNCTION 0x04   - Read Input Registers
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   790
 * return the array with the data to the calling function
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   791
 */
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   792
int read_input_words_u16_ref(u8  slave,
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   793
                             u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   794
                             u16 count,
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   795
                             u16 **dest,
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   796
                             int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   797
                             int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   798
                             u8  *error_code,
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   799
                             const struct timespec *response_timeout) {
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   800
  if( count > MAX_READ_REGS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   801
    count = MAX_READ_REGS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   802
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   803
    fprintf( stderr, "Too many input registers requested.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   804
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   805
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   806
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   807
  return read_registers_u16_ref(0x04 /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   808
                                slave, start_addr, count, dest, ttyfd, send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   809
                                error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   810
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   811
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   812
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   813
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   814
/* Execute a transaction for functions that WRITE a sinlge BIT.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   815
 * Called by:  write_output_bit()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   816
 *             write_output_word()
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   817
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   818
static int set_single(u8  function,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   819
                      u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   820
                      u16 addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   821
                      u16 value,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   822
                      int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   823
                      int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   824
                      u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   825
                      const struct timespec *response_timeout,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   826
                      pthread_mutex_t *data_access_mutex) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   827
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   828
  u8 *data;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   829
  int query_length, response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   830
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   831
  if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   832
  query_length = build_packet(slave, function, addr, value, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   833
  if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   834
  if (query_length < 0)    return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   835
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   836
  response_length = mb_transaction(packet, query_length, &data, ttyfd, send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   837
                                   error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   838
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   839
  if (response_length  < 0)  return response_length;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   840
  if (response_length != 6)  return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   841
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   842
  if ((data[2] != packet[2]) || (data[3] != packet[3]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   843
      (data[4] != packet[4]) || (data[5] != packet[5]))
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   844
    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   845
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   846
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   847
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   848
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   849
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   850
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   851
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   852
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   853
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   854
/* FUNCTION 0x05   - Force Single Coil */
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   855
int write_output_bit(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   856
                     u16 coil_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   857
                     u16 state,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   858
                     int fd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   859
                     int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   860
                     u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   861
                     const struct timespec *response_timeout,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   862
                     pthread_mutex_t *data_access_mutex) {
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   863
  if (state) state = 0xFF00;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   864
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   865
  return set_single(0x05 /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   866
                    slave, coil_addr, state, fd, send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   867
                    error_code, response_timeout, data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   868
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   869
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   870
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   871
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   872
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   873
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   874
/* FUNCTION 0x06   - Write Single Register */
1
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   875
int write_output_word(u8  slave,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   876
                      u16 reg_addr,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   877
                      u16 value,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   878
                      int fd,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   879
                      int send_retries,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   880
                      u8  *error_code,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   881
                      const struct timespec *response_timeout,
59783e8ee3d2 Remove incorrect inlines
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 0
diff changeset
   882
                      pthread_mutex_t *data_access_mutex) {
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   883
  return set_single(0x06 /* function */, 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   884
                    slave, reg_addr, value, fd, send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   885
                    error_code, response_timeout, data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   886
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   887
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   888
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   889
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   890
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   891
/* FUNCTION 0x0F   - Force Multiple Coils */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   892
int write_output_bits(u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   893
                      u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   894
                      u16 coil_count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   895
                      u16 *data,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   896
                      int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   897
                      int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   898
                      u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   899
                      const struct timespec *response_timeout,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   900
                      pthread_mutex_t *data_access_mutex) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   901
  int byte_count, i;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   902
  u8  bit;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   903
  int coil_check = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   904
  int data_array_pos = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   905
  int query_length, response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   906
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   907
  u8  *rdata;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   908
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   909
  if( coil_count > MAX_WRITE_COILS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   910
    coil_count = MAX_WRITE_COILS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   911
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   912
    fprintf( stderr, "Writing to too many coils.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   913
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   914
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   915
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   916
  query_length = build_packet(slave, 0x0F /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   917
                              start_addr, coil_count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   918
  if (query_length < 0)    return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   919
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   920
  /* NOTE: Integer division. (count+7)/8 is equivalent to ceil(count/8) */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   921
  byte_count = (coil_count+7)/8;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   922
  packet[query_length] = byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   923
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   924
  if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   925
  bit = 0x01;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   926
  for(i = 0; i < byte_count; i++) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   927
    packet[++query_length] = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   928
    while((bit & 0xFF) && (coil_check++ < coil_count)) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   929
      if(data[data_array_pos++]) {packet[query_length] |=  bit;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   930
      else                       {packet[query_length] &= ~bit;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   931
      bit <<= 1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   932
    }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   933
    bit = 0x01;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   934
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   935
  if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   936
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   937
  response_length = mb_transaction(packet, ++query_length, &rdata, ttyfd, send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   938
                                   error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   939
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   940
  if (response_length  < 0)      return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   941
  if (response_length != 6)      return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   942
  if ((rdata[2] != packet[2]) || 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   943
      (rdata[3] != packet[3]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   944
      (rdata[4] != packet[4]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   945
      (rdata[5] != packet[5]))   return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   946
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   947
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   948
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   949
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   950
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   951
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   952
/* FUNCTION 0x0F   - Force Multiple Coils
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   953
 * Bits should be stored on an u32 array, 32 bits per u32.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   954
 * Unused bits in last u32 should be set to 0.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   955
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   956
int write_output_bits_u32(u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   957
                          u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   958
                          u16 coil_count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   959
                          u32 *data,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   960
                          int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   961
                          int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   962
                          u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   963
                          const struct timespec *response_timeout) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   964
  int org_pos, byte_count, i;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   965
  int query_length, response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   966
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   967
  u8  *rdata;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   968
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   969
  if( coil_count > MAX_WRITE_COILS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   970
    coil_count = MAX_WRITE_COILS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   971
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   972
    fprintf( stderr, "Writing to too many coils.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   973
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   974
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   975
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   976
  query_length = build_packet(slave, 0x0F /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   977
                              start_addr, coil_count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   978
  if (query_length < 0)    return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   979
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   980
  /* NOTE: Integer division. This is equivalent of determining the ceil(count/8) */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   981
  byte_count = (coil_count+7)/8;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   982
  packet[query_length] = byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   983
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   984
  /* handle groups of 4 bytes... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   985
  for(i = 0, org_pos = 0; i + 3 < byte_count; i += 4, org_pos++) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   986
    packet[++query_length] = data[org_pos] & 0xFF; data[org_pos] >>= 8;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   987
    packet[++query_length] = data[org_pos] & 0xFF; data[org_pos] >>= 8;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   988
    packet[++query_length] = data[org_pos] & 0xFF; data[org_pos] >>= 8;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   989
    packet[++query_length] = data[org_pos] & 0xFF;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   990
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   991
  /* handle any remaining bytes... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   992
  for(; i < byte_count; i++) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   993
    packet[++query_length] = data[org_pos] & 0xFF; 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   994
    data[org_pos] >>= 8;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   995
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   996
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   997
  response_length = mb_transaction(packet, ++query_length, &rdata, ttyfd, send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   998
                                   error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   999
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1000
  if (response_length  < 0)       return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1001
  if (response_length != 6)       return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1002
  if ((rdata[2] != packet[2]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1003
      (rdata[3] != packet[3]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1004
      (rdata[4] != packet[4]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1005
      (rdata[5] != packet[5]))    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1006
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1007
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1008
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1009
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1010
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1011
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1012
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1013
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1014
/* FUNCTION 0x10   - Force Multiple Registers */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1015
int write_output_words(u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1016
                       u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1017
                       u16 reg_count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1018
                       u16 *data,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1019
                       int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1020
                       int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1021
                       u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1022
                       const struct timespec *response_timeout,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1023
                       pthread_mutex_t *data_access_mutex) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1024
  u8  byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1025
  int i, query_length, response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1026
  u8 packet[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1027
  u8  *rdata;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1028
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1029
  if( reg_count > MAX_WRITE_REGS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1030
    reg_count = MAX_WRITE_REGS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1031
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1032
    fprintf( stderr, "Trying to write to too many registers.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1033
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1034
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1035
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1036
  query_length = build_packet(slave, 0x10 /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1037
                              start_addr, reg_count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1038
  if (query_length < 0)    return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1039
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1040
  byte_count = reg_count*2;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1041
  packet[query_length] = byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1042
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1043
  if (NULL != data_access_mutex) pthread_mutex_lock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1044
  for( i = 0; i < reg_count; i++ ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1045
    packet[++query_length] = data[i] >> 8;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1046
    packet[++query_length] = data[i] & 0x00FF;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1047
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1048
  if (NULL != data_access_mutex) pthread_mutex_unlock(data_access_mutex);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1049
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1050
  response_length = mb_transaction(packet, ++query_length, &rdata, ttyfd, send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1051
                                   error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1052
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1053
  if (response_length  < 0)       return response_length;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1054
  if (response_length != 6)       return INVALID_FRAME;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1055
  if ((rdata[2] != packet[2]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1056
      (rdata[3] != packet[3]) ||      
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1057
      (rdata[4] != packet[4]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1058
      (rdata[5] != packet[5]))    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1059
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1060
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1061
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1062
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1063
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1064
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1065
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1066
/* FUNCTION 0x10   - Force Multiple Registers
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1067
 * u16 registers are stored in array of u32, two registers per u32.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1068
 * Unused bits of last u32 element are set to 0.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1069
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1070
int write_output_words_u32(u8  slave,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1071
                           u16 start_addr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1072
                             /* number of 16 bit registers packed in the u32 array! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1073
                           u16 reg_count,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1074
                           u32 *data,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1075
                           int ttyfd,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1076
                           int send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1077
                           u8  *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1078
                           const struct timespec *response_timeout) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1079
  u8  byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1080
  int i, query_length, response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1081
  u8 packet_[QUERY_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1082
  u8 *packet = packet_; /* remove the const'ness of packet_ */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1083
  u8  *rdata;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1084
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1085
  if( reg_count > MAX_WRITE_REGS ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1086
    reg_count = MAX_WRITE_REGS;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1087
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1088
    fprintf( stderr, "Trying to write to too many registers.\n" );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1089
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1090
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1091
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1092
  /* Make sure that the de-referencing and up-casting going on later on in 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1093
   * this function, i.e. code like the following line:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1094
   * *((u16 *)packet) = XXX
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1095
   * will result in u16 words starting off on even addresses.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1096
   * If we don't do this, some compilers (e.g. AVR32 cross-compiler) will 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1097
   * generate code which, when executed, will result in 'bus error'.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1098
   *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1099
   * The following packet++ means that the first byte of the packet array is
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1100
   * essentially never used. Notice too that the size of thepacket array
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1101
   * already takes into account this un-used byte.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1102
   */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1103
  packet++;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1104
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1105
  query_length = build_packet(slave, 0x10 /* function */,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1106
                              start_addr, reg_count, packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1107
  if (query_length < 0)  return INTERNAL_ERROR;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1108
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1109
  byte_count = reg_count*2;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1110
  packet[query_length] = byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1111
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1112
  /* handle groups of 4 bytes... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1113
  for(i = 0; 4*i + 3 < byte_count; i++) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1114
    *((u16 *)(packet+(++query_length))) = mb_hton(data[i]);        ++query_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1115
    *((u16 *)(packet+(++query_length))) = mb_hton(data[i] >> 16);  ++query_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1116
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1117
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1118
  /* handle any remaining bytes...
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1119
   * since byte_count is supposed to be multiple of 2,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1120
   * (and has already been verified above 'if (data[2] != 2*count)')
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1121
   * this will be either 2, or none at all!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1122
   */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1123
  if (4*i + 1 < byte_count) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1124
    *((u16 *)(packet+(++query_length))) = mb_hton(data[i]);        ++query_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1125
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1126
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1127
  response_length = mb_transaction(packet, ++query_length, &rdata, ttyfd, send_retries,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1128
                                   error_code, response_timeout);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1129
    
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1130
  if (response_length  < 0)       return response_length;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1131
  if (response_length != 6)       return INVALID_FRAME;  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1132
  if ((rdata[2] != packet[2]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1133
      (rdata[3] != packet[3]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1134
      (rdata[4] != packet[4]) ||
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1135
      (rdata[5] != packet[5]))    return INVALID_FRAME;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1136
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1137
  return response_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1138
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1139
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1140
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1141
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1142
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1143
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1144
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1145
/************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1146
/************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1147
/**                                            **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1148
/**   Modbus Library Management Functions.     **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1149
/**                                            **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1150
/************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1151
/************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1152
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1153
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1154
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1155
/* Initialise the Modbus Master Layer */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1156
int mb_master_init__(int extra_bytes) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1157
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1158
  fprintf(stderr, "mb_master_init__(extra_bytes=%d), QUERY_BUFFER_SIZE=%d\n", extra_bytes, QUERY_BUFFER_SIZE);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1159
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1160
  buff_extra_bytes_ = extra_bytes;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1161
  return 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1162
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1163
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1164
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1165
/* Shut down the Modbus Master Layer */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1166
int mb_master_done__(void) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1167
        return 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1168
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1169
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1170
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1171
#if 0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1172
int mb_master_init(int nd_count) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1173
  int extra_bytes;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1174
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1175
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1176
  fprintf( stderr, "mb_master_init()\n");
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1177
  fprintf( stderr, "creating %d nodes\n", nd_count);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1178
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1179
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1180
  /* initialise layer 1 library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1181
  if (modbus_init(nd_count, DEF_OPTIMIZATION, &extra_bytes) < 0)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1182
    goto error_exit_0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1183
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1184
  /* initialise this library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1185
  if (mb_master_init__(extra_bytes) < 0)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1186
    goto error_exit_1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1187
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1188
  return 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1189
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1190
error_exit_1:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1191
  modbus_done();
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1192
error_exit_0:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1193
  return -1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1194
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1195
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1196
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1197
int mb_master_done(void) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1198
  mb_master_done__();
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1199
  return modbus_done();
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1200
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1201
#endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1202
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1203
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1204
/* Establish a connection to a remote server/slave */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1205
/* NOTE: We use the lower 2 bits of the returned node id to identify which 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1206
 *       layer1 implementation to use. 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1207
 *           0 -> TCP 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1208
 *           1 -> RTU 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1209
 *           2 -> ASCII 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1210
 *           4 -> unused 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1211
 *       The node id used by the layer1 is shifted left 2 bits
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1212
 *       before returning the node id to the caller!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1213
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1214
int mb_master_connect(node_addr_t node_addr) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1215
  int res = -1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1216
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1217
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1218
  fprintf( stderr, "mb_master_tcp connect()\n");
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1219
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1220
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1221
  /* call layer 1 library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1222
  switch(node_addr.naf) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1223
    case naf_tcp:  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1224
      res = modbus_tcp_connect(node_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1225
      if (res >= 0) res = res*4 + 0 /* offset into fptr_ with TCP functions */;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1226
      return res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1227
    case naf_rtu:  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1228
      res = modbus_rtu_connect(node_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1229
      if (res >= 0) res = res*4 + 1 /* offset into fptr_ with RTU functions */;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1230
      return res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1231
    case naf_ascii:  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1232
      res = modbus_ascii_connect(node_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1233
      if (res >= 0) res = res*4 + 2 /* offset into fptr_ with ASCII functions */;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1234
      return res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1235
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1236
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1237
  return -1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1238
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1239
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1240
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1241
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1242
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1243
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1244
/* Shut down a connection to a remote server/slave */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1245
int mb_master_close(int fd) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1246
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1247
  fprintf( stderr, "mb_master_close(): nd = %d\n", fd);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1248
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1249
  get_ttyfd(); /* declare the ttyfd variable, ... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1250
  /* call layer 1 library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1251
  return modbus_close(ttyfd);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1252
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1253
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1254
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1255
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1256
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1257
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1258
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1259
/* Tell the library that communications will be suspended for some time. */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1260
/* RTU and ASCII versions ignore this function
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1261
 * TCP version closes all the open tcp connections (connections are automatically
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1262
 *   re-established the next time an IO function to the slave is requested).
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1263
 *   To be more precise, the TCP version makes an estimate of how long
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1264
 *   the silence will be based on previous invocations to this exact same
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1265
 *   function, and will only close the connections if this silence is
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1266
 *   expected to be longer than 1 second!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1267
 *   (The closing of connections is specified in Modbus specification)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1268
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1269
int mb_master_tcp_silence_init(void) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1270
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1271
  fprintf( stderr, "mb_master_silence_init():\n");
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1272
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1273
  /* call layer 1 library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1274
  return modbus_tcp_silence_init();
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1275
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1276
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
  1277