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 }