+static void make_exception(mb_exception code)
+{
+ buffer[1] |= 0x80;
+ buffer[2] = code;
+ buf_len = 3;
+}
+
+#define get_word(ptr, off) (((uint16_t)ptr[off] << 8) | ptr[off+1])
+void put_byte(uint8_t byte)
+{
+ buffer[buf_len++] = byte;
+}
+
+void put_word(uint16_t word)
+{
+ buffer[buf_len++] = word >> 8;
+ buffer[buf_len++] = 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)