|
@@ -680,137 +680,220 @@ int modbus_validate_setting(uint16_t reg_addr, uint16_t value, uint16_t battery_
|
|
|
return 0;
|
|
return 0;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
-int modbus_write_register(modbus_client_t *ctx, uint8_t dev_addr,
|
|
|
|
|
- uint16_t reg_addr, uint16_t value)
|
|
|
|
|
|
|
+/*
|
|
|
|
|
+ * Issue one write command under an already-held sock_mutex.
|
|
|
|
|
+ * Uses the given function code (0x06 write-single or 0x10 write-multiple).
|
|
|
|
|
+ * Returns:
|
|
|
|
|
+ * 0 success
|
|
|
|
|
+ * -1 transport / CRC error (retry / reconnect may help)
|
|
|
|
|
+ * 1 device exception response (rejected; retrying same FC usually pointless)
|
|
|
|
|
+ */
|
|
|
|
|
+static int write_once(int sockfd, uint8_t fc, uint8_t dev_addr,
|
|
|
|
|
+ uint16_t reg_addr, uint16_t value, int timeout_sec)
|
|
|
{
|
|
{
|
|
|
- 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];
|
|
uint8_t frame[11];
|
|
|
|
|
+ int frame_len;
|
|
|
|
|
+
|
|
|
frame[0] = dev_addr;
|
|
frame[0] = dev_addr;
|
|
|
- frame[1] = 0x10; /* Function code: Write Multiple Registers */
|
|
|
|
|
- frame[2] = (reg_addr >> 8) & 0xFF; /* Start address high */
|
|
|
|
|
- frame[3] = reg_addr & 0xFF; /* Start address low */
|
|
|
|
|
- frame[4] = 0x00; /* Number of registers high */
|
|
|
|
|
- frame[5] = 0x01; /* Number of registers low (1 register) */
|
|
|
|
|
- frame[6] = 0x02; /* Byte count (2 bytes for 1 register) */
|
|
|
|
|
- frame[7] = (value >> 8) & 0xFF; /* Value high byte */
|
|
|
|
|
- frame[8] = value & 0xFF; /* Value low byte */
|
|
|
|
|
-
|
|
|
|
|
- uint16_t crc = crc16_modbus(frame, 9);
|
|
|
|
|
- frame[9] = crc & 0xFF; /* CRC low */
|
|
|
|
|
- frame[10] = (crc >> 8) & 0xFF; /* CRC high */
|
|
|
|
|
-
|
|
|
|
|
- /* Drain socket before sending */
|
|
|
|
|
- drain_socket(ctx->sockfd);
|
|
|
|
|
-
|
|
|
|
|
- /* Send frame */
|
|
|
|
|
- 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);
|
|
|
|
|
|
|
+ if (fc == MODBUS_FC_WRITE_SINGLE) {
|
|
|
|
|
+ /* FC 0x06: addr + 0x06 + reg(2) + value(2) + crc(2) = 8 bytes */
|
|
|
|
|
+ frame[1] = 0x06;
|
|
|
|
|
+ frame[2] = (reg_addr >> 8) & 0xFF;
|
|
|
|
|
+ frame[3] = reg_addr & 0xFF;
|
|
|
|
|
+ frame[4] = (value >> 8) & 0xFF;
|
|
|
|
|
+ frame[5] = value & 0xFF;
|
|
|
|
|
+ frame_len = 6;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ /* FC 0x10: addr + 0x10 + reg(2) + qty(2) + bytecount(1) + value(2) + crc(2) = 11 */
|
|
|
|
|
+ frame[1] = 0x10;
|
|
|
|
|
+ frame[2] = (reg_addr >> 8) & 0xFF;
|
|
|
|
|
+ frame[3] = reg_addr & 0xFF;
|
|
|
|
|
+ frame[4] = 0x00;
|
|
|
|
|
+ frame[5] = 0x01;
|
|
|
|
|
+ frame[6] = 0x02;
|
|
|
|
|
+ frame[7] = (value >> 8) & 0xFF;
|
|
|
|
|
+ frame[8] = value & 0xFF;
|
|
|
|
|
+ frame_len = 9;
|
|
|
|
|
+ }
|
|
|
|
|
+ uint16_t crc = crc16_modbus(frame, frame_len);
|
|
|
|
|
+ frame[frame_len] = crc & 0xFF;
|
|
|
|
|
+ frame[frame_len + 1] = (crc >> 8) & 0xFF;
|
|
|
|
|
+ frame_len += 2;
|
|
|
|
|
+
|
|
|
|
|
+ /* Drain leftover bus data, then give the bus a short settle gap */
|
|
|
|
|
+ drain_socket(sockfd);
|
|
|
|
|
+
|
|
|
|
|
+ ssize_t sent = send(sockfd, frame, frame_len, 0);
|
|
|
|
|
+ if (sent != frame_len) {
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Send failed: %zd/%d (fc=0x%02X)\n",
|
|
|
|
|
+ sent, frame_len, fc);
|
|
|
return -1;
|
|
return -1;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- printf("[MODBUS-WRITE] Sent to dev %d reg 0x%04X value %u\n",
|
|
|
|
|
- dev_addr, reg_addr, value);
|
|
|
|
|
-
|
|
|
|
|
/*
|
|
/*
|
|
|
- * 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)
|
|
|
|
|
|
|
+ * Both FC 0x06 and FC 0x10 normal responses are 8 bytes:
|
|
|
|
|
+ * addr(1) + fc(1) + reg(2) + value-or-qty(2) + crc(2)
|
|
|
|
|
+ * Exception response is 5 bytes: addr + (fc|0x80) + code + crc(2)
|
|
|
*/
|
|
*/
|
|
|
uint8_t resp[8];
|
|
uint8_t resp[8];
|
|
|
ssize_t total = 0;
|
|
ssize_t total = 0;
|
|
|
- int retries = 0;
|
|
|
|
|
- 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,
|
|
|
|
|
|
|
+ int idle_loops = 0;
|
|
|
|
|
+ int max_loops = timeout_sec * 10 + 20; /* 100ms per loop */
|
|
|
|
|
+
|
|
|
|
|
+ while (total < (ssize_t)sizeof(resp) && idle_loops < max_loops) {
|
|
|
|
|
+ struct timeval tv = { .tv_sec = 0, .tv_usec = 100000 };
|
|
|
|
|
+ fd_set rfds;
|
|
|
|
|
+ FD_ZERO(&rfds);
|
|
|
|
|
+ FD_SET(sockfd, &rfds);
|
|
|
|
|
+ int r = select(sockfd + 1, &rfds, NULL, NULL, &tv);
|
|
|
|
|
+ if (r > 0) {
|
|
|
|
|
+ ssize_t n = recv(sockfd, resp + total,
|
|
|
(ssize_t)sizeof(resp) - total, 0);
|
|
(ssize_t)sizeof(resp) - total, 0);
|
|
|
- if (n <= 0) break;
|
|
|
|
|
|
|
+ if (n == 0) {
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] connection closed by peer (fc=0x%02X)\n", fc);
|
|
|
|
|
+ return -1;
|
|
|
|
|
+ }
|
|
|
|
|
+ if (n < 0) {
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] recv error: %s (fc=0x%02X)\n",
|
|
|
|
|
+ strerror(errno), fc);
|
|
|
|
|
+ return -1;
|
|
|
|
|
+ }
|
|
|
total += n;
|
|
total += n;
|
|
|
- /* Exception response: 5 bytes is enough */
|
|
|
|
|
- if (total >= 5 && (resp[1] & 0x80)) break;
|
|
|
|
|
- if (total >= 8) break; /* Normal response complete */
|
|
|
|
|
|
|
+ idle_loops = 0;
|
|
|
|
|
+ if (total >= 5 && (resp[1] & 0x80)) break; /* exception */
|
|
|
|
|
+ if (total >= 8) break; /* normal complete */
|
|
|
} else {
|
|
} else {
|
|
|
- retries++;
|
|
|
|
|
|
|
+ idle_loops++;
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
if (total < 5) {
|
|
if (total < 5) {
|
|
|
- fprintf(stderr, "[MODBUS-WRITE] Response too short: %zd bytes (< 5)\n", total);
|
|
|
|
|
- pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] no/short response: %zd bytes (fc=0x%02X)\n",
|
|
|
|
|
+ total, fc);
|
|
|
return -1;
|
|
return -1;
|
|
|
}
|
|
}
|
|
|
-
|
|
|
|
|
- /* Verify address */
|
|
|
|
|
if (resp[0] != dev_addr) {
|
|
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);
|
|
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] addr mismatch exp %d got %d (fc=0x%02X)\n",
|
|
|
|
|
+ dev_addr, resp[0], fc);
|
|
|
return -1;
|
|
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);
|
|
|
|
|
|
|
+ if (resp[1] & 0x80) {
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] device exception fc=0x%02X code=%d\n",
|
|
|
|
|
+ fc, resp[2]);
|
|
|
|
|
+ return 1;
|
|
|
|
|
+ }
|
|
|
|
|
+ if (resp[1] != fc) {
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] fc mismatch exp 0x%02X got 0x%02X\n",
|
|
|
|
|
+ fc, resp[1]);
|
|
|
return -1;
|
|
return -1;
|
|
|
}
|
|
}
|
|
|
- /* Normal response must be 8 bytes */
|
|
|
|
|
if (total < 8) {
|
|
if (total < 8) {
|
|
|
- fprintf(stderr, "[MODBUS-WRITE] Response too short: %zd/8 bytes\n", total);
|
|
|
|
|
- pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] short normal response %zd/8 (fc=0x%02X)\n",
|
|
|
|
|
+ total, fc);
|
|
|
return -1;
|
|
return -1;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- /* Verify start address echoed */
|
|
|
|
|
|
|
+ /* Echoed register address must match */
|
|
|
uint16_t resp_reg = (uint16_t)((resp[2] << 8) | resp[3]);
|
|
uint16_t resp_reg = (uint16_t)((resp[2] << 8) | resp[3]);
|
|
|
if (resp_reg != reg_addr) {
|
|
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);
|
|
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] reg mismatch exp 0x%04X got 0x%04X (fc=0x%02X)\n",
|
|
|
|
|
+ reg_addr, resp_reg, fc);
|
|
|
return -1;
|
|
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;
|
|
|
|
|
|
|
+ /* FC 0x06 echoes value; FC 0x10 echoes quantity=1 */
|
|
|
|
|
+ uint16_t resp_field = (uint16_t)((resp[4] << 8) | resp[5]);
|
|
|
|
|
+ if (fc == MODBUS_FC_WRITE_SINGLE) {
|
|
|
|
|
+ if (resp_field != value) {
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] value echo mismatch exp %u got %u\n",
|
|
|
|
|
+ value, resp_field);
|
|
|
|
|
+ return -1;
|
|
|
|
|
+ }
|
|
|
|
|
+ } else {
|
|
|
|
|
+ if (resp_field != 1) {
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] quantity echo mismatch exp 1 got %u\n",
|
|
|
|
|
+ resp_field);
|
|
|
|
|
+ return -1;
|
|
|
|
|
+ }
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- /* Verify CRC over first 6 bytes */
|
|
|
|
|
uint16_t recv_crc = (uint16_t)(resp[6] | (resp[7] << 8));
|
|
uint16_t recv_crc = (uint16_t)(resp[6] | (resp[7] << 8));
|
|
|
uint16_t calc_crc = crc16_modbus(resp, 6);
|
|
uint16_t calc_crc = crc16_modbus(resp, 6);
|
|
|
if (recv_crc != calc_crc) {
|
|
if (recv_crc != calc_crc) {
|
|
|
- fprintf(stderr, "[MODBUS-WRITE] CRC mismatch: recv=0x%04X calc=0x%04X\n",
|
|
|
|
|
- recv_crc, calc_crc);
|
|
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] CRC mismatch (fc=0x%02X)\n", fc);
|
|
|
|
|
+ return -1;
|
|
|
|
|
+ }
|
|
|
|
|
+ return 0;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+int modbus_write_register(modbus_client_t *ctx, uint8_t dev_addr,
|
|
|
|
|
+ uint16_t reg_addr, uint16_t value)
|
|
|
|
|
+{
|
|
|
|
|
+ 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);
|
|
pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Not connected\n");
|
|
|
return -1;
|
|
return -1;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
+ int sockfd = ctx->sockfd;
|
|
|
|
|
+ int timeout_sec = ctx->config.connect_timeout_sec;
|
|
|
pthread_mutex_unlock(&ctx->sock_mutex);
|
|
pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
- printf("[MODBUS-WRITE] Success: dev %d reg 0x%04X = %u\n",
|
|
|
|
|
- dev_addr, reg_addr, value);
|
|
|
|
|
- return 0;
|
|
|
|
|
|
|
+
|
|
|
|
|
+ /*
|
|
|
|
|
+ * Try FC 0x06 (Write Single Register) first - broadly supported by MPPT
|
|
|
|
|
+ * controllers. If the device explicitly rejects it (exception response),
|
|
|
|
|
+ * retry with FC 0x10 (Write Multiple Registers).
|
|
|
|
|
+ */
|
|
|
|
|
+ int rc;
|
|
|
|
|
+
|
|
|
|
|
+ pthread_mutex_lock(&ctx->sock_mutex);
|
|
|
|
|
+ rc = write_once(sockfd, MODBUS_FC_WRITE_SINGLE, dev_addr,
|
|
|
|
|
+ reg_addr, value, timeout_sec);
|
|
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
|
|
+
|
|
|
|
|
+ if (rc == 0) {
|
|
|
|
|
+ printf("[MODBUS-WRITE] Success (FC 0x06): dev %d reg 0x%04X = %u\n",
|
|
|
|
|
+ dev_addr, reg_addr, value);
|
|
|
|
|
+ return 0;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (rc == 1) {
|
|
|
|
|
+ /* Device rejected FC 0x06 explicitly - give it a moment, try FC 0x10 */
|
|
|
|
|
+ usleep(300000);
|
|
|
|
|
+ pthread_mutex_lock(&ctx->sock_mutex);
|
|
|
|
|
+ int rc2 = write_once(sockfd, MODBUS_FC_WRITE_MULTI, dev_addr,
|
|
|
|
|
+ reg_addr, value, timeout_sec);
|
|
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
|
|
+ if (rc2 == 0) {
|
|
|
|
|
+ printf("[MODBUS-WRITE] Success (FC 0x10 fallback): dev %d reg 0x%04X = %u\n",
|
|
|
|
|
+ dev_addr, reg_addr, value);
|
|
|
|
|
+ return 0;
|
|
|
|
|
+ }
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Failed both FC 0x06 and FC 0x10\n");
|
|
|
|
|
+ return -1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ /*
|
|
|
|
|
+ * rc == -1 with FC 0x06 was a transport/CRC/timeout issue, not an explicit
|
|
|
|
|
+ * reject. Still attempt FC 0x10 once (different framing sometimes helps).
|
|
|
|
|
+ */
|
|
|
|
|
+ usleep(300000);
|
|
|
|
|
+ pthread_mutex_lock(&ctx->sock_mutex);
|
|
|
|
|
+ rc = write_once(sockfd, MODBUS_FC_WRITE_MULTI, dev_addr,
|
|
|
|
|
+ reg_addr, value, timeout_sec);
|
|
|
|
|
+ pthread_mutex_unlock(&ctx->sock_mutex);
|
|
|
|
|
+
|
|
|
|
|
+ if (rc == 0) {
|
|
|
|
|
+ printf("[MODBUS-WRITE] Success (FC 0x10 after retry): dev %d reg 0x%04X = %u\n",
|
|
|
|
|
+ dev_addr, reg_addr, value);
|
|
|
|
|
+ return 0;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ fprintf(stderr, "[MODBUS-WRITE] Write failed for dev %d reg 0x%04X\n",
|
|
|
|
|
+ dev_addr, reg_addr);
|
|
|
|
|
+ return -1;
|
|
|
}
|
|
}
|