123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514 |
- #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;
- }
|