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