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