/*
 * A Command parser for RTL Switch configuration
 */

// #define DEBUG
// #define REGDBG 1

#include <8051.h>
#include "cmd_parser.h"
#include "rtl837x_common.h"
#include "rtl837x_port.h"
#include "rtl837x_flash.h"
#include "rtl837x_phy.h"
#include "rtl837x_regs.h"
#include "rtl837x_sfr.h"
#include "rtl837x_stp.h"
#include "rtl837x_igmp.h"
#include "rtl837x_bandwidth.h"
#include "sfp.h"
#include "dhcp.h"
#include "syslog.h"
#include "uip/uip.h"
#include "version.h"

#include "machine.h"
#include "phy.h"

#pragma codeseg BANK2
#pragma constseg BANK2

extern __code const struct machine machine;
extern __xdata bool stp_enabled;
extern __code const uint8_t log_to_phys_port[9];

extern volatile __xdata uint32_t ticks;
extern volatile __xdata uint8_t sfr_data[4];

extern __code const uint8_t * __code const greeting;
extern __code const uint8_t * __code const hex;

extern __xdata uint8_t flash_buf[FLASH_BUF_SIZE];
extern __xdata struct flash_region_t flash_region;

extern __xdata char passwd[21];

extern __xdata uint16_t session_timeout;

extern __xdata struct dhcp_state dhcp_state;

__xdata uint8_t vlan_names[VLAN_NAMES_SIZE];
__xdata uint16_t vlan_ptr;

__xdata char port_names[9][PORT_NAME_SIZE];

extern __xdata uint16_t management_vlan;
extern __xdata struct uip_eth_addr uip_ethaddr;
__xdata uint8_t gpio_last_value[8] = { 0 };

// Temporatly for str to hex convertion value.
// Support up to 32_bits.
__xdata uint8_t hexvalue[4] = { 0 };


// Buffer for writing to flash 0x1fd000, copy to 0x1fe000
__xdata uint8_t cmd_buffer[CMD_BUF_SIZE];

__xdata	char save_cmd;

__xdata uint8_t ip[4];
__xdata uint8_t mac_parse_result[6];

/* Scratch for parse_syslog(), in xdata: a local here would take internal RAM
 * the linker has none of. */
__xdata uint8_t  syslog_port_was_on;

// These variables combined create a Fixed-capacity vector/bounded buffer.
// `N_WORDS`: The total number of command arguments that can be tracked.
// `cmd_words_len` stores the number of arguments found inside `cmd_buffer`
// `cmd_words_b` stores the index into `cmd_buffer`, check `cmd_words_len` is index is valid.
#define N_WORDS 15
__xdata uint8_t cmd_words_len;
__xdata uint8_t cmd_words_b[N_WORDS];

__xdata uint8_t cmd_history[CMD_HISTORY_SIZE];
__xdata uint16_t cmd_history_ptr;

// Error set by commands
__xdata uint8_t err_status;

__xdata uint16_t atoi_results_short;
__xdata uint8_t atoi_results_u8;

inline uint8_t isletter(uint8_t l)
{
	// return (l >= 'a' && l <= 'z') || (l >= 'A' && l <= 'Z');

	// Make it lowercase
	l |= 0x20;
	l -= 'a';
	return (l <= ('z'-'a'));
}


inline uint8_t isnumber(uint8_t l)
{
	// return (l >= '0' && l <= '9');
	l -= '0';
	return (l <= ('9'-'0'));
}

uint8_t cmd_compare(uint8_t start, __code const uint8_t * cmd)
{
	if (cmd_words_len == 0 || start > (cmd_words_len - 1)) {
		return 0;
	}
	uint8_t i = cmd_words_b[start];
	uint8_t j = 0;

	do {
		uint8_t c = cmd[j];
		uint8_t b = cmd_buffer[i];

		// cmd is guaranteed to be NUL-terminated.
        if (c == NUL) {
            if ((b == ' ') || (b == NUL)) {
				// Match
                return 1;
            }
            break;
        }
        if (b != c) {
            break;
        }

		j += 1;
        i += 1;
	} while (i < CMD_BUF_SIZE);

	// No match
	return 0;
}


/* Converts ascii-hex array into value.
	returns number of hexvalue[] entries has been written.
	return value = 0 means error.
*/
uint8_t atoi_hex(uint8_t idx)
{
	uint8_t h_idx = 0;
	uint8_t val = 0;
	uint8_t c;

	while(1) {
		c = cmd_buffer[idx];

		if (c == NUL || c == ' ') {
			break;
		}

		// swap hex nibbles
		val = (val >> 4) | (val << 4);

		if (c - '0' < 10) {
			val |= c - '0';
		} else {
			c |= 0x20;
			c -= 'a';
			if (c > 5) {
				h_idx = 0;
				break;
			}
			val |= c + 10;
		}

		idx++;
		hexvalue[h_idx >> 1] = val;

		if (h_idx & 1 == 1) {
			val = 0;
		}
		h_idx++;
	}

	if (h_idx & 1) {
		hexvalue[h_idx >> 1] <<= 4;
		hexvalue[3] = hexvalue[3] >> 4 | (hexvalue[2] << 4);
		hexvalue[2] = hexvalue[2] >> 4 | (hexvalue[1] << 4);
		hexvalue[1] = hexvalue[1] >> 4 | (hexvalue[0] << 4);
		hexvalue[0] >>= 4;
	}

	return ((h_idx + 1) >> 1);
}


// Returns 0 or the number of digits taken into account for conversion.
// Stops at any non-digit '0'-'9' char or more than 3 bytes.
uint8_t atoi_byte(uint8_t idx)
{
	uint8_t cnt = 0;
	uint8_t num = 0;

	uint8_t * ptr = &cmd_buffer[idx];

	while (1) {
		uint8_t val = *ptr++ - '0';
		if (val > 9)
			break;
		if (num > 25 || (num == 25 && val > 5) || cnt >= 3)
			return 0;
		num = (num * 10) + val;
		cnt++;
	}

	atoi_results_u8 = num;
	return cnt;
}


// Returns 0 or the number of digits taken into account for conversion.
// Stops at any non-digit '0'-'9' char or bytes is more then 5.
uint8_t atoi_short(uint8_t idx)
{
	uint8_t cnt = 0;
	atoi_results_short = 0;

	uint8_t *ptr = &cmd_buffer[idx];

	while (1) {
		uint8_t val = *ptr++ - '0';
		if (val > 9)
			break;
		if (atoi_results_short > 6553 || (atoi_results_short == 6553 && val > 5) || cnt >= 5)
			return 0;
		atoi_results_short = (atoi_results_short * 10) + val;
		cnt++;
	}

	return cnt;
}

/* Parse, validate and translate phys_to_log_port physical port argument.
 * The CPU-port, i.e. port 0, is not a valid argument.
 * returns 0 when on parser error or invalid value.
 * returns non-zero number of characters consumed.
 * Store the value in atoi_results_u8.
 */
uint8_t cmd_parse_port(uint8_t idx) {
	uint8_t port = cmd_buffer[idx] - '0' - 1;
	if (port > 8)
		return 0;

	port = machine.phys_to_log_port[port];
	if (port < machine.min_port || port > machine.max_port)
		return 0;

	atoi_results_u8 = port;
	return 1;
}


// Same as cmd_parse_port() but additionally check for trailing SPACE or NUL.
// returns 0 when on parser error or invalid value or no SPACE or no NUL.
// returns non-zero number of characters consumed including the SPACE.
uint8_t cmd_parse_port_separator(uint8_t idx) {
	uint8_t ret = cmd_parse_port(idx);
	if (ret != 0) {
		idx += ret;
		uint8_t c = cmd_buffer[idx];
		if (c == ' ') {
			ret++;
		} else if (c != NUL)
			ret = 0;
	}
	return ret;
}

// Same as cmd_parse_port_separator() but additionally allow CPU-port.
// returns 0 when on parser error or invalid value or no SPACE or no NUL.
// returns number of characters consumed including the SPACE.
uint8_t cmd_parse_port_cpu_separator(uint8_t idx) {
	uint8_t ret = cmd_parse_port(idx);

	if (ret == 0 && cmd_buffer[idx] == '0') {
		ret = 1;
		atoi_results_u8 = CPU_PORT;
	}

	if (ret != 0) {
		idx += ret;
		uint8_t c = cmd_buffer[idx];
		if (c == ' ') {
			ret++;
		} else if (c != NUL)
			ret = 0;
	}
	return ret;
}


// check if the cmd_buffer[idx] is a SPACE.
__bit cmd_is_space(uint8_t idx) {
	return cmd_buffer[idx] == ' ';
}

// check if the cmd_buffer[idx] is a SPACE or NUL.
__bit cmd_is_space_or_nul(uint8_t idx) {
	uint8_t c = cmd_buffer[idx];
	return c == ' ' || c == NUL;
}

// Parse an IPv4 address
// returns 0 when on parse error or invalid value or it don't ends with SPACE or NUL.
// returns non-zero number of characters consumed including the SPACE.
uint8_t parse_ip(uint8_t idx)
{
	uint8_t b = 0;
	uint8_t ret;
	uint8_t idx_start = idx;

	while(1) {
		ret = atoi_byte(idx);
		if (ret == 0)
			goto err;

		idx += ret;
		ip[b++] = atoi_results_u8;

		ret = cmd_buffer[idx];
		if (b == 4) {
			if (ret == ' ') {
				idx++;
				break;
			}
			if (ret == NUL)
				break;
			goto err;
		}
		idx++;

		if (ret != '.')
			goto err;
	}
	return idx - idx_start;

err:
	print_string("Error in IP format\n");
	return 0;
}

// Prints an IPv4 address.
void print_ip(__xdata uint8_t * ptr) __banked
{
	uint8_t idx = 0;
	uint8_t num;

	while(1) {
		num = *ptr++;
		itoa(num);
		if (++idx == 4)
			break;

		write_char('.');
	}
}

// Prints a MAC address.
void print_mac(__xdata uint8_t *ptr) __banked
{
	uint8_t idx = 0;
	uint8_t num;

	while (1) {
		num = *ptr++;
		print_byte(num);
		if (++idx == 6)
			break;

		write_char(':');
	}
}

// Parses a MAC address "aa:bb:cc:dd:ee:ff" (":" or "-" separators) from
// cmd_buffer starting at idx into the global mac_parse_result[6]. Each octet
// must be exactly two hex digits with a separator between octets and nothing
// trailing, so repeated separators, missing digits or a trailing hex digit are
// rejected. Returns the number of bytes consumed, or 0 on error.
uint8_t parse_mac(uint8_t idx)
{
	uint8_t b = 0;
	uint8_t c = ':';

	do {
		if (c != ':' && c != '-')
			goto err;

		__bit again = true;
		uint8_t v = 0;

		while(1) {
			c = cmd_buffer[idx++];
			c -= '0';
			if (c > 9) {
				c |= 0x20;
				c += '0' - 'a';
				if (c > 5)
					goto err;
				c += 10;
			}
			// force swap-instr.
			v = (v << 4) | (v >> 4);
			v |= c;

			if (again)
				again = false;
			else
				break;
		}
		mac_parse_result[b++] = v;

		c = cmd_buffer[idx++];
	} while(b < 6);

	if (c == ' ' || c == NUL)
		return idx;

err:
	return 0;
}

static void cmd_error(__code const char *msg)
{
	err_status = ERR_INVALID_ARGUMENT;
	print_string("Error: ");
	print_string(msg);
}


static uint8_t name_char(uint8_t c)
{
	if (c < 0x20 || c > 0x7e || c == '"' || c == '\\')
		return '.';
	return c;
}


// Sets the management MAC from a "mac <aa:bb:cc:dd:ee:ff>" command (or prints
// the current one with a bare "mac"). Refuses blank/multicast/locally
// administered/all-zero-OUI addresses so the running MAC stays reachable, and
// re-registers the static L2 management entry so the change applies without a
// reboot. As a regular command a "mac ..." line in the startup config applies
// on every boot (execute_config runs before the final static L2 entry).
void parse_mac_cmd(void)
{
	if (cmd_words_len == 1) {
		print_string("Current MAC: ");
		print_mac(uip_ethaddr.addr);
		write_char('\n');
		return;
	}
	if (cmd_words_len != 2 || !parse_mac(cmd_words_b[1])) {
		cmd_error("mac [<aa:bb:cc:dd:ee:ff>]\n");
		return;
	}
	if ((mac_parse_result[0] & 0x03) || ((mac_parse_result[0] | mac_parse_result[1] | mac_parse_result[2]) == 0)) {
		cmd_error("the MAC must be unicast and globally administered\n");
		return;
	}
	if (memcmp(mac_parse_result, uip_ethaddr.addr, 6) == 0) {
		print_string("MAC unchanged\n");
		return;
	}

	// Swap the static management entry over to the new address: remove the
	// old one (still in uip_ethaddr) before overwriting it.
	port_l2_static_mgmt(uip_ethaddr.addr, management_vlan, true);
	memcpy(uip_ethaddr.addr, mac_parse_result, 6);
	port_l2_static_mgmt(uip_ethaddr.addr, management_vlan, false);

	print_string("MAC set to: ");
	print_mac(uip_ethaddr.addr);
	write_char('\n');
	print_string("Save the startup config to keep it across reboots\n");
}


void parse_lag(void)
{
	__xdata uint8_t group;
	__xdata uint16_t members = 0;

	if (cmd_compare(1, "show")) {
		print_string("LAG status:\n");
		for (uint8_t i = 0; i < 4; i++) {
			write_char(' '); write_char('1' + i);
			members = port_lag_members_get(i);
			if (!members) {
				print_string(" disabled\n");
				continue;
			}
			print_string(" member ports: ");
			for (uint8_t j = 0; j < 10; j++) {
				if (members & 1) {
					print_phys_port(j);
					write_char(' ');
				}
				members >>= 1;
			}
			print_string(" (hash: 0x"); 
			reg_read_m(RTL837X_TRK_HASH_CTRL_BASE + (i << 2));
			print_byte(sfr_data[3]);
			print_string(")\n");
		}
		return;
	}

	if (cmd_words_len < 3)
		goto err;

	// Parse group, expect only one number 0-9;
	if (atoi_byte(cmd_words_b[1]) != 1)
		goto err;

	group = atoi_results_u8 - 1;
	if (group > 3)
		goto err;

	if (cmd_compare(2, "d")) {
		port_lag_members_set(group, 0);
		return;
	}

	uint8_t w = 2;
	while (w < cmd_words_len) {
		// Parse port.
		if (cmd_parse_port_separator(cmd_words_b[w++]) == 0)
			goto err;

		members |= ((uint16_t)1) << atoi_results_u8;
	}
	port_lag_members_set(group, members);
	return;
err:
	cmd_error("lag (show | <1-4> (d | <port>...))\n");
}


void parse_lag_hash(void)
{
	// Parse group, expect only one number 0-9.
	if (cmd_words_len < 3 || atoi_byte(cmd_words_b[1]) != 1)
		goto err;

	uint8_t group = atoi_results_u8 - 1;
	if (group > 3)
		goto err;

	uint8_t hash = 0;
	uint8_t w = 2;
	while (w < cmd_words_len) {
		if (cmd_compare(w, "spa"))
			hash |= LAG_HASH_SOURCE_PORT_NUMBER;
		else if (cmd_compare(w, "smac"))
			hash |= LAG_HASH_L2_SMAC;
		else if (cmd_compare(w, "dmac"))
			hash |= LAG_HASH_L2_DMAC;
		else if (cmd_compare(w, "sip"))
			hash |= LAG_HASH_L3_SIP;
		else if (cmd_compare(w, "dip"))
			hash |= LAG_HASH_L3_DIP;
		else if (cmd_compare(w, "sport"))
			hash |= LAG_HASH_L4_SPORT;
		else if (cmd_compare(w, "dport"))
			hash |= LAG_HASH_L4_DPORT;
		else {
			print_string("Error: invalid hash type:");
			print_string_x(&cmd_buffer[cmd_words_b[w]]);
			write_char('\n');
			goto err;
		}
		w++;
	}
	port_lag_hash_set(group, hash);
	return;
err:
	cmd_error("laghash <1-4> [smac|dmac|sip|dip|sport|dport]\n");
}


void parse_vlan(void)
{
	vlan_settings.vlan = 0;
	vlan_settings.members = 0;
	vlan_settings.tagged = 0;

	if (cmd_words_len < 2)
		goto err;

	// Parse the VLAN number
	if (atoi_short(cmd_words_b[1]) != 0) {
		if (atoi_results_short > 4094)
			goto err;
		vlan_settings.vlan = atoi_results_short;

		if (cmd_compare(2, "mgmt")) {
			port_l2_static_mgmt(uip_ethaddr.addr, management_vlan, true);
			management_vlan = vlan_settings.vlan;
			port_l2_static_mgmt(uip_ethaddr.addr, management_vlan, false);
			if (!vlan_settings.vlan)
				print_string("Management VLAN disabled\n");
			else
				print_string("Management VLAN set to "); print_short(management_vlan); write_char('\n');
			return;
		}

		// Other commands vlan 0 is invalid
		if (!vlan_settings.vlan)
			goto err;

		if (cmd_words_len == 3 && cmd_buffer[cmd_words_b[2]] == 'd') {
			vlan_delete(vlan_settings.vlan);
			return;
		}

		uint8_t w = 2;
		uint8_t pos = cmd_words_b[w];
		if (cmd_words_len > w && isletter(cmd_buffer[pos])) {
			uint8_t i = 0;
			while (!cmd_is_space_or_nul(pos + i))
				i++;
			if (vlan_ptr + i > (VLAN_NAMES_SIZE - 5)) {
				print_string("VLAN name table full, name ignored\n");
			} else {
				vlan_name_remove(vlan_settings.vlan);
				__xdata uint8_t *vlan_str = &vlan_names[vlan_ptr];
				*vlan_str++ = hex[(vlan_settings.vlan >> 8) & 0xf];
				*vlan_str++ = hex[(vlan_settings.vlan >> 4) & 0xf] ;
				*vlan_str++ = hex[vlan_settings.vlan & 0xf];
				while(!cmd_is_space_or_nul(pos)) {
					uint8_t c = name_char(cmd_buffer[pos++]);
					write_char(c);
					*vlan_str++ = c;
				}
				*vlan_str++ = ' '; *vlan_str = NUL;
				vlan_ptr = vlan_str - vlan_names;
				print_string("<\n");
			}
			w++;
		}

		uint8_t ret;
		uint8_t idx;
		while (cmd_words_len > w) {
			idx = cmd_words_b[w++];
			ret = cmd_parse_port(idx);
			if (ret == 0)
				goto err;

			idx += ret;
			uint16_t pmask = ((uint16_t)1) << atoi_results_u8;
			vlan_settings.members |= pmask;

			if (cmd_buffer[idx] == 't') {
				vlan_settings.tagged |= pmask;
				idx++;
			}

			if (!cmd_is_space_or_nul(idx))
				goto err;
		}
		vlan_create();
	} else if (cmd_compare(1, "show")) {
		vlan_dump();
	} else {
		goto err;
	}

	if (cmd_words_len >= 3 && isletter(cmd_buffer[cmd_words_b[2]])) {
		print_string("vlan_ptr "); print_short(vlan_ptr); write_char(':');
		write_char('>'); print_string_x(&vlan_names[0]); write_char('<'); write_char('\n');
	}
	return;
err:
	cmd_error("vlan (<vlan-id>|show) [port][t]...\n");
}


void parse_isolate(void)
{
	uint16_t members = 0;

	if (cmd_words_len < 3)
		goto err;

	print_string("\nISOLATE ");

	if (cmd_parse_port_separator(cmd_words_b[1]) == 0)
		goto err;
	uint8_t port_configured = atoi_results_u8;

	print_byte(port_configured); write_char('\n');

	if (cmd_compare(2, "show")) {
		members = port_isolation_get(port_configured);
		for (uint8_t i = 0; i < 10; i++) {
			if (members & 1) {
				print_phys_port(i);
				write_char(' ');
			}
			members >>= 1;
		}
		return;
    }

	if (cmd_compare(2, "off")) {
		for (uint8_t i = machine.min_port; i <= machine.max_port; i++)
			members |= ((uint16_t)1) << i;
		members |= 0x200; // CPU-port
		port_isolate(port_configured, members);
		return;
	}

	uint8_t w = 2;
	while (w < cmd_words_len) {
		if (cmd_parse_port_cpu_separator(cmd_words_b[w++]) == 0)
			goto err;
		uint8_t port = atoi_results_u8;
		members |= ((uint16_t)1) << port;
	}

	port_isolate(port_configured, members);
	return;

err:
	cmd_error("isolate <port> [show|off] [port]...\n");
}


bool vlan_ingress_mode_parse(char c, __xdata vlan_ingress_mode_t *mode)
{
	switch (c) {
	case 'u':
		*mode = VLAN_UNTAGGED;
		return true;
	case 't':
		*mode = VLAN_TAGGED;
		return true;
	case 'a':
		*mode = VLAN_ALL;
		return true;
	default:
		*mode = VLAN_INVALID;
		return false;
	}
}

void parse_ingress(void)
{
	if (cmd_words_len < 2) {
		goto err;
	}
	uint8_t log_port = 0;
	__xdata vlan_ingress_mode_t mode = VLAN_INVALID;
	uint8_t idx = cmd_words_b[1];

	if (vlan_ingress_mode_parse(cmd_buffer[idx++], &mode)) {
		if (!cmd_is_space_or_nul(idx))
			goto err;
		// Setting mode for all ports at once
		for (log_port = machine.min_port; log_port <= machine.max_port; log_port++) {
			if (!port_ingress_filter(log_port, mode)) {
				cmd_error("cannot set the ingress filter for port "); print_phys_port(log_port); write_char('\n');
				return;
			}
			print_string("All ports ingress filter set to: ");
			print_port_ingress_filter_mode(mode); write_char('\n');
		}
	} else {
		for(uint8_t w = 1; w < cmd_words_len; w++) {
			idx = cmd_words_b[w];
			uint8_t ret = cmd_parse_port(idx);
			if (ret != 1) {
				cmd_error("Invalid physical port number\n");
				continue;
			}
			log_port = atoi_results_u8;
			idx += ret;

			if (!vlan_ingress_mode_parse(cmd_buffer[idx++], &mode) || !cmd_is_space_or_nul(idx)) {
				cmd_error("Invalid ingress mode for port "); print_phys_port(log_port); print_string(" in ingress command\n");
				goto err;
			}
			if (!port_ingress_filter(log_port, mode)) {
				cmd_error("cannot set the ingress filter for port "); print_phys_port(log_port); write_char('\n');
				return;
			}
			print_string("Port "); print_phys_port(log_port);
			print_string(" ingress filter set to: ");
			print_port_ingress_filter_mode(mode); write_char('\n');
		}
	}
	return;
err:
	cmd_error("ingress [p]<u/t/a>...\n");
}

void parse_mirror(void)
{
	__xdata uint8_t mirroring_port = 0;
	__xdata uint16_t rx_pmask = 0;
	__xdata uint16_t tx_pmask = 0;

	if (cmd_compare(1, "status")) {
		reg_read_m(RTL837x_MIRROR_CTRL);
		uint8_t mPort = sfr_data[3];
		if (mPort & 1) {
			print_string("Enabled: ");
		} else {
			print_string("NOT Enabled: ");
		}
		print_string("Mirroring port: ");
		print_phys_port(mirroring_port);
		reg_read_m(RTL837x_MIRROR_CONF);
		uint16_t m = sfr_data[0];
		m = (m << 8) | sfr_data[1];
		print_string(", Port mask RX: ");
		print_short(m);
		m = sfr_data[2];
		m = (m << 8) | sfr_data[3];
		print_string(", Port mask TX: ");
		print_short(m);
		write_char('\n');
		return;
	} else if (cmd_compare(1, "off")) {
		port_mirror_del();
		return;
	}

	if (cmd_words_len < 2)
		goto err;

	uint8_t w = 2;
	uint8_t port;
	uint8_t ret;
	while (w < cmd_words_len) {
		uint8_t idx = cmd_words_b[w++];
		ret = cmd_parse_port(idx);
		if (ret == 0)
			goto err;

		idx += ret;
		port = atoi_results_u8;

		// Use the first port argument as mirroring_port
		if (w == 2)
			mirroring_port = port;

		ret = cmd_buffer[idx];
		uint16_t pmask = ((uint16_t)1) << port;
		if (ret != 't')
			rx_pmask |= pmask;
		if (ret != 'r')
			tx_pmask |= pmask;
	}
	port_mirror_set(mirroring_port, rx_pmask, tx_pmask);
	return;

err:
	cmd_error("Port/command missing: mirror [status/off/<mirroring port> [port][t/r]]...\n");
	return;
}


void parse_port(void)
{
	if (cmd_words_len < 3) {
		cmd_error("\nUsage:" \
					 "\nport <port> [show|on|off]" \
					 "\nport <port> [10m|100m|1g|2g5|duplex] [half|full]" \
					 "\nport <port> name [custom port name]\n");
		return;
	}

	if (cmd_parse_port_separator(cmd_words_b[1]) == 0) {
		cmd_error("Invalid port number\n");
		return;
	}
	phy_settings.port = atoi_results_u8;

	print_string("Logical Port: "); print_byte(phy_settings.port); write_char('\n');
	phy_settings.duplex = PHY_DUPLEX_BOTH;

	if (cmd_compare(2, "show")) {
		print_string("Name: ");
		print_string_x(port_names[phy_settings.port]);
		if (!machine.is_sfp[phy_settings.port]) {
			phy_show(phy_settings.port);
		} else {
			write_char('\n');
		}
		port_media_show(phy_settings.port);
	} else if (cmd_compare(2, "name")) {
		uint8_t i = 0;
		while ( cmd_words_len > 3 && (i < PORT_NAME_SIZE-1) && (cmd_buffer[cmd_words_b[3] + i] != NUL) ) {
			port_names[phy_settings.port][i] = name_char(cmd_buffer[cmd_words_b[3] + i]);
			i++;
		}
		port_names[phy_settings.port][i] = NUL;
		print_string("\nName set to: \"");
		print_string_x(port_names[phy_settings.port]);
		print_string("\"\n");
	} else if (machine.is_sfp[phy_settings.port]) {
		print_string(" is SFP no PHY information available.\n");
	} else if (cmd_compare(2, "10m")) {
		print_string(" 10M\n");
		phy_settings.speed = PHY_SPEED_10M;
		if (cmd_compare(3, "half"))
			phy_settings.duplex = PHY_DUPLEX_HALF;
		else if (cmd_compare(3, "full"))
			phy_settings.duplex = PHY_DUPLEX_FULL;
		phy_set_speed();
	} else if (cmd_compare(2, "100m")) {
		print_string(" 100M\n");
		phy_settings.speed = PHY_SPEED_100M;
		if (cmd_compare(3, "half"))
			phy_settings.duplex = PHY_DUPLEX_HALF;
		else if (cmd_compare(3, "full"))
			phy_settings.duplex = PHY_DUPLEX_FULL;
		phy_set_speed();
	} else if (cmd_compare(2, "10g")) {
		print_string(" 10G\n");
		phy_settings.speed = PHY_SPEED_10G;
		phy_set_speed();
	} else if (cmd_compare(2, "5g")) {
		print_string(" 5G\n");
		phy_settings.speed = PHY_SPEED_5G;
		phy_set_speed();
	} else if (cmd_compare(2, "2g5")) {
		print_string(" 2.5G\n");
		phy_settings.speed = PHY_SPEED_2G5;
		phy_set_speed();
	} else if (cmd_compare(2, "1g")) {
		print_string(" 1G\n");
		phy_settings.speed = PHY_SPEED_1G;
		phy_set_speed();
	} else if (cmd_compare(2, "auto")) {
		print_string(" AUTO\n");
		phy_settings.speed = PHY_SPEED_AUTO;
		phy_set_speed();
	} else if (cmd_compare(2, "off")) {
		print_string(" OFF\n");
		phy_settings.speed = PHY_OFF;
		phy_set_speed();
	} else if (cmd_compare(2, "on")) {
		print_string(" ON\n");
		phy_settings.speed = PHY_SPEED_AUTO;
		phy_set_speed();
	} else if (cmd_compare(2, "duplex")) {
		print_string(" DUPLEX\n");
		if (cmd_compare(3, "full"))
			phy_settings.duplex = PHY_DUPLEX_FULL;
		else
			phy_settings.duplex = PHY_DUPLEX_HALF;
		phy_set_duplex();
	} else {
		cmd_error("Unknown port command\n");
	}
}


void parse_mtu(void)
{
	uint8_t p;

	if (cmd_compare(1, "show")) {
		for (p = machine.min_port; p <= machine.max_port; p++) {
			reg_read_m(RTL8373_REG_MAC_L2_PORT_MAX_LEN + ((uint16_t) p << 8));
			uint16_t mtu = SFR_DATA_U16 & 0x3fff;
			print_string("Port "); print_phys_port(p);
			write_char(' '); print_short(mtu); write_char('\n');
		}
		return;
	}
	if (cmd_words_len != 3)
		goto err;

	if (cmd_parse_port_separator(cmd_words_b[1]) == 0)
		goto err;

	p = atoi_results_u8;
	print_byte(p);

	if (atoi_short(cmd_words_b[2]) == 0 || atoi_results_short < 64 || atoi_results_short > 0x3fff) {
		cmd_error("MTU must be 64..16383\n");
		return;
	}
	REG_WRITE(RTL8373_REG_MAC_L2_PORT_MAX_LEN + ((uint16_t) p << 8), (atoi_results_short >> 10) & 0xf, (atoi_results_short >> 2) & 0xff,
		  ((atoi_results_short & 0x3) << 6) | ((atoi_results_short >> 8) & 0x3f), atoi_results_short & 0xff);
	write_char('\n');
	return;

err:
	cmd_error("mtu [port] [size]\n");
	return;
}

bool sfp_print_measurements(uint8_t sfp)
{
	if (!sfp_read_block(sfp, 92, 1))
		return false;

	print_string("Options: "); print_byte(sfp_buf[0]); write_char('\n');
	if (!(sfp_options[sfp] & 0x40))
		return true;
	if (!sfp_read_block(sfp, 224, 16))
		return false;
	print_string("Temp: "); print_byte(sfp_buf[0]); print_byte(sfp_buf[1]); write_char('\n');
	print_string("Vcc: "); print_byte(sfp_buf[2]); print_byte(sfp_buf[3]); write_char('\n');
	print_string("TX Bias: "); print_byte(sfp_buf[4]); print_byte(sfp_buf[5]); write_char('\n');
	print_string("TX Power: "); print_byte(sfp_buf[6]); print_byte(sfp_buf[7]); write_char('\n');
	print_string("RX Power: "); print_byte(sfp_buf[8]); print_byte(sfp_buf[9]); write_char('\n');
	print_string("Laser: "); print_byte(sfp_buf[10]); print_byte(sfp_buf[11]); write_char('\n');
	print_string("State: "); print_byte(sfp_buf[14]); write_char('\n');

	return true;
}


void parse_sfp(void)
{
	uint8_t slot;

	if (cmd_words_len != 1 && cmd_words_len != 3)
		goto err;

	if (cmd_words_len == 1) {
		for (slot = 0; slot < machine.n_sfp; slot++) {
			print_string("\nSlot "); write_char('1' + slot);
			if (gpio_pin_test(machine.sfp_port[slot].pin_detect)) {
				print_string(" - empty\n");
				continue;
			}
			if (!sfp_read_block(slot, 11, 2)) {
				print_string(" - I2C read failed on this slot\n");
				continue;
			}
			print_string(" - Rate: "); print_byte(sfp_buf[1]);
			print_string("  Encoding: "); print_byte(sfp_buf[0]);
			write_char('\n');
			if (!sfp_print_info(slot) || !sfp_print_measurements(slot))
				print_string("I2C read failed on this slot\n");
		}
		return;
	}
	uint8_t idx = cmd_words_b[1];
	uint8_t ret = atoi_byte(idx);
	idx += ret;
	slot = atoi_results_u8 - 1;
	if (ret == 0 || !cmd_is_space(idx) || slot > 1) {
		cmd_error("Illegal SFP slot number\n");
		return;
	}
	if (slot >= machine.n_sfp) {
		cmd_error("SFP slot not present\n");
		return;
	}

	if (cmd_compare(2, "10g")) {
		print_string(" 10G\n");
		sfp_speed[slot] = SFP_SPEED_10G;
	} else if (cmd_compare(2, "2g5")) {
		print_string(" 2.5G\n");
		sfp_speed[slot] = SFP_SPEED_2G5;
	} else if (cmd_compare(2, "1g")) {
		print_string(" 1G\n");
		sfp_speed[slot] = SFP_SPEED_1G;
	} else if (cmd_compare(2, "100m")) {
		print_string(" 100M\n");
		sfp_speed[slot] = SFP_SPEED_100M;
	} else if (cmd_compare(2, "auto")) {
		print_string(" AUTO\n");
		sfp_speed[slot] = SFP_SPEED_AUTO;
	} else {
		goto err;
	}
	sfp_pins_last |= 0x1 << (slot << 2);
	handle_sfp();
	return;
err:
	cmd_error("\nUsage:\n\tsfp\n\tsfp [1|2] [1g|2g5|10g]\n");
}


void parse_regget(void)
{
	if (cmd_words_len != 2) {
		goto err;
	}

	uint8_t hex_size = atoi_hex(cmd_words_b[1]);

	if (hex_size == 0 || hex_size > 2) {
		goto err;
	}

	uint16_t reg = hexvalue[0];
	if (hex_size == 2) {
		reg <<= 8;
		reg |= hexvalue[1];
	}

	print_string("REGGET: ");
	print_short(reg);
	print_string(": VAL: ");

	reg_read_m(reg);
	print_sfr_data();
	write_char('\n');
	return;

err:
	cmd_error("regget <hexvalue>\n\tlike: regget 0BB0 or regget 0c");
	return;
}


void parse_regset(void)
{
	if (cmd_words_len != 3) {
		goto err;
	}

	uint8_t hex_size = atoi_hex(cmd_words_b[1]);
	if (hex_size == 0 || hex_size > 2) {
		goto err;
	}

	uint16_t reg = hexvalue[0];
	if (hex_size == 2) {
		reg <<= 8;
		reg |= hexvalue[1];
	}

	hex_size = atoi_hex(cmd_words_b[2]);
	if (hex_size == 0 || hex_size > 4) {
		goto err;
	}

	// zero sfr memory data
	sfr_set_zero();

	// copy data over sfr memory
	uint8_t offset = 4 - hex_size;
	while(hex_size) {
		hex_size -= 1;
		sfr_data[offset + hex_size] = hexvalue[hex_size];
	}
	print_string("REGSET: ");
	print_short(reg);

	reg_write_m(reg);

	print_string(": VAL: ");
	print_sfr_data();
	write_char('\n');
	return;

err:
	cmd_error("regset <hexvalue> <hexvalue>\n\tlike regset 0b abcd1234.");
}


static void print_temperature(uint16_t reg)
{
	int16_t v;

	reg_read(reg);
	v = SFR_DATA_U16;
	if (v < 0) {
		write_char('-');
		v = -v;
	}
	itoa(v >> 7);
	write_char('.');
	itoa(((v & 0x7f) * 10) >> 7);
	print_string(" C\n");
}


void parse_temp(void)
{
	print_string("\nChip temperature: ");
	print_temperature(RTL837X_TM_RESULT);
	print_string("At power-on:      ");
	print_temperature(RTL837X_TM_RESULT_POWERON);
}


void parse_sdsget(void)
{
	__xdata uint8_t sds_id, page, reg, hex_size;

	if (cmd_words_len != 4) {
		goto err;
	}

	if (!atoi_byte(cmd_words_b[1])) {
		goto err;
	}
	sds_id = atoi_results_u8;

	hex_size = atoi_hex(cmd_words_b[2]);
	if (hex_size != 1) {
		goto err;
	}
	page = hexvalue[0];

	hex_size = atoi_hex(cmd_words_b[3]);
	if (hex_size != 1) {
		goto err;
	}
	reg = hexvalue[0];

	print_string("SDSGET: ");
	itoa(sds_id);
	print_string(":0x");
	print_byte(page);
	print_string(":0x");
	print_byte(reg);
	print_string(": VAL: ");

	sds_read(sds_id, page, reg);
	print_phy_data();
	write_char('\n');
	return;

err:
	cmd_error("sdsget <sds-id> <hex:page> <hex:reg>\n");
	return;
}


void parse_sdsset(void)
{
	__xdata uint8_t sds_id, page, reg, hex_size;
	__xdata uint16_t val;

	if (cmd_words_len != 5) {
		goto err;
	}

	if (!atoi_byte(cmd_words_b[1])) {
		goto err;
	}
	sds_id = atoi_results_u8;

	hex_size = atoi_hex(cmd_words_b[2]);
	if (hex_size != 1) {
		goto err;
	}
	page = hexvalue[0];

	hex_size = atoi_hex(cmd_words_b[3]);
	if (hex_size != 1) {
		goto err;
	}
	reg = hexvalue[0];

	hex_size = atoi_hex(cmd_words_b[4]);
	if (hex_size == 0 || hex_size > 2) {
		goto err;
	}
	val = hexvalue[0];
	if (hex_size == 2) {
		val <<= 8;
		val |= hexvalue[1];
	}

	print_string("SDSSET: ");
	itoa(sds_id);
	print_string(":0x");
	print_byte(page);
	print_string(":0x");
	print_byte(reg);
	print_string(": VAL: ");

	sds_write_v(sds_id, page, reg, val);

	print_short(val);
	write_char('\n');
	return;

err:
	cmd_error("sdsset <sds-id> <hex:page> <hex:reg> <hex:val>\n");
	return;
}


void parse_phyget(void)
{
	__xdata uint8_t phy_id, dev_id, hex_size;
	__xdata uint16_t reg;

	if (cmd_words_len != 4) {
		goto err;
	}

	if (!atoi_byte(cmd_words_b[1])) {
		goto err;
	}
	phy_id = atoi_results_u8;


	if (!atoi_byte(cmd_words_b[2])) {
		goto err;
	}
	dev_id = atoi_results_u8;

	hex_size = atoi_hex(cmd_words_b[3]);
	if (hex_size == 0 || hex_size > 2) {
		goto err;
	}
	reg = hexvalue[0];
	if (hex_size == 2) {
		reg <<= 8;
		reg |= hexvalue[1];
	}

	print_string("PHYGET: ");
	itoa(phy_id);
	print_string(":");
	itoa(dev_id);
	print_string(".");
	print_short(reg);
	print_string(": VAL: ");

	phy_read(phy_id, dev_id, reg);
	print_phy_data();
	write_char('\n');
	return;

err:
	cmd_error("phyget <phy-id> <dev-id> <hex:reg>\n");
	return;
}


void parse_physet(void)
{
	__xdata uint8_t phy_id, dev_id, hex_size;
	__xdata uint16_t reg, val;

	if (cmd_words_len != 5) {
		goto err;
	}

	if (!atoi_byte(cmd_words_b[1])) {
		goto err;
	}
	phy_id = atoi_results_u8;


	if (!atoi_byte(cmd_words_b[2])) {
		goto err;
	}
	dev_id = atoi_results_u8;

	hex_size = atoi_hex(cmd_words_b[3]);
	if (hex_size == 0 || hex_size > 2) {
		goto err;
	}
	reg = hexvalue[0];
	if (hex_size == 2) {
		reg <<= 8;
		reg |= hexvalue[1];
	}

	hex_size = atoi_hex(cmd_words_b[4]);
	if (hex_size == 0 || hex_size > 2) {
		goto err;
	}
	val = hexvalue[0];
	if (hex_size == 2) {
		val <<= 8;
		val |= hexvalue[1];
	}

	print_string("PHYSET: ");
	itoa(phy_id);
	print_string(":");
	itoa(dev_id);
	print_string(".");
	print_short(reg);
	print_string(": VAL: ");

	phy_write(phy_id, dev_id, reg, val);

	print_short(val);
	write_char('\n');
	return;

err:
	cmd_error("physet <phy-id> <dev-id> <hex:reg> <hex:val>\n");
	return;
}


void parse_rnd(void)
{
	// In order to get a new random numner, this bit has to be set each time!
	reg_bit_set(RTL837X_RLDP_RLPP, RLDP_RND_EN);
	reg_read_m(RTL837X_RAND_NUM1);
	print_byte(sfr_data[2]);
	print_byte(sfr_data[3]);
	reg_read_m(RTL837X_RAND_NUM0);
	print_byte(sfr_data[0]);
	print_byte(sfr_data[1]);
	print_byte(sfr_data[2]);
	print_byte(sfr_data[3]);
	write_char('\n');
}


void parse_passwd(void)
{
	// cmd_words_len can be more then 2 if a SPACE in the password.
	if (cmd_words_len >= 2) {
		uint8_t i = cmd_words_b[1];		
		uint8_t c = 0;
		uint8_t j = 0;
		do {
			c = cmd_buffer[i++];
			passwd[j++] = c;
		} while (c != NUL && j < 20);
		passwd[j] = NUL;
		return;
	}
	cmd_error("Missing password\n");
}


void parse_eee(void)
{
	__xdata int8_t port = -1;
	__xdata uint8_t speed = EEE_2G5;
	__xdata uint8_t speed_word = 0;

	if (machine.n_10g)
		speed = EEE_10G;
	// Check if word 2 is a speed (contains 'g' or 'm') or a port number
	if (cmd_words_len >= 3) {
		uint8_t idx = cmd_words_b[2];
		// Skip digits to check if there's a letter after
		while (isnumber(cmd_buffer[idx]))
			idx++;
		if (cmd_buffer[idx] == 'g' || cmd_buffer[idx] == 'm') {
			// Word 2 is a speed (e.g., "2g5", "100m", "1g")
			speed_word = 2;
		} else if (cmd_is_space_or_nul(idx)) {
			// Word 2 is a port number
			if (cmd_parse_port_separator(cmd_words_b[2]) == 0) {
				cmd_error("Speed word invalid, use: [100m|1g|2g5]\n");
				return;
			}
			port = atoi_results_u8;
			// Check if word 3 is a speed
			if (cmd_words_len >= 4)
				speed_word = 3;
		}
	}
	// Parse speed if found
	if (speed_word > 0) {
		if (cmd_compare(speed_word, "100m"))
			speed = EEE_100;
		else if (cmd_compare(speed_word, "1g"))
			speed = EEE_1000;
		else if (cmd_compare(speed_word, "2g5"))
			speed = EEE_2G5;
		else 
		{
			cmd_error("Speed word invalid, use: [100m|1g|2g5]\n");
			return;
		}
	}
	if (cmd_compare(1, "on")) {
		if (port >= 0)
			port_eee_enable(port, speed);
		else
			port_eee_enable_all(speed);
	} else if (cmd_compare(1, "off")) {
		if (port >= 0)
			port_eee_disable(port);
		else
			port_eee_disable_all();
	} else if (cmd_compare(1, "status")) {
		if (port >= 0)
			port_eee_status(port);
		else
			port_eee_status_all();
	} else {
		cmd_error("eee [on|off|status] [port] [100m|1g|2g5]\n");
	}
}


void parse_bw(void)
{
	if (cmd_words_len < 3) // Check for at least 3 arguments
		goto err;

	if (cmd_parse_port_separator(cmd_words_b[2]) == 0)
		goto err;
	uint8_t port = atoi_results_u8;

	if (cmd_compare(1, "status")) {
		bandwidth_status(port);
		return;
	}

	if (cmd_words_len < 4) // Check for at least 4 arguments
		goto err;

	// Ensure first argument is only `in` or `out`.
	__bit is_in = cmd_compare(1, "in") != 0;
	if (!(is_in || cmd_compare(1, "out")))
		goto err;

	if (cmd_compare(3, "drop")) {
		if (is_in) {
			bandwidth_ingress_drop(port);
			return;
		}
		goto err;
	}

	if (cmd_compare(3, "fc")) {
		if (is_in) {
			bandwidth_ingress_fc(port);
			return;
		}
		goto err;
	}

	if (cmd_compare(3, "off")) {
		if (is_in) {
			bandwidth_ingress_disable(port);
		} else  {
			bandwidth_egress_disable(port);
		}
		return;
	}

	__xdata uint32_t bw = 0;
	uint8_t hex_size = atoi_hex(cmd_words_b[3]);
	if (hex_size == 0 || hex_size > 4)
		goto err;

	uint8_t i = 0;
	do {
		hex_size--;
		*(((uint8_t *) &bw) + hex_size) = hexvalue[i++];
	} while (hex_size);

	if (is_in) {
		bandwidth_ingress_set(port, bw);
	} else {
		bandwidth_egress_set(port, bw);
	}
	return;

err:
	cmd_error("bw [in|out|status] <port> [<hexvalue>|off|drop|fc]\n");
}

void parse_syslog(void)
{
	if (cmd_words_len < 2) // no argument -> print status
	{
		print_string("Current syslog status: ");
		if (syslog_state.enabled) {
			print_string("enabled, sending to ");
			print_ip(syslog_state.server_ip);
			write_char(':'); itoa_short(syslog_state.server_port);
			write_char('\n');
		} else {
			print_string("disabled\n");
		}
		return;
	}

	if (cmd_compare(1, "on")) {
		syslog_start();
	} else if (cmd_compare(1, "off")){
		syslog_stop();
	} else if (cmd_compare(1, "ip")) {
		if (cmd_words_len < 3) { // no additional arguemnt -> print current ip
			print_string("Current syslog IP: ");
			print_ip(syslog_state.server_ip);
			return;
		} else if (parse_ip(cmd_words_b[2]) != 0) {
			uint8_t was_enabled = syslog_state.enabled;
			if (was_enabled)
				syslog_stop();
			print_string("Setting new syslog IP.\n");
			syslog_state.server_ip[0] = ip[0]; syslog_state.server_ip[1] = ip[1];
			syslog_state.server_ip[2] = ip[2]; syslog_state.server_ip[3] = ip[3];
			if (was_enabled)
				syslog_start();
		} else {
			cmd_error("Invalid IP address\n");
		}
	} else if (cmd_compare(1, "port")) {
		if (cmd_words_len < 3) { // no additional argument -> print current port
			print_string("Current syslog port: ");
			itoa_short(syslog_state.server_port); write_char('\n');
			return;
		}
		/* atoi_short() returns zero for no digits and for a value past
		 * 65535; port zero is not a port either, so both tests are needed. */
		if (!atoi_short(cmd_words_b[2]) || !atoi_results_short) {
			cmd_error("Invalid port\n");
			return;
		}
		/* The port is frozen into the connection by uip_udp_new(), so
		 * changing it has to tear the connection down and build a new one -
		 * same dance as setting the address above. */
		syslog_port_was_on = syslog_state.enabled;
		if (syslog_port_was_on)
			syslog_stop();
		print_string("Setting new syslog port.\n");
		syslog_state.server_port = atoi_results_short;
		if (syslog_port_was_on)
			syslog_start();
	}
	else
	{
		cmd_error("syslog [on|off|ip [ip-address]|port [number]]\n"
			  "  on/off enables or disables syslog, ip sets the syslog server IP address,\n"
			  "  port sets the destination UDP port (default 514)\n");
	}
}

void parse_session(void)
{
	if (cmd_words_len >= 2) {
		/* atoi_short() returns zero for no digits and for a value past
		 * 65535; a zero-second timeout is not useful either, so both
		 * tests are needed. */
		if (!atoi_short(cmd_words_b[1]) || !atoi_results_short) {
			print_string("Must be 1-65535 s\n");
			return;
		}
		session_timeout = atoi_results_short;
	}
	/* Also the confirmation after setting: one message for both. */
	print_string("Session timeout: ");
	itoa_short(session_timeout);
	print_string(" s\n");
}


// Parse command into words
// cmd_words_len contains the number of words found.
// cmd_words_b[] contains only start of a word offset.
// Returns the parsing status via `err_status`-variable.
void cmd_tokenize(void) __banked
{
#ifdef DEBUG
	print_string("Tokenizing command\n");
	print_string_x(&cmd_buffer[0]);
	write_char('<'); write_char('\n');
#endif
	err_status = ERR_OK;
	uint8_t line_ptr = 0;
	uint8_t is_white = 1;
	uint8_t word = 0;
	uint8_t c = 0;

	while(1) {
		c = cmd_buffer[line_ptr];
		
		if (c == NUL) {
			// Store the word count
			cmd_words_len = word;
			break;
		}

		if (line_ptr == CMD_BUF_SIZE - 1) {
			err_status = ERR_CMD_TOO_LONG;
			return;
		}

		if (is_white && c != ' ') {
			is_white = 0;

			cmd_words_b[word++] = line_ptr;
			if (word >= N_WORDS) {
				cmd_words_len = 0;
				print_string("\nSyntax error: too many arguments.");
				err_status = ERR_TOO_MANY_ARGUMENTS;
				return;
			}
		} else if (c == ' ') {
			is_white = 1;
		}

		line_ptr++;
	}
}

// Print GPIO status
void print_gpio_status(void) {
	for (uint8_t idx = 0; idx < 2; idx++) {
		reg_read(RTL837X_REG_GPIO_00_31_INPUT + (idx * 4));
		print_string("GPIO ");
		write_char(idx + '0');
		write_char(':');
		write_char(' ');

		print_byte(SFR_DATA_24);
		print_byte(SFR_DATA_16);
		print_byte(SFR_DATA_8);
		print_byte(SFR_DATA_0);

		write_char(' ');
		print_byte( gpio_last_value[(idx *4)] ^ SFR_DATA_24);
		gpio_last_value[(idx *4)] = SFR_DATA_24;
		print_byte( gpio_last_value[(idx *4) + 1] ^ SFR_DATA_16);
		gpio_last_value[(idx *4) + 1] = SFR_DATA_16;
		print_byte( gpio_last_value[(idx *4) + 2] ^ SFR_DATA_8);
		gpio_last_value[(idx *4) + 2] = SFR_DATA_8;
		print_byte( gpio_last_value[(idx *4) + 3] ^ SFR_DATA_0);
		gpio_last_value[(idx *4) + 3] = SFR_DATA_0;
		write_char('\n');
	}
}

// Show software version
void print_sw_version(void) __banked {
	print_string("Software version: " VERSION_SW);
	print_string("\nBuild date: " BUILD_DATE);
	print_string("\nHardware: ");
	print_string(machine.machine_name);
	write_char('\n');
}


// Identify command
void cmd_parser(void) __banked
{
#ifdef DEBUG
	print_long(ticks);
	print_string("Parsing command\n");
	print_string_x(&cmd_buffer[0]);
	write_char('<'); write_char('\n');
	print_string("CMD-words: ");
	print_byte(cmd_words_len); write_char(' ');
	print_byte(cmd_words_b[0]); write_char(' ');
	print_byte(cmd_words_b[1]); write_char(' ');
	print_byte(cmd_words_b[2]); write_char(' ');
	print_byte(cmd_words_b[3]); write_char(' ');
	print_byte(cmd_words_b[4]); write_char(' ');
	print_byte(cmd_words_b[5]); write_char(' ');
	print_byte(cmd_words_b[6]); write_char('\n');
#endif
	if (cmd_words_len >= 1) {
		if (cmd_compare(0, "reset")) {
			print_string("\nRESET\n\n");
			reset_chip();
		} else if (cmd_compare(0, "sfp")) {
			parse_sfp();
		} else if (cmd_compare(0, "stat")) {
			port_stats_print();
		} else if (cmd_compare(0, "flash") && cmd_words_len == 2) {
			uint8_t c = cmd_buffer[cmd_words_b[1]];
			if (c == 's') {
				print_string("\nSECURITY REGISTERS\n");
				// The following will only show something else than 0xff if it was programmed for a managed switch
				print_string("Region 1: ");
				flash_region.addr = 0x0001000;
				flash_region.len = 40;
				flash_read_security();
				print_string("\nRegion 2: ");
				flash_region.addr = 0x0002000;
				flash_region.len = 40;
				flash_read_security();
				print_string("\nRegion 3: ");
				flash_region.addr = 0x0003000;
				flash_region.len = 40;
				flash_read_security();
			} else if (c == 'j') {
				print_string("\nJEDEC ID\n");
				flash_read_jedecid();
			} else if (c == 'u') {
				print_string("\nUNIQUE ID (note: only 4 bytes are likely correct here!)\n");
				flash_read_uid();
			}
		} else if (cmd_compare(0, "port")) {
			parse_port();
		} else if (cmd_compare(0, "mtu")) {
			parse_mtu();
		} else if (cmd_compare(0, "syslog")) {
			parse_syslog();
		} else if (cmd_compare(0, "ip")) {
			if (cmd_compare(1, "dhcp")) {
				dhcp_start();
			} else if (cmd_words_len == 1) {
				print_string("Current IP: ");
				print_ip(uip_hostaddr);
				if (dhcp_state.state == DHCP_LEASING) {
					print_string(" (dhcp, renewal in sec: ");
					print_short(dhcp_state.dhcp_timer);
					write_char(')');
				} else {
					print_string(" (static)");
				}
				write_char('\n');
			} else {
				if (dhcp_state.state)
					dhcp_stop();
				if (parse_ip(cmd_words_b[1]) != 0) {
					uip_ipaddr(&uip_hostaddr, ip[0], ip[1], ip[2], ip[3]);
					print_string("Setting ip: ");
					print_ip(ip); write_char('\n');
				} else {
					cmd_error("Invalid IP address\n" \
								 "Error: ip [<ip-address>|dhcp]\n" \
								 "  The dhcp option enables the dhcp client, calling ip without options prints the current IP\n" \
								 "  Calling with a valid IP address will stop any ongoing dhcp client and set the IP address\n");
				}
			}
		} else if (cmd_compare(0, "gw")) {
			if (cmd_words_len == 1) {
				print_string("Current gw: ");
				print_ip(uip_draddr); write_char('\n');
			} else {
				if (parse_ip(cmd_words_b[1]) != 0) {
					uip_ipaddr(&uip_draddr, ip[0], ip[1], ip[2], ip[3]);
					print_string("Setting gw: ");
					print_ip(ip); write_char('\n');
				} else {
					cmd_error("Invalid IP address\n" \
								 "Error: gw <ip-address>\n");
				}
			}
		} else if (cmd_compare(0, "netmask")) {
			if (cmd_words_len == 1) {
				print_string("Current netmask: ");
				print_ip(uip_netmask); write_char('\n');
			} else {
				if (parse_ip(cmd_words_b[1]) != 0) {
					uip_ipaddr(&uip_netmask, ip[0], ip[1], ip[2], ip[3]);
					print_string("Setting netmask: ");
					print_ip(ip); write_char('\n');
				} else {
					cmd_error("Invalid IP address\n");
				}
			}
			write_char('\n');
		} else if (cmd_compare(0, "l2")) {
			if (cmd_compare(1, "forget"))
				port_l2_forget();
			else
				port_l2_learned();
		} else if (cmd_compare(0, "igmp")) {
			if (cmd_compare(1, "on"))
				igmp_enable();
			else if (cmd_compare(1, "off"))
				igmp_setup();
			else if (cmd_compare(1, "show"))
				igmp_show();
			else {
				cmd_error("igmp on|off|show\n");
			}
		} else if (cmd_compare(0, "mac")) {
			parse_mac_cmd();
		} else if (cmd_compare(0, "hostname")) {
			/* "hostname" alone reports the current name; "hostname <text>"
			 * sets it, sanitized to JSON-safe printable ASCII. A name with
			 * spaces would tokenize into several words - reject it instead
			 * of silently keeping the first one. */
			if (cmd_words_len == 1) {
				print_string_x(hostname);
				write_char('\n');
			} else if (cmd_words_len == 2) {
				__xdata uint8_t *hp = &cmd_buffer[cmd_words_b[1]];
				__xdata char *dst = hostname;
				for (uint8_t hn = 0; hn < sizeof(hostname) - 1; hn++) {
					uint8_t c = *hp++;
					if (c == NUL || c == '\r' || c == '\n')
						break;
					*dst++ = name_char(c);
				}
				*dst = NUL;
			} else {
				cmd_error("hostname [name] - the name must not contain spaces\n");
			}
		} else if (cmd_compare(0, "stp")) {
			stp_parse();
#ifdef HEALTH
		} else if (cmd_compare(0, "health")) {
			health_show();
#endif
		} else if (cmd_compare(0, "pvid")) {
			if (cmd_words_len == 3 && cmd_parse_port_separator(cmd_words_b[1]) != 0
			    && atoi_short(cmd_words_b[2]) && atoi_results_short && atoi_results_short <= 4094)
				port_pvid_set(atoi_results_u8, atoi_results_short);
			else {
				cmd_error("pvid <port> <1-4094>\n");
			}
		} else if (cmd_compare(0, "vlan")) {
			parse_vlan();
		} else if (cmd_compare(0, "isolate")) {
			parse_isolate();
		} else if (cmd_compare(0, "mirror")) {
			parse_mirror();
		} else if (cmd_compare(0, "lag")) {
			parse_lag();
		} else if (cmd_compare(0, "laghash")) {
			parse_lag_hash();
		} else if (cmd_compare(0, "sds")) {
			print_reg(RTL837X_REG_SDS_MODES);
			write_char('\n');
		} else if (cmd_compare(0, "gpio")) {
			print_gpio_status();
		} else if (cmd_compare(0, "regget")) {
			parse_regget();
		} else if (cmd_compare(0, "regset")) {
			parse_regset();
		} else if (cmd_compare(0, "sdsget")) {
			parse_sdsget();
		} else if (cmd_compare(0, "sdsset")) {
			parse_sdsset();
		} else if (cmd_compare(0, "phyget")) {
			parse_phyget();
		} else if (cmd_compare(0, "physet")) {
			parse_physet();
		} else if (cmd_compare(0, "rnd")) {
			parse_rnd();
		} else if (cmd_compare(0, "passwd")) {
			parse_passwd();
		} else if (cmd_compare(0, "eee")) {
			parse_eee();
		} else if (cmd_compare(0, "bw")) {
			parse_bw();
		} else if (cmd_compare(0, "temp")) {
			parse_temp();
		} else if (cmd_compare(0, "version")) {
			print_sw_version();
		} else if (cmd_compare(0, "time")) {
			print_string("  Tick counter: "); print_long(ticks); print_string("   Sec Counter: ");
			reg_read_m(RTL837X_REG_SEC_COUNTER);
			print_sfr_data();
			write_char('\n');
		} else if (cmd_compare(0, "history")) {
			__xdata uint16_t p = (cmd_history_ptr + 1) & CMD_HISTORY_MASK;
			__xdata uint8_t found_begin = 0;
//			print_string("History ptr: ");
//			print_short(cmd_history_ptr); write_char('\n');
			while (p != cmd_history_ptr) {
//				print_short(p); write_char(' ');
				if (!cmd_history[p] || cmd_history[p] == '\n')
					found_begin = 1;
				if (found_begin && cmd_history[p])
					write_char(cmd_history[p]);
				p = (p + 1) & CMD_HISTORY_MASK;
			}
		} else if (cmd_compare(0, "ingress")) {
			parse_ingress();
		} else if (cmd_compare(0, "session")) {
			parse_session();
		}
		else {
			cmd_error("Unknown command\n");
		}


		if (save_cmd && cmd_words_len && err_status == ERR_OK) {
			// Find end of the cmd-buffer, looking for the NUL-byte.
			uint8_t i = cmd_words_b[cmd_words_len - 1];
			do {
				i++;
			} while(cmd_buffer[i] != NUL);

			// Copy last cmd-buffer to history.
			cmd_history_ptr = (cmd_history_ptr + i) & CMD_HISTORY_MASK;
			__xdata uint16_t p = cmd_history_ptr;
			cmd_history[cmd_history_ptr] = '\n';
			cmd_history_ptr = (cmd_history_ptr + 1) & CMD_HISTORY_MASK;
			do {
				i--;
				cmd_history[--p & CMD_HISTORY_MASK] = cmd_buffer[i];
			} while (i);
		}
	}
}

void clear_command_history(void) __banked
{
	for (cmd_history_ptr = 0; cmd_history_ptr < CMD_HISTORY_SIZE; cmd_history_ptr++)
		cmd_history[cmd_history_ptr] = 0;
	cmd_history_ptr = 0;
	return;
}


#define FLASH_READ_BURST_SIZE 0x100
#define PASSWORD "1234"

#if CONFIG_LEN % FLASH_READ_BURST_SIZE
	#error "CONFIG_LEN not a multiple of FLASH_READ_BURST_SIZE"
#endif
void execute_config(void) __banked
{
	__xdata uint32_t pos = CONFIG_START;
	__xdata uint8_t pages_left = CONFIG_LEN / FLASH_READ_BURST_SIZE;

	// Set default password, it can be overwritten in the configuration file
	strtox(passwd, PASSWORD);
	save_cmd = 0;

	uint8_t cmd_idx = 0;
	uint8_t skipping = 0;
	do {
		flash_region.addr = pos;
		flash_region.len = FLASH_READ_BURST_SIZE;
		flash_read_bulk(flash_buf);

		__xdata uint8_t cfg_idx = 0;
		uint8_t c = 0;
		do {
			if (cmd_idx >= (CMD_BUF_SIZE - 1)) {
				cmd_buffer[cmd_idx] = NUL;
				print_string("ERROR: Command too long, skipped: ");
				print_string_x(cmd_buffer);
				write_char('\n');
				cmd_idx = 0;
				skipping = 1;
			}
			c = flash_buf[cfg_idx++];
			if (c == 0 || c == '\n') {
				if (skipping) {
					skipping = 0;
				} else {
					cmd_buffer[cmd_idx] = NUL;
					if (cmd_idx) {
						cmd_tokenize();
						if (err_status == ERR_OK)
							cmd_parser();
					}
				}
				if (c == 0)
					goto config_done;
				cmd_idx = 0;
				continue;
			}

			if (!skipping) {
				cmd_buffer[cmd_idx] = c;
				cmd_idx++;
			}
		} while (cfg_idx);

		pages_left--;
		pos += FLASH_READ_BURST_SIZE;
	} while(pages_left);

config_done:
	// Start saving commands to cmd_history
	clear_command_history();
	save_cmd = 1;
}

// Execute multiple commands
// If a command is too long or can't be tokenized, remaining commands are not executed
// Returns the status via `err_status`-variable.
void execute_commands(__xdata uint8_t *p) __banked {
	__xdata uint8_t failed = ERR_OK;
	err_status = ERR_OK;
	uint8_t cmd_idx = 0;
	while (1) {
		if (*p == 0 || *p == '\n' || *p == '\r') {
			if (cmd_idx) {
				cmd_buffer[cmd_idx] = NUL;
				cmd_tokenize();
				if (err_status != ERR_OK)
					return;
				cmd_parser();
				if (failed == ERR_OK)
					failed = err_status;
			}
			if (*p == 0) {
				err_status = failed;
				return;
			}
			cmd_idx = 0;
		} else {
			if (cmd_idx < (CMD_BUF_SIZE - 1)) {
				cmd_buffer[cmd_idx++] = *p;
			} else {
				cmd_buffer[CMD_BUF_SIZE - 1] = NUL;
				print_string("ERROR: Command too long: ");
				print_string_x(cmd_buffer);
				write_char('\n');
				err_status = ERR_CMD_TOO_LONG;
				return;
			}
		}
		p++;
	};
}

#ifdef HEALTH
/*
 * Health instrumentation. The counters live in xdata and are fed from the
 * main loop through the banked entry points below; the "health" command
 * prints everything in one go. All figures are raw hex, diffing two dumps
 * gives the rates. Phase times are in system ticks (SYS_TICK_HZ = 200,
 * 5 ms each); a phase counts as slow when it spans 2 or more ticks.
 */
extern __xdata uint16_t len_left;	/* httpd: bytes still to send of the current file */
extern __xdata uint8_t entry;		/* httpd: index of the file being sent */

static __xdata uint32_t health_loops;
__xdata uint16_t health_rx_frames;

static __xdata uint16_t ph_last_t;
static __xdata uint8_t  ph_max[HEALTH_PHASES];
static __xdata uint16_t ph_slow[HEALTH_PHASES];

/* The ISR increments ticks between the two byte reads of a 16-bit load, so
 * read until two loads agree. */
static uint16_t ticks_lo(void)
{
	uint16_t a, b;
	a = (uint16_t)ticks;
	do {
		b = a;
		a = (uint16_t)ticks;
	} while (a != b);
	return a;
}


void health_loop_start(void) __banked
{
	health_loops++;
	ph_last_t = ticks_lo();
}


void health_phase(uint8_t id) __banked
{
	__xdata uint16_t now = ticks_lo();
	__xdata uint16_t d = now - ph_last_t;
	ph_last_t = now;
	if (d > 0xff)
		d = 0xff;
	if ((uint8_t)d > ph_max[id])
		ph_max[id] = d;
	if (d >= 2)
		ph_slow[id]++;
}


/* Fill the stack area above the current frame with a pattern; the dump scans
 * for how much of it survived. The 8051 stack grows upward, so everything
 * above SP is free at this point. Interrupts are held off so an ISR does not
 * push into the area mid-paint. */
void health_stack_paint(void) __banked
{
	__xdata uint8_t from = SP + 4;
	__xdata uint8_t ea = EA;
	EA = 0;
	for (uint8_t i = 0xff; i >= from; i--)
		*(__idata uint8_t *)i = 0xa5;
	EA = ea;
}


static void ph_line(__code const char *name, uint8_t id)
{
	print_string(name);
	print_string(" max ");
	print_byte(ph_max[id]);
	print_string(" slow ");
	print_short(ph_slow[id]);
	write_char('\n');
}


void health_show(void) __banked
{
	__xdata uint8_t i;

	print_string("up ");
	reg_read_m(RTL837X_REG_SEC_COUNTER);
	print_byte(sfr_data[0]); print_byte(sfr_data[1]);
	print_byte(sfr_data[2]); print_byte(sfr_data[3]);
	print_string(" ticks ");
	print_long(ticks);
	print_string(" loops ");
	print_long(health_loops);
	print_string(" rx ");
	print_short(health_rx_frames);
	write_char('\n');

	/* Phase timing: max duration seen (ticks of 5 ms) and slow-pass count */
	ph_line("link", HEALTH_PH_LINK);
	ph_line("sfp ", HEALTH_PH_SFP);
	ph_line("rx  ", HEALTH_PH_RX);
	ph_line("tx  ", HEALTH_PH_TX);
	ph_line("stp ", HEALTH_PH_STP);
	ph_line("cmd ", HEALTH_PH_CMD);

	/* Stack: current depth plus how much of the painted area is untouched */
	print_string("sp ");
	print_byte(SP);
	print_string(" untouched ");
	i = 0;
	while (*(__idata uint8_t *)(0xff - i) == 0xa5 && i < 0x86)
		i++;
	print_byte(i);
	write_char('\n');

	/* The TCP connection slot. One on this configuration; a build with more
	 * has to say which slot each line is, so make that a visible decision. */
#if UIP_CONNS != 1
#error "health: the tcp dump prints a single slot, extend it for UIP_CONNS > 1"
#endif
	i = 0;
	{
		print_string("tcp ");
		print_string("st ");
		print_byte(uip_conns[i].tcpstateflags);
		print_string(" tmr ");
		print_byte(uip_conns[i].timer);
		print_string(" rtx ");
		print_byte(uip_conns[i].nrtx);
		print_string(" rto ");
		print_byte(uip_conns[i].rto);
		print_string(" len ");
		print_short(uip_conns[i].len);
		print_string(" mss ");
		print_short(uip_conns[i].mss);
		write_char('\n');
		print_string("  lport ");
		print_short(HTONS(uip_conns[i].lport));
		print_string(" rport ");
		print_short(HTONS(uip_conns[i].rport));
		print_string(" rip ");
		print_short(HTONS(uip_conns[i].ripaddr[0]));
		write_char(' ');
		print_short(HTONS(uip_conns[i].ripaddr[1]));
		write_char('\n');
	}

	/* httpd transfer state and the protocol flags */
	print_string("httpd left ");
	print_short(len_left);
	print_string(" entry ");
	print_byte(entry);
	print_string(" stp ");
	print_byte(stp_enabled);
	print_string(" mvlan ");
	print_short(management_vlan);
	write_char('\n');
}
#endif