test-modbus/modbus.c

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