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 }