|
|
@@ -206,15 +206,20 @@ static void drain_socket(int sockfd)
|
|
|
}
|
|
|
|
|
|
/* ---- Send Modbus RTU frame and receive complete response ---- */
|
|
|
-static int modbus_send_recv(int sockfd, const uint8_t *tx_frame, int tx_len,
|
|
|
+static int modbus_send_recv(int sockfd, pthread_mutex_t *sock_mutex,
|
|
|
+ const uint8_t *tx_frame, int tx_len,
|
|
|
uint8_t *rx_buf, int rx_buf_size, int timeout_sec)
|
|
|
{
|
|
|
+ /* Serialize with write operations (both share the same TCP socket) */
|
|
|
+ pthread_mutex_lock(sock_mutex);
|
|
|
+
|
|
|
/* Drain any leftover data before sending */
|
|
|
drain_socket(sockfd);
|
|
|
|
|
|
/* Send */
|
|
|
ssize_t sent = send(sockfd, tx_frame, tx_len, 0);
|
|
|
if (sent != tx_len) {
|
|
|
+ pthread_mutex_unlock(sock_mutex);
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
@@ -231,10 +236,11 @@ static int modbus_send_recv(int sockfd, const uint8_t *tx_frame, int tx_len,
|
|
|
tv.tv_usec = 0;
|
|
|
|
|
|
int ret = select(sockfd + 1, &rfds, NULL, NULL, &tv);
|
|
|
- if (ret < 0) return -1; /* Error */
|
|
|
+ if (ret < 0) { pthread_mutex_unlock(sock_mutex); return -1; } /* Error */
|
|
|
if (ret == 0) {
|
|
|
/* Timeout */
|
|
|
if (total_received > 0) break; /* Got some data, use it */
|
|
|
+ pthread_mutex_unlock(sock_mutex);
|
|
|
return -1; /* No data at all */
|
|
|
}
|
|
|
|
|
|
@@ -247,6 +253,7 @@ static int modbus_send_recv(int sockfd, const uint8_t *tx_frame, int tx_len,
|
|
|
usleep(10000); /* 10ms */
|
|
|
continue;
|
|
|
}
|
|
|
+ pthread_mutex_unlock(sock_mutex);
|
|
|
return -1;
|
|
|
}
|
|
|
if (received == 0) break; /* Connection closed */
|
|
|
@@ -267,11 +274,12 @@ static int modbus_send_recv(int sockfd, const uint8_t *tx_frame, int tx_len,
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ pthread_mutex_unlock(sock_mutex);
|
|
|
return total_received;
|
|
|
}
|
|
|
|
|
|
/* ---- Build and send read input registers command (0x04) ---- */
|
|
|
-static int read_input_registers(int sockfd, uint8_t addr,
|
|
|
+static int read_input_registers(modbus_client_t *client, uint8_t addr,
|
|
|
uint16_t start_reg, uint16_t count,
|
|
|
uint16_t *regs, int timeout_sec)
|
|
|
{
|
|
|
@@ -287,7 +295,8 @@ static int read_input_registers(int sockfd, uint8_t addr,
|
|
|
tx[7] = (crc >> 8) & 0xFF; /* CRC high */
|
|
|
|
|
|
uint8_t rx[512];
|
|
|
- int rx_len = modbus_send_recv(sockfd, tx, 8, rx, sizeof(rx), timeout_sec);
|
|
|
+ int rx_len = modbus_send_recv(client->sockfd, &client->sock_mutex,
|
|
|
+ tx, 8, rx, sizeof(rx), timeout_sec);
|
|
|
if (rx_len < 0) {
|
|
|
return -1;
|
|
|
}
|
|
|
@@ -339,7 +348,7 @@ static int read_input_registers(int sockfd, uint8_t addr,
|
|
|
}
|
|
|
|
|
|
/* ---- Build and send read output registers command (0x03) ---- */
|
|
|
-static int read_output_registers(int sockfd, uint8_t addr,
|
|
|
+static int read_output_registers(modbus_client_t *client, uint8_t addr,
|
|
|
uint16_t start_reg, uint16_t count,
|
|
|
uint16_t *regs, int timeout_sec)
|
|
|
{
|
|
|
@@ -355,7 +364,8 @@ static int read_output_registers(int sockfd, uint8_t addr,
|
|
|
tx[7] = (crc >> 8) & 0xFF;
|
|
|
|
|
|
uint8_t rx[512];
|
|
|
- int rx_len = modbus_send_recv(sockfd, tx, 8, rx, sizeof(rx), timeout_sec);
|
|
|
+ int rx_len = modbus_send_recv(client->sockfd, &client->sock_mutex,
|
|
|
+ tx, 8, rx, sizeof(rx), timeout_sec);
|
|
|
if (rx_len < 0) {
|
|
|
return -1;
|
|
|
}
|
|
|
@@ -403,7 +413,7 @@ static int poll_device(modbus_client_t *client, mppt_device_data_t *dev)
|
|
|
int ret;
|
|
|
|
|
|
/* Read Input Registers (0x04): 11 registers starting at 0x0000 */
|
|
|
- ret = read_input_registers(client->sockfd, dev->device_addr,
|
|
|
+ ret = read_input_registers(client, dev->device_addr,
|
|
|
0x0000, MODBUS_INPUT_REG_COUNT,
|
|
|
regs, client->config.connect_timeout_sec);
|
|
|
if (ret < 0) {
|
|
|
@@ -427,7 +437,7 @@ static int poll_device(modbus_client_t *client, mppt_device_data_t *dev)
|
|
|
/* Protocol spec: two commands must have 1 second interval */
|
|
|
usleep(1000000); /* 1 second delay per protocol specification */
|
|
|
|
|
|
- ret = read_output_registers(client->sockfd, dev->device_addr,
|
|
|
+ ret = read_output_registers(client, dev->device_addr,
|
|
|
0x0000, MODBUS_OUTPUT_REG_COUNT,
|
|
|
regs, client->config.connect_timeout_sec);
|
|
|
if (ret >= 0) {
|
|
|
@@ -576,6 +586,7 @@ void modbus_init(modbus_client_t *client, const modbus_config_t *config)
|
|
|
client->running = false;
|
|
|
client->device_count = config->device_count;
|
|
|
pthread_mutex_init(&client->data_mutex, NULL);
|
|
|
+ pthread_mutex_init(&client->sock_mutex, NULL);
|
|
|
|
|
|
/* Initialize device entries */
|
|
|
for (int i = 0; i < config->device_count && i < MODBUS_MAX_DEVICES; i++) {
|
|
|
@@ -608,6 +619,7 @@ void modbus_stop(modbus_client_t *client)
|
|
|
}
|
|
|
client->connected = false;
|
|
|
pthread_mutex_destroy(&client->data_mutex);
|
|
|
+ pthread_mutex_destroy(&client->sock_mutex);
|
|
|
}
|
|
|
|
|
|
void modbus_get_all_data(modbus_client_t *client,
|
|
|
@@ -671,7 +683,16 @@ int modbus_validate_setting(uint16_t reg_addr, uint16_t value, uint16_t battery_
|
|
|
int modbus_write_register(modbus_client_t *ctx, uint8_t dev_addr,
|
|
|
uint16_t reg_addr, uint16_t value)
|
|
|
{
|
|
|
- if (!ctx || !ctx->connected) return -1;
|
|
|
+ if (!ctx) return -1;
|
|
|
+
|
|
|
+ /* Serialize socket access with the polling thread (both share the same TCP socket) */
|
|
|
+ pthread_mutex_lock(&ctx->sock_mutex);
|
|
|
+
|
|
|
+ if (!ctx->connected || ctx->sockfd < 0) {
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Not connected\n");
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
|
|
|
/* Build Write Multiple Registers frame (0x10) */
|
|
|
uint8_t frame[11];
|
|
|
@@ -696,60 +717,99 @@ int modbus_write_register(modbus_client_t *ctx, uint8_t dev_addr,
|
|
|
ssize_t sent = send(ctx->sockfd, frame, 11, 0);
|
|
|
if (sent != 11) {
|
|
|
fprintf(stderr, "[MODBUS-WRITE] Send failed: %zd/11\n", sent);
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
printf("[MODBUS-WRITE] Sent to dev %d reg 0x%04X value %u\n",
|
|
|
dev_addr, reg_addr, value);
|
|
|
|
|
|
- /* Read response (should be 6 bytes: addr + 0x10 + start_addr(2) + count(2) + crc(2)) */
|
|
|
- uint8_t resp[6];
|
|
|
+ /*
|
|
|
+ * Read response.
|
|
|
+ * Normal FC 0x10 response is 8 bytes:
|
|
|
+ * addr(1) + func(1) + start_addr(2) + quantity(2) + crc(2)
|
|
|
+ * Exception response is 5 bytes:
|
|
|
+ * addr(1) + func|0x80(1) + exception_code(1) + crc(2)
|
|
|
+ */
|
|
|
+ uint8_t resp[8];
|
|
|
ssize_t total = 0;
|
|
|
int retries = 0;
|
|
|
- while (total < 6 && retries < 20) {
|
|
|
+ while (total < (ssize_t)sizeof(resp) && retries < 60) {
|
|
|
struct timeval tv = { .tv_sec = 0, .tv_usec = 100000 }; /* 100ms */
|
|
|
fd_set readfds;
|
|
|
FD_ZERO(&readfds);
|
|
|
FD_SET(ctx->sockfd, &readfds);
|
|
|
int ret = select(ctx->sockfd + 1, &readfds, NULL, NULL, &tv);
|
|
|
if (ret > 0) {
|
|
|
- ssize_t n = recv(ctx->sockfd, resp + total, 6 - total, 0);
|
|
|
+ ssize_t n = recv(ctx->sockfd, resp + total,
|
|
|
+ (ssize_t)sizeof(resp) - total, 0);
|
|
|
if (n <= 0) break;
|
|
|
total += n;
|
|
|
+ /* Exception response: 5 bytes is enough */
|
|
|
+ if (total >= 5 && (resp[1] & 0x80)) break;
|
|
|
+ if (total >= 8) break; /* Normal response complete */
|
|
|
} else {
|
|
|
retries++;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- if (total < 6) {
|
|
|
- fprintf(stderr, "[MODBUS-WRITE] Response too short: %zd/6 bytes\n", total);
|
|
|
+ if (total < 5) {
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Response too short: %zd bytes (< 5)\n", total);
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
- /* Verify response */
|
|
|
+ /* Verify address */
|
|
|
if (resp[0] != dev_addr) {
|
|
|
fprintf(stderr, "[MODBUS-WRITE] Address mismatch: expected %d, got %d\n",
|
|
|
dev_addr, resp[0]);
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+ /* Verify function code byte (check exception flag) */
|
|
|
+ if (resp[1] != 0x10) {
|
|
|
+ if (resp[1] & 0x80) {
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Error response: code 0x%02X\n", resp[2]);
|
|
|
+ } else {
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Unexpected function code: 0x%02X\n", resp[1]);
|
|
|
+ }
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
return -1;
|
|
|
}
|
|
|
- if (resp[1] == 0x90) {
|
|
|
- /* Error response: 0x10 + 0x80 = 0x90 */
|
|
|
- fprintf(stderr, "[MODBUS-WRITE] Error response: code %d\n", resp[2]);
|
|
|
+ /* Normal response must be 8 bytes */
|
|
|
+ if (total < 8) {
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Response too short: %zd/8 bytes\n", total);
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
return -1;
|
|
|
}
|
|
|
- if (resp[1] != 0x10) {
|
|
|
- fprintf(stderr, "[MODBUS-WRITE] Unexpected function code: 0x%02X\n", resp[1]);
|
|
|
+
|
|
|
+ /* Verify start address echoed */
|
|
|
+ uint16_t resp_reg = (uint16_t)((resp[2] << 8) | resp[3]);
|
|
|
+ if (resp_reg != reg_addr) {
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Start address mismatch: expected 0x%04X, got 0x%04X\n",
|
|
|
+ reg_addr, resp_reg);
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+ /* Verify register quantity echoed (must be 1) */
|
|
|
+ uint16_t resp_qty = (uint16_t)((resp[4] << 8) | resp[5]);
|
|
|
+ if (resp_qty != 1) {
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Quantity mismatch: expected 1, got %u\n", resp_qty);
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
- /* Verify CRC */
|
|
|
- uint16_t recv_crc = resp[4] | (resp[5] << 8);
|
|
|
- uint16_t calc_crc = crc16_modbus(resp, 4);
|
|
|
+ /* Verify CRC over first 6 bytes */
|
|
|
+ uint16_t recv_crc = (uint16_t)(resp[6] | (resp[7] << 8));
|
|
|
+ uint16_t calc_crc = crc16_modbus(resp, 6);
|
|
|
if (recv_crc != calc_crc) {
|
|
|
- fprintf(stderr, "[MODBUS-WRITE] CRC mismatch\n");
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] CRC mismatch: recv=0x%04X calc=0x%04X\n",
|
|
|
+ recv_crc, calc_crc);
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
printf("[MODBUS-WRITE] Success: dev %d reg 0x%04X = %u\n",
|
|
|
dev_addr, reg_addr, value);
|
|
|
return 0;
|