Implementation of PCCC protocol over EtherNet/IP
This commit is contained in:
parent
deb05e5cd6
commit
485d1d7f1a
|
@ -18,7 +18,7 @@
|
|||
//
|
||||
// This file has all the EtherNet/IP functions supported by the OpenPLC. If any
|
||||
// other function is to be added to the project, it must be added here
|
||||
// Thiago Alves, Apr 2019
|
||||
// Hannah Hanback, Sep 2019
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
#include <stdio.h>
|
||||
|
@ -32,41 +32,104 @@
|
|||
|
||||
#define ENIP_MIN_LENGTH 28
|
||||
|
||||
struct enip_header
|
||||
thread_local unsigned char enip_session[4]; // opens thread for session
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Obtains the Length in Item2_Length as variable type uint16_t
|
||||
// used to know the length of pccc data within Item2_Data
|
||||
// ENIP Type: Unknown
|
||||
//-----------------------------------------------------------------------------
|
||||
int getEnipType(struct enip_data_Unknown *enipDataUnknown, struct enip_header *header)
|
||||
{
|
||||
unsigned char *command;//[2];
|
||||
unsigned char *length;//[2];
|
||||
unsigned char *session_handle;//[4];
|
||||
unsigned char *status;//[4];
|
||||
unsigned char *sender_context;//[8];
|
||||
unsigned char *options;//[4];
|
||||
unsigned char *data;
|
||||
};
|
||||
|
||||
struct enip_data
|
||||
{
|
||||
unsigned char *interface_handle;
|
||||
unsigned char *timeout;
|
||||
unsigned char *item_count;
|
||||
|
||||
unsigned char *item1_id;
|
||||
unsigned char *item1_length;
|
||||
unsigned char *item1_data;
|
||||
|
||||
unsigned char *item2_id;
|
||||
unsigned char *item2_length;
|
||||
unsigned char *item2_data;
|
||||
};
|
||||
|
||||
|
||||
thread_local unsigned char enip_session[4];
|
||||
|
||||
int respondToError(unsigned char *buffer, int buffer_size, int error_code)
|
||||
{
|
||||
if (header->command[0] == 65 || header->command[0] == 0x70)
|
||||
{
|
||||
// ENIP Type Unnecessary for command execution
|
||||
// 0x65 --- register session
|
||||
return 0;
|
||||
}
|
||||
else if (enipDataUnknown->item1_id[0] == 0x81)
|
||||
{
|
||||
// PCCC type 1 - Unknown
|
||||
return 1;
|
||||
}
|
||||
else if (enipDataUnknown->item1_data[0] == 0xb2 && enipDataUnknown->item2_length[1] == 0x4b) // There is an offset of the bytes within the
|
||||
{ // Unconnected and Connected type data
|
||||
// PCCC type 2 - Unconnected Data Item // that is accounted for to check enipType // This means the labels "item1_data and item2_length
|
||||
return 2; // do not contain what is stated but what it would be for the correct type
|
||||
}
|
||||
else if (enipDataUnknown->item1_data[0] == 0xb2 && ( (enipDataUnknown->item2_length[1]==0x54) || (enipDataUnknown->item2_length[1]==0x4e) ) ) //0x54 opens connection
|
||||
{ //0x4e closes connection
|
||||
// PCCC type 3 - Connected Data Item
|
||||
return 3;
|
||||
}
|
||||
else if (enipDataUnknown->item1_id[0] == 0xa1)
|
||||
{
|
||||
// PCCC type 3 - Connected for 0x70 command SEND UNIT DATA
|
||||
return 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Unknown type ID
|
||||
// Respond with error_code
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
int parseEnipHeader(unsigned char *buffer, int buffer_size, struct enip_header *header)
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Obtains the Length in the Header Length as variable type uint16_t
|
||||
// used to know the length of CIP object data
|
||||
//-----------------------------------------------------------------------------
|
||||
uint16_t get_HeaderLength(struct enip_header *header)
|
||||
{
|
||||
uint16_t dataLength = ((uint16_t)header->length[1] << 8) | (uint16_t)header->length[0];
|
||||
return dataLength;
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Obtains the Length in Item2_Length as variable type uint16_t
|
||||
// used to know the length of pccc data within Item2_Data
|
||||
//-----------------------------------------------------------------------------
|
||||
uint16_t get_Item2_DataLength(struct enip_data_Unknown *enipDataUnknown)
|
||||
{
|
||||
uint16_t dataLength = ((uint16_t)enipDataUnknown->item2_length[1] << 8) | (uint16_t)enipDataUnknown->item2_length[0];
|
||||
return dataLength;
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Obtains the Length in Item2_Length as variable type uint16_t
|
||||
// used to know the length of pccc data within Item2_Data
|
||||
// ENIP Type: Unconnected
|
||||
//-----------------------------------------------------------------------------
|
||||
uint16_t get_Item2_DataLength_Unconnected(struct enip_data_Unconnected *enipDataUnconnected)
|
||||
{
|
||||
uint16_t dataLength = ((uint16_t)enipDataUnconnected->item2_length[1] << 8) | (uint16_t)enipDataUnconnected->item2_length[0];
|
||||
return dataLength;
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Obtains the Length in Item2_Length as variable type uint16_t
|
||||
// used to know the length of pccc data within Item2_Data
|
||||
// ENIP Type: Connected_0x70
|
||||
//-----------------------------------------------------------------------------
|
||||
uint16_t get_Item2_DataLength_Connected_0x70(struct enip_data_Connected_0x70 *enipDataConnected_0x70)
|
||||
{
|
||||
uint16_t dataLength = ((uint16_t)enipDataConnected_0x70->item2_length[1] << 8) | (uint16_t)enipDataConnected_0x70->item2_length[0];
|
||||
return dataLength;
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Parses the Header information into struct
|
||||
//-----------------------------------------------------------------------------
|
||||
int parseEnipHeader(unsigned char *buffer, int buffer_size, struct enip_header *header, struct enip_data_Unknown *enipDataUnknown)
|
||||
{
|
||||
//verify if message is big enough
|
||||
if (buffer_size < ENIP_MIN_LENGTH)
|
||||
|
@ -78,14 +141,6 @@ int parseEnipHeader(unsigned char *buffer, int buffer_size, struct enip_header *
|
|||
header->status = &buffer[8];
|
||||
header->sender_context = &buffer[12];
|
||||
header->options = &buffer[20];
|
||||
/*
|
||||
memcpy(header->command, &buffer[0], 2);
|
||||
memcpy(header->length, &buffer[2], 2);
|
||||
memcpy(header->session_handle, &buffer[4], 4);
|
||||
memcpy(header->status, &buffer[8], 4);
|
||||
memcpy(header->sender_context, &buffer[12], 8);
|
||||
memcpy(header->options, &buffer[20], 4);
|
||||
*/
|
||||
header->data = &buffer[24];
|
||||
|
||||
uint16_t enip_data_size = ((uint16_t)header->length[1] << 8) | (uint16_t)header->length[0];
|
||||
|
@ -97,24 +152,116 @@ int parseEnipHeader(unsigned char *buffer, int buffer_size, struct enip_header *
|
|||
return enip_data_size;
|
||||
}
|
||||
|
||||
int parseEnipData(struct enip_header *header, struct enip_data *data)
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Parses the ENIP data from the buffer
|
||||
// ENIP Type: UNKNOWN
|
||||
//-----------------------------------------------------------------------------
|
||||
void parseEnipUnknown(unsigned char *buffer, struct enip_data_Unknown *enipDataUnknown)
|
||||
{
|
||||
data->interface_handle = &header->data[0];
|
||||
data->timeout = &header->data[4];
|
||||
data->item_count = &header->data[6];
|
||||
data->item1_id = &header->data[8];
|
||||
data->item1_length = &header->data[10];
|
||||
data->item1_data = &header->data[12];
|
||||
enipDataUnknown->interface_handle = &buffer[24];
|
||||
enipDataUnknown->timeout = &buffer[28];
|
||||
enipDataUnknown->item_count = &buffer[30];
|
||||
|
||||
uint16_t item_length = ((uint16_t)data->item1_length[1] << 8) | (uint16_t)data->item1_length[0];
|
||||
enipDataUnknown->item1_id = &buffer[32];
|
||||
enipDataUnknown->item1_length = &buffer[34];
|
||||
enipDataUnknown->item1_data = &buffer[36];
|
||||
|
||||
data->item2_id = &header->data[12+item_length];
|
||||
data->item2_length = &header->data[14+item_length];
|
||||
data->item2_data = &header->data[16+item_length];
|
||||
|
||||
return 1;
|
||||
enipDataUnknown->item2_id = &buffer[37];
|
||||
enipDataUnknown->item2_length = &buffer[39];
|
||||
enipDataUnknown->item2_data = &buffer[41];
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Parses the ENIP data from the buffer
|
||||
// ENIP Type: Unconnected
|
||||
//-----------------------------------------------------------------------------
|
||||
void parseEnipUnconnected(unsigned char *buffer, struct enip_data_Unconnected *enipDataUnconnected)
|
||||
{
|
||||
enipDataUnconnected->interface_handle = &buffer[24];
|
||||
enipDataUnconnected->timeout = &buffer[28];
|
||||
enipDataUnconnected->item_count = &buffer[30];
|
||||
|
||||
enipDataUnconnected->item1_id = &buffer[32];
|
||||
enipDataUnconnected->item1_length = &buffer[34];
|
||||
|
||||
enipDataUnconnected->item2_id = &buffer[36];
|
||||
enipDataUnconnected->item2_length = &buffer[38];
|
||||
|
||||
enipDataUnconnected->service = &buffer[40]; //0x4b (Request)
|
||||
enipDataUnconnected->request_pathSize = &buffer[41];//[1]
|
||||
enipDataUnconnected->request_path = &buffer[42];//[4]
|
||||
enipDataUnconnected->requestor_idLength = &buffer[46];//[1]
|
||||
enipDataUnconnected->vendor_id = &buffer[47];//[2]
|
||||
enipDataUnconnected->serial_number = &buffer[49];//[4]
|
||||
enipDataUnconnected->data = &buffer[53];
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Parses the ENIP data from the buffer
|
||||
// ENIP Type: Connected
|
||||
//-----------------------------------------------------------------------------
|
||||
void parseEnipConnected(unsigned char *buffer, struct enip_data_Connected *enipDataConnected)
|
||||
{
|
||||
enipDataConnected->interface_handle = &buffer[24];//[4]
|
||||
enipDataConnected->timeout = &buffer[28];//[2]
|
||||
enipDataConnected->item_count = &buffer[30];//[2]
|
||||
|
||||
enipDataConnected->item1_id = &buffer[32];//[2]
|
||||
enipDataConnected->item1_length = &buffer[34];//[2]
|
||||
|
||||
enipDataConnected->item2_id = &buffer[36];//[2]
|
||||
enipDataConnected->item2_length = &buffer[38];//[2]
|
||||
|
||||
enipDataConnected->service = &buffer[40];//[1] 0x4b (Request)
|
||||
enipDataConnected->request_pathSize = &buffer[41];//[1] -----------size in words
|
||||
enipDataConnected->request_path = &buffer[42];//[4]
|
||||
enipDataConnected->actual_timeout = &buffer[46];//[2]
|
||||
enipDataConnected->o2t_netConnectID = &buffer[48];//[4]
|
||||
enipDataConnected->t2o_netConnectID = &buffer[52];//[4]
|
||||
enipDataConnected->connect_serialNo = &buffer[56];//[2]
|
||||
enipDataConnected->orig_vendorNo = &buffer[58];//[2]
|
||||
enipDataConnected->orig_serialNo = &buffer[60];//[4]
|
||||
enipDataConnected->timeout_multiplier = &buffer[64];//[1]
|
||||
enipDataConnected->reserved = &buffer[65];//[3]
|
||||
enipDataConnected->o2t_rpi = &buffer[68];//[4]
|
||||
enipDataConnected->o2t_netConnectParam = &buffer[72];//[2]
|
||||
enipDataConnected->t2o_rpi = &buffer[74];//[4]
|
||||
enipDataConnected->t2o_netConnectParam = &buffer[78];//[2]
|
||||
enipDataConnected->transport_trigger = &buffer[80];//[1]
|
||||
enipDataConnected->connection_pathSize = &buffer[81];//[1] ----- size in words
|
||||
enipDataConnected->connection_path = &buffer[82];//[?]
|
||||
}
|
||||
|
||||
|
||||
void parseEnipDataConnected_0x70(unsigned char *buffer, struct enip_data_Connected_0x70 *enipDataConnected_0x70)
|
||||
{
|
||||
enipDataConnected_0x70->interface_handle = &buffer[24];
|
||||
enipDataConnected_0x70->timeout = &buffer[28];
|
||||
enipDataConnected_0x70->item_count = &buffer[30];
|
||||
|
||||
enipDataConnected_0x70->item1_id = &buffer[32];
|
||||
enipDataConnected_0x70->item1_length = &buffer[34];
|
||||
enipDataConnected_0x70->connection_id = &buffer[36];
|
||||
|
||||
enipDataConnected_0x70->item2_id = &buffer[40];
|
||||
enipDataConnected_0x70->item2_length = &buffer[42];
|
||||
enipDataConnected_0x70->sequence_count = &buffer[44];
|
||||
|
||||
enipDataConnected_0x70->service = &buffer[46];
|
||||
enipDataConnected_0x70->request_pathSize = &buffer[47];
|
||||
enipDataConnected_0x70->request_path = &buffer[48];
|
||||
enipDataConnected_0x70->requestor_id = &buffer[52];
|
||||
enipDataConnected_0x70->pcccData = &buffer[59];
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Registers a ENIP Session
|
||||
// Command Code: 0x65
|
||||
//-----------------------------------------------------------------------------
|
||||
int registerEnipSession(struct enip_header *header)
|
||||
{
|
||||
unsigned char r[4];
|
||||
|
@ -130,6 +277,233 @@ int registerEnipSession(struct enip_header *header)
|
|||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// SendRRData
|
||||
// Receives a PCCC msg and Responds
|
||||
// Command Code: 0x65
|
||||
//-----------------------------------------------------------------------------
|
||||
int sendRRData(int enipType, struct enip_header *header, struct enip_data_Unknown *enipDataUnknown, struct enip_data_Unconnected *enipDataUnconnected, struct enip_data_Connected *enipDataConnected)
|
||||
{
|
||||
if (enipType == 1)
|
||||
{
|
||||
|
||||
//writeDataContents(enipDataUnknown);
|
||||
|
||||
uint16_t currentHeaderLength = get_HeaderLength(header); // get length of current stored size
|
||||
uint16_t currentPcccSize = get_Item2_DataLength(enipDataUnknown); // get length of stored pccc size
|
||||
|
||||
//change timeout value
|
||||
enipDataUnknown->timeout[0] = 0x00;
|
||||
enipDataUnknown->timeout[1] = 0x04;
|
||||
|
||||
//get pointer to beginning of pccc data to be passed
|
||||
unsigned char* pcccData = enipDataUnknown->item2_data;
|
||||
|
||||
//send pccc Data to pccc.cpp to be parsed and craft response
|
||||
// returns the new PCCC data size
|
||||
uint16_t newPcccSize = processPCCCMessage(pcccData, currentPcccSize);
|
||||
if (newPcccSize == -1)
|
||||
return -1; //error in PCCC.cpp
|
||||
|
||||
//change enipDataUnknown->item2_length to match new pccc data size
|
||||
enipDataUnknown->item2_length[0] = newPcccSize & 0xFF;
|
||||
enipDataUnknown->item2_length[1] = newPcccSize >> 8;
|
||||
|
||||
//calculate new header length size
|
||||
uint16_t len = currentHeaderLength - (currentPcccSize - newPcccSize);
|
||||
|
||||
//change header->length to match new enip data size
|
||||
header->length[0] = len & 0xFF;
|
||||
header->length[1] = len >> 8;
|
||||
|
||||
//calculate total size of enip response message in bytes
|
||||
uint16_t messageSize = len + 24;
|
||||
|
||||
return messageSize; // total message size in bytes
|
||||
}
|
||||
else if (enipType == 2)
|
||||
{
|
||||
//change timeout value
|
||||
enipDataUnconnected->timeout[0] = 0x00;
|
||||
enipDataUnconnected->timeout[1] = 0x04;
|
||||
|
||||
uint16_t currentHeaderLength = get_HeaderLength(header); // get length of current stored size
|
||||
uint16_t currentItem2Size = get_Item2_DataLength_Unconnected(enipDataUnconnected); // get length of stored pccc size
|
||||
|
||||
//get pointer to beginning of pccc data to be passed
|
||||
unsigned char* pcccData = enipDataUnconnected->data;
|
||||
|
||||
//send pccc Data to pccc.cpp to be parsed and craft response
|
||||
// returns the new PCCC data size
|
||||
uint16_t newPcccSize = processPCCCMessage(pcccData, currentItem2Size - 13); // get length of new pccc size
|
||||
if (newPcccSize == -1)
|
||||
return -1; //error in PCCC.cpp
|
||||
|
||||
//item2_length is the length of the PCCC Data + 11 (11 for number of bytes after item2_length excluding PCCC Data)
|
||||
uint16_t newItem2Size = 11 + newPcccSize;
|
||||
|
||||
//change enipDataUnconnected->item2_length to match new pccc data size
|
||||
enipDataUnconnected->item2_length[0] = newItem2Size & 0xFF;
|
||||
enipDataUnconnected->item2_length[1] = newItem2Size >> 8;
|
||||
|
||||
//calculate new header length size
|
||||
uint16_t newHeaderLength = currentHeaderLength - (currentItem2Size - newItem2Size);
|
||||
|
||||
//change header->length to match new enip data size
|
||||
header->length[0] = newHeaderLength & 0xFF;
|
||||
header->length[1] = newHeaderLength >> 8;
|
||||
|
||||
//change service 0x4b to 0xcb
|
||||
enipDataUnconnected->service[0] = 0xcb;
|
||||
|
||||
//change request path to 0
|
||||
enipDataUnconnected->request_pathSize[0] = 0x00;
|
||||
enipDataUnconnected->request_path[0] = 0x00;
|
||||
enipDataUnconnected->request_path[1] = 0x00;
|
||||
|
||||
//move data forward
|
||||
memmove(&enipDataUnconnected->request_path[2], enipDataUnconnected->requestor_idLength, newPcccSize + 7);//11);
|
||||
|
||||
//obtain total size of response message in bytes
|
||||
uint16_t messageSize = newHeaderLength + 24; // 24 is the static header size
|
||||
|
||||
return messageSize; // total message size in bytes
|
||||
}
|
||||
else if (enipType == 3)
|
||||
{
|
||||
//change timeout value
|
||||
enipDataConnected->timeout[0] = 0x00;
|
||||
enipDataConnected->timeout[1] = 0x04;
|
||||
|
||||
//change item2_length value (always 30?)
|
||||
enipDataConnected->item2_length[0] = 0x1e;
|
||||
enipDataConnected->item2_length[1] = 0x00;
|
||||
|
||||
//change service response 0x54->0xd4
|
||||
enipDataConnected->service[0] = 0xd4;
|
||||
|
||||
//change request path to 0
|
||||
enipDataConnected->request_pathSize[0] = 0x00;
|
||||
enipDataConnected->request_path[0] = 0x00;
|
||||
enipDataConnected->request_path[1] = 0x00;
|
||||
|
||||
// change o2t_netConnectID
|
||||
enipDataConnected->request_path[2] = 0x5a;
|
||||
enipDataConnected->request_path[3] = 0xf0;
|
||||
enipDataConnected->actual_timeout[0] = 0xb8;
|
||||
enipDataConnected->actual_timeout[1] = 0x01;
|
||||
|
||||
// start at the back and move up forward
|
||||
|
||||
// overwrite t2o_netConnectParam with response of reserved 0x00 00
|
||||
enipDataConnected->t2o_netConnectParam[0] = 0x00;
|
||||
enipDataConnected->t2o_netConnectParam[1] = 0x00;
|
||||
|
||||
// move up to overwrite o2t_netConnectParam
|
||||
memmove(&enipDataConnected->o2t_netConnectParam[0], enipDataConnected->t2o_rpi, 6); //6 = 80841e00 00 00
|
||||
|
||||
// move to overwrite timeout multiplier
|
||||
memmove(&enipDataConnected->timeout_multiplier[0], enipDataConnected->o2t_rpi, 10);//10 = 80841e00 80841e00 0000
|
||||
|
||||
// move to overwrite o2t_netConnectID
|
||||
memmove(&enipDataConnected->o2t_netConnectID[0], enipDataConnected->t2o_netConnectID, 22);
|
||||
|
||||
//change length inside header
|
||||
header->length[0] = 0x2e;
|
||||
header->length[1] = 0x00;
|
||||
|
||||
//calculate total size of response message in bytes
|
||||
// this will be length from header +24
|
||||
// uint16_t enip_dataSize = len + 24;
|
||||
uint16_t messageSize = 70;
|
||||
|
||||
return messageSize;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
// log error to openPLC
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// SendUnitData
|
||||
// Receives a PCCC msg and Responds
|
||||
// Command Code: 0x70
|
||||
//-----------------------------------------------------------------------------
|
||||
int sendUnitData(struct enip_header *header, struct enip_data_Connected_0x70 *enipDataConnected_0x70)
|
||||
{
|
||||
//change the service response 0x4b -> 0xcb
|
||||
enipDataConnected_0x70->service[0] = 0xcb;
|
||||
|
||||
//overwrite request path
|
||||
enipDataConnected_0x70->request_pathSize[0] = 0x00;
|
||||
enipDataConnected_0x70->request_path[0] = 0x00;
|
||||
enipDataConnected_0x70->request_path[1] = 0x00;
|
||||
|
||||
//get pointer to beginning of pccc data to be passed
|
||||
unsigned char* pcccData = enipDataConnected_0x70->pcccData;
|
||||
|
||||
int masked_write_check = 0;
|
||||
int write_check = 0;
|
||||
int write_check_2 = 0;
|
||||
if(pcccData[4] == 0xab)
|
||||
{
|
||||
masked_write_check = pcccData[5]; //Byte Size
|
||||
}
|
||||
|
||||
if(pcccData[4] == 0xaa)
|
||||
{
|
||||
//write_check = 2;
|
||||
write_check = 2;
|
||||
}
|
||||
if(pcccData[4] == 0xa2 && pcccData[7] == 0x8a)
|
||||
{
|
||||
write_check_2 = -6;
|
||||
}
|
||||
//if(pcccData[4] == 0xa2 && pcccData[7] == 0x89)
|
||||
//{
|
||||
//write_check_2 = -2;
|
||||
//}
|
||||
|
||||
//get the current Item2_Length
|
||||
uint16_t currentItem2Size = get_Item2_DataLength_Connected_0x70(enipDataConnected_0x70);
|
||||
|
||||
//calculate the currentPcccSize
|
||||
uint16_t currentPcccSize = abs(currentItem2Size - 15);
|
||||
|
||||
//send pccc Data to pccc.cpp to be parsed and craft response
|
||||
// returns the new PCCC data size
|
||||
uint16_t newPcccSize = processPCCCMessage(pcccData, currentPcccSize);
|
||||
if (newPcccSize == -1)
|
||||
return -1; //error in PCCC.cpp
|
||||
|
||||
//calculate Data Sizes
|
||||
uint16_t newItem2Size = newPcccSize + 13;
|
||||
uint16_t newHeaderSize = newPcccSize + newItem2Size + 14 + masked_write_check + write_check + write_check_2; // Use to be '+ 16' uint16_t newHeaderSize = newPcccSize + newItem2Size + 16;
|
||||
|
||||
//change item2_length to match new cip data size
|
||||
enipDataConnected_0x70->item2_length[0] = newItem2Size & 0xFF;
|
||||
enipDataConnected_0x70->item2_length[1] = newItem2Size >> 8;
|
||||
|
||||
//change header->length to match new enip data size
|
||||
header->length[0] = newHeaderSize & 0xFF;
|
||||
header->length[1] = newHeaderSize >> 8;
|
||||
|
||||
//move data forward
|
||||
memmove(&enipDataConnected_0x70->request_path[2], enipDataConnected_0x70->requestor_id, newPcccSize + 7);
|
||||
|
||||
//calculate total size of enip response message in bytes
|
||||
uint16_t messageSize = newHeaderSize + 24;
|
||||
|
||||
return messageSize; // total message size in bytes
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// This function must parse and process the client request and write back the
|
||||
// response for it. The return value is the size of the response message in
|
||||
|
@ -137,52 +511,86 @@ int registerEnipSession(struct enip_header *header)
|
|||
//-----------------------------------------------------------------------------
|
||||
int processEnipMessage(unsigned char *buffer, int buffer_size)
|
||||
{
|
||||
int error_code;
|
||||
// initialize logging system
|
||||
unsigned char log_msg[1000];
|
||||
unsigned char *p = log_msg;
|
||||
|
||||
// initailize structs
|
||||
struct enip_header header;
|
||||
struct enip_data_Unknown enipDataUnknown;
|
||||
struct enip_data_Unconnected enipDataUnconnected;
|
||||
struct enip_data_Connected enipDataConnected;
|
||||
struct enip_data_Connected_0x70 enipDataConnected_0x70;
|
||||
|
||||
error_code = parseEnipHeader(buffer, buffer_size, &header);
|
||||
if (error_code < 0)
|
||||
return respondToError(buffer, buffer_size, error_code);
|
||||
if (parseEnipHeader(buffer, buffer_size, &header, &enipDataUnknown) < 0)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
parseEnipUnknown(buffer, &enipDataUnknown);
|
||||
|
||||
// Register a Session
|
||||
if (header.command[0] == 0x65)
|
||||
return registerEnipSession(&header);
|
||||
|
||||
else if (header.command[0] == 0x6f)
|
||||
if (header.command[0] == 0x70) // Send Unit Data ---> works with Connected Type
|
||||
{
|
||||
struct enip_data data;
|
||||
error_code = parseEnipData(&header, &data);
|
||||
if (error_code < 0)
|
||||
return respondToError(buffer, buffer_size, error_code);
|
||||
|
||||
if (data.item2_id[0] == 0x91)
|
||||
{
|
||||
//PCCC type 1 - Unknown
|
||||
}
|
||||
else if (data.item2_id[0] == 0xb2)
|
||||
{
|
||||
//PCCC type 2 - Unconnected Data Item
|
||||
}
|
||||
else if (data.item1_id[0] == 0xa1 && data.item2_id[0] == 0xb1)
|
||||
{
|
||||
//PCCC type 3 - Connected Data Item
|
||||
}
|
||||
else
|
||||
{
|
||||
//Unknown type ID. Respond with error_code
|
||||
}
|
||||
parseEnipDataConnected_0x70(buffer, &enipDataConnected_0x70);
|
||||
uint16_t size = sendUnitData(&header, &enipDataConnected_0x70);
|
||||
return size; //sendUnitData()
|
||||
}
|
||||
|
||||
|
||||
//writeDataContents(&enipDataUnknown);
|
||||
|
||||
// select Enip type-----------------------------
|
||||
// 1 = UNKNOWN
|
||||
// 2 = Unconnected
|
||||
// 3 = Connected
|
||||
// -1 = ERROR: unsupported enip type------------
|
||||
int enipType = getEnipType(&enipDataUnknown, &header);
|
||||
|
||||
if (enipType == 2)
|
||||
{
|
||||
parseEnipUnconnected(buffer, &enipDataUnconnected);
|
||||
}
|
||||
else if (enipType == 3)
|
||||
{
|
||||
parseEnipConnected(buffer, &enipDataConnected);
|
||||
}
|
||||
else if (enipType < 0)
|
||||
{
|
||||
// log UNKNOWN Enip Type message to open plc
|
||||
sprintf(log_msg, "ENIP: Received unsupported EtherNet/IP Type\n");
|
||||
log(log_msg);
|
||||
}
|
||||
|
||||
//writeDataContents(&enipDataUnknown);
|
||||
|
||||
//if (header.command[0] == 0x65) // Register a Session
|
||||
// return registerEnipSession(&header);
|
||||
|
||||
if (header.command[0] == 0x6f) // Send RR Data
|
||||
{
|
||||
//writeDataContents(&enipDataUnknown);
|
||||
uint16_t size = sendRRData(enipType, &header, &enipDataUnknown, &enipDataUnconnected, &enipDataConnected);
|
||||
return size;
|
||||
}
|
||||
/*else if (header.command[0] == 0x70) // Send Unit Data ---> works with Connected Type
|
||||
{
|
||||
parseEnipDataConnected_0x70(buffer, &enipDataConnected_0x70);
|
||||
uint16_t size = sendUnitData(&header, &enipDataConnected_0x70);
|
||||
return size; //sendUnitData()
|
||||
}*/
|
||||
else
|
||||
{
|
||||
unsigned char log_msg[1000];
|
||||
unsigned char *p = log_msg;
|
||||
p += sprintf(p, "Unknown EtherNet/IP request: ");
|
||||
for (int i = 0; i < buffer_size; i++)
|
||||
{
|
||||
p += sprintf(p, "%02x ", (unsigned char)buffer[i]);
|
||||
}
|
||||
p += sprintf(p, "\n");
|
||||
printf(log_msg);
|
||||
log(log_msg);
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
|
|
@ -0,0 +1,126 @@
|
|||
//-----------------------------------------------------------------------------
|
||||
// Copyright 2019 Thiago Alves
|
||||
// This file is part of the OpenPLC Software Stack.
|
||||
//
|
||||
// OpenPLC is free software: you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation, either version 3 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// OpenPLC is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU General Public License
|
||||
// along with OpenPLC. If not, see <http://www.gnu.org/licenses/>.
|
||||
//------
|
||||
//
|
||||
// This file contains the structured used by enip.cpp to process
|
||||
// EtherNet/IP requests.
|
||||
// UAH, Sep 2019
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
|
||||
struct enip_header
|
||||
{
|
||||
unsigned char *command;//[2];
|
||||
unsigned char *length;//[2];
|
||||
unsigned char *session_handle;//[4];
|
||||
unsigned char *status;//[4];
|
||||
unsigned char *sender_context;//[8];
|
||||
unsigned char *options;//[4];
|
||||
unsigned char *data;
|
||||
};
|
||||
|
||||
struct enip_data_Unknown
|
||||
{
|
||||
unsigned char *interface_handle;//[4]
|
||||
unsigned char *timeout;//[2]
|
||||
unsigned char *item_count;//[2]
|
||||
|
||||
unsigned char *item1_id;//[2]
|
||||
unsigned char *item1_length;//[2]
|
||||
unsigned char *item1_data;//[1]
|
||||
|
||||
unsigned char *item2_id;//[2]
|
||||
unsigned char *item2_length;//[2]
|
||||
unsigned char *item2_data;
|
||||
};
|
||||
|
||||
|
||||
struct enip_data_Unconnected
|
||||
{
|
||||
unsigned char *interface_handle;//[4]
|
||||
unsigned char *timeout;//[2]
|
||||
unsigned char *item_count;//[2]
|
||||
|
||||
unsigned char *item1_id;//[2]
|
||||
unsigned char *item1_length;//[2]
|
||||
|
||||
unsigned char *item2_id;//[2]
|
||||
unsigned char *item2_length;//[2]
|
||||
|
||||
unsigned char *service;//[1] 0x4b (Request)
|
||||
unsigned char *request_pathSize;//[1]
|
||||
unsigned char *request_path;//[4]
|
||||
unsigned char *requestor_idLength;//[1]
|
||||
unsigned char *vendor_id;//[2]
|
||||
unsigned char *serial_number;//[4]
|
||||
unsigned char *data;
|
||||
};
|
||||
|
||||
|
||||
struct enip_data_Connected
|
||||
{
|
||||
unsigned char *interface_handle;//[4]
|
||||
unsigned char *timeout;//[2]
|
||||
unsigned char *item_count;//[2]
|
||||
|
||||
unsigned char *item1_id;//[2]
|
||||
unsigned char *item1_length;//[2]
|
||||
|
||||
unsigned char *item2_id;//[2]
|
||||
unsigned char *item2_length;//[2]
|
||||
|
||||
unsigned char *service;//[1] 0x4b (Request)
|
||||
unsigned char *request_pathSize;//[1] -----------size in words
|
||||
unsigned char *request_path;//[4]
|
||||
unsigned char *actual_timeout;//[2]
|
||||
unsigned char *o2t_netConnectID;//[4]
|
||||
unsigned char *t2o_netConnectID;//[4]
|
||||
unsigned char *connect_serialNo;//[2]
|
||||
unsigned char *orig_vendorNo;//[2]
|
||||
unsigned char *orig_serialNo;//[4]
|
||||
unsigned char *timeout_multiplier;//[1]
|
||||
unsigned char *reserved;//[3]
|
||||
unsigned char *o2t_rpi;//[4]
|
||||
unsigned char *o2t_netConnectParam;//[2]
|
||||
unsigned char *t2o_rpi;//[4]
|
||||
unsigned char *t2o_netConnectParam;//[2]
|
||||
unsigned char *transport_trigger;//[1]
|
||||
unsigned char *connection_pathSize;//[1] ----- size in words
|
||||
unsigned char *connection_path;//[?]
|
||||
};
|
||||
|
||||
struct enip_data_Connected_0x70
|
||||
{
|
||||
unsigned char *interface_handle;//[4]
|
||||
unsigned char *timeout;//[2]
|
||||
unsigned char *item_count;//[2]
|
||||
|
||||
unsigned char *item1_id;//[2] --- 0x a1 00
|
||||
unsigned char *item1_length;//[2] --- 0x 04 00
|
||||
unsigned char *connection_id;//[4]
|
||||
|
||||
unsigned char *item2_id;//[2] --- 0x b1 00
|
||||
unsigned char *item2_length;//[2] length in bytes after this byte
|
||||
unsigned char *sequence_count;//[2]
|
||||
|
||||
unsigned char *service;//[1] --- 0x4b
|
||||
unsigned char *request_pathSize;//[1] --- 0x 20 67 24 01
|
||||
unsigned char *request_path;//[4]
|
||||
unsigned char *requestor_id;//[7]
|
||||
unsigned char *pcccData;//[?]
|
||||
};
|
|
@ -25,6 +25,7 @@
|
|||
|
||||
#include <pthread.h>
|
||||
#include <stdint.h>
|
||||
#include "enipStruct.h" //This header file contains necessary structs for enip.cpp
|
||||
|
||||
#define MODBUS_PROTOCOL 0
|
||||
#define DNP3_PROTOCOL 1
|
||||
|
@ -143,6 +144,9 @@ void mapUnusedIO();
|
|||
//enip.cpp
|
||||
int processEnipMessage(unsigned char *buffer, int buffer_size);
|
||||
|
||||
//pccc.cpp ADDED Ulmer
|
||||
uint16_t processPCCCMessage(unsigned char *buffer, int buffer_size);
|
||||
|
||||
//modbus_master.cpp
|
||||
void initializeMB();
|
||||
void *querySlaveDevices(void *arg);
|
||||
|
|
|
@ -0,0 +1,579 @@
|
|||
//-----------------------------------------------------------------------------
|
||||
// Copyright 2019 Thiago Alves
|
||||
// This file is part of the OpenPLC Software Stack.
|
||||
//
|
||||
// OpenPLC is free software: you can redistribute it and/or modify
|
||||
// it under the terms of the GNU General Public License as published by
|
||||
// the Free Software Foundation, either version 3 of the License, or
|
||||
// (at your option) any later version.
|
||||
//
|
||||
// OpenPLC is distributed in the hope that it will be useful,
|
||||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// GNU General Public License for more details.
|
||||
//
|
||||
// You should have received a copy of the GNU General Public License
|
||||
// along with OpenPLC. If not, see <http://www.gnu.org/licenses/>.
|
||||
//------
|
||||
//
|
||||
// This file has all the PCCC functions supported by the OpenPLC. If any
|
||||
// other function is to be added to the project, it must be added here
|
||||
// UAH, Sep 2019
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
//------------Libraries-------------//
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#include <time.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "ladder.h"
|
||||
|
||||
//--------------------------------------------------------------Defines--------------------------------------------------------------------------------//
|
||||
|
||||
/*------------Maximum/Minimum Sizes for each buffer------------------*/
|
||||
#define MAX_DISCRETE_INPUT 1024 // Digital Inputs
|
||||
#define MAX_COILS 1024 // Digital Outputs
|
||||
#define MAX_HOLD_REGS 1024 // Pure registers Analog Outputs
|
||||
//#define MAX_INP_REGS 1024 // Analog Inputs
|
||||
|
||||
#define MIN_16B_RANGE 1024 //Holding Register Size 16bit (memory)
|
||||
#define MAX_16B_RANGE 2047 //Holding Register Size 16bit (memory)
|
||||
#define MAX_32B_RANGE 2047 //Holding Register Size 32bit (memory)
|
||||
|
||||
/*------------File Type for PCCC--------------*/
|
||||
#define PCCC_INPUT_LOGICAL_SLOT 0x8c
|
||||
#define PCCC_OUTPUT_LOGICAL_SLOT 0x8b
|
||||
#define PCCC_INTEGER 0x89
|
||||
#define PCCC_FLOATING_POINT 0x8A
|
||||
|
||||
#define PCCC_FN_OUTPUT 0x00
|
||||
#define PCCC_FN_INPUT 0x01
|
||||
#define PCCC_FN_INT 0x07
|
||||
#define PCCC_FN_FLOAT 0x08
|
||||
|
||||
/*----------------Define functions for bit/byte operations-------------------*/
|
||||
#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
|
||||
#define bitSet(value, bit) ((value) |= (1UL << (bit)))
|
||||
#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
|
||||
#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit))
|
||||
#define lowByte(w) ((unsigned char) ((w) & 0xff))
|
||||
#define highByte(w) ((unsigned char) ((w) >> 8))
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
/*-----PLC Buffers - These buffer store the contents from OpenPLC-----------*/
|
||||
IEC_BOOL pccc_discrete_input[MAX_DISCRETE_INPUT];
|
||||
IEC_BOOL pccc_coils[MAX_COILS];
|
||||
//IEC_UINT pccc_input_regs[MAX_INP_REGS];
|
||||
IEC_UINT pccc_holding_regs[MAX_HOLD_REGS];
|
||||
//-----------------------------------------------------------------------------------------------------------------------------------------------------//
|
||||
|
||||
thread_local int Pccc_MessageLength;
|
||||
|
||||
using namespace std;
|
||||
//-----------------------------------------------------------Structure Defines--------------------------------------------------//
|
||||
struct pccc_header //Structure for the Header Information for EthernetIP
|
||||
{
|
||||
unsigned char *Data;
|
||||
unsigned char *Data_Size;
|
||||
|
||||
unsigned char *HD_length = 5;//[5] -> Typical Header Length for Command; Response Header Length is 4
|
||||
unsigned char *HD_CMD_Code;//[1] -> Command Code
|
||||
unsigned char *HD_Status;//[1] -> Status Code
|
||||
unsigned char *HD_TransactionNum;//[2] -> Transaction Number
|
||||
unsigned char *HD_Data_Function_Code;//[1] -> Function code MSB
|
||||
unsigned char *HD_Ext_Status; //Ext Status -> only appended if Status = 0x0f[1]
|
||||
|
||||
unsigned char resp_cod_hex = 0x4f; //Response Hex Value
|
||||
unsigned char *RP_CMD_Code = &resp_cod_hex;//[1] -> Reply Command Code = 0x4f
|
||||
};
|
||||
|
||||
struct protected_logical_read_command //Struct for Reply and Command values of Read
|
||||
{
|
||||
unsigned char *CMD_Byte_Size;//[1]
|
||||
unsigned char *RP_EXT_Status;//[1]* -> Ext Status -> only appended if Status = 0x0f[1]
|
||||
};
|
||||
|
||||
struct protected_logical_write_command
|
||||
{
|
||||
unsigned char *CMD_Byte_Size;//[1]*
|
||||
unsigned char *RP_EXT_Status;//[1]* -> Ext Status -> only appended if Status = 0x0f[1]
|
||||
};
|
||||
//--------------------------------------------------------------------------------------------------------------------------------------//
|
||||
|
||||
//------------------------Function Declaration---------------------------------//
|
||||
|
||||
uint16_t Command_Protocol(pccc_header header,unsigned char *buffer, int buffer_size);
|
||||
uint16_t ParsePCCCData(unsigned char *buffer, int buffer_size);
|
||||
uint16_t Protected_Logical_Read_Reply(pccc_header, unsigned char *buffer, int buffer_size);
|
||||
uint16_t Protected_Logical_Write_Reply(pccc_header, unsigned char *buffer, int buffer_size);
|
||||
|
||||
void Pccc_ReadCoils(unsigned char *buffer, int buffer_size);
|
||||
void Pccc_WriteCoil(unsigned char *buffer, int buffer_size);
|
||||
void Pccc_ReadDiscreteInputs(unsigned char *buffer, int buffer_size);
|
||||
void Pccc_ReadHoldingRegisters(unsigned char *buffer, int buffer_size);
|
||||
//void Pccc_ReadInputRegisters(unsigned char *buffer, int buffer_size);
|
||||
void Pccc_WriteRegister(unsigned char *buffer, int buffer_size);
|
||||
|
||||
int word_pccc(unsigned char byte1, unsigned char byte2);
|
||||
int an_word_pccc(unsigned char byte1, unsigned char byte2);
|
||||
|
||||
//----------------------------------------------------------------------------//
|
||||
|
||||
//This function takes in the data from enip.cpp and places the data in the appropriate structure variables
|
||||
uint16_t processPCCCMessage(unsigned char *buffer, int buffer_size)
|
||||
{
|
||||
/* Variables */
|
||||
int new_pccc_length; //New PCCC Length
|
||||
pccc_header header;
|
||||
header.Data = buffer;
|
||||
header.Data_Size = buffer_size;
|
||||
|
||||
/*Determine the new pccc length*/
|
||||
new_pccc_length = ParsePCCCData(buffer,buffer_size);
|
||||
return new_pccc_length; //Return the length to enip.cpp
|
||||
}
|
||||
|
||||
uint16_t ParsePCCCData(unsigned char *buffer, int buffer_size)
|
||||
{
|
||||
/*Variables*/
|
||||
int new_pccc_length; //Variable for new PCCC length
|
||||
pccc_header header;
|
||||
|
||||
header.HD_CMD_Code = &buffer[0];//[1] -> Command Code
|
||||
header.HD_Status = &buffer[1];////[1] -> Status Code
|
||||
header.HD_TransactionNum = &buffer[2];//[2] -> Transaction Number
|
||||
header.HD_Data_Function_Code = &buffer[4];//[1] -> Data Function Code
|
||||
|
||||
/*Determine what command is being requested*/
|
||||
new_pccc_length = Command_Protocol(header,buffer,buffer_size);
|
||||
|
||||
return new_pccc_length; //Return the new pccc length
|
||||
}
|
||||
|
||||
/* Determine the Command that is being requested to execute */
|
||||
uint16_t Command_Protocol(pccc_header header, unsigned char *buffer, int buffer_size)
|
||||
{
|
||||
uint16_t var_pccc_length;
|
||||
|
||||
/*If Statement to determine the command code from the Command Packet*/
|
||||
if(((unsigned int)*header.HD_CMD_Code == 0x0f) && ((unsigned int)*header.HD_Data_Function_Code == 0xA2))//Protected Logical Read
|
||||
{
|
||||
var_pccc_length = Protected_Logical_Read_Reply(header,buffer,buffer_size);
|
||||
return var_pccc_length;
|
||||
}
|
||||
else if(((unsigned int)*header.HD_CMD_Code == 0x0f) && ( ((unsigned int)*header.HD_Data_Function_Code == 0xAA) || ((unsigned int)*header.HD_Data_Function_Code == 0xAB)))//Protected Logical Write
|
||||
{
|
||||
var_pccc_length = Protected_Logical_Write_Reply(header,buffer,buffer_size);
|
||||
return var_pccc_length;
|
||||
}
|
||||
else
|
||||
{
|
||||
/*initialize logging system*/
|
||||
unsigned char log_msg[1000];
|
||||
unsigned char *p = log_msg;
|
||||
sprintf(log_msg, "PCCC: Unsupportedd Command/Data Function Code!\n");
|
||||
log(log_msg);
|
||||
return -1;
|
||||
}//return length as -1 to signify that the CMD Code/Function Code was not recognize
|
||||
}
|
||||
|
||||
uint16_t Protected_Logical_Read_Reply(pccc_header header, unsigned char *buffer, int buffer_size)
|
||||
{
|
||||
/*Variables*/
|
||||
protected_logical_read_command protected_LR;
|
||||
protected_LR.CMD_Byte_Size = &buffer[5];//Byte Size of data to be read
|
||||
|
||||
/*Determining Data Length*/
|
||||
unsigned int len_resp = 4;
|
||||
len_resp = len_resp + (unsigned int)*protected_LR.CMD_Byte_Size;
|
||||
|
||||
/*check if the message is long enough- Left in for future error handling setup*/
|
||||
/*if (buffer_size < 8)
|
||||
{
|
||||
//PCCC Error Handling; Make sure that the buffer size is at least 8
|
||||
}*/
|
||||
|
||||
//****************** Read Coils **********************//
|
||||
if(buffer[6] == PCCC_FN_OUTPUT && buffer[7] == PCCC_OUTPUT_LOGICAL_SLOT) // Done/Tested
|
||||
{
|
||||
Pccc_ReadCoils(buffer, buffer_size);
|
||||
}
|
||||
|
||||
//*************** Read Discrete Inputs ***************//
|
||||
else if(buffer[6] == PCCC_FN_INPUT && buffer[7] == PCCC_INPUT_LOGICAL_SLOT)// Done/Tested
|
||||
{
|
||||
Pccc_ReadDiscreteInputs(buffer, buffer_size);
|
||||
}
|
||||
|
||||
//****************** Read Holding Registers[PURE, 16Bit Mem, 32bit MEM] ******************//
|
||||
else if((buffer[6] == PCCC_FN_INT || buffer[6] == PCCC_FN_FLOAT) && (buffer[7] == PCCC_INTEGER || buffer[7] == PCCC_FLOATING_POINT))//Done/Tested
|
||||
{
|
||||
Pccc_ReadHoldingRegisters(buffer, buffer_size);
|
||||
}
|
||||
else
|
||||
{
|
||||
unsigned char log_msg[1000];
|
||||
unsigned char *p = log_msg;
|
||||
sprintf(log_msg, "PCCC: Error occured while processing Protected Logical Read\n");
|
||||
log(log_msg);
|
||||
return -1;
|
||||
}//return length as -1 to signify that the CMD Code/Function Code was not recognize
|
||||
|
||||
/*Creating the reply packet and memcpy the data into the buffer*/
|
||||
memmove(&buffer[0], (unsigned int)header.RP_CMD_Code, 1); //0x4f Response Code
|
||||
memmove(&buffer[1], (unsigned int)header.HD_Status, 1); //Same from COMMAND REQUEST
|
||||
memmove(&buffer[2], (unsigned int)header.HD_TransactionNum, 2);//Same from COMMAND REQUEST
|
||||
|
||||
return len_resp; //Return the Resonse Packet Length for PCCC
|
||||
}
|
||||
|
||||
uint16_t Protected_Logical_Write_Reply(pccc_header header,unsigned char *buffer, int buffer_size) // Connected
|
||||
{
|
||||
/*Variables*/
|
||||
protected_logical_write_command protected_LW;
|
||||
protected_LW.CMD_Byte_Size = &buffer[5];//Byte Size of data to be read
|
||||
|
||||
/*Determining link of new PCCC Packet*/
|
||||
uint16_t len_resp = header.HD_length - 1;
|
||||
|
||||
/*Creating the reply packet and memcpy the data into the buffer*/
|
||||
memmove(&buffer[0], (unsigned int)header.RP_CMD_Code, 1);
|
||||
memmove(&buffer[1], (unsigned int)header.HD_Status, 1);
|
||||
memmove(&buffer[2], (unsigned int)header.HD_TransactionNum, 2);
|
||||
|
||||
/*check if the message is long enough- Left in for future error handling setup*/
|
||||
/*if (buffer_size < 8)
|
||||
{
|
||||
//PCCC Error Handling; Make sure that the buffer size is at least 8
|
||||
}*/
|
||||
|
||||
//****************** Write Coil **********************//
|
||||
if(buffer[6] == PCCC_FN_OUTPUT && buffer[7] == PCCC_OUTPUT_LOGICAL_SLOT)// Done/Tested
|
||||
{
|
||||
Pccc_WriteCoil(buffer, buffer_size);
|
||||
}
|
||||
|
||||
//****************** Write Register ******************//
|
||||
else if((buffer[6] == PCCC_FN_FLOAT || buffer[6] == PCCC_FN_INT) && (buffer[7] == PCCC_INTEGER || buffer[7] == PCCC_FLOATING_POINT))//Done/Tested
|
||||
{
|
||||
Pccc_WriteRegister(buffer, buffer_size);
|
||||
}
|
||||
|
||||
//****************** Function Code Error ******************/
|
||||
/*Left in for future error handling setup*/
|
||||
else
|
||||
{
|
||||
//PCCC Error Handling; Make sure that the buffer size is at least 8. If none of the defined File Type and File Numbers match, error unrecognized File Type and File Number.
|
||||
}
|
||||
return len_resp;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Concatenate two bytes into an int
|
||||
//-----------------------------------------------------------------------------
|
||||
int word_pccc(unsigned char byte1, unsigned char byte2)
|
||||
{
|
||||
int returnValue;
|
||||
returnValue = (int)(byte1) | (int)byte2;
|
||||
|
||||
return returnValue;
|
||||
}
|
||||
//-----------------------------------------------------------------------------
|
||||
// Concatenate two bytes into an int
|
||||
//-----------------------------------------------------------------------------
|
||||
int an_word_pccc(unsigned char byte1, unsigned char byte2)
|
||||
{
|
||||
int returnValue;
|
||||
returnValue = (int)(byte1) | (int)(byte2 << 8);
|
||||
|
||||
return returnValue;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Implementation of PCCC Read Coils
|
||||
//-----------------------------------------------------------------------------
|
||||
void Pccc_ReadCoils(unsigned char *buffer, int buffer_size) //Working QX Read
|
||||
{
|
||||
int Start, ByteDataLength, Mask;
|
||||
|
||||
/*check if the message is long enough- Left in for future error handling setup*/
|
||||
/*if (buffer_size < 10)
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); This Request must have at least 10 bytes. If it doesn't, its a corrupted message
|
||||
}*/
|
||||
|
||||
Start = word_pccc(buffer[8],buffer[9]); //Start based on the Element and Subelemnt values in the Command Packet
|
||||
Mask = log2( word_pccc(buffer[10],buffer[11]) ); //Save the byte size or byte data length to the variable from the command packet
|
||||
ByteDataLength = buffer[5];
|
||||
pthread_mutex_lock(&bufferLock);
|
||||
|
||||
/*----Reading the values from the PLC bool_output buffer and writing to the PCCC buffer based on position----*/
|
||||
for (int i = 0; i < ByteDataLength; i++)
|
||||
{
|
||||
for(int j = 0; j < 8; j++)
|
||||
{
|
||||
int position = Start + i * 8 + j;
|
||||
if (position < MAX_COILS)
|
||||
{
|
||||
if(bool_output[position/8][position%8] != NULL)
|
||||
{
|
||||
bitWrite(buffer[4+i], j, *bool_output[position/8][position%8]);
|
||||
}
|
||||
else
|
||||
{
|
||||
bitWrite(buffer[4+i],j,0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); If the position is greater than the MAX COILS, ERROR Overflow?
|
||||
}
|
||||
}
|
||||
}
|
||||
pthread_mutex_unlock(&bufferLock);
|
||||
|
||||
/*Left in for future error handling setup*/
|
||||
/*if (pccc_error != ERR_NONE)
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); Deetermine if there was an error:
|
||||
}*/
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Implementation of PCCC Read Discrete Inputs
|
||||
//-----------------------------------------------------------------------------
|
||||
void Pccc_ReadDiscreteInputs(unsigned char *buffer, int buffer_size) //Working IX Read Only
|
||||
{
|
||||
int Start, ByteDataLength;
|
||||
|
||||
/*This Request must have at least 10 bytes. If it doesn't, its a corrupted messageLeft in for future error handling setup*/
|
||||
/*if (buffer_size < 10)
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); This Request must have at least 10 bytes. If it doesn't, its a corrupted message
|
||||
}*/
|
||||
|
||||
Start = word_pccc(buffer[8],buffer[9]);//Start based on the Element and Subelemnt values in the Command Packet
|
||||
ByteDataLength = buffer[5];//Save the byte size or byte data length to the variable from the command packet
|
||||
pthread_mutex_lock(&bufferLock);
|
||||
|
||||
/*--------Reading the values from the PLC bool_input buffer and writing to the PCCC buffer based on position--------*/
|
||||
for (int i = 0; i < ByteDataLength; i++)
|
||||
{
|
||||
for(int j = 0; j < 8; j++)
|
||||
{
|
||||
int position = Start + i * 8 + j;
|
||||
if (position < MAX_DISCRETE_INPUT)
|
||||
{
|
||||
if(bool_input[position/8][position%8] != NULL)
|
||||
{
|
||||
bitWrite(buffer[4+i], j, *bool_input[position/8][position%8]);
|
||||
}
|
||||
else
|
||||
{
|
||||
bitWrite(buffer[4+i],j,0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); If the position is greater than the MAX, ERROR Overflow?
|
||||
}
|
||||
}
|
||||
}
|
||||
pthread_mutex_unlock(&bufferLock);
|
||||
|
||||
/*Left in for future error handling setup*/
|
||||
/*if (mb_error != ERR_NONE)
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); Deetermine if there was an error:
|
||||
}*/
|
||||
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Implementation of PCCC Read Holding Registers
|
||||
//-----------------------------------------------------------------------------
|
||||
void Pccc_ReadHoldingRegisters(unsigned char *buffer, int buffer_size) // QW Read
|
||||
{
|
||||
int Start, an_Start, WordDataLength, ByteDataLength;
|
||||
|
||||
/*this request must have at least 10 bytes. If it doesn't, it's a corrupted message - Left in for future error handling setup*/
|
||||
/*if (buffer_size < 10)
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); This Request must have at least 10 bytes. If it doesn't, its a corrupted message
|
||||
}*/
|
||||
|
||||
Start = word_pccc(buffer[8],buffer[9]);//Start based on the Element and Subelemnt values in the Command Packet
|
||||
ByteDataLength = buffer[5];//Save the byte size or byte data length to the variable from the command packet
|
||||
WordDataLength = ByteDataLength / 2;//Calculate the word data length based on the byte data length
|
||||
unsigned int Temp_FileT = buffer[7];//Value will be changed potentially during this process, save the File Type Value from command packet
|
||||
unsigned int Temp_FileN = buffer[6];//Value will be changed potentially during this process, save the File Number Value from command packet
|
||||
|
||||
/*asked for too many registers - Left in for future error handling setup*/
|
||||
/*if (ByteDataLength > 255)
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); This Request must have at greater than 255 bytes. If it does, its a corrupted message
|
||||
//return;
|
||||
}*/
|
||||
|
||||
pthread_mutex_lock(&bufferLock);
|
||||
/*--------Reading the values from the PLC int_output, int_memory, and dint_memory buffer and writing to the PCCC buffer based on position--------*/
|
||||
for(int i = 0; i < WordDataLength; i++)
|
||||
{
|
||||
int position = Start + i;
|
||||
//int an_position = an_Start + i;
|
||||
if ((position <= MIN_16B_RANGE) && (Temp_FileN == PCCC_FN_INT && Temp_FileT == PCCC_INTEGER))
|
||||
{
|
||||
if (int_output[position] != NULL)
|
||||
{
|
||||
buffer[ 4 + position * 2] = lowByte(*int_output[position]);
|
||||
buffer[5 + position * 2] = highByte(*int_output[position]);
|
||||
}
|
||||
else
|
||||
{
|
||||
buffer[ 4 + position * 2] = 0;
|
||||
buffer[5 + position * 2] = 0;
|
||||
}
|
||||
}
|
||||
//accessing memory
|
||||
//16-bit registers
|
||||
else if ((position >= MIN_16B_RANGE && position <= MAX_16B_RANGE) && (Temp_FileN == PCCC_FN_INT && Temp_FileT == PCCC_INTEGER))
|
||||
{
|
||||
if (int_memory[position - MIN_16B_RANGE] != NULL)
|
||||
{
|
||||
buffer[ 4 + position * 2] = lowByte(*int_memory[position - MIN_16B_RANGE]);
|
||||
buffer[5 + position * 2] = highByte(*int_memory[position - MIN_16B_RANGE]);
|
||||
}
|
||||
else
|
||||
{
|
||||
buffer[ 4 + position * 2] = 0;
|
||||
buffer[5 + position * 2] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
//32-bit registers
|
||||
else if (Temp_FileN == PCCC_FN_FLOAT && Temp_FileT == PCCC_FLOATING_POINT && (position % 2 == 0))
|
||||
{
|
||||
position = position/2;
|
||||
uint32_t tempValue = *dint_memory[position];
|
||||
|
||||
buffer[4+(4*position)] = tempValue;
|
||||
buffer[5+(4*position)] = tempValue >> 8;
|
||||
buffer[6+(4*position)] = tempValue >> 16;
|
||||
buffer[7+(4*position)] = tempValue >> 24;
|
||||
|
||||
}
|
||||
/*Left in for future error handling setup-Invalid Address*/
|
||||
else
|
||||
{
|
||||
//PCCC Error Handling (Fill in?); If none of the above are recognized, error
|
||||
}
|
||||
|
||||
}
|
||||
pthread_mutex_unlock(&bufferLock);
|
||||
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Implementation of PCCC Write Coil
|
||||
//-----------------------------------------------------------------------------
|
||||
void Pccc_WriteCoil(unsigned char *buffer, int buffer_size) //QX Write NEEDS WRITE MULTIPLE
|
||||
{
|
||||
int Start, Mask;
|
||||
int mask_offset = 0;
|
||||
|
||||
/*Left in for future error handling setup*/
|
||||
/*if(buffer_size < 10)
|
||||
{
|
||||
//ModbusError(buffer, ERR_ILLEGAL_DATA_ADDRESS);
|
||||
//return;
|
||||
}
|
||||
*/
|
||||
|
||||
/*For the Write Mask, there has to be a maskoffset due to an extra two bytes */
|
||||
if((unsigned int)buffer[4] == 0xAB)
|
||||
{
|
||||
mask_offset = buffer[5]; //Byte Size
|
||||
}
|
||||
|
||||
Start = word_pccc(buffer[8],buffer[9]);//Start based on the Element and Subelemnt values in the Command Packet
|
||||
Mask = log2( word_pccc(buffer[10],buffer[11]) );// Maskoffset based on the mask value in Masked Protected Write Command Packet
|
||||
|
||||
/*--------Determines if the values inside the PCCC data has a 1 or 0 in it. Writes that value to the bool_output based on the contents of the data in PCCC Buffer-------*/
|
||||
if(Start < MAX_COILS)
|
||||
{
|
||||
unsigned char value;
|
||||
if(word_pccc(buffer[10 + mask_offset],buffer[11 + mask_offset]) > 0)
|
||||
{
|
||||
value = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
value = 0;
|
||||
}
|
||||
pthread_mutex_lock(&bufferLock);
|
||||
if(bool_output[Start][Mask] != NULL)
|
||||
{
|
||||
*bool_output[Start][Mask] = value;
|
||||
}
|
||||
pthread_mutex_unlock(&bufferLock);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
// Implementation of PCCC Write Holding Register
|
||||
//-----------------------------------------------------------------------------
|
||||
void Pccc_WriteRegister(unsigned char *buffer, int buffer_size) // QW Write
|
||||
{
|
||||
int Start, WordDataLength, ByteDataLength;
|
||||
|
||||
Start = word_pccc(buffer[8],buffer[9]);//Start based on the Element and Subelemnt values in the Command Packet
|
||||
int an_Start = an_word_pccc(buffer[8],buffer[9]);//Different Start method for INTs based on the Element and Subelemnt values in the Command Packet
|
||||
ByteDataLength = buffer[5];//Save the byte size or byte data length to the variable from the command packet
|
||||
WordDataLength = ByteDataLength / 2;//Calculate the word data length based on the byte data length
|
||||
unsigned int Temp_FileT = buffer[7];//Value will be changed potentially during this process, save the File Type Value from command packet
|
||||
unsigned int Temp_FileN = buffer[6];//Value will be changed potentially during this process, save the File Number Value from command packet
|
||||
|
||||
pthread_mutex_lock(&bufferLock);
|
||||
|
||||
/*--------Determines if the values inside the PCCC data has data. Writes that value to the appropriate PLC Buffer based on the contents of the data in PCCC Buffer-------*/
|
||||
for(int i = 0; i < WordDataLength; i++)
|
||||
{
|
||||
int position = Start + i;
|
||||
//analog outputs
|
||||
if ((position <= MIN_16B_RANGE) && (Temp_FileN == PCCC_FN_INT && (Temp_FileT == PCCC_INTEGER)))
|
||||
{
|
||||
if (int_output[position] != NULL) *int_output[position] = an_word_pccc(buffer[10 + i], buffer[11 + i]);//look at this closer
|
||||
}
|
||||
//accessing memory
|
||||
//16-bit registers
|
||||
else if ((position >= MIN_16B_RANGE && position <= MAX_16B_RANGE) && (Temp_FileN == PCCC_FN_OUTPUT && (Temp_FileT == PCCC_INTEGER)))
|
||||
{
|
||||
if (int_memory[position - MIN_16B_RANGE] != NULL) *int_memory[position - MIN_16B_RANGE] = an_word_pccc(buffer[10 + i], buffer[11 + i]);//look at this closer
|
||||
}
|
||||
//32-bit registers
|
||||
if (Temp_FileN == PCCC_FN_FLOAT && (Temp_FileT == PCCC_FLOATING_POINT))
|
||||
{
|
||||
if (dint_memory[position] != NULL)
|
||||
{
|
||||
uint32_t tempValue = buffer[10 + i] | buffer[11 + i] << 8 | buffer[12 + i] << 16 | buffer[13 + i] <<24;//look at this closer
|
||||
|
||||
*dint_memory[position] = tempValue;
|
||||
|
||||
i += 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
pccc_holding_regs[position] = an_word_pccc(buffer[10 + i], buffer[11 + i]);//look at this closer might need to copy from temp
|
||||
}
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&bufferLock);
|
||||
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue