- txbuf[tx_head++] = unit_id;
- txbuf[tx_head++] = func | 0x80;
- txbuf[tx_head++] = code;
+ txbuf[tx_head++] = word >> 8;
+ txbuf[tx_head++] = word & 0xFF;
+}
+
+static mb_exception read_holding_regs(uint16_t start, uint16_t len)
+{
+ if (len > BUFSIZE/2 - 3)
+ return MB_ILLEGAL_ADDR;
+
+ if (start < MB_HOLD_REGS_BASE
+ || start + len > MB_HOLD_REGS_BASE + MB_N_HOLD_REGS)
+ return MB_ILLEGAL_ADDR;
+
+ put_byte(2*len);
+
+ start -= MB_HOLD_REGS_BASE;
+ while(len--)
+ put_word(hold_regs[start++]);
+
+ return MB_OK;
+}
+
+static mb_exception write_single_reg(uint16_t reg, uint16_t val)
+{
+ if (reg < MB_HOLD_REGS_BASE
+ || reg >= MB_HOLD_REGS_BASE + MB_N_HOLD_REGS)
+ return MB_ILLEGAL_ADDR;
+
+ if (!hold_reg_is_valid(reg, val))
+ return MB_ILLEGAL_VAL;
+
+ reg -= MB_HOLD_REGS_BASE;
+ hold_regs[reg] = val;
+#if MB_N_HOLD_REGS_EEPROM > 0
+ if (reg < MB_N_HOLD_REGS_EEPROM)
+ eeprom_write_word(&hold_regs_ee[reg], val);
+#endif
+ put_word(reg + MB_HOLD_REGS_BASE);
+ put_word(val);
+
+ return MB_OK;