X-Git-Url: https://www.fi.muni.cz/~kas/git//home/kas/public_html/git/?a=blobdiff_plain;f=firmware%2Fmodbus.c;h=a49bd877e1c778357e5f3f39f3f1b5ece6defbec;hb=e9281b1cc49e6f5f43e45038f8f90f6f75c8a4a0;hp=937d9d90b0f543775c81cd8507729311a7eff57b;hpb=86f8e5bfd1c753034b9ca876dac7d18720ab8808;p=openparking.git

diff --git a/firmware/modbus.c b/firmware/modbus.c
old mode 100755
new mode 100644
index 937d9d9..a49bd87
--- a/firmware/modbus.c
+++ b/firmware/modbus.c
@@ -5,11 +5,14 @@
  * All bugs by Jan "Yenya" Kasprzak <kas@fi.muni.cz> :-)
  */
 
+#include <avr/eeprom.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/atomic.h>
 #include <util/delay.h>
 
+#include "modbus.h"
+
 #define BUFSIZE 128	// must be a power of two
 
 // configure the control pin
@@ -33,7 +36,9 @@ typedef uint8_t  bufptr_t;
 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;
+#ifndef mb_unit_id
+static uint8_t mb_unit_id;
+#endif
 
 #define UART_BAUD	9600
 #define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)
@@ -47,12 +52,32 @@ static volatile uint8_t unit_id;
  */
 #define TIMEOUT		(28*CLOCK_SPEED/UART_BAUD)
 
-void modbus_init()
+uint16_t hold_regs[MB_N_HOLD_REGS];
+
+#if MB_N_HOLD_REGS_EEPROM > 0
+static uint16_t hold_regs_ee[MB_N_HOLD_REGS_EEPROM] EEMEM = {
+	42, 
+	0, 0, 0, 30, 30, 30, 30, 0, 0, 0, 0, 30,
+	(1 << 4) | (1 << 11), // LED 1
+	0, // LED 2
+};
+
+#endif
+
+void modbus_init(uint8_t unit)
 {
 	rx_bytes = 0;
 	tx_head = tx_tail = 0;
 
-	unit_id = 42;
+	if (unit)
+		mb_unit_id = unit;
+#if MB_N_HOLD_REGS_EEPROM > 0
+	do {
+		int i;
+		for (i = 0; i < MB_N_HOLD_REGS_EEPROM; i++)
+			hold_regs[i] = eeprom_read_word(&hold_regs_ee[i]);
+	} while (0);
+#endif
 
 	ctl_pin_off();
 	ctl_pin_setup();
@@ -103,17 +128,69 @@ static uint16_t compute_crc(volatile uint8_t *buf, bufptr_t len)
 	return crc;
 }
 
-static void make_exception(uint8_t func, uint8_t code)
+static void make_exception(mb_exception code)
+{
+	txbuf[1] |= 0x80;
+	txbuf[2] = code;
+	tx_head = 3;
+}
+
+#define get_word(ptr, off) (((uint16_t)ptr[off] << 8) | ptr[off+1])
+void put_byte(uint8_t byte)
+{
+	txbuf[tx_head++] = byte;
+}
+
+void put_word(uint16_t word)
 {
-	txbuf[tx_head++] = unit_id;
-	txbuf[tx_head++] = func | 0x80;
-	txbuf[tx_head++] = code;
+	txbuf[tx_head++] = word >> 8;
+	txbuf[tx_head++] = word & 0xFF;
+}
+
+static mb_exception read_holding_regs(uint16_t start, uint16_t len)
+{
+	if (len > BUFSIZE/2 - 3)
+		return MB_ILLEGAL_ADDR;
+
+	if (start < MB_HOLD_REGS_BASE
+		|| start + len > MB_HOLD_REGS_BASE + MB_N_HOLD_REGS)
+		return MB_ILLEGAL_ADDR;
+
+	put_byte(2*len);
+
+	start -= MB_HOLD_REGS_BASE;
+	while(len--)
+		put_word(hold_regs[start++]);
+
+	return MB_OK;
+}
+
+static mb_exception write_single_reg(uint16_t reg, uint16_t val)
+{
+	if (reg < MB_HOLD_REGS_BASE
+		|| reg >= MB_HOLD_REGS_BASE + MB_N_HOLD_REGS)
+		return MB_ILLEGAL_ADDR;
+
+	if (!hold_reg_is_valid(reg, val))
+		return MB_ILLEGAL_VAL;
+
+	reg -= MB_HOLD_REGS_BASE;
+	hold_regs[reg] = val;
+#if MB_N_HOLD_REGS_EEPROM > 0
+	if (reg < MB_N_HOLD_REGS_EEPROM)
+		eeprom_write_word(&hold_regs_ee[reg], val);
+#endif
+	put_word(reg + MB_HOLD_REGS_BASE);
+	put_word(val);
+
+	return MB_OK;
 }
 
 void modbus_poll()
 {
 	bufptr_t packet_len;
 	uint16_t crc;
+	uint8_t rv;
 
 	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
 		if (rx_bytes == 0) // nothing received yet
@@ -127,7 +204,7 @@ void modbus_poll()
 			return;
 		}
 
-		if (rxbuf[0] != unit_id) { // not for myself
+		if (rxbuf[0] != mb_unit_id) { // not for myself
 			rx_bytes = 0;
 			return;
 		}
@@ -146,13 +223,24 @@ void modbus_poll()
 		|| (crc >> 8) != rxbuf[packet_len-1]) // bad crc
 		goto out;
 
-	tx_head = 0;
+	txbuf[0] = rxbuf[0]; // not mb_unit_id in case it gets changed
+	txbuf[1] = rxbuf[1];
+	tx_head = 2;
 
+	rv = MB_OK;
 	switch (rxbuf[1]) { // function
+	case 3:
+		rv = read_holding_regs(get_word(rxbuf, 2), get_word(rxbuf, 4));
+		break;
+	case 6:
+		rv = write_single_reg(get_word(rxbuf, 2), get_word(rxbuf, 4));
+		break;
 	default:
-		make_exception(rxbuf[1], 1); // illegal function
+		make_exception(MB_ILLEGAL_FUNC); // illegal function
 	}
-
+	
+	if (rv)
+		make_exception(rv);
 send:
 	if (tx_head) {
 		crc = compute_crc(txbuf, tx_head);