tcp2rtu/tcp2rtu.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <sys/epoll.h>
#include <arpa/inet.h>
#include <linux/serial.h>

#define MAX_EVENTS 10
#define QUEUE_SIZE 256

typedef struct
{
  unsigned short transaction_id;
  unsigned char unit_id;
  unsigned char function_code;
  unsigned short address;
  unsigned short quantity;
} TransactionEntry;

typedef struct
{
  TransactionEntry entries[QUEUE_SIZE];
  int front;
  int rear;
  int count;
} TransactionQueue;

void initQueue(TransactionQueue* queue)
{
  queue->front = 0;
  queue->rear = 0;
  queue->count = 0;
}

int isQueueEmpty(TransactionQueue* queue)
{
  return queue->count == 0;
}

int isQueueFull(TransactionQueue* queue)
{
  return queue->count == QUEUE_SIZE;
}

void enqueue(TransactionQueue* queue,TransactionEntry entry)
{
  if(!isQueueFull(queue))
  {
    queue->entries[queue->rear] = entry;
    queue->rear = (queue->rear + 1) % QUEUE_SIZE;
    queue->count++;
  }
  else
  {
    fprintf(stderr,"Transaction queue is full!\n");
  }
}

TransactionEntry dequeue(TransactionQueue* queue,unsigned char unit_id,unsigned char function_code,unsigned short address,unsigned short quantity,int compare_address,int compare_quantity)
{
  TransactionEntry entry = { 0, 0, 0, 0, 0 };
  if(!isQueueEmpty(queue))
  {
    for(int i = queue->front; i != queue->rear; i = (i + 1) % QUEUE_SIZE)
    {
      if(queue->entries[i].unit_id == unit_id &&
        queue->entries[i].function_code == function_code &&
        (!compare_address || (queue->entries[i].address == address)) &&
        (!compare_quantity || (queue->entries[i].quantity == quantity)))
      {
        entry = queue->entries[i];
        queue->front = (i + 1) % QUEUE_SIZE;
        queue->count--;
        break;
      }
    }
  }
  else
  {
    fprintf(stderr,"Transaction queue is empty!\n");
  }
  return entry;
}

unsigned short crc16(const unsigned char* buf,int len)
{
  unsigned short crc = 0xFFFF;
  for(int pos = 0; pos < len; pos++)
  {
    crc ^= (unsigned short)buf[pos];
    for(int i = 8; i != 0; i--)
    {
      if((crc & 1) != 0)
      {
        crc >>= 1;
        crc ^= 0xA001;
      }
      else
      {
        crc >>= 1;
      }
    }
  }
  return crc;
}

void configure_serial_port(int fd,int baud_rate)
{
  struct termios options;
  tcgetattr(fd,&options);
  cfsetispeed(&options,baud_rate);
  cfsetospeed(&options,baud_rate);
  options.c_cflag |= (CLOCAL | CREAD);
  options.c_cflag &= ~CSIZE;
  options.c_cflag |= CS8;
  options.c_cflag &= ~PARENB;
  options.c_cflag &= ~CSTOPB;
  options.c_cflag &= ~CRTSCTS;
  options.c_iflag &= ~(IXON | IXOFF | IXANY);
  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  options.c_oflag &= ~OPOST;
  tcsetattr(fd,TCSANOW,&options);
}

void configure_rs485_mode(int fd)
{
  struct serial_rs485 rs485conf;
  memset(&rs485conf,0,sizeof(rs485conf));

  /* Enable RS485 mode: */
  rs485conf.flags |= SER_RS485_ENABLED;
  /* Set logical level for RTS pin equal to 1 when sending: */
  rs485conf.flags |= SER_RS485_RTS_ON_SEND;
  /* Set logical level for RTS pin equal to 0 after sending: */
  rs485conf.flags &= ~SER_RS485_RTS_AFTER_SEND;
  /* Set this flag if you want to receive data even whilst sending data */
  rs485conf.flags &= ~SER_RS485_RX_DURING_TX;
  /* Set rts delay before send, if needed: */
  rs485conf.delay_rts_before_send = 0;  // in miliseconds
  /* Set rts delay after send, if needed: */
  rs485conf.delay_rts_after_send = 0;  // in miliseconds
  if(ioctl(fd,TIOCSRS485,&rs485conf) < 0)
  {
    perror("ERROR setting RS-485 mode");
    exit(1);
  }
}

int get_baud_rate(int baud_rate)
{
  switch(baud_rate)
  {
  case 1200: return B1200;
  case 2400: return B2400;
  case 4800: return B4800;
  case 9600: return B9600;
  case 19200: return B19200;
  case 38400: return B38400;
  case 57600: return B57600;
  case 115200: return B115200;
  default:
    fprintf(stderr,"Unsupported baud rate: %d\n",baud_rate);
    exit(1);
  }
}

void print_binary(const unsigned char* data,int length)
{
  for(int i = 0; i < length; i++)
  {
    printf("%02X ",data[i]);
  }
  printf("\n");
}

void parse_and_enqueue_request(TransactionQueue* queue,const unsigned char* buffer,int length)
{
  unsigned short transaction_id = (buffer[0] << 8) | buffer[1];
  unsigned char unit_id = buffer[6];
  unsigned char function_code = buffer[7];
  unsigned short address = (buffer[8] << 8) | buffer[9];
  unsigned short quantity = (buffer[10] << 8) | buffer[11];

  TransactionEntry entry = { transaction_id, unit_id, function_code, address, quantity };
  enqueue(queue,entry);
}

void parse_and_send_response(TransactionQueue* queue,const unsigned char* buffer,int length,int newsockfd,int verbose)
{
  unsigned char unit_id = buffer[0];
  unsigned char function_code = buffer[1];
  unsigned short address = 0;
  unsigned short quantity = 0;
  int compare_address = 0;
  int compare_quantity = 0;

  if(newsockfd > 0)
  {
    if(write(newsockfd,buffer,length) < 0)
    {
      perror("ERROR writing to socket");
    }
  }
  return;

  if(verbose)
  {
    printf("Converted Modbus TCP frame: ");
    print_binary(buffer,length);
  }

  if(function_code == 0x05 || function_code == 0x06 || function_code == 0x0F || function_code == 0x10)
  {
    address = (buffer[2] << 8) | buffer[3];
    compare_address = 1;
  }
  else if(function_code == 0x01 || function_code == 0x02 || function_code == 0x03 || function_code == 0x04)
  {
    quantity = (buffer[2] / 2); // Modbus RTU 字节数除以2是 quantity
    compare_quantity = 1;
  }
  else
  {
    fprintf(stderr,"Unsupported function code: %d\n",function_code);
  }

  TransactionEntry entry = dequeue(queue,unit_id,function_code,address,quantity,compare_address,compare_quantity);

  if(entry.transaction_id == 0)
  {
    // No matching transaction entry found
    fprintf(stderr,"No matching transaction entry found\n");
    return;
  }

  unsigned char tcp_frame[256];
  int data_length;
  unsigned short protocol_id = 0;

  if(function_code == 0x01 || function_code == 0x02 || function_code == 0x03 || function_code == 0x04)
  {
    unsigned char byte_count = buffer[2];
    data_length = byte_count + 1; // 包含 1 字节的 byte_count
    length = data_length + 2; // 单元标识符和功能码

    tcp_frame[0] = (entry.transaction_id >> 8) & 0xFF;
    tcp_frame[1] = entry.transaction_id & 0xFF;
    tcp_frame[2] = (protocol_id >> 8) & 0xFF;
    tcp_frame[3] = protocol_id & 0xFF;
    tcp_frame[4] = (length >> 8) & 0xFF;
    tcp_frame[5] = length & 0xFF;
    tcp_frame[6] = unit_id;
    tcp_frame[7] = function_code;
    tcp_frame[8] = byte_count;
    memcpy(tcp_frame + 9,buffer + 3,byte_count);
  }
  else if(function_code == 0x05 || function_code == 0x06)
  {
    data_length = 4;
    length = data_length + 2; // 单元标识符和功能码

    tcp_frame[0] = (entry.transaction_id >> 8) & 0xFF;
    tcp_frame[1] = entry.transaction_id & 0xFF;
    tcp_frame[2] = (protocol_id >> 8) & 0xFF;
    tcp_frame[3] = protocol_id & 0xFF;
    tcp_frame[4] = (length >> 8) & 0xFF;
    tcp_frame[5] = length & 0xFF;
    tcp_frame[6] = unit_id;
    tcp_frame[7] = function_code;
    tcp_frame[8] = (address >> 8) & 0xFF;
    tcp_frame[9] = address & 0xFF;
    memcpy(&tcp_frame[10],buffer + 4,2);
    // tcp_frame[10] = (quantity >> 8) & 0xFF;
    // tcp_frame[11] = quantity & 0xFF;
  }
  else if(function_code == 0x0F || function_code == 0x10)
  {
    data_length = 4;
    length = data_length + 4; // 单元标识符和功能码

    tcp_frame[0] = (entry.transaction_id >> 8) & 0xFF;
    tcp_frame[1] = entry.transaction_id & 0xFF;
    tcp_frame[2] = (protocol_id >> 8) & 0xFF;
    tcp_frame[3] = protocol_id & 0xFF;
    tcp_frame[4] = (length >> 8) & 0xFF;
    tcp_frame[5] = length & 0xFF;
    tcp_frame[6] = unit_id;
    tcp_frame[7] = function_code;
    tcp_frame[8] = (address >> 8) & 0xFF;
    tcp_frame[9] = address & 0xFF;
    memcpy(&tcp_frame[10],buffer + 4,2);
    // tcp_frame[10] = (quantity >> 8) & 0xFF;
    // tcp_frame[11] = quantity & 0xFF;
  }

}

int main(int argc,char* argv[])
{
  int verbose = 0;
  const char* serial_port;
  int baud_rate;
  int tcp_port;

  for(int i = 1; i < argc; i++)
  {
    if(strcmp(argv[i],"-v") == 0)
    {
      verbose = 1;
    }
    else if(i == argc - 3)
    {
      serial_port = argv[i];
    }
    else if(i == argc - 2)
    {
      baud_rate = atoi(argv[i]);
    }
    else if(i == argc - 1)
    {
      tcp_port = atoi(argv[i]);
    }
  }

  if(argc < 4 || (argc == 5 && !verbose))
  {
    fprintf(stderr,"Usage: %s [-v] <serial_port> <baud_rate> <tcp_port>\n",argv[0]);
    exit(1);
  }

  int baud_rate_flag = get_baud_rate(baud_rate);

  int serial_fd = open(serial_port,O_RDWR | O_NOCTTY | O_NDELAY);
  if(serial_fd == -1)
  {
    perror("ERROR opening serial port");
    exit(1);
  }

  configure_serial_port(serial_fd,baud_rate_flag);
  configure_rs485_mode(serial_fd);

  int sockfd,newsockfd = -1;
  struct sockaddr_in serv_addr,cli_addr;
  socklen_t clilen;
  char buffer[256];
  int n;

  sockfd = socket(AF_INET,SOCK_STREAM,0);
  if(sockfd < 0)
  {
    perror("ERROR opening socket");
    close(serial_fd);
    exit(1);
  }

  memset((char*)&serv_addr,0,sizeof(serv_addr));
  serv_addr.sin_family = AF_INET;
  serv_addr.sin_addr.s_addr = INADDR_ANY;
  serv_addr.sin_port = htons(tcp_port);

  if(bind(sockfd,(struct sockaddr*)&serv_addr,sizeof(serv_addr)) < 0)
  {
    perror("ERROR on binding");
    close(sockfd);
    close(serial_fd);
    exit(1);
  }

  listen(sockfd,5);
  clilen = sizeof(cli_addr);

  int epoll_fd = epoll_create1(0);
  if(epoll_fd == -1)
  {
    perror("ERROR creating epoll");
    close(sockfd);
    close(serial_fd);
    exit(1);
  }

  struct epoll_event ev,events[MAX_EVENTS];
  ev.events = EPOLLIN;
  ev.data.fd = sockfd;
  if(epoll_ctl(epoll_fd,EPOLL_CTL_ADD,sockfd,&ev) == -1)
  {
    perror("ERROR adding socket to epoll");
    close(sockfd);
    close(serial_fd);
    close(epoll_fd);
    exit(1);
  }

  ev.events = EPOLLIN;
  ev.data.fd = serial_fd;
  if(epoll_ctl(epoll_fd,EPOLL_CTL_ADD,serial_fd,&ev) == -1)
  {
    perror("ERROR adding serial port to epoll");
    close(sockfd);
    close(serial_fd);
    close(epoll_fd);
    exit(1);
  }

  TransactionQueue queue;
  initQueue(&queue);

  while(1)
  {
    int nfds = epoll_wait(epoll_fd,events,MAX_EVENTS,-1);
    if(nfds == -1)
    {
      perror("ERROR in epoll_wait");
      close(sockfd);
      close(serial_fd);
      close(epoll_fd);
      exit(1);
    }

    for(int i = 0; i < nfds; i++)
    {
      if(events[i].data.fd == sockfd)
      {
        newsockfd = accept(sockfd,(struct sockaddr*)&cli_addr,&clilen);
        if(newsockfd == -1)
        {
          perror("ERROR on accept");
          continue;
        }

        ev.events = EPOLLIN | EPOLLET;
        ev.data.fd = newsockfd;
        if(epoll_ctl(epoll_fd,EPOLL_CTL_ADD,newsockfd,&ev) == -1)
        {
          perror("ERROR adding new socket to epoll");
          close(newsockfd);
          continue;
        }
      }
      else if(events[i].data.fd == serial_fd)
      {
        memset(buffer,0,256);
        n = read(serial_fd,buffer,255);
        if(n < 0)
        {
          perror("ERROR reading from serial port");
          continue;
        }

        if(verbose)
        {
          printf("Received Modbus RTU frame: ");
          print_binary((unsigned char*)buffer,n);
        }

        parse_and_send_response(&queue,buffer,n,newsockfd,verbose);
      }
      else
      {
        memset(buffer,0,256);
        n = read(events[i].data.fd,buffer,255);
        if(n < 0)
        {
          perror("ERROR reading from socket");
          close(events[i].data.fd);
          continue;
        }
        else if(n == 0)
        {
          close(events[i].data.fd);
          continue;
        }

        if(verbose)
        {
          printf("Received Modbus TCP frame: ");
          print_binary((unsigned char*)buffer,n);
        }

        // parse_and_enqueue_request(&queue,buffer,n);

        // unsigned char rtu_frame[256];
        // int rtu_length = n - 6;
        // memcpy(rtu_frame,buffer + 6,rtu_length);

        // unsigned short crc = crc16(rtu_frame,rtu_length);
        // rtu_frame[rtu_length] = crc & 0xFF;          // CRC LSB
        // rtu_frame[rtu_length + 1] = (crc >> 8) & 0xFF; // CRC MSB
        // rtu_length += 2;

        if(verbose)
        {
          printf("Converted Modbus RTU frame: ");
          print_binary(buffer,n);
        }

        if(write(serial_fd,buffer,n) < 0)
        {
          perror("ERROR writing to serial port");
        }
      }
    }
  }

  close(sockfd);
  close(serial_fd);
  close(epoll_fd);
  return 0;
}