first push
This commit is contained in:
1 parent
53c85e94af
commit
32f5ef9d3b
398 files changed
+1675
-140886
No files matched your search
+896
@@ -0,0 +1,896 @@
|
||||
#include <stdio.h>
|
||||
#include "loopback.h"
|
||||
#include "socket.h"
|
||||
#include "wizchip_conf.h"
|
||||
#include "modbusSlave.h"
|
||||
#include "modbus_crc.h"
|
||||
#include "usart.h"
|
||||
#include <string.h>
|
||||
#include "slmpMaster.h"
|
||||
#include "rtuMaster.h"
|
||||
#include "gpio.h"
|
||||
|
||||
#define DBID1_IDX 1000
|
||||
#define START1_IDX 1001
|
||||
#define SIZE1_IDX 1002
|
||||
#define TOMOD_IDX 1003
|
||||
#define DBID2_IDX 1005
|
||||
#define START2_IDX 1006
|
||||
#define SIZE2_IDX 1007
|
||||
#define FROMMOD_IDX 1008
|
||||
|
||||
#if LOOPBACK_MODE == LOOPBACK_MAIN_NOBLCOK
|
||||
|
||||
uint8_t loopback_tcps_modbus_rxbuf[1024];
|
||||
uint8_t loopback_tcps_modbus_txbuf[1024];
|
||||
|
||||
uint16_t comm_count = 0;
|
||||
|
||||
int32_t loopback_tcps_modbus(uint8_t sn, uint16_t port) {
|
||||
int32_t ret;
|
||||
uint16_t size = 0, sentsize = 0;
|
||||
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
uint8_t destip[4];
|
||||
uint16_t destport;
|
||||
#endif
|
||||
|
||||
switch (getSn_SR(sn)) {
|
||||
case SOCK_ESTABLISHED:
|
||||
|
||||
if (getSn_IR(sn) & Sn_IR_CON) {
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
getSn_DIPR(sn, destip);
|
||||
destport = getSn_DPORT(sn);
|
||||
|
||||
printf("%d:Connected - %d.%d.%d.%d : %d\r\n", sn, destip[0],
|
||||
destip[1], destip[2], destip[3], destport);
|
||||
#endif
|
||||
setSn_IR(sn, Sn_IR_CON);
|
||||
}
|
||||
if ((size = getSn_RX_RSR(sn)) > 0) // Don't need to check SOCKERR_BUSY because it doesn't not occur.
|
||||
{
|
||||
|
||||
if (size > DATA_BUF_SIZE)
|
||||
size = DATA_BUF_SIZE;
|
||||
ret = recv(sn, loopback_tcps_modbus_rxbuf, size);
|
||||
|
||||
if (ret <= 0)
|
||||
return ret; // check SOCKERR_BUSY & SOCKERR_XXX. For showing the occurrence of SOCKERR_BUSY.
|
||||
|
||||
uint16_t rxbuf_size = handleModbusTcp(loopback_tcps_modbus_rxbuf, loopback_tcps_modbus_txbuf);
|
||||
|
||||
if (rxbuf_size > 0) {
|
||||
sentsize = 0;
|
||||
|
||||
while (rxbuf_size != sentsize) {
|
||||
ret = send(sn, loopback_tcps_modbus_txbuf + sentsize, rxbuf_size - sentsize);
|
||||
if (ret < 0) {
|
||||
close(sn);
|
||||
return ret;
|
||||
}
|
||||
sentsize += ret; // Don't care SOCKERR_BUSY, because it is zero.
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
case SOCK_CLOSE_WAIT:
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
//printf("%d:CloseWait\r\n",sn);
|
||||
#endif
|
||||
if ((ret = disconnect(sn)) != SOCK_OK)
|
||||
return ret;
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
printf("%d:Socket Closed\r\n", sn);
|
||||
#endif
|
||||
break;
|
||||
case SOCK_INIT:
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
printf("%d:Listen, TCP server loopback, port [%d]\r\n", sn, port);
|
||||
#endif
|
||||
if ((ret = listen(sn)) != SOCK_OK)
|
||||
return ret;
|
||||
break;
|
||||
case SOCK_CLOSED:
|
||||
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
//printf("%d:TCP server loopback start\r\n",sn);
|
||||
#endif
|
||||
if ((ret = socket(sn, Sn_MR_TCP, port, 0x00)) != sn)
|
||||
return ret;
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
//printf("%d:Socket opened\r\n",sn);
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 将Modbus TCP转RTU
|
||||
uint16_t tcp_to_rtu(uint8_t *tcp_buf, uint8_t *rtu_buf) {
|
||||
uint16_t len = ((tcp_buf[4] << 8) | tcp_buf[5]) - 1; // 去掉UnitID前的长度
|
||||
memcpy(rtu_buf, &tcp_buf[6], len + 1); // 从UnitID开始拷贝
|
||||
uint16_t crc = crc16(rtu_buf, len + 1);
|
||||
rtu_buf[len + 1] = crc & 0xFF;
|
||||
rtu_buf[len + 2] = (crc >> 8) & 0xFF;
|
||||
return len + 3; // 返回总长度
|
||||
}
|
||||
|
||||
// 将Modbus RTU转TCP
|
||||
|
||||
uint16_t rtu_to_tcp(uint8_t *rtu_buf, uint8_t *tcp_buf, uint16_t rtu_len, uint8_t *rx_buf) {
|
||||
|
||||
uint16_t crc_calc = crc16(rtu_buf, rtu_len - 2);
|
||||
uint16_t crc_recv = rtu_buf[rtu_len - 2] | (rtu_buf[rtu_len - 1] << 8);
|
||||
if (crc_calc != crc_recv) {
|
||||
return 0; // CRC错误
|
||||
}
|
||||
|
||||
// 还原MBAP头
|
||||
memcpy(tcp_buf, rx_buf, 6); // Transaction ID, Protocol ID, Length先拷贝
|
||||
tcp_buf[6] = rtu_buf[0]; // UnitID
|
||||
memcpy(&tcp_buf[7], &rtu_buf[1], rtu_len - 3); // 功能码+数据
|
||||
|
||||
uint16_t pdu_len = rtu_len - 3; // PDU长度
|
||||
uint16_t mbap_len = pdu_len + 1; // **必须 +1 包含 UnitID**
|
||||
tcp_buf[4] = (mbap_len >> 8) & 0xFF;
|
||||
tcp_buf[5] = (mbap_len) & 0xFF;
|
||||
|
||||
return 7 + pdu_len; // MBAP(7) + PDU
|
||||
}
|
||||
|
||||
enum {
|
||||
STATE_IDLE, STATE_WAIT_RTU
|
||||
} modbus_state = STATE_IDLE;
|
||||
|
||||
int32_t loopback_tcps_modbus_to485(uint8_t sn, uint8_t *buf, uint16_t port, uint16_t *timeout, RtuRxST *rtu_rx) {
|
||||
int32_t ret;
|
||||
uint16_t size = 0, sentsize = 0;
|
||||
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
uint8_t destip[4];
|
||||
uint16_t destport;
|
||||
#endif
|
||||
|
||||
switch (getSn_SR(sn)) {
|
||||
case SOCK_ESTABLISHED:
|
||||
if (getSn_IR(sn) & Sn_IR_CON) {
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
getSn_DIPR(sn, destip);
|
||||
destport = getSn_DPORT(sn);
|
||||
|
||||
printf("%d:Connected - %d.%d.%d.%d : %d\r\n", sn, destip[0], destip[1], destip[2],
|
||||
destip[3], destport);
|
||||
#endif
|
||||
setSn_IR(sn, Sn_IR_CON);
|
||||
}
|
||||
|
||||
uint16_t rxbuf_size = 0;
|
||||
uint8_t sendbuf[1024];
|
||||
|
||||
switch (modbus_state) {
|
||||
|
||||
case STATE_WAIT_RTU:
|
||||
if (rtu_rx->done) {
|
||||
rxbuf_size = rtu_to_tcp(rtu_rx->buf, sendbuf, rtu_rx->size, buf);
|
||||
}
|
||||
|
||||
if (rtu_rx->done || *timeout > 1000) {
|
||||
rtu_rx->done = 0;
|
||||
modbus_state = STATE_IDLE;
|
||||
}
|
||||
|
||||
if ((size = getSn_RX_RSR(sn)) > 0) {
|
||||
ret = recv(sn, buf, size);
|
||||
if (ret <= 0) {
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE_IDLE:
|
||||
if ((size = getSn_RX_RSR(sn)) > 0) {
|
||||
if (size > DATA_BUF_SIZE)
|
||||
size = DATA_BUF_SIZE;
|
||||
ret = recv(sn, buf, size);
|
||||
|
||||
if (ret <= 0)
|
||||
return ret; // check SOCKERR_BUSY & SOCKERR_XXX. For showing the occurrence of SOCKERR_BUSY.
|
||||
|
||||
if (buf[6] == get_slaveid()) {
|
||||
rxbuf_size = handleModbusTcp(buf, sendbuf);
|
||||
} else {
|
||||
uint8_t rtu_tx[256];
|
||||
uint16_t rtu_tx_len = tcp_to_rtu(buf, rtu_tx);
|
||||
HAL_GPIO_WritePin(re485_GPIO_Port, re485_Pin, GPIO_PIN_SET);
|
||||
HAL_UART_Transmit_DMA(&huart1, rtu_tx, rtu_tx_len);
|
||||
*timeout = 0;
|
||||
rtu_rx->done = 0;
|
||||
modbus_state = STATE_WAIT_RTU;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (rxbuf_size > 0) {
|
||||
sentsize = 0;
|
||||
while (rxbuf_size != sentsize) {
|
||||
ret = send(sn, sendbuf + sentsize, rxbuf_size - sentsize);
|
||||
if (ret < 0) {
|
||||
close(sn);
|
||||
return ret;
|
||||
}
|
||||
sentsize += ret; // Don't care SOCKERR_BUSY, because it is zero.
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
case SOCK_CLOSE_WAIT:
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
//printf("%d:CloseWait\r\n",sn);
|
||||
#endif
|
||||
if ((ret = disconnect(sn)) != SOCK_OK)
|
||||
return ret;
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
printf("%d:Socket Closed\r\n", sn);
|
||||
#endif
|
||||
break;
|
||||
case SOCK_INIT:
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
printf("%d:Listen, TCP server loopback, port [%d]\r\n", sn, port);
|
||||
#endif
|
||||
if ((ret = listen(sn)) != SOCK_OK)
|
||||
return ret;
|
||||
break;
|
||||
case SOCK_CLOSED:
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
//printf("%d:TCP server loopback start\r\n",sn);
|
||||
#endif
|
||||
if ((ret = socket(sn, Sn_MR_TCP, port, 0x00)) != sn)
|
||||
return ret;
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
//printf("%d:Socket opened\r\n",sn);
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
void to_big_endian_16(uint16_t value, uint8_t *dest) {
|
||||
dest[0] = (uint8_t) (value >> 8); // 高字节
|
||||
dest[1] = (uint8_t) (value & 0xFF); // 低字节
|
||||
}
|
||||
|
||||
void to_big_endian_24(uint32_t value, uint8_t *dest) {
|
||||
dest[0] = (uint8_t) (value >> 16); // 最高字节
|
||||
dest[1] = (uint8_t) (value >> 8); // 次高字节
|
||||
dest[2] = (uint8_t) (value & 0xFF); // 最低字节
|
||||
}
|
||||
|
||||
uint8_t s7_rx_done = 1;
|
||||
uint8_t comm_step = 0;
|
||||
uint8_t read_write_flag = 0;
|
||||
|
||||
uint8_t sendbuf[1024];
|
||||
uint16_t sendbufsize;
|
||||
uint8_t comm1[] = { 0x03, 0x00, 0x00, 0x16, 0x11, 0xE0, 0x00, 0x00, 0x00, 0x01, 0x00, 0xC0, 0x01, 0x0A, 0xC1, 0x02, 0x01, 0x00, 0xC2, 0x02, 0x03, 0x02 };
|
||||
uint8_t comm2[] = { 0x03, 0x00, 0x00, 0x19, 0x02, 0xF0, 0x80, 0x32, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0xF0, 0x00, 0x00, 0x01, 0x00, 0x01,
|
||||
0x01, 0xE0 };
|
||||
uint8_t read[] = { 0x03, 0x00, 0x00, 0x1f, 0x02, 0xf0, 0x80, 0x32, 0x01, 0x00, 0x00, 0x02, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x04, 0x01, 0x12, 0x0A, 0x10, 0x02,
|
||||
0x00, 0x02, 0x00, 0x01, 0x84, 0x00, 0x00, 0x00 };
|
||||
uint8_t write[] = { 0x03, 0x00, 0x00, 0x29, 0x02, 0xf0, 0x80, 0x32, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x0e, 0x00, 0x0a, 0x05, 0x01, 0x12, 0x0a, 0x10, 0x02,
|
||||
0x00, 0x06, 0x00, 0x01, 0x84, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x30, 0x00, 0xff, 0x00, 0xff, 0x00, 0xff };
|
||||
uint16_t comm1_size = sizeof(comm1);
|
||||
uint16_t comm2_size = sizeof(comm2);
|
||||
uint16_t read_size = sizeof(read);
|
||||
uint16_t write_size = sizeof(write);
|
||||
|
||||
typedef union {
|
||||
uint8_t frame[31];
|
||||
struct {
|
||||
uint8_t padding1[23];
|
||||
uint8_t read_size[2];
|
||||
uint8_t db_id[2];
|
||||
uint8_t padding2[1];
|
||||
uint8_t start_address[3];
|
||||
|
||||
};
|
||||
} s7ReadFrame;
|
||||
|
||||
typedef union {
|
||||
uint8_t frame[1024];
|
||||
struct {
|
||||
uint8_t padding1[2];
|
||||
uint8_t frame_size[2];
|
||||
uint8_t padding2[11];
|
||||
uint8_t dataframe_size[2];
|
||||
uint8_t padding3[6];
|
||||
uint8_t data_size[2];
|
||||
uint8_t db_id[2];
|
||||
uint8_t padding4[1];
|
||||
uint8_t start_address[3];
|
||||
uint8_t padding5[2];
|
||||
uint8_t databyte_size_t8[2];
|
||||
uint8_t data[800];
|
||||
};
|
||||
} s7WriteFrame;
|
||||
|
||||
s7ReadFrame readframe = { .frame = { 0x03, 0x00, 0x00, 0x1f, 0x02, 0xf0, 0x80, 0x32, 0x01, 0x00, 0x00, 0x02, 0x00, 0x00, 0x0e, 0x00, 0x00, 0x04, 0x01, 0x12,
|
||||
0x0A, 0x10, 0x02, 0x00, 0x02, 0x00, 0x01, 0x84, 0x00, 0x00, 0x00 } };
|
||||
|
||||
s7WriteFrame writeframe = { .frame = { 0x03, 0x00, 0x00, 0x29, 0x02, 0xf0, 0x80, 0x32, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x0e, 0x00, 0x0a, 0x05, 0x01, 0x12,
|
||||
0x0a, 0x10, 0x02, 0x00, 0x06, 0x00, 0x01, 0x84, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } };
|
||||
|
||||
uint16_t prepare_write_frame(uint8_t *write, uint16_t writebyte_size) {
|
||||
if (writebyte_size == 0) {
|
||||
return 0;
|
||||
}
|
||||
uint16_t frame_size = 35 + writebyte_size;
|
||||
to_big_endian_16(frame_size, writeframe.frame_size);
|
||||
to_big_endian_16(4 + writebyte_size, writeframe.dataframe_size);
|
||||
to_big_endian_16(writebyte_size, writeframe.data_size);
|
||||
to_big_endian_16(get_register(DBID2_IDX), writeframe.db_id);
|
||||
to_big_endian_24(get_register(START2_IDX) * 8, writeframe.start_address);
|
||||
to_big_endian_16(writebyte_size * 8, writeframe.databyte_size_t8);
|
||||
memcpy(writeframe.data, write, writebyte_size);
|
||||
return frame_size;
|
||||
}
|
||||
|
||||
void print_list(uint8_t *buf, uint16_t size) {
|
||||
for (uint16_t i = 0; i < size; i++) {
|
||||
printf("0x%x ", buf[i]);
|
||||
}
|
||||
printf("\r\n");
|
||||
}
|
||||
|
||||
uint8_t write_to_s7[1024] = { 0 };
|
||||
|
||||
int32_t loopback_tcpc_s7(uint8_t sn, uint8_t *recvbuf, uint8_t *destip, uint16_t destport) {
|
||||
int32_t ret; // return value for SOCK_ERRORs
|
||||
uint16_t sentsize = 0;
|
||||
|
||||
// Port number for TCP client (will be increased)
|
||||
static uint16_t any_port = 50000;
|
||||
|
||||
// Socket Status Transitions
|
||||
// Check the W5500 Socket n status register (Sn_SR, The 'Sn_SR' controlled by Sn_CR command or Packet send/recv status)
|
||||
uint8_t sr_status = getSn_SR(sn);
|
||||
switch (sr_status) {
|
||||
case SOCK_ESTABLISHED:
|
||||
if (getSn_IR(sn) & Sn_IR_CON) // Socket n interrupt register mask; TCP CON interrupt = connection with peer is successful
|
||||
{
|
||||
#ifdef _LOOPBACK_DEBUG_S7_
|
||||
printf("%d:Connected to - %d.%d.%d.%d : %d\r\n", sn, destip[0], destip[1], destip[2], destip[3], destport);
|
||||
#endif
|
||||
setSn_IR(sn, Sn_IR_CON); // this interrupt should be write the bit cleared to '1'
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// Data Transaction Parts; Handle the [data receive and send] process
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
if ((s7_rx_done == 1 || client_wait_timeout == 0) && s7Timegap == 0) {
|
||||
sentsize = 0;
|
||||
if (comm_step == 0) {
|
||||
memcpy(sendbuf, comm1, comm1_size);
|
||||
sendbufsize = comm1_size;
|
||||
} else if (comm_step == 1) {
|
||||
memcpy(sendbuf, comm2, comm2_size);
|
||||
sendbufsize = comm2_size;
|
||||
} else {
|
||||
if (read_write_flag == 0) {
|
||||
read_write_flag = 1;
|
||||
// S7 > MODBUS
|
||||
if (get_register(SIZE1_IDX) == 0) {
|
||||
sendbufsize = 0;
|
||||
} else {
|
||||
to_big_endian_16(get_register(DBID1_IDX), readframe.db_id);
|
||||
to_big_endian_24(get_register(START1_IDX) * 8, readframe.start_address);
|
||||
to_big_endian_16(get_register(SIZE1_IDX) * 2, readframe.read_size);
|
||||
memcpy(sendbuf, readframe.frame, read_size);
|
||||
sendbufsize = read_size;
|
||||
}
|
||||
|
||||
} else {
|
||||
read_write_flag = 0;
|
||||
// MODBUS > S7
|
||||
get_register_range_to_u8(get_register(FROMMOD_IDX), get_register(SIZE2_IDX), write_to_s7);
|
||||
uint16_t write_size = prepare_write_frame(write_to_s7, (uint16_t) get_register(SIZE2_IDX) * 2);
|
||||
memcpy(sendbuf, writeframe.frame, write_size);
|
||||
sendbufsize = write_size;
|
||||
}
|
||||
}
|
||||
|
||||
if (sendbufsize != 0) {
|
||||
s7_rx_done = 0;
|
||||
client_wait_timeout = 2000;
|
||||
|
||||
} else {
|
||||
s7_rx_done = 1;
|
||||
client_wait_timeout = 50;
|
||||
}
|
||||
|
||||
#ifdef _LOOPBACK_DEBUG_S7_
|
||||
if (get_register(1900) > 0) {
|
||||
printf("TX step %d -> ", comm_step);
|
||||
if (sendbufsize > 0) {
|
||||
print_list(sendbuf, sendbufsize);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
while (sendbufsize != sentsize) {
|
||||
|
||||
s7Timegap = get_register(1030);
|
||||
|
||||
ret = send(sn, sendbuf + sentsize, sendbufsize - sentsize); // Data send process (User's buffer -> Destination through H/W Tx socket buffer)
|
||||
if (ret < 0) // Send Error occurred (sent data length < 0)
|
||||
{
|
||||
close(sn); // socket close
|
||||
return ret;
|
||||
}
|
||||
sentsize += ret; // Don't care SOCKERR_BUSY, because it is zero.
|
||||
|
||||
#ifdef _LOOPBACK_DEBUG_S7_
|
||||
if (get_register(1900) > 0) {
|
||||
printf("TX send return %ld -> ", ret);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (s7_rx_done == 0) {
|
||||
uint16_t clientrxsize = 0;
|
||||
if ((clientrxsize = getSn_RX_RSR(sn)) > 0) {
|
||||
int16_t ret = recv(sn, recvbuf, clientrxsize);
|
||||
#ifdef _LOOPBACK_DEBUG_S7_
|
||||
if (get_register(1900) > 0) {
|
||||
printf("RX step %d -> ", comm_step);
|
||||
print_list(recvbuf, clientrxsize);
|
||||
}
|
||||
|
||||
#endif
|
||||
if (ret > 0) {
|
||||
if (comm_step < 2) {
|
||||
comm_step++;
|
||||
} else {
|
||||
// 处理接收到的数据
|
||||
if (read_write_flag == 1 && clientrxsize >= 27) {
|
||||
uint16_t idx_in_modbus = get_register(TOMOD_IDX);
|
||||
for (uint16_t idx = 25; idx < clientrxsize; idx += 2) {
|
||||
if (get_register(1004) == 10) {
|
||||
set_register(idx_in_modbus++, (recvbuf[idx + 1]) | (recvbuf[idx] << 8));
|
||||
} else {
|
||||
set_register(idx_in_modbus++, (recvbuf[idx]) | (recvbuf[idx + 1] << 8));
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
s7_rx_done = 1;
|
||||
} else {
|
||||
// 处理错误或断开连接
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
break;
|
||||
|
||||
case SOCK_CLOSE_WAIT:
|
||||
#ifdef _LOOPBACK_DEBUG_S7_
|
||||
//printf("%d:CloseWait\r\n",sn);
|
||||
#endif
|
||||
if ((ret = disconnect(sn)) != SOCK_OK)
|
||||
return ret;
|
||||
#ifdef _LOOPBACK_DEBUG_S7_
|
||||
printf("%d:Socket Closed\r\n", sn);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case SOCK_INIT:
|
||||
#ifdef _LOOPBACK_DEBUG_S7_
|
||||
printf("%d:Try to connect to the %d.%d.%d.%d : %d\r\n", sn, destip[0], destip[1], destip[2], destip[3], destport);
|
||||
#endif
|
||||
if ((ret = connect(sn, destip, destport)) != SOCK_OK) {
|
||||
return ret; // Try to TCP connect to the TCP server (destination)
|
||||
}
|
||||
break;
|
||||
|
||||
case SOCK_CLOSED:
|
||||
comm_step = 0;
|
||||
close(sn);
|
||||
if ((ret = socket(sn, Sn_MR_TCP, any_port++, SF_IO_NONBLOCK)) != sn) {
|
||||
if (any_port == 0xffff)
|
||||
any_port = 50000;
|
||||
return ret; // TCP socket open with 'any_port' port number
|
||||
}
|
||||
#ifdef _LOOPBACK_DEBUG_S7_
|
||||
//printf("%d:TCP client loopback start\r\n",sn);
|
||||
//printf("%d:Socket opened\r\n",sn);
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int32_t loopback_udps(uint8_t sn, uint8_t *buf, uint16_t port) {
|
||||
int32_t ret;
|
||||
uint16_t size, sentsize;
|
||||
uint8_t destip[4];
|
||||
uint16_t destport;
|
||||
|
||||
switch (getSn_SR(sn)) {
|
||||
case SOCK_UDP:
|
||||
if ((size = getSn_RX_RSR(sn)) > 0) {
|
||||
if (size > DATA_BUF_SIZE)
|
||||
size = DATA_BUF_SIZE;
|
||||
ret = recvfrom(sn, buf, size, destip, (uint16_t*) &destport);
|
||||
if (ret <= 0) {
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
printf("%d: recvfrom error. %ld\r\n", sn, ret);
|
||||
#endif
|
||||
return ret;
|
||||
}
|
||||
size = (uint16_t) ret;
|
||||
sentsize = 0;
|
||||
while (sentsize != size) {
|
||||
ret = sendto(sn, buf + sentsize, size - sentsize, destip, destport);
|
||||
if (ret < 0) {
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
printf("%d: sendto error. %ld\r\n", sn, ret);
|
||||
#endif
|
||||
return ret;
|
||||
}
|
||||
sentsize += ret; // Don't care SOCKERR_BUSY, because it is zero.
|
||||
}
|
||||
}
|
||||
break;
|
||||
case SOCK_CLOSED:
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
//printf("%d:UDP loopback start\r\n",sn);
|
||||
#endif
|
||||
if ((ret = socket(sn, Sn_MR_UDP, port, 0x00)) != sn)
|
||||
return ret;
|
||||
#ifdef _LOOPBACK_DEBUG_
|
||||
printf("%d:Opened, UDP loopback, port [%d]\r\n", sn, port);
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
// ******************************* SLMP *******************************
|
||||
uint8_t slmp_txbuf[1024], slmp_rxbuf[1024];
|
||||
uint16_t read_address, read_size, write_address, write_size, read_idx_slmp = 0, write_idx_slmp = 0, curr_read_idx_slmp, curr_write_idx_slmp;
|
||||
|
||||
_Bool isReadRx(uint16_t clientrxsize) {
|
||||
return clientrxsize == read_size * 2 + 11 && memcmp(slmp_rxbuf, readRespHeader, 7) == 0;
|
||||
}
|
||||
|
||||
void mapReadRx() {
|
||||
|
||||
for (uint8_t i = 0; i < read_size; i++) {
|
||||
uint16_t thisValue = slmp_rxbuf[i * 2 + 11] | (slmp_rxbuf[i * 2 + 12] << 8);
|
||||
set_register(curr_read_idx_slmp * 20 + 1200 + i, thisValue);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
_Bool update_read() {
|
||||
curr_read_idx_slmp = read_idx_slmp;
|
||||
uint16_t slave_id = get_register(writeST[read_idx_slmp].slave_id);
|
||||
uint16_t plc_address = get_register(writeST[read_idx_slmp].plc_address);
|
||||
uint16_t size = get_register(writeST[read_idx_slmp].size);
|
||||
read_idx_slmp = (read_idx_slmp + 1) % 10;
|
||||
if (slave_id < 0x01 || slave_id > 0xF7 || size < 0x01 || size > 20)
|
||||
return 0;
|
||||
read_address = plc_address;
|
||||
read_size = size;
|
||||
return 1;
|
||||
}
|
||||
|
||||
_Bool update_write() {
|
||||
curr_write_idx_slmp = write_idx_slmp;
|
||||
uint16_t slave_id = get_register(readST[write_idx_slmp].slave_id);
|
||||
uint16_t plc_address = get_register(readST[write_idx_slmp].plc_address);
|
||||
uint16_t size = get_register(readST[write_idx_slmp].size);
|
||||
write_idx_slmp = (write_idx_slmp + 1) % 10;
|
||||
if (slave_id < 0x01 || slave_id > 0xF7 || size < 0x01 || size > 20)
|
||||
return 0;
|
||||
write_address = plc_address;
|
||||
write_size = size;
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint16_t slmpTimegap = 0;
|
||||
void update_timeout_slmp() {
|
||||
if (slmpTimegap < 0xffff) {
|
||||
slmpTimegap++;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t loopback_tcpc_slmp(uint8_t sn, uint8_t *destip, uint16_t destport) {
|
||||
int32_t ret = 0;
|
||||
|
||||
// Port number for TCP client (will be increased)
|
||||
static uint16_t any_port = 50000;
|
||||
|
||||
// Socket Status Transitions
|
||||
// Check the W5500 Socket n status register (Sn_SR, The 'Sn_SR' controlled by Sn_CR command or Packet send/recv status)
|
||||
uint8_t sr_status = getSn_SR(sn);
|
||||
switch (sr_status) {
|
||||
case SOCK_ESTABLISHED:
|
||||
|
||||
if (getSn_IR(sn) & Sn_IR_CON) // Socket n interrupt register mask; TCP CON interrupt = connection with peer is successful
|
||||
{
|
||||
setSn_IR(sn, Sn_IR_CON); // this interrupt should be write the bit cleared to '1'
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// Data Transaction Parts; Handle the [data receive and send] process
|
||||
//////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// 处理接受到的报文
|
||||
if (slmp_master_status == readySend) {
|
||||
uint16_t clientrxsize = getSn_RX_RSR(sn);
|
||||
if (clientrxsize > 0) {
|
||||
|
||||
ret = recv(sn, slmp_rxbuf, clientrxsize);
|
||||
if (ret > 0) {
|
||||
comm_count++;
|
||||
if (isReadRx(clientrxsize)) {
|
||||
mapReadRx();
|
||||
slmp_master_status = slmp_readRxDone;
|
||||
} else {
|
||||
slmp_master_status = slmp_writeRxDone;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 准备需要发送的报文
|
||||
|
||||
uint16_t sendbufsize = 0;
|
||||
|
||||
if (slmpTimegap > 1000 || slmp_master_status != readySend) {
|
||||
slmpTimegap = 0;
|
||||
switch (slmp_master_status) {
|
||||
case slmp_writeRxDone:
|
||||
if (update_read()) {
|
||||
slmp_master_status = readySend;
|
||||
|
||||
if (read_size <= 20 && (read_address + read_size) < 8000) {
|
||||
sendbufsize = handleSMLP_ReadTX(read_address, read_size);
|
||||
memcpy(slmp_txbuf, frame.frameU8, 21);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
case slmp_readRxDone:
|
||||
if (update_write()) {
|
||||
slmp_master_status = readySend;
|
||||
|
||||
if (write_size <= 20 && (write_address + write_size) < 8000) {
|
||||
sendbufsize = handleSLMP_WriteTX(write_address, write_size);
|
||||
memcpy(slmp_txbuf, frame.frameU8, 21);
|
||||
for (uint8_t i = 0; i < write_size; i++) {
|
||||
uint16_t val = get_register(curr_write_idx_slmp * 20 + 1000 + i);
|
||||
slmp_txbuf[i * 2 + 21] = val & 0xFF;
|
||||
slmp_txbuf[i * 2 + 22] = (val >> 8) & 0xFF;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
default:
|
||||
slmp_master_status = slmp_writeRxDone;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 发送该报文
|
||||
uint16_t sentsize = 0;
|
||||
|
||||
while (sendbufsize != sentsize) {
|
||||
// Data send process (User's buffer -> Destination through H/W Tx socket buffer)
|
||||
ret = send(sn, slmp_txbuf + sentsize, sendbufsize - sentsize);
|
||||
if (ret < 0) // Send Error occurred (sent data length < 0)
|
||||
{
|
||||
close(sn); // socket close
|
||||
return ret;
|
||||
}
|
||||
sentsize += ret; // Don't care SOCKERR_BUSY, because it is zero.
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case SOCK_CLOSE_WAIT:
|
||||
if ((ret = disconnect(sn)) != SOCK_OK)
|
||||
return ret;
|
||||
break;
|
||||
|
||||
case SOCK_INIT:
|
||||
if ((ret = connect(sn, destip, destport)) != SOCK_OK) {
|
||||
return ret; // Try to TCP connect to the TCP server (destination)
|
||||
}
|
||||
break;
|
||||
|
||||
case SOCK_CLOSED:
|
||||
|
||||
close(sn);
|
||||
if ((ret = socket(sn, Sn_MR_TCP, any_port++, SF_IO_NONBLOCK)) != sn) {
|
||||
if (any_port == 0xffff)
|
||||
any_port = 50000;
|
||||
return ret; // TCP socket open with 'any_port' port number
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/////////////////FINS start
|
||||
|
||||
uint8_t myRtumSTIdx = 0;
|
||||
uint8_t omrfins_buf[1024];
|
||||
|
||||
uint8_t readrx[14] = { 0xc0, 0x00, 0x02, 0x00, 0x0A, 0x00, 0x00, 0x01, 0x00, 0x00, 0x01, 0x01, 0x00, 0x40 };
|
||||
_Bool handleReadRecv(uint16_t size, uint16_t local_node) {
|
||||
readrx[4] = local_node;
|
||||
for (int i = 0; i < 14; i++) {
|
||||
if (readrx[i] != omrfins_buf[i]) {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
uint16_t lastMyRtumSTIdx;
|
||||
if (myRtumSTIdx == 0) {
|
||||
lastMyRtumSTIdx = 19;
|
||||
} else {
|
||||
lastMyRtumSTIdx = myRtumSTIdx - 1;
|
||||
}
|
||||
|
||||
uint16_t thisSize = get_register(myRtumST[lastMyRtumSTIdx].size);
|
||||
|
||||
if (thisSize * 2 + 14 != size) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (int i = 0; i < thisSize; i++) {
|
||||
uint8_t byte_h = omrfins_buf[i * 2 + 14];
|
||||
uint8_t byte_l = omrfins_buf[i * 2 + 15];
|
||||
uint16_t value = ((uint16_t) byte_h << 8) | byte_l;
|
||||
set_register(1400 + (20 * lastMyRtumSTIdx) + i, value);
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint8_t send_read[18] = { 0x80, 0x00, 0x02, 0x00, 0x01, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x01, 0x01, 0x82, 0x00, 0x64, 0x00, 0x00, 0x0a };
|
||||
uint8_t write_send[58] = { 0x80, 0x00, 0x02, 0x00, 0x01, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x01, 0x02, 0x82, 0x00, 0x64, 0x00, 0x00, 0x02 };
|
||||
uint16_t handleSend(uint16_t local_node) {
|
||||
uint16_t sendsize = 0;
|
||||
uint16_t slave_id = get_register(myRtumST[myRtumSTIdx].slave_id);
|
||||
uint16_t plc_address = get_register(myRtumST[myRtumSTIdx].plc_address);
|
||||
uint16_t size = get_register(myRtumST[myRtumSTIdx].size);
|
||||
|
||||
if (slave_id == 0 || slave_id >= 127 || size > 20 || size == 0) {
|
||||
sendsize = 0;
|
||||
} else {
|
||||
if (myRtumSTIdx % 2 == 1) {
|
||||
send_read[7] = local_node;
|
||||
send_read[13] = (plc_address >> 8) & 0xFF;
|
||||
send_read[14] = plc_address & 0xFF;
|
||||
send_read[17] = size;
|
||||
sendsize = 18;
|
||||
for (int i = 0; i < sendsize; i++) {
|
||||
omrfins_buf[i] = send_read[i];
|
||||
}
|
||||
|
||||
} else {
|
||||
write_send[7] = local_node;
|
||||
write_send[13] = (plc_address >> 8) & 0xFF;
|
||||
write_send[14] = plc_address & 0xFF;
|
||||
write_send[17] = size;
|
||||
for (int i = 0; i < size; i++) {
|
||||
uint16_t value = get_register(1400 + (20 * myRtumSTIdx) + i);
|
||||
write_send[i * 2 + 18] = (value >> 8) & 0xFF;
|
||||
write_send[i * 2 + 19] = value & 0xFF;
|
||||
}
|
||||
sendsize = (size * 2) + 18;
|
||||
for (int i = 0; i < sendsize; i++) {
|
||||
omrfins_buf[i] = write_send[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
myRtumSTIdx = (myRtumSTIdx + 1) % 20;
|
||||
return sendsize;
|
||||
}
|
||||
|
||||
uint16_t fins_timeout = 0;
|
||||
void update_timeout_fins() {
|
||||
if (fins_timeout < 0xffff) {
|
||||
fins_timeout++;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t loopback_udpc_omrfins(uint8_t sn, uint8_t *destip, uint16_t destport, uint16_t local_node) {
|
||||
int32_t ret;
|
||||
uint16_t size = 0, sentsize = 0;
|
||||
static uint16_t any_port = 50000;
|
||||
switch (getSn_SR(sn)) {
|
||||
case SOCK_UDP:
|
||||
if ((size = getSn_RX_RSR(sn)) > 0) {
|
||||
if (size > DATA_BUF_SIZE) {
|
||||
size = DATA_BUF_SIZE;
|
||||
}
|
||||
ret = recvfrom(sn, omrfins_buf, size, destip, (uint16_t*) &destport);
|
||||
|
||||
if (ret <= 0) {
|
||||
return ret;
|
||||
} else {
|
||||
comm_count++;
|
||||
handleReadRecv(ret, local_node);
|
||||
fins_timeout = 490;
|
||||
}
|
||||
}
|
||||
|
||||
size = 0;
|
||||
if (fins_timeout > 500) {
|
||||
size = handleSend(local_node);
|
||||
if (size == 0) {
|
||||
fins_timeout = 1000;
|
||||
}
|
||||
}
|
||||
|
||||
sentsize = 0;
|
||||
while (sentsize != size) {
|
||||
ret = sendto(sn, omrfins_buf + sentsize, size - sentsize, destip, destport);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
sentsize += ret;
|
||||
}
|
||||
break;
|
||||
case SOCK_CLOSED:
|
||||
if ((ret = socket(sn, Sn_MR_UDP, any_port, 0x00)) != sn) {
|
||||
return ret;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 1;
|
||||
|
||||
}
|
||||
//////////////FINS end
|
||||
|
||||
void loopback_comm(uint8_t *ip, uint16_t port, uint8_t node) {
|
||||
if (master.plc_type == 0) {
|
||||
loopback_tcpc_slmp(1, ip, port);
|
||||
} else if (master.plc_type == 1) {
|
||||
loopback_udpc_omrfins(1, ip, port, node);
|
||||
}
|
||||
loopback_tcps_modbus(0, 502);
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in new issue
Block a user