mb_slave.c
author Mario de Sousa <msousa@fe.up.pt>
Tue, 10 Dec 2019 15:51:32 +0000
changeset 6 fe4088d5573a
parent 0 ae252e0fd9b8
child 8 f14859c24751
permissions -rw-r--r--
change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
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,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_slave.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
#include <fcntl.h>	/* File control definitions */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    27
#include <stdio.h>	/* Standard input/output */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    28
#include <string.h>
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    29
#include <stdlib.h>
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    30
#include <termio.h>	/* POSIX terminal control definitions */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    31
#include <sys/time.h>	/* Time structures for select() */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    32
#include <unistd.h>	/* POSIX Symbolic Constants */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    33
#include <errno.h>	/* Error definitions */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    34
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    35
#include <netinet/in.h> /* required for htons() and ntohs() */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    36
#include "mb_layer1.h"
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    37
#include "mb_slave.h"
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    38
#include "mb_slave_private.h"
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    39
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    40
/* #define DEBUG */		/* uncomment to see the data sent and received */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    41
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 modbus_write             fptr_[layer1_fin].modbus_write           
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    44
#define modbus_read              fptr_[layer1_fin].modbus_read            
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    45
#define modbus_init              fptr_[layer1_fin].modbus_init            
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    46
#define modbus_done              fptr_[layer1_fin].modbus_done            
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    47
#define modbus_connect           fptr_[layer1_fin].modbus_connect         
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    48
#define modbus_listen            fptr_[layer1_fin].modbus_listen          
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    49
#define modbus_close             fptr_[layer1_fin].modbus_close           
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    50
#define modbus_silence_init      fptr_[layer1_fin].modbus_silence_init    
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    51
#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
    52
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    53
/* 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
    54
/* 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
    55
#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
    56
                        if (fd < 0) {ttyfd = fd; layer1_fin = 0; /* use modbusTCP */}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    57
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) implementations will be adding some 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    69
 *  header and tail bytes (e.g. CRC) to the packet we build here. Since
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    70
 *  layer1 will re-use the same buffer allocated in this slave layer 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    71
 *  (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
    72
 *  we need to allocate more bytes than those strictly required for this
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    73
 *  slave layer. Therefore, the extra_bytes parameter.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    74
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    75
 *  Note that we add one more extra byte to the response buffer.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    76
 *  This is because some response packets will not be starting off
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    77
 *  at byte 0, but rather at byte 1 of the buffer. This is in order
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    78
 *  to guarantee that the data that is sent on the buffer is aligned
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    79
 *  on even bytes (the 16 bit words!). This will allow the application
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    80
 *  (layer above the one implemented in this file - i.e. the callback 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    81
 *  functions) to reference this memory as an u16 *, without producing
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    82
 *  'bus error' messages in some embedded devices that do not allow
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    83
 *   acessing u16 on odd numbered addresses.
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
static int buff_extra_bytes_;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    86
#define RESP_BUFFER_SIZE       (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    87
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
/**       Local Utility functions...     **/
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
/******************************************/
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    97
/*
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
    98
 * Function to determine next transaction id.
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
 * We use a library wide transaction id, which means that we
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   101
 * 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
   102
 * be sending the request...
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
static inline u16 next_transaction_id(void) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   105
  static u16 next_id = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   106
  return next_id++;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   107
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   108
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
/*
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   111
 * Functions to convert u16 variables
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   112
 * between network and host byte order
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   113
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   114
 * NOTE: Modbus uses MSByte first, just like
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   115
 *       tcp/ip, so we could be tempted to use the htons() and
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   116
 *       ntohs() functions to guarantee code portability.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   117
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   118
 *       However, on some embedded systems running Linux
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   119
 *       these functions only work if the 16 bit words are 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   120
 *       stored on even addresses. This is not always the 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   121
 *       case in our code, so we have to define our own
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   122
 *       conversion functions...
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
/* if using gcc, use it to determine byte order... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   126
#ifndef __BYTE_ORDER
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   127
#if defined(__GNUC__) 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   128
  /* We have GCC, which should define __LITTLE_ENDIAN__ */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   129
#  if defined(__LITTLE_ENDIAN__)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   130
#    define __BYTE_ORDER __LITTLE_ENDIAN
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   131
#  else
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   132
#    define __BYTE_ORDER __BIG_ENDIAN
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   133
#  endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   134
#endif /* __GNUC__ */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   135
#endif /* __BYTE_ORDER */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   136
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   137
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   138
/* If we still don't know byte order, try to get it from <sys/param.h> */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   139
#ifndef __BYTE_ORDER
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   140
#include <sys/param.h>
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   141
#endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   142
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   143
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   144
#ifndef __BYTE_ORDER
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   145
#  ifdef BYTE_ORDER
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   146
#   if BYTE_ORDER == LITTLE_ENDIAN
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   147
#    define __BYTE_ORDER __LITTLE_ENDIAN
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   148
#   else
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   149
#    if BYTE_ORDER == BIG_ENDIAN
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   150
#      define __BYTE_ORDER __BIG_ENDIAN
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   151
#    endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   152
#   endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   153
#  endif /* BYTE_ORDER */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   154
#endif /* __BYTE_ORDER */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   155
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   158
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   159
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   160
#ifdef __BYTE_ORDER
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   161
# if __BYTE_ORDER == __LITTLE_ENDIAN
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   162
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   163
/**************************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   164
/* u16 conversion functions to use on little endian platforms */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   165
/**************************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   166
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   167
/* NOTE: The input parameter must be the address 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   168
 *          of an u16 passed as a pointer to u8
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   169
 * 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   170
 *       We use u8 *ptr as input parameter and read both (ptr+0) and (ptr+1)
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   171
 *       instead of using u16 *ptr because we sometimes receive data in packtes
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   172
 *       that are not aligned on even addresses, so some compilers recognize that
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   173
 *       the given odd address cannot be used as a pointer to u16 and therefore 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   174
 *       adjust the pointer by (+1) or (-1), basicaly breacking our code!
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   175
 *       So, we revert to u8 pointers... for u16 values.
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   176
 */
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   177
static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   178
  u16_to_ptr[0] =  u16_from_ptr[1];
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   179
  u16_to_ptr[1] =  u16_from_ptr[0];
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   180
}
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   181
#define mb_ntoh(a, b) mb_hton(a, b)
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   182
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   183
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   184
static inline void mb_hton_count(u8 *u16_ptr, unsigned count) {
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   185
  unsigned i;
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   186
  for (i = 0; i < count*2; i += 2) {
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   187
    /* swap the bytes around... 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   188
     *  a = a ^ b;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   189
     *  b = a ^ b;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   190
     *  a = a ^ b;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   191
     */
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   192
    (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   193
    (u16_ptr+i)[1] ^= (u16_ptr+i)[0]; 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   194
    (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; 
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   195
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   196
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   197
#define mb_ntoh_count(w, count) mb_hton_count(w, count)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   198
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   199
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   200
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   201
# else
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   202
#  if __BYTE_ORDER == __BIG_ENDIAN
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   203
/***********************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   204
/* u16 conversion functions to use on big endian platforms */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   205
/***********************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   206
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   207
/* We don't need to swap the bytes around! */
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   208
static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   209
  u16_to_ptr[0] =  u16_from_ptr[0];
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   210
  u16_to_ptr[1] =  u16_from_ptr[1];
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   211
}
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   212
#define mb_ntoh(a, b) mb_hton(a, b)
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   213
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   214
#define mb_hton_count(w, count) /* empty ! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   215
#define mb_ntoh_count(w, count) /* empty ! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   216
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
#  else
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   219
/********************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   220
/* u16 conversion functions to use on generic platforms */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   221
/********************************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   222
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   223
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   224
/* We can't determine endiannes at compile time, so we do it at runtime.
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   225
 * With any luck the compiler will be able to determine the result of the
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   226
 * comparison at compile time and end up discarding the non-used code
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   227
 * and the 'if' itself from the final executable.
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   228
 */
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   229
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   230
static union {u16 u16;
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   231
              u8  u8[2];} endian_ = 0x0102;
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   232
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   233
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   234
static inline void mb_hton(u8 *u16_from_ptr, u8 *u16_to_ptr) {
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   235
  if (endian_.u8[0] == 0x01) {
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   236
    /* machine is big endian -> no swapping */    
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   237
    u16_to_ptr[0] =  u16_from_ptr[0];
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   238
    u16_to_ptr[1] =  u16_from_ptr[1];
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   239
  } else {
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   240
    /* machine is little endian -> we swap bytes around */    
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   241
    u16_to_ptr[0] =  u16_from_ptr[1];
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   242
    u16_to_ptr[1] =  u16_from_ptr[0];
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   243
  }
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   244
}
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   245
#define mb_ntoh(a, b) mb_hton(a, b)
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   246
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   247
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   248
static inline void mb_hton_count(u8 *u16_ptr, unsigned count) {
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   249
  unsigned i;
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   250
  
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   251
  if (endian_.u8[0] == 0x01)
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   252
      /* machine is big endian. Nothing to do */
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   253
      return;
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   254
  
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   255
  /* machine is little endian -> we swap bytes around */    
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   256
  for (i = 0; i < count*2; i += 2) {
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   257
    /* swap the bytes around... 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   258
     *  a = a ^ b;
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   259
     *  b = a ^ b;
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   260
     *  a = a ^ b;
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   261
     */
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   262
    (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   263
    (u16_ptr+i)[1] ^= (u16_ptr+i)[0]; 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   264
    (u16_ptr+i)[0] ^= (u16_ptr+i)[1]; 
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   265
  }
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   266
}
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   267
#define mb_ntoh_count(w, count) mb_hton_count(w, count)
0
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
#  endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   270
# endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   271
#endif /* __BYTE_ORDER */
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   278
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   279
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   280
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
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   283
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   284
/**                                           **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   285
/**    Handle requests from master/client     **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   286
/**                                           **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   287
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   288
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   289
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   290
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   291
/* Handle functions 0x01 and 0x02 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   292
typedef int (*read_bits_callback_t)(void *arg, u16 start_addr, u16 bit_count,  u8  *data_bytes);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   293
static int handle_read_bits (u8 *query_packet,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   294
                             u8 **resp_packet_ptr,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   295
                             u8 *error_code,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   296
                             read_bits_callback_t read_bits_callback,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   297
                             void *callback_arg
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   298
                            ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   299
  u16 start_addr, count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   300
  int res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   301
  u8 *resp_packet;
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
  /* If no callback, handle as if function is not supported... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   304
  if (read_bits_callback == NULL) 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   305
    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   306
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   307
  /* in oprder for the data in this packet to be aligned on even numbered addresses, this 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   308
   *  response packet will start off at an odd numbered byte...
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   309
   *  We therefore add 1 to the address where the packet starts.
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
  (*resp_packet_ptr)++;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   312
  resp_packet = *resp_packet_ptr;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   313
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   314
    /* NOTE:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   315
     *  Modbus uses high level addressing starting off from 1, but
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   316
     *  this is sent as 0 on the wire!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   317
     *  We could expect the user to specify high level addressing 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   318
     *   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
   319
     *   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
   320
     *   to correctly hold the address supplied by the user (which could
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   321
     *   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
   322
     *   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
   323
     *   is correct (i.e. <= 65536). 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   324
     *  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
   325
     *   requiring the user to use addressing starting off at 0! 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   326
     */
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   327
  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   328
  mb_ntoh(&(query_packet[4]), (u8 *)&count); 
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   329
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   330
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   331
  printf("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   332
          query_packet[0], query_packet[1], start_addr, count);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   333
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   334
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   335
  if ((count > MAX_READ_BITS) || (count < 1)) 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   336
    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   337
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   338
  /* Remember, we are using addressing starting off at 0, in the start_addr variable! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   339
  /*  This means that he highest acceptable address is 65535, when count=1 .... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   340
  /* Note the use of 65536 in the comparison will force automatic upgrade of u16 variables! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   341
  /*    => start_addr + count will nver overflow the u16 type!                              */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   342
  if (start_addr + count > 65536) 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   343
    {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   344
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   345
  /* start building response frame... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   346
  resp_packet[0] = query_packet[0]; /* slave */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   347
  resp_packet[1] = query_packet[1]; /* function (either 0x01 or 0x02 ! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   348
  resp_packet[2] = (count + 7) / 8; /* number of data bytes = ceil(count/8) */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   349
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   350
  res = read_bits_callback(callback_arg, start_addr, count, &(resp_packet[3]));
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   351
  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   352
  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
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
  return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   355
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   356
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   357
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
/* Handle function 0x01 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   360
int handle_read_output_bits  (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   361
  {return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_outbits, callbacks->arg);}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   362
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   363
/* Handle function 0x02 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   364
int handle_read_input_bits   (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   365
  {return handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_inbits, callbacks->arg);}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   366
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   367
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   370
/* Handle functions 0x03 and 0x04 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   371
typedef int (*read_words_callback_t)(void *arg, u16 start_addr, u16 word_count, u16 *data_words);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   372
static int handle_read_words (u8 *query_packet, 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   373
                              u8 **resp_packet_ptr, 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   374
                              u8 *error_code, 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   375
                              read_words_callback_t read_words_callback,
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   376
                              void *callback_arg
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   377
                             ) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   378
  u16 start_addr, count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   379
  int res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   380
  u8 *resp_packet;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   381
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   382
  /* If no callback, handle as if function is not supported... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   383
  if (read_words_callback == NULL) 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   384
    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   385
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   386
  /* See equivalent comment in handle_read_bits() */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   387
  (*resp_packet_ptr)++;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   388
  resp_packet = *resp_packet_ptr;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   389
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   390
  /* See equivalent comment in handle_read_bits() */ 
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   391
  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   392
  mb_ntoh(&(query_packet[4]), (u8 *)&count);
0
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
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   395
  printf("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n", 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   396
         query_packet[0], query_packet[1], start_addr, count);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   397
  #endif
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
  if ((count > MAX_READ_REGS) || (count < 1))
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   400
    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   401
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   402
  /* See equivalent comment in handle_read_bits() */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   403
  if (start_addr + count > 65536)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   404
    {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   405
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   406
  /* start building response frame... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   407
  resp_packet[0] = query_packet[0]; /* slave     */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   408
  resp_packet[1] = query_packet[1]; /* function code, either 0x03 or 0x04 !!!*/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   409
  resp_packet[2] = count * 2;       /* number of bytes of data... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   410
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   411
  res = read_words_callback(callback_arg, start_addr, count, (u16 *)&(resp_packet[3]));
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   412
  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   413
  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   414
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   415
  /* convert all data from host to network byte order. */
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   416
  mb_hton_count(&(resp_packet[3]), count);
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   417
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   418
  return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   421
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   422
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
/* Handle function 0x03 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   425
int handle_read_output_words (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   426
  {return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_outwords, callbacks->arg);}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   427
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   428
/* Handle function 0x04 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   429
int handle_read_input_words  (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   430
  {return handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_inwords, callbacks->arg);}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   431
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   432
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   433
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   434
/* Handle function 0x05 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   435
int handle_write_output_bit  (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   436
  u16 start_addr;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   437
  int res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   438
  u8 *resp_packet;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   439
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   440
  /* If no callback, handle as if function is not supported... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   441
  if (callbacks->write_outbits == NULL)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   442
    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   443
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   444
  resp_packet = *resp_packet_ptr;
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
  /* See equivalent comment in handle_read_bits() */ 
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   447
  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   448
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   449
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   450
  printf("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n", 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   451
         query_packet[0], query_packet[1], start_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   452
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   453
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   454
  // byte 5 Must be 0x00, byte 4 must be 0x00 or 0xFF !!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   455
  if ( (query_packet[5] != 0) || 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   456
      ((query_packet[4] != 0) && (query_packet[4] != 0xFF)))
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   457
    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   458
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   459
  /* Address will always be valid, no need to check! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   460
  // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   461
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   462
  /* start building response frame... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   463
  resp_packet[0] = query_packet[0]; /* slave */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   464
  resp_packet[1] = query_packet[1]; /* function */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   465
  resp_packet[2] = query_packet[2]; /* start address - hi byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   466
  resp_packet[3] = query_packet[3]; /* start address - lo byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   467
  resp_packet[4] = query_packet[4]; /* value: 0x00 or 0xFF */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   468
  resp_packet[5] = query_packet[5]; /* value: must be 0x00 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   469
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   470
  res = (callbacks->write_outbits)(callbacks->arg, start_addr, 1, &(query_packet[4]));
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   471
  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   472
  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   473
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   474
  return 6; /* response packet size, including slave id in byte 0 */
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
/* Handle function 0x06 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   480
int handle_write_output_word (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   481
  u16 start_addr;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   482
  int res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   483
  u8 *resp_packet;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   484
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   485
  /* If no callback, handle as if function is not supported... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   486
  if (callbacks->write_outwords == NULL)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   487
    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   488
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   489
  resp_packet = *resp_packet_ptr;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   490
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   491
  /* See equivalent comment in handle_read_bits() */ 
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   492
  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   493
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   494
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   495
  printf("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n", 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   496
         query_packet[0], query_packet[1], start_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   497
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   498
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   499
  /* Address will always be valid, no need to check! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   500
  // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   501
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   502
  /* start building response frame... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   503
  resp_packet[0] = query_packet[0]; /* slave           */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   504
  resp_packet[1] = query_packet[1]; /* function        */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   505
  resp_packet[2] = query_packet[2]; /* start address - hi byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   506
  resp_packet[3] = query_packet[3]; /* start address - lo byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   507
  resp_packet[4] = query_packet[4]; /* value - hi byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   508
  resp_packet[5] = query_packet[5]; /* value - lo byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   509
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   510
  /* convert data from network to host byte order */
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   511
  mb_ntoh_count(&(query_packet[4]), 1);
0
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
  res = (callbacks->write_outwords)(callbacks->arg, start_addr, 1, (u16 *)&(query_packet[4]));
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   514
  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   515
  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   516
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   517
  return 6; /* packet size is 6 -> slave, function, addr(2), value(2) */
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
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   522
/* Handle function 0x0F */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   523
int handle_write_output_bits (u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   524
  u16 start_addr, count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   525
  int res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   526
  u8 *resp_packet;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   527
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   528
  /* If no callback, handle as if function is not supported... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   529
  if (callbacks->write_outbits == NULL)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   530
    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
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
  resp_packet = *resp_packet_ptr;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   533
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   534
  /* See equivalent comment in handle_read_bits() */ 
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   535
  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   536
  mb_ntoh(&(query_packet[4]), (u8 *)&count); 
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   537
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   538
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   539
  printf("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n", 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   540
         query_packet[0], query_packet[1], start_addr, count);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   541
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   542
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   543
  if ((count > MAX_WRITE_COILS) || (count < 1) || ((count+7)/8 != query_packet[6]) )
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   544
    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   545
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   546
  /* See equivalent comment in handle_read_bits() */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   547
  if (start_addr + count > 65536)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   548
    {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
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
  /* start building response frame... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   551
  resp_packet[0] = query_packet[0]; /* slave */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   552
  resp_packet[1] = query_packet[1]; /* function */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   553
  resp_packet[2] = query_packet[2]; /* start address - hi byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   554
  resp_packet[3] = query_packet[3]; /* start address - lo byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   555
  resp_packet[4] = query_packet[4]; /* count - hi byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   556
  resp_packet[5] = query_packet[5]; /* count - lo byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   557
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   558
  res = (callbacks->write_outbits)(callbacks->arg, start_addr, count, &(query_packet[7]));
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   559
  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   560
  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   561
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   562
  return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   563
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   564
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   565
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   566
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
/* Handle function 0x10 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   569
int handle_write_output_words(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_t *callbacks) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   570
  u16 start_addr, count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   571
  int res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   572
  u8 *resp_packet;
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
  /* If no callback, handle as if function is not supported... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   575
  if (callbacks->write_outwords == NULL)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   576
    {*error_code = ERR_ILLEGAL_FUNCTION; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   577
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   578
  resp_packet = *resp_packet_ptr;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   579
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   580
  /* See equivalent comment in handle_read_bits() */ 
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   581
  mb_ntoh(&(query_packet[2]), (u8 *)&start_addr);
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   582
  mb_ntoh(&(query_packet[4]), (u8 *)&count); 
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   583
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   584
  if ((count > MAX_WRITE_REGS) || (count < 1) || (count*2 != query_packet[6]) )
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   585
    {*error_code = ERR_ILLEGAL_DATA_VALUE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   586
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   587
  /* See equivalent comment in handle_read_bits() */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   588
  if (start_addr + count > 65536)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   589
    {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   590
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   591
  /* start building response frame... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   592
  resp_packet[0] = query_packet[0]; /* slave           */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   593
  resp_packet[1] = query_packet[1]; /* function        */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   594
  resp_packet[2] = query_packet[2]; /* start address - hi byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   595
  resp_packet[3] = query_packet[3]; /* start address - lo byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   596
  resp_packet[4] = query_packet[4]; /* count - hi byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   597
  resp_packet[5] = query_packet[5]; /* count - lo byte */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   598
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   599
  /* convert all data from network to host byte order */
6
fe4088d5573a change hton() and ntoh() so we don't use (u8 *) to (u16 *) casts. Some compilers incorrectly change the pointer to force alignment with even addresses.
Mario de Sousa <msousa@fe.up.pt>
parents: 0
diff changeset
   600
  mb_ntoh_count(&(query_packet[7]), count);
0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   601
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   602
  res = (callbacks->write_outwords)(callbacks->arg, start_addr, count, (u16 *)&(query_packet[7]));
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   603
  if (res == -2) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   604
  if (res  <  0) {*error_code = ERR_SLAVE_DEVICE_FAILURE; return -1;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   605
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   606
  return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   607
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   608
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   611
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   612
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   613
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   614
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   615
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   616
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   617
/***********************************************/
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
/**    initialise / shutdown the library      **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   620
/**                                           **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   621
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   622
/***********************************************/
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
int mb_slave_init__(int extra_bytes) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   625
  buff_extra_bytes_ = extra_bytes;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   626
  return 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   627
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   628
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   629
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   630
int mb_slave_done__(void) 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   631
  {return 0;}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   632
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   633
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   634
#if 0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   635
int mb_slave_init(int nd_count) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   636
  int extra_bytes;
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
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   639
  fprintf( stderr, "mb_slave_init()\n");
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   640
  fprintf( stderr, "creating %d nodes\n", nd_count);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   641
  #endif
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
  /* initialise layer 1 library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   644
  if (modbus_init(nd_count, DEF_OPTIMIZATION, &extra_bytes) < 0)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   645
    goto error_exit_0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   646
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   647
  /* initialise this library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   648
  if (mb_slave_init__(extra_bytes) < 0)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   649
    goto error_exit_1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   650
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   651
  return 0;
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
error_exit_1:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   654
  modbus_done();
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   655
error_exit_0:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   656
  return -1;
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   660
int mb_slave_done(void) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   661
  mb_slave_done__(void)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   662
  return modbus_done();
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   663
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   664
#endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   665
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   666
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   667
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   668
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   669
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   670
/**                                           **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   671
/**        open/close slave connection        **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   672
/**                                           **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   673
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   674
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   675
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   676
/* Create a new slave/server */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   677
/* 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
   678
 *       layer1 implementation to use. 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   679
 *           0 -> TCP 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   680
 *           1 -> RTU 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   681
 *           2 -> ASCII 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   682
 *           4 -> unused 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   683
 *       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
   684
 *       before returning the node id to the caller!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   685
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   686
int mb_slave_new(node_addr_t node_addr) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   687
  int res = -1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   688
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   689
  fprintf( stderr, "mb_slave_connect()\n");
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   690
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   691
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   692
  /* call layer 1 library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   693
  switch(node_addr.naf) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   694
    case naf_tcp:  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   695
      res = modbus_tcp_listen(node_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   696
      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
   697
      return res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   698
    case naf_rtu:  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   699
      res = modbus_rtu_listen(node_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   700
      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
   701
      return res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   702
    case naf_ascii:  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   703
      res = modbus_ascii_listen(node_addr);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   704
      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
   705
      return res;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   706
  }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   707
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   708
  return -1;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   709
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   710
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   711
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   712
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   713
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   714
int mb_slave_close(int fd) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   715
  #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   716
  fprintf( stderr, "mb_slave_close(): nd = %d\n", fd);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   717
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   718
  get_ttyfd(); /* declare the ttyfd variable!! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   719
  /* call layer 1 library */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   720
  /* will call one of modbus_tcp_close(), modbus_rtu_close(), modbus_ascii_close() */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   721
  return modbus_close(ttyfd);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   722
}
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   723
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   724
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   725
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   726
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   727
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   728
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   729
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   730
/**                                           **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   731
/**               Run the slave               **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   732
/**                                           **/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   733
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   734
/***********************************************/
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   735
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   736
/* Execute infinite loop waiting and replying to requests coming from clients/master
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   737
 * This function enters an infinite loop wating for new connection requests, 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   738
 * and for modbus requests over previoulsy open connections...
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   739
 *
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   740
 * The frames are read from:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   741
 *   -  the node descriptor nd, if nd >= 0
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   742
 *       When using TCP, if the referenced node nd was created to listen for new connections
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   743
 *       [mb_slave_listen()], then this function will also reply to Modbus data requests arriving
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   744
 *       on other nodes that were created as a consequence of accepting connections requests to
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   745
 *       the referenced node nd.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   746
 *       All other nodes are ignored!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   747
 *       
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   748
 *   -  any valid and initialised TCP node descriptor, if nd = -1
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   749
 *      In this case, will also accept connection requests arriving from a previously
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   750
 *       created node to listen for new connection requests [mb_slave_listen() ].
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   751
 *      NOTE: (only avaliable if using TCP)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   752
 * 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   753
 * slaveid identifies the address (RTU and ASCII) or slaveid (TCP) that we implement.
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   754
 *     Any requests that we receive sent with a slaveid different
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   755
 *     than the one specified, and also different to 0, will be silently ignored!
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   756
 *     Whatever the slaveid specified, we always reply to requests
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   757
 *     to slaveid 0 (the modbus broadcast address).
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   758
 *     Calling this function with a slaveid of 0 means to ignore this 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   759
 *     parameter and to reply to all requests (whatever the slaveid
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   760
 *     used in the request). This should mostly be used by TCP servers... 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   761
 */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   762
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   763
int mb_slave_run(int fd, mb_slave_callback_t callback_functions, u8 slaveid) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   764
  int byte_count;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   765
  u16 transaction_id;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   766
  int nd;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   767
  u8 function, error_code = 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   768
  int resp_length;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   769
  u8 *query_packet = NULL;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   770
  u8 *resp_packet;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   771
  u8  resp_buffer_[RESP_BUFFER_SIZE];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   772
  u8  slave;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   773
  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   774
  get_ttyfd(); /* declare the ttyfd variable!! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   775
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   776
  #ifdef DEBUG  
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   777
  fprintf(stderr,"[%lu] mb_slave_run(): Called... fd=%d, ttyfd=%d\n", pthread_self(), fd, ttyfd);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   778
  #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   779
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   780
  while(1) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   781
    nd = ttyfd;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   782
    /* will call one of modbus_tcp_read(), modbus_rtu_read(), modbus_ascii_read() */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   783
    do {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   784
        byte_count = modbus_read(&nd,              /* node descriptor          */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   785
                                 &query_packet,    /* u8 **recv_data_ptr,      */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   786
                                 &transaction_id,  /* u16 *transaction_id,     */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   787
                                 NULL,             /* const u8 *send_data,     */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   788
                                 0,                /* int send_length,         */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   789
                                 NULL  /* wait indefenitely */ /* const struct timespec *recv_timeout); */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   790
                                );
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   791
    } while (byte_count <= 2);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   792
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   793
    #ifdef DEBUG
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   794
    {/* display the hex code of each character received */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   795
      int i;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   796
      printf("[%lu] mb_slave_run() received %d bytes (ptr=%p): \n", pthread_self(), byte_count, query_packet);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   797
      for (i=0; i < byte_count; i++)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   798
        printf("<0x%2X>", query_packet[i]);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   799
      printf("\n");
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   800
    }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   801
    #endif
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   802
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   803
    slave    = query_packet[0];
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   804
    function = query_packet[1];
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
    /* We only reply if:
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   807
     *       - request was sent to broadcast address   (slave   == 0)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   808
     *  OR   - we were asked to reply to every request (slaveid == 0)
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   809
     *  OR   - request matches the slaveid we were asked to accept (slave == slaveid)
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
     * Otherwise, silently ignore the received request!!!
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
    if ((slaveid == 0) || (slave == 0) || (slave == slaveid)) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   814
      resp_packet = resp_buffer_;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   815
      
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   816
      switch(function) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   817
        case 0x01: resp_length = handle_read_output_bits  (query_packet, &resp_packet, &error_code, &callback_functions); break;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   818
        case 0x02: resp_length = handle_read_input_bits   (query_packet, &resp_packet, &error_code, &callback_functions); break;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   819
        case 0x03: resp_length = handle_read_output_words (query_packet, &resp_packet, &error_code, &callback_functions); break;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   820
        case 0x04: resp_length = handle_read_input_words  (query_packet, &resp_packet, &error_code, &callback_functions); break;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   821
        case 0x05: resp_length = handle_write_output_bit  (query_packet, &resp_packet, &error_code, &callback_functions); break;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   822
        case 0x06: resp_length = handle_write_output_word (query_packet, &resp_packet, &error_code, &callback_functions); break;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   823
        case 0x0F: resp_length = handle_write_output_bits (query_packet, &resp_packet, &error_code, &callback_functions); break;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   824
        case 0x10: resp_length = handle_write_output_words(query_packet, &resp_packet, &error_code, &callback_functions); break;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   825
        /* return exception code 0x01 -> function not supported! */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   826
        default :  resp_length = -1; error_code = 0x01; break; 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   827
      }; /* switch(function) */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   828
      
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   829
      if (resp_length < 0) {
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   830
        /* return error... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   831
        /* build exception response frame... */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   832
        resp_packet = resp_buffer_;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   833
        resp_packet[0] = query_packet[0]; /* slave */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   834
        resp_packet[1] = query_packet[1] | 0x80; /* function code with error bit activated! */ 
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   835
        resp_packet[2] = error_code; 	
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   836
        resp_length = 3;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   837
      }
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   838
      modbus_write(nd, resp_packet, resp_length, transaction_id, NULL /*transmit_timeout*/);
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   839
    }; /* if not ignore request */
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   840
  }; /* while(1) */
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
  /* humour the compiler... */	
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   843
  return 0;
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   844
}
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
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
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   855
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   856
ae252e0fd9b8 Initial commit.
Mario de Sousa <msousa@fe.up.pt>
parents:
diff changeset
   857