test-modbus/modbus.c

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