Preliminary modbus rtu implementation.

[openparking.git] / firmware / modbus.c

diff --git a/firmware/modbus.c b/firmware/modbus.c
new file mode 100755 (executable)
index 0000000..937d9d9
--- /dev/null
+++ b/firmware/modbus.c
@@ -0,0 +1,196 @@
+/*
+ * Loosely modelled after AVR-RS485 by Yoshinori Kohyama (http://algobit.jp/),
+ * available at https://github.com/kohyama/AVR-RS485/
+ *
+ * All bugs by Jan "Yenya" Kasprzak <kas@fi.muni.cz> :-)
+ */
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <util/atomic.h>
+#include <util/delay.h>
+
+#define BUFSIZE 128    // must be a power of two
+
+// configure the control pin
+#define ctl_pin_setup()        do { DDRD  |=  _BV(PD2); } while (0)
+#define ctl_pin_on()   do { PORTD |=  _BV(PD2); } while (0)
+#define ctl_pin_off()  do { PORTD &= ~_BV(PD2); } while (0)
+
+#define BUFMASK (BUFSIZE-1)
+#if (BUFSIZE & BUFMASK)
+#error BUFSIZE must be a power of two
+#endif
+
+#if BUFSIZE > 255
+typedef uint16_t bufptr_t;
+#else
+typedef uint8_t  bufptr_t;
+#endif
+
+#define bufptr_inc(x)  ((x + 1) & BUFMASK)
+
+static volatile bufptr_t rx_bytes, tx_head, tx_tail;
+static volatile uint8_t rxbuf[BUFSIZE], txbuf[BUFSIZE];
+static volatile uint16_t last_rx;
+static volatile uint8_t unit_id;
+
+#define UART_BAUD      9600
+#define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)
+#define wait_one_byte()        _delay_us(10*1000000/UART_BAUD)
+
+#define get_clock()    (TCNT1)
+#define CLOCK_SPEED    (F_CPU/1024)
+/*
+ * According to Wikipedia, it is indeed 28 bits = 3.5 bytes without
+ * start- and stopbits.
+ */
+#define TIMEOUT                (28*CLOCK_SPEED/UART_BAUD)
+
+void modbus_init()
+{
+       rx_bytes = 0;
+       tx_head = tx_tail = 0;
+
+       unit_id = 42;
+
+       ctl_pin_off();
+       ctl_pin_setup();
+
+       UBRR0 = UBRR_VAL;
+       UCSR0A = 0;
+       UCSR0B = _BV(RXCIE0)|_BV(RXEN0)|_BV(TXEN0);
+       UCSR0C = _BV(UCSZ01)|_BV(UCSZ00);
+}
+
+void rs485_send(char *p)
+{
+       bufptr_t next;
+
+       if (*p == '\0')
+               return;
+
+       ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+               next = bufptr_inc(tx_head);
+               while (next != tx_tail && *p != '\0') {
+                       txbuf[tx_head] = *p++;
+                       tx_head = next;
+                       next = bufptr_inc(tx_head);
+               }
+               ctl_pin_on();
+               UCSR0B |= _BV(UDRIE0);
+       }
+}
+
+static uint16_t compute_crc(volatile uint8_t *buf, bufptr_t len)
+{
+       bufptr_t i;
+       uint16_t crc = 0xFFFF;
+
+       for (i = 0; i < len; i++) {
+               uint8_t j;
+               crc ^= (uint16_t)(buf[i]);
+               for(j = 0; j < 8; j++) {
+                       if (crc & 0x0001) {
+                               crc >>= 1;
+                               crc ^= 0xA001;
+                       } else {
+                               crc >>= 1;
+                       }
+               }
+       }
+
+       return crc;
+}
+
+static void make_exception(uint8_t func, uint8_t code)
+{
+       txbuf[tx_head++] = unit_id;
+       txbuf[tx_head++] = func | 0x80;
+       txbuf[tx_head++] = code;
+}
+
+void modbus_poll()
+{
+       bufptr_t packet_len;
+       uint16_t crc;
+
+       ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+               if (rx_bytes == 0) // nothing received yet
+                       return;
+
+               if (get_clock() - last_rx < TIMEOUT) // still receiving
+                       return;
+
+               if (rx_bytes < 4) { // too short
+                       rx_bytes = 0;
+                       return;
+               }
+
+               if (rxbuf[0] != unit_id) { // not for myself
+                       rx_bytes = 0;
+                       return;
+               }
+
+               if (tx_tail) { // still sending?
+                       rx_bytes = 0;
+                       return;
+               }
+
+               packet_len = rx_bytes; // make a copy
+       }
+
+       crc = compute_crc(rxbuf, packet_len - 2);
+
+       if ((crc & 0xFF) != rxbuf[packet_len-2]
+               || (crc >> 8) != rxbuf[packet_len-1]) // bad crc
+               goto out;
+
+       tx_head = 0;
+
+       switch (rxbuf[1]) { // function
+       default:
+               make_exception(rxbuf[1], 1); // illegal function
+       }
+
+send:
+       if (tx_head) {
+               crc = compute_crc(txbuf, tx_head);
+               txbuf[tx_head++] = crc & 0xFF;
+               txbuf[tx_head++] = crc >> 8;
+
+               tx_tail = 0;
+
+               ctl_pin_on();
+               UCSR0B |= _BV(UDRIE0);
+       }
+out:
+       ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+               rx_bytes = 0;
+       }
+}
+
+ISR(USART_RX_vect)
+{
+       ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+               rxbuf[rx_bytes] = UDR0;
+               if (rx_bytes + 1 < BUFSIZE) // ignore overruns
+                       rx_bytes++;
+               last_rx = get_clock();
+       }
+}
+
+ISR(USART_UDRE_vect)
+{
+       ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+               if (tx_head == tx_tail) {
+                       UCSR0B &= ~_BV(UDRIE0);
+                       tx_tail = tx_head = 0;
+                       wait_one_byte(); // FIXME: too long busy-wait
+                       ctl_pin_off();
+               } else {
+                       UDR0 = txbuf[tx_tail];
+                       tx_tail = bufptr_inc(tx_tail);
+               }
+       }
+}