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