test-modbus/modbus.c

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