]> mj.ucw.cz Git - home-hw.git/blob - lib/modbus.c
c6336460d307d1a8fe814e77b3159409ddaeb113
[home-hw.git] / lib / modbus.c
1 /*
2  *      Generic MODBUS Library for STM32
3  *
4  *      (c) 2019 Martin Mareš <mj@ucw.cz>
5  */
6
7 #include "util.h"
8 #include "modbus.h"
9
10 #include <stddef.h>
11 #include <string.h>
12
13 #include <libopencm3/cm3/nvic.h>
14 #include <libopencm3/stm32/gpio.h>
15 #include <libopencm3/stm32/usart.h>
16 #include <libopencm3/stm32/timer.h>
17
18 /*** Configuration ***/
19
20 // You should set the following parameters in config.h
21
22 // USART (pins are expected to be configured by the caller)
23 // #define MODBUS_USART USART2
24 // #define MODBUS_NVIC_USART_IRQ NVIC_USART2_IRQ
25 // #define MODBUS_USART_ISR usart2_isr
26
27 // GPIO pin for transmitter enable (pins is expected to be configured by the caller)
28 // #define MODBUS_TXEN_GPIO_PORT GPIOA
29 // #define MODBUS_TXEN_GPIO_PIN GPIO1
30
31 // Timer
32 // #define MODBUS_TIMER TIM2
33 // #define MODBUS_NVIC_TIMER_IRQ NVIC_TIM2_IRQ
34 // #define MODBUS_TIMER_ISR tim2_isr
35
36 // Slave address we are responding at
37 // #define MODBUS_OUR_ADDRESS 42
38
39 // Baud rate
40 #ifndef MODBUS_BAUD_RATE
41 #define MODBUS_BAUD_RATE 19200
42 #endif
43
44 // CPU clock frequency
45 // #define CPU_CLOCK_MHZ 72
46
47 // Receive buffer size (standard specifies 256 bytes, you can make it shorter if necessary)
48 #ifndef MODBUS_RX_BUFSIZE
49 #define MODBUS_RX_BUFSIZE 256
50 #endif
51
52 // Transmit buffer size (standard specifies 256 bytes, you can make it shorter if necessary)
53 #ifndef MODBUS_TX_BUFSIZE
54 #define MODBUS_TX_BUFSIZE 256
55 #endif
56
57 // Receive timeout in microseconds
58 #ifndef MODBUS_RX_TIMEOUT
59 #if MODBUS_BAUD_RATE <= 19200
60 // For low baud rates, the standard specifies timeout of 1.5 character times
61 // (1 character = start bit + 8 data bits + parity bit + stop bit = 11 bits)
62 #define MODBUS_RX_TIMEOUT (1000000*11*3/2/MODBUS_BAUD_RATE)
63 #else
64 // For high rates, the timeout is fixed to 750 μs
65 #define MODBUS_RX_TIMEOUT 750
66 #endif
67 #endif
68
69 /*** State ***/
70
71 enum mb_state {
72         STATE_RX,
73         STATE_RX_DONE,
74         STATE_PROCESSING,
75         STATE_TX,
76         STATE_TX_LAST,
77         STATE_TX_DONE,
78 };
79
80 static byte rx_buf[MODBUS_RX_BUFSIZE];
81 static u16 rx_size;
82 static byte rx_bad;
83 static byte state;              // STATE_xxx
84
85 static byte *rx_frame;
86 static byte *rx_frame_end;
87
88 static byte tx_buf[MODBUS_TX_BUFSIZE];
89 static u16 tx_size;
90 static u16 tx_pos;
91
92 static bool check_frame(void);
93 static void process_frame(void);
94
95 /*** Low-level layer ***/
96
97 static void rx_init(void)
98 {
99         state = STATE_RX;
100         rx_size = 0;
101         rx_bad = 0;
102         usart_set_mode(MODBUS_USART, USART_MODE_RX);
103         usart_enable_rx_interrupt(MODBUS_USART);
104 }
105
106 static void rx_done(void)
107 {
108         state = STATE_RX_DONE;
109         usart_disable_rx_interrupt(MODBUS_USART);
110 }
111
112 static void tx_init(void)
113 {
114         state = STATE_TX;
115         tx_pos = 0;
116         gpio_set(MODBUS_TXEN_GPIO_PORT, MODBUS_TXEN_GPIO_PIN);
117         usart_set_mode(MODBUS_USART, USART_MODE_TX);
118         usart_enable_tx_interrupt(MODBUS_USART);
119 }
120
121 static void tx_done(void)
122 {
123         state = STATE_TX_DONE;
124         // usart_disable_tx_interrupt(MODBUS_USART);            // Already done by irq handler
125         gpio_clear(MODBUS_TXEN_GPIO_PORT, MODBUS_TXEN_GPIO_PIN);
126 }
127
128 void modbus_init(void)
129 {
130         timer_set_prescaler(MODBUS_TIMER, CPU_CLOCK_MHZ-1);     // 1 tick = 1 μs
131         timer_set_mode(MODBUS_TIMER, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_DOWN);
132         timer_update_on_overflow(MODBUS_TIMER);
133         timer_disable_preload(MODBUS_TIMER);
134         timer_one_shot_mode(MODBUS_TIMER);
135         timer_enable_irq(MODBUS_TIMER, TIM_DIER_UIE);
136         nvic_enable_irq(MODBUS_NVIC_TIMER_IRQ);
137
138         gpio_clear(MODBUS_TXEN_GPIO_PORT, MODBUS_TXEN_GPIO_PIN);
139
140         usart_set_baudrate(MODBUS_USART, MODBUS_BAUD_RATE);
141         usart_set_databits(MODBUS_USART, 9);
142         usart_set_stopbits(MODBUS_USART, USART_STOPBITS_1);
143         usart_set_parity(MODBUS_USART, USART_PARITY_EVEN);
144         usart_set_flow_control(MODBUS_USART, USART_FLOWCONTROL_NONE);
145
146         rx_init();
147
148         nvic_enable_irq(MODBUS_NVIC_USART_IRQ);
149         usart_enable(MODBUS_USART);
150 }
151
152 void MODBUS_USART_ISR(void)
153 {
154         u32 status = USART_SR(MODBUS_USART);
155
156         if (status & USART_SR_RXNE) {
157                 uint ch = usart_recv(MODBUS_USART);
158                 if (state == STATE_RX) {
159                         if (status & (USART_SR_FE | USART_SR_ORE | USART_SR_NE)) {
160                                 rx_bad = 1;
161                         } else if (rx_size < MODBUS_RX_BUFSIZE) {
162                                 rx_buf[rx_size++] = ch;
163                         } else {
164                                 // Frame too long
165                                 rx_bad = 2;
166                         }
167                         timer_set_period(MODBUS_TIMER, MODBUS_RX_TIMEOUT);
168                         timer_generate_event(MODBUS_TIMER, TIM_EGR_UG);
169                         timer_enable_counter(MODBUS_TIMER);
170                 }
171         }
172
173         if (state == STATE_TX) {
174                 if (status & USART_SR_TXE) {
175                         if (tx_pos < tx_size) {
176                                 usart_send(MODBUS_USART, tx_buf[tx_pos++]);
177                         } else {
178                                 // The transmitter is double-buffered, so at this moment, it is transmitting
179                                 // the last byte of the frame. Wait until transfer is completed.
180                                 usart_disable_tx_interrupt(MODBUS_USART);
181                                 USART_CR1(MODBUS_USART) |= USART_CR1_TCIE;
182                                 state = STATE_TX_LAST;
183                         }
184                 }
185         } else if (state == STATE_TX_LAST) {
186                 if (status & USART_SR_TC) {
187                         // Transfer of the last byte is complete. Release the bus.
188                         USART_CR1(MODBUS_USART) &= ~USART_CR1_TCIE;
189                         tx_done();
190                         rx_init();
191                 }
192         }
193 }
194
195 void MODBUS_TIMER_ISR(void)
196 {
197         if (TIM_SR(MODBUS_TIMER) & TIM_SR_UIF) {
198                 TIM_SR(MODBUS_TIMER) &= ~TIM_SR_UIF;
199                 if (state == STATE_RX)
200                         rx_done();
201         }
202 }
203
204 void modbus_loop(void)
205 {
206         if (state != STATE_RX_DONE)
207                 return;
208         state = STATE_PROCESSING;
209
210         if (!check_frame()) {
211                 rx_init();
212                 return;
213         }
214
215         if (rx_buf[0] == MODBUS_OUR_ADDRESS) {
216                 // Frame addressed to us: process and reply
217                 process_frame();
218                 tx_init();
219         } else if (rx_buf[0] == 0x00) {
220                 // Broadcast frame: process, but do not reply
221                 process_frame();
222                 rx_init();
223         } else {
224                 // Somebody else's frame: discard
225                 rx_init();
226         }
227 }
228
229 /** CRC ***/
230
231 static const byte crc_hi[] = {
232         0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
233         0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
234         0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
235         0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
236         0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
237         0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41,
238         0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
239         0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
240         0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
241         0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
242         0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
243         0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
244         0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
245         0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
246         0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
247         0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
248         0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
249         0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
250         0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
251         0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
252         0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
253         0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
254         0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
255         0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
256         0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
257         0x80, 0x41, 0x00, 0xc1, 0x81, 0x40
258 };
259
260 static const byte crc_lo[] = {
261         0x00, 0xc0, 0xc1, 0x01, 0xc3, 0x03, 0x02, 0xc2, 0xc6, 0x06,
262         0x07, 0xc7, 0x05, 0xc5, 0xc4, 0x04, 0xcc, 0x0c, 0x0d, 0xcd,
263         0x0f, 0xcf, 0xce, 0x0e, 0x0a, 0xca, 0xcb, 0x0b, 0xc9, 0x09,
264         0x08, 0xc8, 0xd8, 0x18, 0x19, 0xd9, 0x1b, 0xdb, 0xda, 0x1a,
265         0x1e, 0xde, 0xdf, 0x1f, 0xdd, 0x1d, 0x1c, 0xdc, 0x14, 0xd4,
266         0xd5, 0x15, 0xd7, 0x17, 0x16, 0xd6, 0xd2, 0x12, 0x13, 0xd3,
267         0x11, 0xd1, 0xd0, 0x10, 0xf0, 0x30, 0x31, 0xf1, 0x33, 0xf3,
268         0xf2, 0x32, 0x36, 0xf6, 0xf7, 0x37, 0xf5, 0x35, 0x34, 0xf4,
269         0x3c, 0xfc, 0xfd, 0x3d, 0xff, 0x3f, 0x3e, 0xfe, 0xfa, 0x3a,
270         0x3b, 0xfb, 0x39, 0xf9, 0xf8, 0x38, 0x28, 0xe8, 0xe9, 0x29,
271         0xeb, 0x2b, 0x2a, 0xea, 0xee, 0x2e, 0x2f, 0xef, 0x2d, 0xed,
272         0xec, 0x2c, 0xe4, 0x24, 0x25, 0xe5, 0x27, 0xe7, 0xe6, 0x26,
273         0x22, 0xe2, 0xe3, 0x23, 0xe1, 0x21, 0x20, 0xe0, 0xa0, 0x60,
274         0x61, 0xa1, 0x63, 0xa3, 0xa2, 0x62, 0x66, 0xa6, 0xa7, 0x67,
275         0xa5, 0x65, 0x64, 0xa4, 0x6c, 0xac, 0xad, 0x6d, 0xaf, 0x6f,
276         0x6e, 0xae, 0xaa, 0x6a, 0x6b, 0xab, 0x69, 0xa9, 0xa8, 0x68,
277         0x78, 0xb8, 0xb9, 0x79, 0xbb, 0x7b, 0x7a, 0xba, 0xbe, 0x7e,
278         0x7f, 0xbf, 0x7d, 0xbd, 0xbc, 0x7c, 0xb4, 0x74, 0x75, 0xb5,
279         0x77, 0xb7, 0xb6, 0x76, 0x72, 0xb2, 0xb3, 0x73, 0xb1, 0x71,
280         0x70, 0xb0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
281         0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9c, 0x5c,
282         0x5d, 0x9d, 0x5f, 0x9f, 0x9e, 0x5e, 0x5a, 0x9a, 0x9b, 0x5b,
283         0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4b, 0x8b,
284         0x8a, 0x4a, 0x4e, 0x8e, 0x8f, 0x4f, 0x8d, 0x4d, 0x4c, 0x8c,
285         0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
286         0x43, 0x83, 0x41, 0x81, 0x80, 0x40
287 };
288
289 static u16 crc16(byte *buf, u16 len)
290 {
291         byte hi = 0xff, lo = 0xff;
292
293         while (len--) {
294                 byte i = hi ^ *buf++;
295                 hi = lo ^ crc_hi[i];
296                 lo = crc_lo[i];
297         }
298
299         return (hi << 8 | lo);
300 }
301
302 /*** High-level layer ***/
303
304 static bool check_frame(void)
305 {
306         if (rx_bad) {
307                 // FIXME: Error counters?
308                 return false;
309         }
310         
311         if (rx_size < 4) {
312                 // FIXME: Error counters?
313                 return false;
314         }
315
316         u16 crc = crc16(rx_buf, rx_size - 2);
317         u16 rx_crc = (rx_buf[rx_size-2] << 8) | rx_buf[rx_size-1];
318         if (crc != rx_crc) {
319                 // FIXME: Error counters?
320                 return false;
321         }
322
323         rx_frame = rx_buf + 1;
324         rx_frame_end = rx_frame + rx_size - 2;
325         return true;
326 }
327
328 enum mb_function {
329         FUNC_READ_COILS = 0x01,
330         FUNC_READ_DISCRETE_INPUTS = 0x02,
331         FUNC_READ_HOLDING_REGISTERS = 0x03,
332         FUNC_READ_INPUT_REGISTERS = 0x04,
333         FUNC_WRITE_SINGLE_COIL = 0x05,
334         FUNC_WRITE_SINGLE_REGISTER = 0x06,
335         FUNC_READ_EXCEPTION_STATUS = 0x07,
336         FUNC_DIAGNOSTICS = 0x08,
337         FUNC_GET_COMM_EVENT_COUNTER = 0x0b,
338         FUNC_GET_COMM_EVENT_LOG = 0x0c,
339         FUNC_WRITE_MULTIPLE_COILS = 0x0f,
340         FUNC_WRITE_MULTIPLE_REGISTERS = 0x10,
341         FUNC_REPORT_SLAVE_ID = 0x11,
342         FUNC_READ_FILE_RECORD = 0x14,
343         FUNC_WRITE_FILE_RECORD = 0x15,
344         FUNC_MASK_WRITE_REGISTER = 0x16,
345         FUNC_READ_WRITE_MULTIPLE_REGISTERS = 0x17,
346         FUNC_READ_FIFO_QUEUE = 0x18,
347         FUNC_ENCAPSULATED_INTERFACE_TRANSPORT = 0x2b,
348 };
349
350 enum mb_error {
351         ERR_ILLEGAL_FUNCTION = 0x01,
352         ERR_ILLEGAL_DATA_ADDRESS = 0x02,
353         ERR_ILLEGAL_DATA_VALUE = 0x03,
354 };
355
356 static uint read_remains(void)
357 {
358         return rx_frame_end - rx_frame;
359 }
360
361 static byte read_byte(void)
362 {
363         return *rx_frame++;
364 }
365
366 static u16 read_u16(void)
367 {
368         byte hi = *rx_frame++;
369         byte lo = *rx_frame++;
370         return (hi << 8) | lo;
371 }
372
373 static void write_byte(byte v)
374 {
375         tx_buf[tx_size++] = v;
376 }
377
378 static void write_u16(u16 v)
379 {
380         write_byte(v >> 8);
381         write_byte(v);
382 }
383
384 static bool body_fits(uint body_len)
385 {
386         // body_len excludes slave address, function code, and CRC
387         return (2 + body_len + 2 <= MODBUS_TX_BUFSIZE);
388 }
389
390 static void report_error(byte code)
391 {
392         // Discard the partially constructed body of the reply and rewrite the header
393         tx_buf[1] |= 0x80;
394         tx_buf[2] = code;
395         tx_size = 3;
396 }
397
398 static void func_read_bits(bool coils)
399 {
400         if (read_remains() < 4)
401                 return report_error(ERR_ILLEGAL_DATA_VALUE);
402
403         u16 start = read_u16();
404         u16 count = read_u16();
405
406         uint bytes = (count+7) / 8;
407         if (!body_fits(1 + bytes))
408                 return report_error(ERR_ILLEGAL_DATA_VALUE);
409
410         for (u16 i = 0; i < count; i++)
411                 if (!(coils ? modbus_check_coil : modbus_check_discrete_input)(start + i))
412                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
413
414         write_byte(bytes);
415         for (u16 i = 0; i < bytes; i++) {
416                 byte b = 0;
417                 for (byte j = 0; j < 8 && 8*i + j < count; j++) {
418                         uint addr = start + 8*i + j;
419                         if ((coils ? modbus_get_coil : modbus_get_discrete_input)(addr))
420                                 b |= 1 << j;
421                 }
422                 write_byte(b);
423         }
424 }
425
426 static void func_read_registers(byte holding)
427 {
428         if (read_remains() < 4)
429                 return report_error(ERR_ILLEGAL_DATA_VALUE);
430
431         u16 start = read_u16();
432         u16 count = read_u16();
433
434         uint bytes = 2*count;
435         if (!body_fits(1 + bytes))
436                 return report_error(ERR_ILLEGAL_DATA_VALUE);
437
438         for (u16 i = 0; i < count; i++)
439                 if (!(holding ? modbus_check_holding_register : modbus_check_input_register)(start + i))
440                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
441
442         // FIXME: Reporting of slave failures?
443         write_byte(bytes);
444         for (u16 i = 0; i < count; i++)
445                 write_u16((holding ? modbus_get_holding_register : modbus_get_input_register)(start + i));
446 }
447
448 static void func_write_single_coil(void)
449 {
450         if (read_remains() < 4)
451                 return report_error(ERR_ILLEGAL_DATA_VALUE);
452
453         u16 addr = read_u16();
454         u16 value = read_u16();
455
456         if (!modbus_check_coil(addr))
457                 return report_error(ERR_ILLEGAL_DATA_ADDRESS);
458         if (value != 0x0000 && value != 0xff00)
459                 return report_error(ERR_ILLEGAL_DATA_VALUE);
460
461         modbus_set_coil(addr, value);
462 }
463
464 static void func_write_single_register(void)
465 {
466         if (read_remains() < 4)
467                 return report_error(ERR_ILLEGAL_DATA_VALUE);
468
469         u16 addr = read_u16();
470         u16 value = read_u16();
471
472         if (!modbus_check_holding_register(addr))
473                 return report_error(ERR_ILLEGAL_DATA_ADDRESS);
474
475         modbus_set_holding_register(addr, value);
476 }
477
478 static void func_write_multiple_coils(void)
479 {
480         if (read_remains() < 5)
481                 return report_error(ERR_ILLEGAL_DATA_VALUE);
482
483         u16 start = read_u16();
484         u16 count = read_u16();
485         byte bytes = read_byte();
486
487         if (read_remains() < bytes || bytes != (count+7) / 8)
488                 return report_error(ERR_ILLEGAL_DATA_VALUE);
489
490         for (u16 i = 0; i < count; i++)
491                 if (!modbus_check_coil(start + i))
492                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
493
494         for (u16 i = 0; i < count; i++)
495                 modbus_set_coil(start + i, rx_frame[i/8] & (1U << (i%8)));
496 }
497
498 static void func_write_multiple_registers(void)
499 {
500         if (read_remains() < 5)
501                 return report_error(ERR_ILLEGAL_DATA_VALUE);
502
503         u16 start = read_u16();
504         u16 count = read_u16();
505         byte bytes = read_byte();
506
507         if (read_remains() < bytes || bytes != 2*count)
508                 return report_error(ERR_ILLEGAL_DATA_VALUE);
509
510         for (u16 i = 0; i < count; i++)
511                 if (!modbus_check_holding_register(start + i))
512                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
513
514         for (u16 i = 0; i < count; i++)
515                 modbus_set_holding_register(start + i, read_u16());
516 }
517
518 static void func_mask_write_register(void)
519 {
520         if (read_remains() < 6)
521                 return report_error(ERR_ILLEGAL_DATA_VALUE);
522
523         u16 addr = read_u16();
524         u16 and_mask = read_u16();
525         u16 or_mask = read_u16();
526
527         if (!modbus_check_holding_register(addr))
528                 return report_error(ERR_ILLEGAL_DATA_ADDRESS);
529
530         u16 reg = modbus_get_holding_register(addr);
531         reg = (reg & and_mask) | (or_mask & ~and_mask);
532         modbus_set_holding_register(addr, reg);
533 }
534
535 static void func_read_write_multiple_registers(void)
536 {
537         if (read_remains() < 9)
538                 return report_error(ERR_ILLEGAL_DATA_VALUE);
539
540         u16 read_start = read_u16();
541         u16 read_count = read_u16();
542         u16 write_start = read_u16();
543         u16 write_count = read_u16();
544         byte write_bytes = read_byte();
545
546         if (read_remains() < write_bytes || write_bytes != 2*write_count)
547                 return report_error(ERR_ILLEGAL_DATA_VALUE);
548
549         for (u16 i = 0; i < read_count; i++)
550                 if (!modbus_check_holding_register(read_start + i))
551                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
552
553         for (u16 i = 0; i < write_count; i++)
554                 if (!modbus_check_holding_register(write_start + i))
555                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
556
557         byte read_bytes = 2*write_count;
558         if (!body_fits(1 + read_bytes))
559                 return report_error(ERR_ILLEGAL_DATA_VALUE);
560
561         for (u16 i = 0; i < write_count; i++)
562                 modbus_set_holding_register(write_start + i, read_u16());
563
564         write_byte(read_bytes);
565         for (u16 i = 0; i < read_count; i++)
566                 modbus_get_holding_register(read_start + i);
567 }
568
569 static void func_encapsulated_interface_transport(void)
570 {
571         if (read_remains() < 3 ||
572             read_byte() != 0x0e)
573                 return report_error(ERR_ILLEGAL_DATA_VALUE);
574
575         byte action = read_byte();
576         byte id = read_byte();
577
578         byte range_min, range_max;
579         switch (action) {
580                 case 1:
581                         // Streaming access to basic identification
582                         range_min = MODBUS_ID_VENDOR_NAME;
583                         range_max = MODBUS_ID_MAJOR_MINOR_REVISION;
584                         break;
585                 case 2:
586                         // Streaming access to regular identification
587                         range_min = MODBUS_ID_VENDOR_URL;
588                         range_max = MODBUS_ID_USER_APP_NAME;
589                         break;
590                 case 4:
591                         // Individual access
592                         if (id >= MODBUS_ID_MAX || !modbus_id_strings[id])
593                                 return report_error(ERR_ILLEGAL_DATA_ADDRESS);
594                         range_min = range_max = id;
595                         break;
596                 default:
597                         return report_error(ERR_ILLEGAL_DATA_VALUE);
598         }
599
600         if (action != 4) {
601                 if (id < range_min || id > range_max)
602                         id = range_min;
603         }
604
605         write_byte(0x0e);       // Repeat a part of the request
606         write_byte(action);
607
608         // Conformity level
609         if (modbus_id_strings[MODBUS_ID_VENDOR_URL] ||
610             modbus_id_strings[MODBUS_ID_PRODUCT_NAME] ||
611             modbus_id_strings[MODBUS_ID_USER_APP_NAME])
612                 write_byte(0x82);       // Regular identification, both stream and individual access supported
613         else
614                 write_byte(0x81);       // Basic identification only
615
616         u16 more_follows_at = tx_size;
617         write_byte(0);          // More follows: so far not
618         write_byte(0);          // Next object ID: so far none
619         write_byte(0);          // Number of objects
620
621         for (id = range_min; id <= range_max; id++) {
622                 if (modbus_id_strings[id]) {
623                         byte len = strlen(modbus_id_strings[id]);
624                         byte remains = MODBUS_TX_BUFSIZE - 4 - tx_size; // 2 for CRC, 2 for object header
625                         if (len > remains) {
626                                 // If it is the only object, cut it
627                                 if (!tx_buf[more_follows_at + 2])
628                                         len = remains;
629                                 else {
630                                         // More follows, report the next ID
631                                         tx_buf[more_follows_at] = 0xff;
632                                         tx_buf[more_follows_at + 1] = id;
633                                         break;
634                                 }
635                         }
636                         tx_buf[more_follows_at + 2] ++;
637                         write_byte(id);
638                         write_byte(len);
639                         memcpy(tx_buf + tx_size, modbus_id_strings[id], len);
640                         tx_size += len;
641                 }
642         }
643 }
644
645 static void process_frame(void)
646 {
647         byte func = read_byte();
648
649         // Prepare reply frame
650         tx_buf[0] = MODBUS_OUR_ADDRESS;
651         tx_buf[1] = rx_buf[1];
652         tx_size = 2;
653
654         switch (func) {
655                 case FUNC_READ_COILS:
656                         func_read_bits(true);
657                         break;
658                 case FUNC_READ_DISCRETE_INPUTS:
659                         func_read_bits(false);
660                         break;
661                 case FUNC_READ_HOLDING_REGISTERS:
662                         func_read_registers(true);
663                         break;
664                 case FUNC_READ_INPUT_REGISTERS:
665                         func_read_registers(false);
666                         break;
667                 case FUNC_WRITE_SINGLE_COIL:
668                         func_write_single_coil();
669                         break;
670                 case FUNC_WRITE_SINGLE_REGISTER:
671                         func_write_single_register();
672                         break;
673                 case FUNC_WRITE_MULTIPLE_COILS:
674                         func_write_multiple_coils();
675                         break;
676                 case FUNC_WRITE_MULTIPLE_REGISTERS:
677                         func_write_multiple_registers();
678                         break;
679                 case FUNC_MASK_WRITE_REGISTER:
680                         func_mask_write_register();
681                         break;
682                 case FUNC_READ_WRITE_MULTIPLE_REGISTERS:
683                         func_read_write_multiple_registers();
684                         break;
685                 case FUNC_ENCAPSULATED_INTERFACE_TRANSPORT:
686                         func_encapsulated_interface_transport();
687                         break;
688                 default:
689                         report_error(ERR_ILLEGAL_FUNCTION);
690         }
691
692         // Finish reply frame
693         write_u16(crc16(tx_buf, tx_size));
694 }