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