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         write_u16(addr);
 483         write_u16(value);
 484 }
 485
 486 static void func_write_single_register(void)
 487 {
 488         if (read_remains() < 4)
 489                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 490
 491         u16 addr = read_u16();
 492         u16 value = read_u16();
 493
 494         if (!modbus_check_holding_register(addr))
 495                 return report_error(ERR_ILLEGAL_DATA_ADDRESS);
 496
 497         modbus_set_holding_register(addr, value);
 498
 499         write_u16(addr);
 500         write_u16(value);
 501 }
 502
 503 static void func_write_multiple_coils(void)
 504 {
 505         if (read_remains() < 5)
 506                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 507
 508         u16 start = read_u16();
 509         u16 count = read_u16();
 510         byte bytes = read_byte();
 511
 512         if (read_remains() < bytes || bytes != (count+7) / 8)
 513                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 514
 515         for (u16 i = 0; i < count; i++)
 516                 if (!modbus_check_coil(start + i))
 517                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
 518
 519         for (u16 i = 0; i < count; i++)
 520                 modbus_set_coil(start + i, rx_frame[i/8] & (1U << (i%8)));
 521
 522         write_u16(start);
 523         write_u16(count);
 524 }
 525
 526 static void func_write_multiple_registers(void)
 527 {
 528         if (read_remains() < 5)
 529                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 530
 531         u16 start = read_u16();
 532         u16 count = read_u16();
 533         byte bytes = read_byte();
 534
 535         if (read_remains() < bytes || bytes != 2*count)
 536                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 537
 538         for (u16 i = 0; i < count; i++)
 539                 if (!modbus_check_holding_register(start + i))
 540                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
 541
 542         for (u16 i = 0; i < count; i++)
 543                 modbus_set_holding_register(start + i, read_u16());
 544
 545         write_u16(start);
 546         write_u16(count);
 547 }
 548
 549 static void func_mask_write_register(void)
 550 {
 551         if (read_remains() < 6)
 552                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 553
 554         u16 addr = read_u16();
 555         u16 and_mask = read_u16();
 556         u16 or_mask = read_u16();
 557
 558         if (!modbus_check_holding_register(addr))
 559                 return report_error(ERR_ILLEGAL_DATA_ADDRESS);
 560
 561         u16 reg = modbus_get_holding_register(addr);
 562         reg = (reg & and_mask) | (or_mask & ~and_mask);
 563         modbus_set_holding_register(addr, reg);
 564
 565         write_u16(addr);
 566         write_u16(and_mask);
 567         write_u16(or_mask);
 568 }
 569
 570 static void func_read_write_multiple_registers(void)
 571 {
 572         if (read_remains() < 9)
 573                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 574
 575         u16 read_start = read_u16();
 576         u16 read_count = read_u16();
 577         u16 write_start = read_u16();
 578         u16 write_count = read_u16();
 579         byte write_bytes = read_byte();
 580
 581         if (read_remains() < write_bytes || write_bytes != 2*write_count)
 582                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 583
 584         for (u16 i = 0; i < read_count; i++)
 585                 if (!modbus_check_holding_register(read_start + i))
 586                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
 587
 588         for (u16 i = 0; i < write_count; i++)
 589                 if (!modbus_check_holding_register(write_start + i))
 590                         return report_error(ERR_ILLEGAL_DATA_ADDRESS);
 591
 592         byte read_bytes = 2*write_count;
 593         if (!body_fits(1 + read_bytes))
 594                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 595
 596         for (u16 i = 0; i < write_count; i++)
 597                 modbus_set_holding_register(write_start + i, read_u16());
 598
 599         write_byte(read_bytes);
 600         for (u16 i = 0; i < read_count; i++)
 601                 modbus_get_holding_register(read_start + i);
 602 }
 603
 604 static void func_encapsulated_interface_transport(void)
 605 {
 606         if (read_remains() < 3 ||
 607             read_byte() != 0x0e)
 608                 return report_error(ERR_ILLEGAL_DATA_VALUE);
 609
 610         byte action = read_byte();
 611         byte id = read_byte();
 612
 613         byte range_min, range_max;
 614         switch (action) {
 615                 case 1:
 616                         // Streaming access to basic identification
 617                         range_min = MODBUS_ID_VENDOR_NAME;
 618                         range_max = MODBUS_ID_MAJOR_MINOR_REVISION;
 619                         break;
 620                 case 2:
 621                         // Streaming access to regular identification
 622                         range_min = MODBUS_ID_VENDOR_URL;
 623                         range_max = MODBUS_ID_USER_APP_NAME;
 624                         break;
 625                 case 4:
 626                         // Individual access
 627                         if (id >= MODBUS_ID_MAX || !modbus_id_strings[id])
 628                                 return report_error(ERR_ILLEGAL_DATA_ADDRESS);
 629                         range_min = range_max = id;
 630                         break;
 631                 default:
 632                         return report_error(ERR_ILLEGAL_DATA_VALUE);
 633         }
 634
 635         if (action != 4) {
 636                 if (id < range_min || id > range_max)
 637                         id = range_min;
 638         }
 639
 640         write_byte(0x0e);       // Repeat a part of the request
 641         write_byte(action);
 642
 643         // Conformity level
 644         if (modbus_id_strings[MODBUS_ID_VENDOR_URL] ||
 645             modbus_id_strings[MODBUS_ID_PRODUCT_NAME] ||
 646             modbus_id_strings[MODBUS_ID_USER_APP_NAME])
 647                 write_byte(0x82);       // Regular identification, both stream and individual access supported
 648         else
 649                 write_byte(0x81);       // Basic identification only
 650
 651         u16 more_follows_at = tx_size;
 652         write_byte(0);          // More follows: so far not
 653         write_byte(0);          // Next object ID: so far none
 654         write_byte(0);          // Number of objects
 655
 656         for (id = range_min; id <= range_max; id++) {
 657                 if (modbus_id_strings[id]) {
 658                         byte len = strlen(modbus_id_strings[id]);
 659                         byte remains = MODBUS_TX_BUFSIZE - 4 - tx_size; // 2 for CRC, 2 for object header
 660                         if (len > remains) {
 661                                 // If it is the only object, cut it
 662                                 if (!tx_buf[more_follows_at + 2])
 663                                         len = remains;
 664                                 else {
 665                                         // More follows, report the next ID
 666                                         tx_buf[more_follows_at] = 0xff;
 667                                         tx_buf[more_follows_at + 1] = id;
 668                                         break;
 669                                 }
 670                         }
 671                         tx_buf[more_follows_at + 2] ++;
 672                         write_byte(id);
 673                         write_byte(len);
 674                         memcpy(tx_buf + tx_size, modbus_id_strings[id], len);
 675                         tx_size += len;
 676                 }
 677         }
 678 }
 679
 680 static void process_frame(void)
 681 {
 682         byte func = read_byte();
 683
 684         // Prepare reply frame
 685         tx_buf[0] = MODBUS_OUR_ADDRESS;
 686         tx_buf[1] = rx_buf[1];
 687         tx_size = 2;
 688
 689         switch (func) {
 690                 case FUNC_READ_COILS:
 691                         func_read_bits(true);
 692                         break;
 693                 case FUNC_READ_DISCRETE_INPUTS:
 694                         func_read_bits(false);
 695                         break;
 696                 case FUNC_READ_HOLDING_REGISTERS:
 697                         func_read_registers(true);
 698                         break;
 699                 case FUNC_READ_INPUT_REGISTERS:
 700                         func_read_registers(false);
 701                         break;
 702                 case FUNC_WRITE_SINGLE_COIL:
 703                         func_write_single_coil();
 704                         break;
 705                 case FUNC_WRITE_SINGLE_REGISTER:
 706                         func_write_single_register();
 707                         break;
 708                 case FUNC_WRITE_MULTIPLE_COILS:
 709                         func_write_multiple_coils();
 710                         break;
 711                 case FUNC_WRITE_MULTIPLE_REGISTERS:
 712                         func_write_multiple_registers();
 713                         break;
 714                 case FUNC_MASK_WRITE_REGISTER:
 715                         func_mask_write_register();
 716                         break;
 717                 case FUNC_READ_WRITE_MULTIPLE_REGISTERS:
 718                         func_read_write_multiple_registers();
 719                         break;
 720                 case FUNC_ENCAPSULATED_INTERFACE_TRANSPORT:
 721                         func_encapsulated_interface_transport();
 722                         break;
 723                 default:
 724                         report_error(ERR_ILLEGAL_FUNCTION);
 725         }
 726
 727         // Finish reply frame
 728         write_u16(crc16(tx_buf, tx_size));
 729 }