ModbusSlaveRTU.cpp

00001 /* Modbus RTU Slave Library
00002  * Copyright (c) 2015 Gabriel Rivas
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "ModbusSlaveRTU.h"
00018 #include "MessageQueue.h"
00019 
00020 #define MODBUS_FRAME_OVERHEAD_BYTES (5)
00021 #define MODBUS_ID_FUNCTION_SIZE (3)
00022 #define CRC_POSITION1 (m_frame.mByteCount + MODBUS_ID_FUNCTION_SIZE)
00023 #define CRC_POSITION2 (m_frame.mByteCount + MODBUS_ID_FUNCTION_SIZE + 1)
00024 
00025 ModbusSlaveRTU::ModbusSlaveRTU( uint8_t id,
00026                                 MessageQueue<uint8_t>* txQueue,
00027                                 MessageQueue<uint8_t>* rxQueue,
00028                                 ThreadSafeArray_t *coilRegisters,
00029                                 ThreadSafeArray_t *inputRegisters,
00030                                 ThreadSafeArray_t *holdingRegisters)
00031 {
00032 //   assert(txQueue != NULL);
00033     m_bTxQueue = txQueue;
00034 
00035 //    assert(rxQueue != NULL);
00036     m_bRxQueue = rxQueue;
00037 
00038     m_state = WAIT_REQUEST_MESSAGE;
00039 
00040     m_address = id;
00041     m_status = LISTENING;
00042 
00043 //   assert(coilRegisters != NULL);
00044     m_memoryMap.coilRegisters = coilRegisters;
00045 
00046 //   assert(inputRegisters != NULL);
00047     m_memoryMap.inputRegisters = inputRegisters;
00048 
00049 //   assert(holdingRegisters != NULL);
00050     m_memoryMap.holdingRegisters = holdingRegisters;
00051 }
00052 
00053 void ModbusSlaveRTU::trigger(void)
00054 {
00055     m_state = GET_RECEIVED_BYTES;
00056 }
00057 
00058 ModbusSlaveRTU::MbStatusTypes_t  ModbusSlaveRTU::getStatus(void)
00059 {
00060     return m_status;
00061 }
00062 
00063 void ModbusSlaveRTU::FSM(void)
00064 {
00065     uint32_t count;
00066 
00067     switch(m_state) {
00068         case WAIT_REQUEST_MESSAGE:
00069             m_status = LISTENING;
00070 
00071             break;
00072         case GET_RECEIVED_BYTES:
00073             count = m_bRxQueue->getWriteIndex();
00074             m_status = PROCESSING;
00075             if ((count > 0) && (count < MODBUS_MAX_LEN)) {
00076                 for (uint32_t i = 0; i < count; i++) {
00077                     uint8_t data = m_bRxQueue->read();
00078                     m_receivedMessage[i] = data;
00079                 }
00080                 m_state = BUILD_RESPONSE_MESSAGE;
00081                 m_bRxQueue->reset();
00082             } else {
00083                 m_state = WAIT_REQUEST_MESSAGE;
00084             }
00085             break;
00086         case BUILD_RESPONSE_MESSAGE:
00087             if (buildResponse() == NO_ERROR) {
00088                 m_status = SENDING;
00089                 m_state = TRANSMIT_RESPONSE_MESSAGE;
00090             } else {
00091                 m_status = LISTENING;
00092                 m_state = WAIT_REQUEST_MESSAGE;
00093             }
00094             break;
00095         case TRANSMIT_RESPONSE_MESSAGE:
00096             for (uint32_t i = 0; i < m_frame.mSize; i++) {
00097                 m_bTxQueue->write(m_responseMessage[i]);
00098             }
00099 
00100             m_status = LISTENING;
00101             m_state = WAIT_REQUEST_MESSAGE;
00102             break;
00103         default:
00104             break;
00105     }
00106 
00107 }
00108 
00109 ModbusSlaveRTU::MbReceivedMessageErrors_t ModbusSlaveRTU::buildResponse(void)
00110 {
00111     MbReceivedMessageErrors_t result = NO_ERROR;
00112 
00113     switch(m_receivedMessage[FUNCTION]) {
00114         case READ_COIL_STATUS:
00115             result = readCoilRegistersHandler();
00116             break;
00117         case READ_HOLDING_REGISTERS:
00118             result = readHoldingRegistersHandler();
00119             break;
00120         case FORCE_SINGLE_COIL:
00121             break;
00122         default:
00123             break;
00124     }
00125 
00126     return result;
00127 }
00128 
00129 uint8_t ModbusSlaveRTU::checkMessageCRC(uint8_t *  data)
00130 {
00131     uint8_t function = data[FUNCTION];
00132     uint8_t byteCount;
00133     uint16_t crcVal;
00134     uint16_t crcCalc;
00135     uint8_t result;
00136 
00137     if (function != READ_EXCEPTION_STATUS) {
00138         byteCount = 8;
00139         crcVal = data[byteCount-1];
00140         crcVal = (crcVal << 8) | data[byteCount - 2];
00141         crcCalc  = getCRC(data, byteCount - 2);
00142 
00143         if (crcVal == crcCalc) {
00144             result = 1;
00145         } else {
00146             result = 0;
00147         }
00148     } else {
00149         result = 0;
00150     }
00151 
00152     return result;
00153 }
00154 
00155 uint16_t ModbusSlaveRTU::getCRC(uint8_t *  data,uint8_t len)
00156 {
00157     uint16_t crc = 0xFFFF;
00158     uint8_t i;
00159 
00160     while (len--) {
00161         i = *data++;
00162         *(uint8_t *)&crc  ^= i;
00163 
00164         for (i = 0; i != 8; i++) {
00165             if (crc & 0x0001) {
00166                 crc = (crc >> 1);
00167                 crc ^= 0xA001;
00168             } else {
00169                 crc = (crc >> 1);
00170             }
00171         }
00172     }
00173     return crc;
00174 }
00175 
00176 ModbusSlaveRTU::MbReceivedMessageErrors_t ModbusSlaveRTU::readHoldingRegistersHandler(void)
00177 {
00178     uint8_t i;
00179     MbReceivedMessageErrors_t result = NO_ERROR;
00180 
00181     m_frame.mAddress = m_receivedMessage[ADDRESS];
00182     m_frame.mFunction  =  static_cast<MbFunctionCodes_t>(m_receivedMessage[FUNCTION]);
00183 
00184     result = checkReceivedMessageErrors(m_memoryMap.holdingRegisters);
00185 
00186     if (result < ADDRESS_ERROR) {
00187         m_frame.mByteCount = ((m_receivedMessage[REGISTER_COUNT] << 8) | (m_receivedMessage[REGISTER_COUNT + 1] & 0xFF));
00188         m_frame.mByteCount <<= 1;
00189 
00190 
00191         if ((m_frame.mByteCount + m_frame.mOffset)  > m_memoryMap.holdingRegisters->length)
00192             m_frame.mByteCount = m_memoryMap.holdingRegisters->length - m_frame.mOffset;
00193 
00194         (m_memoryMap.holdingRegisters)->mutex.lock();
00195 
00196         m_frame.mSize      = m_frame.mByteCount + MODBUS_FRAME_OVERHEAD_BYTES;
00197 
00198         for(i = 0; i < m_frame.mByteCount; i++) {
00199             m_responseMessage[i + DATA_START] = *((m_memoryMap.holdingRegisters)->data + i + m_frame.mOffset);
00200         }
00201 
00202         appendHeaderAndTailToMessage();
00203 
00204         (m_memoryMap.holdingRegisters)->mutex.unlock();
00205     }
00206 
00207     return result;
00208 }
00209 
00210 ModbusSlaveRTU::MbReceivedMessageErrors_t ModbusSlaveRTU::readCoilRegistersHandler(void)
00211 {
00212     MbReceivedMessageErrors_t result = NO_ERROR;
00213     uint16_t initRegister;
00214     uint16_t initCoil;
00215     uint16_t endRegister;
00216     uint16_t i = 0;
00217     uint16_t j = 0;
00218     uint8_t mask;
00219     uint8_t residual_bits;
00220 
00221     m_frame.mAddress = m_receivedMessage[ADDRESS];
00222     m_frame.mFunction  =  static_cast<MbFunctionCodes_t>(m_receivedMessage[FUNCTION]);
00223 
00224     result = checkReceivedMessageErrors(m_memoryMap.coilRegisters);
00225 
00226     if (result < ADDRESS_ERROR) {
00227         m_frame.mByteCount = ((m_receivedMessage[REGISTER_COUNT] << 8) | (m_receivedMessage[REGISTER_COUNT + 1] & 0xFF));
00228 
00229         residual_bits = (m_frame.mByteCount) % 8;
00230         m_frame.mByteCount >>= 3;
00231 
00232         if (residual_bits > 0)
00233             m_frame.mByteCount += 1;
00234 
00235         initRegister = m_frame.mOffset >> 3;
00236         initCoil = m_frame.mOffset % 8;
00237         endRegister = initRegister  + (m_frame.mByteCount);
00238 
00239         if (m_frame.mByteCount == 0)
00240             endRegister -= 1;
00241 
00242         mask = (1 << residual_bits) - 1;
00243 
00244         if ((m_frame.mByteCount + m_frame.mOffset)  > m_memoryMap.holdingRegisters->length)
00245             m_frame.mByteCount = m_memoryMap.holdingRegisters->length - m_frame.mOffset;
00246 
00247         m_frame.mSize      = m_frame.mByteCount + MODBUS_FRAME_OVERHEAD_BYTES;
00248 
00249         m_memoryMap.coilRegisters->mutex.lock();
00250 
00251         //Process coil data to generate the response message.
00252         for (i = initRegister; i < endRegister; i++) {
00253             m_responseMessage[j++ + DATA_START] = (m_memoryMap.coilRegisters->data[i] >> initCoil) |
00254                                                   (m_memoryMap.coilRegisters->data[i+1] << (8 - initCoil));
00255         }
00256 
00257         m_responseMessage[j + DATA_START] = (m_memoryMap.coilRegisters->data[i] >> initCoil) |
00258                                             (m_memoryMap.coilRegisters->data[i+1] << (8 - initCoil));
00259         m_responseMessage[j + DATA_START] &= mask;
00260 
00261         appendHeaderAndTailToMessage();
00262 
00263         m_memoryMap.coilRegisters->mutex.unlock();
00264     }
00265 
00266     return result;
00267 }
00268 
00269 void ModbusSlaveRTU::appendHeaderAndTailToMessage(void)
00270 {
00271     uint16_t mCRC;
00272 
00273     m_responseMessage[ADDRESS]     = m_frame.mAddress;
00274     m_responseMessage[FUNCTION]    = m_frame.mFunction;
00275     m_responseMessage[BYTE_COUNT]  = m_frame.mByteCount;
00276 
00277     mCRC = getCRC(m_responseMessage,m_frame.mByteCount + DATA_START);
00278 
00279     m_responseMessage[CRC_POSITION1] = (uint8_t) mCRC & 0xFF;
00280     m_responseMessage[CRC_POSITION2] = (uint8_t) (mCRC >> 8) & 0xFF;
00281 }
00282 
00283 ModbusSlaveRTU::MbReceivedMessageErrors_t ModbusSlaveRTU::checkReceivedMessageErrors(ThreadSafeArray_t *array)
00284 {
00285     MbReceivedMessageErrors_t result = NO_ERROR;
00286 
00287     //Check if Modbus Address is correct
00288     if (m_frame.mAddress != m_address) {
00289         result = ADDRESS_ERROR;
00290     }
00291 
00292     //Verify message errors
00293     if (checkMessageCRC(m_receivedMessage) == 0) {
00294         result = CRC_ERROR;
00295     }
00296 
00297     //Verify offset
00298     m_frame.mOffset    = ((m_receivedMessage[BYTE_OFFSET] << 8) | (m_receivedMessage[BYTE_OFFSET + 1] & 0xFF));
00299     if ((m_frame.mOffset > array->length - 1) ||
00300             (m_frame.mOffset > MODBUS_MAX_LEN-1)) {
00301         result = OFFSET_ERROR;
00302     }
00303 
00304     return result;
00305 }