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 tx_buf[TX_BUFSIZE];
  27 static u16 tx_size;
  28 static u16 tx_pos;
  29
  30 #define MB_OUR_ADDRESS 42
  31
  32 static void UNUSED xx_write_char(uint c)        // FIXME
  33 {
  34         usart_set_mode(USART2, USART_MODE_TX);
  35         gpio_set(GPIOA, GPIO1);
  36         usart_send_blocking(USART2, c);
  37         while (!usart_get_flag(USART2, USART_SR_TC))
  38                 ;
  39         gpio_clear(GPIOA, GPIO1);
  40         usart_set_mode(USART2, USART_MODE_RX);
  41 }
  42
  43 static void rx_init(void)
  44 {
  45         state = STATE_RX;
  46         rx_size = 0;
  47         rx_bad = 0;
  48         usart_set_mode(USART2, USART_MODE_RX);
  49         usart_enable_rx_interrupt(USART2);
  50 }
  51
  52 static void rx_done(void)
  53 {
  54         state = STATE_RX_DONE;
  55         usart_disable_rx_interrupt(USART2);
  56 }
  57
  58 static void tx_init(void)
  59 {
  60         state = STATE_TX;
  61         tx_pos = 0;
  62         gpio_set(GPIOA, GPIO1);
  63         usart_set_mode(USART2, USART_MODE_TX);
  64         usart_enable_tx_interrupt(USART2);
  65 }
  66
  67 static void tx_done(void)
  68 {
  69         state = STATE_TX_DONE;
  70         // usart_disable_tx_interrupt(USART2);          // Already done by irq handler
  71         gpio_clear(GPIOA, GPIO1);
  72 }
  73
  74 void modbus_init(void)
  75 {
  76         timer_set_prescaler(TIM2, 71);          // 1 tick = 1 μs
  77         timer_set_mode(TIM2, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_DOWN);
  78         timer_update_on_overflow(TIM2);
  79         timer_disable_preload(TIM2);
  80         timer_one_shot_mode(TIM2);
  81         timer_enable_irq(TIM2, TIM_DIER_UIE);
  82         nvic_enable_irq(NVIC_TIM2_IRQ);
  83
  84         gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO_USART2_RX);
  85         gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART2_TX);
  86
  87         gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO1);
  88         gpio_clear(GPIOA, GPIO1);
  89
  90         usart_set_baudrate(USART2, 19200);
  91         usart_set_databits(USART2, 8);
  92         usart_set_stopbits(USART2, USART_STOPBITS_1);
  93         usart_set_parity(USART2, USART_PARITY_NONE);
  94         // usart_set_parity(USART2, USART_PARITY_EVEN); // FIXME
  95         usart_set_flow_control(USART2, USART_FLOWCONTROL_NONE);
  96
  97         rx_init();
  98
  99         nvic_enable_irq(NVIC_USART2_IRQ);
 100         usart_enable(USART2);
 101
 102 #if 0
 103         u32 xxx = USART_CR1(USART2);
 104         usart_set_mode(USART2, USART_MODE_TX);
 105         gpio_set(GPIOA, GPIO1);
 106         debug_printf("%08x\n", xxx);
 107         xx_write_char('\n');
 108 #endif
 109 }
 110
 111 void usart2_isr(void)
 112 {
 113         u32 status = USART_SR(USART2);
 114
 115         // FIXME: Optimize
 116
 117         if (status & USART_SR_RXNE) {
 118                 uint ch = usart_recv(USART2);
 119                 if (state == STATE_RX) {
 120 #if 0
 121                         if (status & (USART_SR_FE | USART_SR_ORE | USART_SR_NE)) {
 122                                 rx_bad = 1;
 123                         } else
 124 #endif
 125                         if (rx_size < RX_BUFSIZE) {
 126                                 rx_buf[rx_size++] = ch;
 127                         } else {
 128                                 // Frame too long
 129                                 rx_bad = 2;
 130                         }
 131                         timer_set_period(TIM2, 7500);   // 0.75 ms timeout for end of frame (FIXME: right value?)
 132                         timer_generate_event(TIM2, TIM_EGR_UG);
 133                         timer_enable_counter(TIM2);
 134                 }
 135         }
 136
 137         if (state == STATE_TX) {
 138                 if (status & USART_SR_TXE) {
 139                         if (tx_pos < tx_size) {
 140                                 usart_send(USART2, tx_buf[tx_pos++]);
 141                         } else {
 142                                 // The transmitter is double-buffered, so at this moment, it is transmitting
 143                                 // the last byte of the frame. Wait until transfer is completed.
 144                                 usart_disable_tx_interrupt(USART2);
 145                                 USART_CR1(USART2) |= USART_CR1_TCIE;
 146                                 state = STATE_TX_LAST;
 147                         }
 148                 }
 149         }
 150
 151         if (state == STATE_TX_LAST) {
 152                 if (status & USART_SR_TC) {
 153                         // Transfer of the last byte is complete. Release the bus.
 154                         USART_CR1(USART2) &= ~USART_CR1_TCIE;
 155                         tx_done();
 156                         rx_init();
 157                 }
 158         }
 159 }
 160
 161 void tim2_isr(void)
 162 {
 163         if (TIM_SR(TIM2) & TIM_SR_UIF) {
 164                 TIM_SR(TIM2) &= ~TIM_SR_UIF;
 165                 if (state == STATE_RX)
 166                         rx_done();
 167         }
 168 }
 169
 170 // CRC tables
 171
 172 static const byte crc_hi[] = {
 173     0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
 174     0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
 175     0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
 176     0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
 177     0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
 178     0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41,
 179     0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
 180     0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
 181     0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
 182     0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
 183     0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
 184     0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
 185     0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
 186     0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
 187     0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
 188     0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
 189     0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
 190     0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
 191     0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
 192     0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
 193     0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
 194     0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
 195     0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
 196     0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
 197     0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
 198     0x80, 0x41, 0x00, 0xc1, 0x81, 0x40
 199 };
 200
 201 static const byte crc_lo[] = {
 202     0x00, 0xc0, 0xc1, 0x01, 0xc3, 0x03, 0x02, 0xc2, 0xc6, 0x06,
 203     0x07, 0xc7, 0x05, 0xc5, 0xc4, 0x04, 0xcc, 0x0c, 0x0d, 0xcd,
 204     0x0f, 0xcf, 0xce, 0x0e, 0x0a, 0xca, 0xcb, 0x0b, 0xc9, 0x09,
 205     0x08, 0xc8, 0xd8, 0x18, 0x19, 0xd9, 0x1b, 0xdb, 0xda, 0x1a,
 206     0x1e, 0xde, 0xdf, 0x1f, 0xdd, 0x1d, 0x1c, 0xdc, 0x14, 0xd4,
 207     0xd5, 0x15, 0xd7, 0x17, 0x16, 0xd6, 0xd2, 0x12, 0x13, 0xd3,
 208     0x11, 0xd1, 0xd0, 0x10, 0xf0, 0x30, 0x31, 0xf1, 0x33, 0xf3,
 209     0xf2, 0x32, 0x36, 0xf6, 0xf7, 0x37, 0xf5, 0x35, 0x34, 0xf4,
 210     0x3c, 0xfc, 0xfd, 0x3d, 0xff, 0x3f, 0x3e, 0xfe, 0xfa, 0x3a,
 211     0x3b, 0xfb, 0x39, 0xf9, 0xf8, 0x38, 0x28, 0xe8, 0xe9, 0x29,
 212     0xeb, 0x2b, 0x2a, 0xea, 0xee, 0x2e, 0x2f, 0xef, 0x2d, 0xed,
 213     0xec, 0x2c, 0xe4, 0x24, 0x25, 0xe5, 0x27, 0xe7, 0xe6, 0x26,
 214     0x22, 0xe2, 0xe3, 0x23, 0xe1, 0x21, 0x20, 0xe0, 0xa0, 0x60,
 215     0x61, 0xa1, 0x63, 0xa3, 0xa2, 0x62, 0x66, 0xa6, 0xa7, 0x67,
 216     0xa5, 0x65, 0x64, 0xa4, 0x6c, 0xac, 0xad, 0x6d, 0xaf, 0x6f,
 217     0x6e, 0xae, 0xaa, 0x6a, 0x6b, 0xab, 0x69, 0xa9, 0xa8, 0x68,
 218     0x78, 0xb8, 0xb9, 0x79, 0xbb, 0x7b, 0x7a, 0xba, 0xbe, 0x7e,
 219     0x7f, 0xbf, 0x7d, 0xbd, 0xbc, 0x7c, 0xb4, 0x74, 0x75, 0xb5,
 220     0x77, 0xb7, 0xb6, 0x76, 0x72, 0xb2, 0xb3, 0x73, 0xb1, 0x71,
 221     0x70, 0xb0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
 222     0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9c, 0x5c,
 223     0x5d, 0x9d, 0x5f, 0x9f, 0x9e, 0x5e, 0x5a, 0x9a, 0x9b, 0x5b,
 224     0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4b, 0x8b,
 225     0x8a, 0x4a, 0x4e, 0x8e, 0x8f, 0x4f, 0x8d, 0x4d, 0x4c, 0x8c,
 226     0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
 227     0x43, 0x83, 0x41, 0x81, 0x80, 0x40
 228 };
 229
 230 static u16 crc16(byte *buf, u16 len)
 231 {
 232         byte hi = 0xff, lo = 0xff;
 233
 234         while (len--) {
 235                 byte i = hi ^ *buf++;
 236                 hi = lo ^ crc_hi[i];
 237                 lo = crc_lo[i];
 238         }
 239
 240         return (hi << 8 | lo);
 241 }
 242
 243 static bool check_frame(void)
 244 {
 245         if (rx_bad) {
 246                 // FIXME: Error counters?
 247                 return false;
 248         }
 249
 250         if (rx_size < 4) {
 251                 // FIXME: Error counters?
 252                 return false;
 253         }
 254
 255         u16 crc = crc16(rx_buf, rx_size - 2);
 256         u16 rx_crc = (rx_buf[rx_size-2] << 8) | rx_buf[rx_size-1];
 257         if (crc != rx_crc) {
 258                 // FIXME: Error counters?
 259                 return false;
 260         }
 261
 262         return true;
 263 }
 264
 265 enum mb_function {
 266         FUNC_READ_HOLDING_REGISTERS = 0x03,
 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 void report_error(byte code)
 276 {
 277         // Discard the partially constructed body of the reply and rewrite the header
 278         tx_buf[1] |= 0x80;
 279         tx_buf[2] = code;
 280         tx_size = 3;
 281 }
 282
 283 static void process_frame(void)
 284 {
 285         byte func = rx_buf[1];
 286
 287         // Prepare reply frame
 288         tx_buf[0] = MB_OUR_ADDRESS;
 289         tx_buf[1] = rx_buf[1];
 290         tx_size = 2;
 291
 292         switch (func) {
 293                 case FUNC_READ_HOLDING_REGISTERS:
 294                         tx_buf[tx_size++] = 2;
 295                         tx_buf[tx_size++] = 0x12;
 296                         tx_buf[tx_size++] = 0x34;
 297                         break;
 298                 default:
 299                         report_error(ERR_ILLEGAL_FUNCTION);
 300         }
 301
 302         // Finish reply frame
 303         u16 crc = crc16(tx_buf, tx_size);
 304         tx_buf[tx_size++] = crc >> 8;
 305         tx_buf[tx_size++] = crc;
 306 }
 307
 308 void modbus_loop(void)
 309 {
 310         if (state != STATE_RX_DONE) {
 311                 // gpio_toggle(GPIOC, GPIO13);
 312                 return;
 313         }
 314         state = STATE_PROCESSING;
 315         gpio_toggle(GPIOC, GPIO13);
 316
 317         if (!check_frame()) {
 318                 rx_init();
 319                 return;
 320         }
 321
 322         if (rx_buf[0] == MB_OUR_ADDRESS) {
 323                 // Frame addressed to us: process and reply
 324                 process_frame();
 325                 tx_init();
 326         } else if (rx_buf[0] == 0x00) {
 327                 // Broadcast frame: process, but do not reply
 328                 process_frame();
 329                 rx_init();
 330         } else {
 331                 // Somebody else's frame: discard
 332                 rx_init();
 333         }
 334 }