test-modbus/modbus.c

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