#include <linux/module.h>
#include <linux/delay.h>	/* for udelay */
#include "xpd.h"
#include "xpp_proto.h"
#include "xpp_zap.h"

static char rcsid[] = "$Id$";

extern	int print_dbg;
#include "zap_debug.h"

typedef struct xpp_command xpp_command_t;
typedef	int (*xpp_handler_t)(xbus_t *xbus, int id, xpp_command_t *cmd, xpacket_t *packet);

struct xpp_command {
	xpp_opcode_t	opcode;
	unsigned int	header_size;
	bool		varsize;
	const char	*name;
	const char	*desc;
	xpp_handler_t	handler;
};

#define	S_(s,l,...)					\
	{						\
		.lines = s,				\
		{					\
			.len = l,			\
			.data = { __VA_ARGS__ },	\
		}					\
	}

struct slic_init_data {
	xpp_line_t	lines;
	slic_data_t	slic_data;
} slic_init_data[] = {
#include "slic_init.inc"
};

static int packet_process(xbus_t *xbus, int xpd_num, xpacket_t *pack);
static int simulate_xpd(xbus_t *xbus, int xpd_num, xpacket_t *sent_packet);
static bool pcm_valid(xpd_t *xpd, xpacket_t *reply);

#define	NEW_PACKET(p, xbus, name, to)	\
	do {				\
		p = xbus->ops->packet_new(xbus, GFP_ATOMIC);	\
		if(!p)				\
			return -ENOMEM;		\
		PACKET_INIT(p, name);		\
		XPD_ADDR_SET(p->content.addr, to); \
	} while(0);

/*------------------------- SLIC Handling --------------------------*/

int proc_xpd_slic_read(char *page, char **start, off_t off, int count, int *eof, void *data)
{
	int		len = 0;
	unsigned long	flags;
	xpd_t		*xpd = data;
	//slic_reply_t	*info;

	BUG_ON(!xpd);
	spin_lock_irqsave(&xpd->lock, flags);
#if 0
	info = (slic_reply_t *)&xpd->slic_info;
	len += sprintf(page + len, "SLIC_REPLY: %s reg_num=0x%X, dataH=0x%X dataL=0x%X\n",
			(info->indirect)?"I":"D",
			info->reg_num, info->data_high, info->data_low);
#endif
	spin_unlock_irqrestore(&xpd->lock, flags);
	if (len <= off+count)
		*eof = 1;
	*start = page + off;
	len -= off;
	if (len > count)
		len = count;
	if (len < 0)
		len = 0;
	return len;
}

static int parse_slic_cmd(const char *buf, slic_cmd_t *sc)
{
	char		op;		/* [W]rite, [R]ead */
	char		reg_type;	/* [D]irect, [I]ndirect */
	int		s1, s2, s3, s4;
	int		reg_num;
	int		data_low, data_high;
	xpp_line_t	lines;
	int		ret;

	ret = sscanf(buf, "%x %x %x %x %c%c %x %x %x",
			&s1, &s2, &s3, &s4, &op, &reg_type, &reg_num, &data_high, &data_low);
	lines = (s4 << 24) | (s3 << 16) | (s2 << 8) | (s1);
	switch(op) {
		case 'R':
			if(reg_type == 'D' && ret == 7) {
				// DBG("0x%X 0x%X 0x%X 0x%X %c %x\n", s1, s2, s3, s4, reg_type, reg_num);
				ret = slic_cmd_direct_read(sc, lines, reg_num);
			} else if(reg_type == 'I' && ret == 7) {
				// DBG("0x%X 0x%X 0x%X 0x%X %c %x\n", s1, s2, s3, s4, reg_type, reg_num);
				ret = slic_cmd_indirect_read(sc, lines, reg_num);
			} else {
				NOTICE("%s: Bad read input: ret=%d buf='%s' reg_type=%c\n", __FUNCTION__, ret, buf, reg_type);
				goto err;
			}
			break;
		case 'W':
			if(reg_type == 'D' && ret == 8) {
				// DBG("0x%X 0x%X 0x%X 0x%X %c %x %X\n", s1, s2, s3, s4, reg_type, reg_num, data_high);
				ret = slic_cmd_direct_write(sc, lines, reg_num, data_high);
			} else if(reg_type == 'I' && ret == 9) {
				// DBG("0x%X 0x%X 0x%X 0x%X %c %x %X %X\n", s1, s2, s3, s4, reg_type, reg_num, data_high, data_low);
				ret = slic_cmd_indirect_write(sc, lines, reg_num, data_low, data_high);
			} else {
				NOTICE("%s: Bad write input: ret=%d buf='%s' reg_type=%c\n", __FUNCTION__, ret, buf, reg_type);
				goto err;
			}
			break;
		default:
			NOTICE("%s: Bad input: ret=%d buf='%s' op=%c\n", __FUNCTION__, ret, buf, op);
			goto err;
	}
	return ret;
err:
	return -EINVAL;
}

static int process_slic_cmdline(xpd_t *xpd, char *cmdline)
{
	xbus_t		*xbus;
	slic_cmd_t	sc;
	xpacket_t	*pack_tx;
	char		*p;
	int		len = strlen(cmdline);

	BUG_ON(!xpd);
	xbus = xpd->xbus;
	if((p = strchr(cmdline, '#')) != NULL)	/* Truncate comments */
		*p = '\0';
	if((p = strchr(cmdline, ';')) != NULL)	/* Truncate comments */
		*p = '\0';
	for(p = cmdline; *p && (*p == ' ' || *p == '\t'); p++) /* Trim leading whitespace */
		;
	if(*p == '\0')
		return 0;
	len = parse_slic_cmd(p, &sc);
	if(len < 0)
		return len;
	sc.lines &= xpd->enabled_chans;	// Ignore disabled channels
	if(!sc.lines) {
		NOTICE("%s: no enabled channels are marked. Skip.\n", __FUNCTION__);
		return 0;
	}
	dump_slic_cmd("WRITE_SLIC", &sc);
	NEW_PACKET(pack_tx, xbus, SLIC_WRITE, xpd->id);
	PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd) = sc;
	pack_tx->datalen = len;
	packet_send(xbus, pack_tx);
	return 0;
}

int proc_xpd_slic_write(struct file *file, const char __user *buffer, unsigned long count, void *data)
{
	xpd_t		*xpd = data;
	const int	LINE_LEN = 500;
	char		buf[LINE_LEN];
	char		*p;
	int		i;
	int		ret;

	BUG_ON(!xpd);
	for(i = 0; i < count; /* noop */) {
		for(p = buf; p < buf + LINE_LEN; p++) {	/* read a line */
			if(i >= count)
				break;
			if(get_user(*p, buffer + i))
				return -EFAULT;
			i++;
			if(*p == '\n' || *p == '\r')	/* whatever */
				break;
		}
		if(p >= buf + LINE_LEN)
			return -E2BIG;
		*p = '\0';
		ret = process_slic_cmdline(xpd, buf);
		if(ret < 0)
			return ret;
	}
	return count;
}



/*------------------------- Protocol Functions ---------------------*/

#define	HOSTCMD(name, ...)		\
			DECLARE_CMD(name, ## __VA_ARGS__ );	\
			EXPORT_SYMBOL(xpp_proto_ ## name);	\
			DECLARE_CMD(name, ## __VA_ARGS__ )


/* 0x04 */ HOSTCMD(DESC_REQ, int xpd_num)
{
	int	ret = 0;
	xpacket_t	*pack_tx;

	DBG("\n");
	if(!xbus) {
		DBG("NO XBUS\n");
		return -EINVAL;
	}
	NEW_PACKET(pack_tx, xbus, DESC_REQ, xpd_num);
	DBG("calling packet_send for a DESC_REQ packet.\n");
	ret = packet_send(xbus, pack_tx);
	DBG("after packet_send, updating counter (ret=%d)\n", ret);
	XBUS_COUNTER(xbus, DESC_REQ)++;
	return ret;
}

/* 0x0F */ HOSTCMD(CHAN_POWER, xpp_line_t lines, bool on)
{
	int		ret = 0;
	xpacket_t	*pack_tx;
	slic_cmd_t	*sc;
	int		len;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	lines &= xpd->enabled_chans;	// Ignore disabled channels
	if(!lines) {
		return 0;
	}
	DBG("Channel Power: 0x%04X %s\n", lines, (on) ? "up" : "down");
	NEW_PACKET(pack_tx, xbus, SLIC_WRITE, xpd->id);
	sc = &PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd);
	if(on) {
		// Power up
		len = slic_cmd_direct_write(sc, lines, 0x42, 0x06);
	} else {
		// Power down
		len = slic_cmd_direct_write(sc, lines, 0x42, 0x00);
	}
	pack_tx->datalen = len;

	packet_send(xbus, pack_tx);
	return ret;
}

/* 0x0F */ HOSTCMD(CHAN_ENABLE, xpp_line_t lines, bool on)
{
	int		ret = 0;
	xpacket_t	*pack_tx;
	slic_cmd_t	*sc;
	int		len;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	lines &= xpd->enabled_chans;	// Ignore disabled channels
	if(!lines) {
		return 0;
	}
	DBG("Channel Activation: 0x%4X %s\n", lines, (on) ? "on" : "off");
	NEW_PACKET(pack_tx, xbus, SLIC_WRITE, xpd->id);
	sc = &PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd);
	len = slic_cmd_direct_write(sc, lines, 0x40, (on)?0x01:0x00);
	pack_tx->datalen = len;

	packet_send(xbus, pack_tx);
	return ret;
}

/* 0x0F */ HOSTCMD(RING, int pos, bool on)
{
	int		ret = 0;
	xpacket_t	*pack_tx;
	slic_cmd_t	*sc;
	xpp_line_t	mask = (1 << pos);
	int		len;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	mask &= xpd->enabled_chans;	// Ignore disabled channels
	if(!mask) {
		return 0;
	}
	DBG("%s pos=%d %s\n", xpd->xpdname, pos, (on) ? "on" : "off");
	NEW_PACKET(pack_tx, xbus, SLIC_WRITE, xpd->id);
	sc = &PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd);
	len = slic_cmd_direct_write(sc, mask, 0x40, (on)?0x04:0x01);
	pack_tx->datalen = len;

	packet_send(xbus, pack_tx);
	return ret;
}

/* 0x0F */ HOSTCMD(SETHOOK, xpp_line_t hook_status)
{
	int		ret = 0;
	xpacket_t	*pack_tx;
	slic_cmd_t	*sc;
	int		len;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	hook_status &= xpd->enabled_chans;	// Ignore disabled channels
	if(!hook_status) {
		return 0;
	}
	DBG("New hook_status: %d\n", hook_status);
	NEW_PACKET(pack_tx, xbus, SLIC_WRITE, xpd->id);
	sc = &PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd);
	/* FIXME: This is fake, until Dima implements FXO */
	len = slic_cmd_direct_write(sc, hook_status, 0x02, 0x00);
	pack_tx->datalen = len;

	packet_send(xbus, pack_tx);
	return ret;
}

/*
 * LED control is done via SLIC register 0x06:
 *         7     6     5     4     3     2     1     0
 * 	+-----+-----+-----+-----+-----+-----+-----+-----+
 * 	| MR  | MG  | MB  |  R  | OG  | OB  |  G  | B   |
 * 	+-----+-----+-----+-----+-----+-----+-----+-----+
 *
 * 	B	- BLUE LED (0 - OFF, 1 - ON)
 * 	G	- GREEN LED (0 - OFF, 1 - ON)
 * 	OB	- Output BLUE (this line is output)
 * 	OG	- Output GREEN (this line is output)
 * 	R	- RED LED (0 - OFF, 1 - ON)
 * 	MB	- Mask BLUE. (1 - B effect the BLUE LED)
 * 	MR	- Mask RED. (1 - R effect the RED LED)
 * 	MG	- Mask GREEN. (1 - G effect the GREEN LED)
 *
 * 	The BLUE LED (actually a relay out) is connected to line 0 and 4 only.
 */

//		                 	GREEN	RED	BLUE
static int	led_mask[NUM_LEDS] = { 	BIT(6),	BIT(7),	BIT(5) };
static int	led_vals[NUM_LEDS] = { 	BIT(1),	BIT(4),	BIT(0) };

/* 0x0F */ HOSTCMD(LED, xpp_line_t lines, byte which, bool on)
{
	int		ret = 0;
	xpacket_t	*pack_tx;
	slic_cmd_t	*sc;
	int		len;
	int		value;
	int		i;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	lines &= xpd->enabled_chans;	// Ignore disabled channels
	if(!lines) {
		return 0;
	}
	DBG("LED: lines=0x%04X which=%d -- %s\n", lines, which, (on) ? "on" : "off");
	which = which % NUM_LEDS;
	value = BIT(2) | BIT(3);
	value |= ((BIT(5) | BIT(6) | BIT(7)) & ~led_mask[which]);
	if(on)
		value |= led_vals[which];
	for(i = 0; i < CHANNELS_PERXPD; i++) {
		if(!IS_SET(lines, i))
				continue;
		NEW_PACKET(pack_tx, xbus, SLIC_WRITE, xpd->id);
		sc = &PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd);
		len = slic_cmd_direct_write(sc, lines, 0x06, value);
		DBG("LED pack: line=%d value=0x%04X\n", i, value);
		pack_tx->datalen = len;
		packet_send(xbus, pack_tx);
	}
	return ret;
}

/* 0x0F */ HOSTCMD(RELAY_OUT, byte which, bool on)
{
	int		ret = 0;
	xpacket_t	*pack_tx;
	slic_cmd_t	*sc;
	int		len;
	int		value;
	xpp_line_t	lines;
	int		relay_channels[] = { 0, 4 };

	BUG_ON(!xbus);
	BUG_ON(!xpd);

	DBG("RELAY_OUT: which=%d -- %s\n", which, (on) ? "on" : "off");
	which = which % ARRAY_SIZE(relay_channels);
	lines = BIT(relay_channels[which]);
	value = BIT(2) | BIT(3);
	value |= ((BIT(5) | BIT(6) | BIT(7)) & ~led_mask[LED_BLUE]);
	if(on)
		value |= led_vals[LED_BLUE];
	NEW_PACKET(pack_tx, xbus, SLIC_WRITE, xpd->id);
	sc = &PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd);
	len = slic_cmd_direct_write(sc, lines, 0x06, value);

	DBG("RELAY_OUT pack: line=%d value=0x%04X\n", lines, value);
	pack_tx->datalen = len;
	packet_send(xbus, pack_tx);
	return ret;
}

/* 0x0F */ HOSTCMD(SLIC_INIT)
{
	int	ret = 0;
	xpacket_t		*pack_tx;
	slic_data_t		*slic;
	struct slic_init_data	*source;
	int			i;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	DBG("INITIALIZING SLIC\n");
	for(i = 0; i < ARRAY_SIZE(slic_init_data); i++) {
		source = &slic_init_data[i];
		NEW_PACKET(pack_tx, xbus, SLIC_INIT, xpd->id);
		PACKET_FIELD(pack_tx, SLIC_INIT, lines) = source->lines;

		slic = &PACKET_FIELD(pack_tx, SLIC_INIT, slic_data);
		slic->len = source->slic_data.len;
		memcpy(slic->data, source->slic_data.data, source->slic_data.len);
		pack_tx->datalen = sizeof(xpp_line_t) + slic->len + 1;
//		dump_packet("SLIC", pack_tx, print_dbg);
		packet_send(xbus, pack_tx);
		mdelay(10);	// FIXME: Temporary -- Dima need to fix it
	}
	return ret;
}

/* 0x0F */ HOSTCMD(SLIC_QUERY, int pos, byte reg_num)
{
	int	ret = 0;
	xpacket_t	*pack_tx;
	slic_cmd_t	*sc;
	int		len;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	NEW_PACKET(pack_tx, xbus, SLIC_WRITE, xpd->id);
	sc = &PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd);
	len = slic_cmd_direct_read(sc, (1<<pos), reg_num);

	pack_tx->datalen = len;

	packet_send(xbus, pack_tx);
	return ret;
}

/* 0x11 */ HOSTCMD(PCM_WRITE, xpp_line_t lines, volatile byte *buf)
{
	int	ret = 0;
	xpacket_t	*pack_tx;
	byte		*pcm;
	byte		*start_pcm;
	int i;
	extern ulong	pcm_gen;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	lines &= xpd->enabled_chans;
	// DBG("PCM_WRITE\n");
	if(pcm_gen != 0)
		return 0;
//	if(lines == 0)
//		return 0;

	/*
	 * FIXME: Workaround a bug in sync code of the Astribank.
	 *        Send dummy PCM for sync.
	 */
	if(lines == 0)
		lines = BIT(0);

	NEW_PACKET(pack_tx, xbus, PCM_WRITE, xpd->id);
	PACKET_FIELD(pack_tx, PCM_WRITE, lines) = lines;
	start_pcm = pcm = PACKET_FIELD(pack_tx, PCM_WRITE, pcm);
	for(i = 0; i < CHANNELS_PERXPD; i++) {
		if(IS_SET(lines, i)) {
			memcpy(pcm, (byte *)buf, ZT_CHUNKSIZE);
			pcm += ZT_CHUNKSIZE;
		}
		buf += ZT_CHUNKSIZE;
	}
	pack_tx->datalen = sizeof(xpp_line_t) + (pcm - start_pcm);
	packet_send(xbus, pack_tx);
	XPD_COUNTER(xpd, PCM_WRITE)++;
	XBUS_COUNTER(xbus, PCM_WRITE)++;
	return ret;
}

/* 0x13 */ HOSTCMD(PCM_GEN, xpp_line_t lines,  volatile byte *buf)
{
	xpacket_t	*pack_tx;
	bool		gen_seq = ((lines != 0) && (buf != NULL));

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	lines &= xpd->enabled_chans;	// Ignore disabled channels
	if(!lines) {
		return 0;
	}
	DBG("PCM_GEN lines=0x%04X %s\n", lines, (gen_seq) ? "seq" : "off");
	NEW_PACKET(pack_tx, xbus, PCM_GEN, xpd->id);
	PACKET_FIELD(pack_tx, PCM_GEN, lines) = lines;
	if(gen_seq) {
		PACKET_FIELD(pack_tx, PCM_GEN, gen) = 0;
		memcpy(&PACKET_FIELD(pack_tx, PCM_GEN, pcm_seq), (byte *)buf, ZT_CHUNKSIZE);
	} else {
		PACKET_FIELD(pack_tx, PCM_GEN, gen) = 2;
	}
	packet_send(xbus, pack_tx);
	return 0;
}

/*
 * Sync source is controled by a mask byte to 0x19 command:
 *         7     6     5     4     3     2     1     0
 * 	+-----+-----+-----+-----+-----+-----+-----+-----+
 * 	|     |     |     |     |     |     | RW  | AB  |
 * 	+-----+-----+-----+-----+-----+-----+-----+-----+
 *
 *	RW	- Read or set (0 - Write, 1 - Read)
 *	AB	- This Astribank provide sync (0 - no, 1 - yes)
 *
 */

/* 0x19 */ HOSTCMD(SYNC_SOURCE, bool setit, bool is_master)
{
	xpacket_t	*pack_tx;
	byte		mask = 0;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	if(is_master)
		mask |= BIT(0);
	if(!setit)
		mask |= BIT(1);
	DBG("SYNC_SOURCE %s setit=%s is_master=%s (mask=0x%X)\n",
			xpd->xpdname, (setit)?"yes":"no", (is_master)?"yes":"no", mask);
	NEW_PACKET(pack_tx, xbus, SYNC_SOURCE, xpd->id);
	PACKET_FIELD(pack_tx, SYNC_SOURCE, mask) = mask;
	packet_send(xbus, pack_tx);
	return 0;
}

/* 0x31 */ HOSTCMD(LOOPBACK_AX, byte *data, unsigned int size)
{
	xpacket_t	*pack_tx;

	BUG_ON(!xbus);
	BUG_ON(!xpd);
	DBG("LOOPBACK_AX %d bytes\n", size);
	NEW_PACKET(pack_tx, xbus, LOOPBACK_AX, xpd->id);
	memcpy(&PACKET_FIELD(pack_tx, LOOPBACK_AX, data), data, size);
	packet_send(xbus, pack_tx);
	return 0;
}

/*------------------------- Protocol Simulator ---------------------*/


static int simulate_xpd(xbus_t *xbus, int xpd_num, xpacket_t *sent_packet)
{
	xpacket_t	*pack = sent_packet;
	struct xpd_sim	*xpd_sim;
	struct xpd_sim	*loopto_sim;
	xpp_opcode_t	opcode;
	int		dest_xpd_num;
	int		ret = 0;

	// Sanity checks
	BUG_ON(!xbus);
	BUG_ON(xpd_num > MAX_XPDS || xpd_num < 0);
	BUG_ON(!sent_packet);
	BUG_ON(!xbus->sim[xpd_num].simulated);

	XBUS_COUNTER(xbus, SIM_PACKETS)++;
	xpd_sim = &xbus->sim[xpd_num];
	opcode = pack->content.opcode;
	dest_xpd_num = xpd_sim->loopto;
	loopto_sim = &xbus->sim[dest_xpd_num];
//	DBG("before: addr=%d, opcode=0x%X\n", xpd_num, opcode);
	switch(opcode) {
		case XPP_DESC_REQ:
			DBG("SIM DESC_REQ (xpd_num=%d)\n", xpd_num);
			PACKET_INIT(pack, DEV_DESC);
			PACKET_FIELD(pack, DEV_DESC, type) = xpd_sim->xpd_type;
			dest_xpd_num = xpd_num;	// Reply as the original XPD
			break;
		case XPP_PCM_WRITE:
			PACKET_INIT(pack, PCM_READ);
			XPD_ADDR_SET(pack->content.addr, dest_xpd_num);
			break;
		case XPP_SLIC_WRITE:
#if FINISHED_DECODING_SLICS
			slic_cmd_t	*sc;
			int		len;

			sc = &PACKET_FIELD(pack_tx, SLIC_WRITE, slic_cmd);
			lines = sc->lines;
			bool	slic_write = ! (slic->data[0] & 0x80);
			int	slic_reg = slic->data[0] & ~0x80;

			if(slic->len == 2 && slic_write && slic_reg == 0x40) {				// RING
				bool		on = (slic->data[1] == 0x04);
				if(on) {
					loopto_sim->hookstate |= lines;
				} else {
					loopto_sim->hookstate &= ~lines;
				}

				DBG("SIM RING to: xpd=%d (type=%d): (%s) ringing=0x%04X lines=0x%04X\n", dest_xpd_num, loopto_sim->xpd_type,
						(on)?"on":"off", loopto_sim->hookstate, lines);
				PACKET_INIT(pack, SIG_CHANGED);
				PACKET_FIELD(pack, SIG_CHANGED, type) = loopto_sim->xpd_type;
				PACKET_FIELD(pack, SIG_CHANGED, sig_status) = loopto_sim->hookstate;
				PACKET_FIELD(pack, SIG_CHANGED, sig_toggles) = lines;
				dump_packet("SIM RING TO", pack, print_dbg);
				break;
			} else if(slic->len == 1 && slic_write && slic_reg == 0x02) {			// SETHOOK
				DBG("SIM SETHOOK: xpd=%d: hookstate=0x%04X lines=0x%04X\n", dest_xpd_num, loopto_sim->hookstate, lines);
				PACKET_INIT(pack, SIG_CHANGED);
				PACKET_FIELD(pack, SIG_CHANGED, type) = loopto_sim->xpd_type;
				PACKET_FIELD(pack, SIG_CHANGED, sig_status) = lines;
				PACKET_FIELD(pack, SIG_CHANGED, sig_toggles) = loopto_sim->hookstate ^ lines;
				loopto_sim->hookstate = lines;
				break;
			} else if(slic->len == 2 && slic_write && slic_reg == 0x06) {			// LED
				DBG("SIM LED: xpd=%d: 0x%04X=%s\n", xpd_num, lines, (0x10)? "on" : "off");
				ret = 0;
				goto junk;
			} else if(slic->len == 2 && ! slic_write) {				// SLIC_QUERY
				DBG("SIM SLIC_QUERY: xpd=%d: register=0x%02X\n", xpd_num, slic_reg);
				ret = 0;
				goto junk;
			} else if(slic->len >= 4) {						// INITIALIZATION?
				DBG("SIM INITIALIZATION? xpd=%d len=%d\n", xpd_num, slic->len);
				ret = 0;
				goto junk;
			}
			NOTICE("%s: xpd=%d: SLIC_WRITE: len=%d\n", __FUNCTION__, xpd_num, slic->len);
#endif
			dump_packet("BAD SLIC_WRITE", pack, print_dbg);
			// FALL THROUGH
		default:
			NOTICE("%s: xpd=%d: CANNOT SIMULATE OPCODE=0x%02X\n", 
					__FUNCTION__, xpd_num, opcode);
//			dump_packet("BAD OPCODE", pack, print_dbg);
			ret = -EINVAL;
			goto junk;
	}
//	DBG("after reversing: addr=%d, opcode=0x%X\n", xpd_num, pack->header.opcode);
	return packet_process(xbus, dest_xpd_num, pack);
junk:
	xbus->ops->packet_free(xbus, pack);
	return ret;
}

#define	VERBOSE_DEBUG			1
#define	ERR_REPORT_LIMIT		20

void dump_packet(const char *msg, xpacket_t *packet, bool print_dbg)
{
	xpp_opcode_t	op = (byte)packet->content.opcode;

	if(!print_dbg)
		return;
	DBG("%s: @0x%02X OP=0x%02X flags=0x%02X LEN=%d\n",
			msg,
			XPD_NUM(packet->content.addr),
			op,
			(byte)packet->flags,
			(byte)packet->datalen);
#if VERBOSE_DEBUG
	{
		int i;
		byte	*p = packet->content.raw;

		for(i = 0; i < packet->datalen; i++) {
			static int limiter = 0;

			if(i >= sizeof(xpp_packet_r_t)) {
				if(limiter < ERR_REPORT_LIMIT) {
					ERR("dump_packet: length overflow i=%d > sizeof(xpp_packet_r_t)=%d\n",
							i+1, sizeof(xpp_packet_r_t));
				} else if(limiter == ERR_REPORT_LIMIT) {
					ERR("dump_packet: error packet #%d... squelsh reports.\n", limiter);
				}
				limiter++;
				break;
			}
			DBG("        %2d> %02X\n", i+1, p[i]);
		}
	}
#endif
}

/*------------------------- Reply Handlers -------------------------*/

#define	HANDLER_DEF(name)		\
	int CALL_PROTO(name, xbus_t *xbus, int xpd_num, xpp_command_t *cmd, xpacket_t *reply)

/*
static HANDLER_DEF(notimp)
{
	NOTICE("xpp protocol error: command %s is not implemented yet\n", cmd->name);
	return -EPROTO;
}
*/
static HANDLER_DEF(DEV_DESC)
{
	byte		type = PACKET_FIELD(reply, DEV_DESC, type) & 0x7;	// 3 LSB's
	byte		rev = PACKET_FIELD(reply, DEV_DESC, rev);
	xpp_line_t	line_status = PACKET_FIELD(reply, DEV_DESC, line_status);
	xpd_t		*xpd = xpd_of(xbus, xpd_num);

	if(xpd) {
		NOTICE("Received DEV_DESC packet for an existing xpd %s of type %d\n", 
				xpd->xpdname, type);
		return 0;
	}
	XBUS_COUNTER(xbus, DEV_DESC)++;
	DBG("xpd=%d type=%d rev=%d line_status=0x%04X\n", xpd_num, type, rev, line_status);
	switch(type) {
	case XPD_TYPE_FXS:
		break;
	case XPD_TYPE_FXO:
		break;
	case XPD_TYPE_NOMODULE:
		DBG("No module at address=%d\n", xpd_num);
		return 0;
	default:
		NOTICE("DEV_DESC: unkown type=%d\n", type);
		return -EPROTO;
	}
	if((xpd = xpd_new(xbus, xpd_num, type, rev)) == NULL) {
		NOTICE("xpd_new failed\n");
	}
	xpp_check_hookstate(xpd, line_status);
	return 0;
}

/**
 * Handle signalling
 */
static HANDLER_DEF(SIG_CHANGED)
{
	xpd_t		*xpd = xpd_of(xbus, xpd_num);
	xpp_line_t	sig_status = PACKET_FIELD(reply, SIG_CHANGED, sig_status);

	if(!xpd) {
		NOTICE("%s: received %s for non-existing xpd: %d\n", __FUNCTION__, cmd->name, xpd_num);
		return -EPROTO;
	}
	if(xpd->direction == TO_PHONE) {		/* Hook state changes */
		DBG("%s (PHONE) sig_status=0x%04X\n", xpd->xpdname, sig_status);
		xpp_check_hookstate(xpd, sig_status);
	} else {					/* TO_TRUNK - line ring changes */
		unsigned long	flags;
		int		i;

		DBG("%s (TRUNK) sig_status=0x%04X\n", xpd->xpdname, sig_status);
		spin_lock_irqsave(&xpd->lock, flags);
		for(i = 0; i < xpd->channels; i++) {
			if(IS_SET(sig_status, i)) {
				xpd->ringing[i] = RINGS_NUM*2;
				zt_hooksig(&xpd->chans[i], ZT_RXSIG_OFFHOOK);
			} else {
				zt_hooksig(&xpd->chans[i], ZT_RXSIG_ONHOOK);
				xpd->ringing[i] = 0;
			}
		}
		spin_unlock_irqrestore(&xpd->lock, flags);
	}
	return 0;
}

static HANDLER_DEF(SLIC_REPLY)
{
	slic_reply_t	*info = &PACKET_FIELD(reply, SLIC_REPLY, info);
	xpd_t		*xpd = xpd_of(xbus, xpd_num);
	unsigned long	flags;

	if(!xpd) {
		NOTICE("%s: received %s for non-existing xpd: %d\n", __FUNCTION__, cmd->name, xpd_num);
		return -EPROTO;
	}
	spin_lock_irqsave(&xpd->lock, flags);
	DBG("SLIC_REPLY: xpd #%d %s reg_num=0x%X, dataL=0x%X dataH=0x%X\n",
			xpd_num, (info->indirect)?"I":"D",
			info->reg_num, info->data_low, info->data_high);
	spin_unlock_irqrestore(&xpd->lock, flags);
	return 0;
}

static HANDLER_DEF(PCM_READ)
{
	/* FIXME: work around temporary hardware bug */
	xpd_num = 0;

	xpp_line_t	lines = PACKET_FIELD(reply, PCM_READ, lines);
	const byte	*pcm = PACKET_FIELD(reply, PCM_READ, pcm);
	xpd_t		*xpd = xpd_of(xbus, xpd_num);
	volatile u_char	*readchunk;
	volatile u_char	*r;
	unsigned long	flags;
	int		i;

	if(!xpd) {
		NOTICE("%s: received %s for non-existing xpd: %d\n", __FUNCTION__, cmd->name, xpd_num);
		return -EPROTO;
	}
	// DBG("lines=0x%04X\n", lines);

	if(!pcm_valid(xpd, reply)) {
		return -EPROTO;
	}
	spin_lock_irqsave(&xpd->lock, flags);
	if (xpd->timer_count & 1) {
		/* First part */
		r = readchunk = xpd->readchunk;
	} else {
		r = readchunk = xpd->readchunk + ZT_CHUNKSIZE * CHANNELS_PERXPD;
	}

	/* Copy PCM and put each channel in its index */
	for (i = 0; i < CHANNELS_PERXPD; i++) {
		if(IS_SET(lines, i)) {
			memcpy((u_char *)r, pcm, ZT_CHUNKSIZE);
			//memset((u_char *)r, 0x5A, ZT_CHUNKSIZE);	// DEBUG
			pcm += ZT_CHUNKSIZE;
		}
		r += ZT_CHUNKSIZE;
	}

	XPD_COUNTER(xpd, PCM_READ)++;
	XBUS_COUNTER(xpd->xbus, PCM_READ)++;
	spin_unlock_irqrestore(&xpd->lock, flags);
	if(xpd->id == 0)
		xpp_tick(0);
	return 0;
}

static HANDLER_DEF(SYNC_REPLY)
{
	xpd_t		*xpd = xpd_of(xbus, xpd_num);

	if(!xpd) {
		NOTICE("%s: received %s for non-existing xpd: %d\n", __FUNCTION__, cmd->name, xpd_num);
		return -EPROTO;
	}
	DBG("SYNC_REPLY: 0x%X\n", PACKET_FIELD(reply, SYNC_REPLY, mask));
	return 0;
}

static HANDLER_DEF(LOOPBACK_XA)
{
	xpd_t		*xpd = xpd_of(xbus, xpd_num);

	if(!xpd) {
		NOTICE("%s: received %s for non-existing xpd: %d\n", __FUNCTION__, cmd->name, xpd_num);
		return -EPROTO;
	}
	dump_packet("LOOPBACK_XA", reply, print_dbg);
	CALL_PROTO(LED, xpd->xbus, xpd, xpd->enabled_chans, LED_RED, 0);		// FIXME: Find usage for extra LED
	return 0;
}

static HANDLER_DEF(DIAG_FE)
{
	dump_packet("DIAG_FE", reply, print_dbg);
	return 0;
}

static const xpp_packet_r_t	FOR_SIZE_CALC;	// Ugly hack, so we have something to sizeof()

#define	C_(op,var,txt)	\
	   [ XPP_##op ] {	\
		   .opcode = XPP_##op, \
		   .varsize = var,	\
		   .header_size = sizeof(FOR_SIZE_CALC.cmd_##op), \
		   .name = #op, \
		   .desc = txt, \
		   .handler = PROTO_FUNC(op) \
	   }

static xpp_command_t xpp_commands[] = {
  /*	OP		V 	DESCRIPTION	*/
  C_(	DEV_DESC,	0,	"Device description reply"),
  C_(	SIG_CHANGED,	0,	"Signaling change (hookstate/ringing)"),
  C_(	SLIC_REPLY,	0,	"Reply to slic state"),
  C_(	PCM_READ,	1,	"Read PCM data"),
  C_(	SYNC_REPLY,	0,	"SYNC_REPLY"),
  C_(	LOOPBACK_XA,	1,	"LOOPBACK Reply"),
  C_(	DIAG_FE,	1,	"DIAG FE Opcode"),
};

#undef	C_

static unsigned int xpp_max_opcode(void)
{
	return ARRAY_SIZE(xpp_commands);
}

static bool xpp_valid_opcode(xpp_opcode_t op)
{
	if(op <= 0 || op >= xpp_max_opcode())
		return 0;
	return xpp_commands[op].opcode != XPP_NOTIMP;
}

static xpp_command_t *xpp_command(xpp_opcode_t op)
{
	if(!xpp_valid_opcode(op))
		return 0;
	return &xpp_commands[op];
}

static bool xpp_valid_size(xpp_opcode_t op, xpacket_t *pack)
{
	xpp_command_t	*cmd = xpp_command(op);
	unsigned int	hsize = cmd->header_size;
	unsigned int	size = pack->datalen;
	int		varsize = cmd->varsize;

	// ERR("op=%d hsize=%d size=%d\n", op, hsize, size);
	return (hsize == size) ||
		(varsize && size <= sizeof(struct xpp_packet_r));
}

static bool pcm_valid(xpd_t *xpd, xpacket_t *reply)
{
	xpp_opcode_t	op;
	xpp_command_t	*cmd;
	xpp_line_t	lines = PACKET_FIELD(reply, PCM_READ, lines);
	int		i;
	int		count = 0;

	BUG_ON(!reply);
	op = reply->content.opcode;
	cmd = xpp_command(op);
	if(!cmd) {
		ERR("xpp: %s -- bad command op=0x%02X\n", __FUNCTION__, op);
		return 0;
	}
	for (i = 0; i < CHANNELS_PERXPD; i++)
		if(IS_SET(lines, i))
			count++;
	if(reply->datalen != (sizeof(xpp_line_t) + count * 8)) {
		static int rate_limit = 0;

		XPD_COUNTER(xpd, RECV_ERRORS)++;
		if((rate_limit++ % 1000) <= 10) {
			ERR("BAD PCM REPLY: reply->datalen=%d, count=%d\n", reply->datalen, count);
		}
		return 0;
	}
	return 1;
}

int packet_receive(xbus_t *xbus, xpacket_t *pack)
{
	int	xpd_num = XPD_NUM(pack->content.addr);

	if(!VALID_XPD_NUM(xpd_num)) {
		dump_packet("martian packet", pack, print_dbg);
		xbus->ops->packet_free(xbus, pack);
		return -EPROTO;
	}
	if(xbus->sim[xpd_num].simulated) {
		//dump_packet("packet_receive -> simulate", pack, print_dbg);
		return simulate_xpd(xbus, xpd_num, pack);
	} else {
		//dump_packet("packet_receive -> process", pack, print_dbg);
		return packet_process(xbus, xpd_num, pack);
	}
}

static int packet_process(xbus_t *xbus, int xpd_num, xpacket_t *pack)
{
	xpp_opcode_t	op;
	xpp_command_t	*cmd;
	xpp_handler_t	handler;
	int		ret = 0;

	BUG_ON(!pack);
	op = pack->content.opcode;
	cmd = xpp_command(op);
	/*-------- Validations -----------*/
	if(!cmd) {
		ERR("xpp: %s -- bad command op=0x%02X\n", __FUNCTION__, op);
		dump_packet("packet_process -- bad command", pack, print_dbg);
		ret = -EPROTO;
		goto out;
	}
	if(!xpp_valid_size(op, pack)) {
		ERR("xpp: %s: wrong size %d for op=0x%02X\n",
					__FUNCTION__, pack->datalen, op);
		dump_packet("packet_process -- wrong size", pack, print_dbg);
		ret = -EPROTO;
		goto out;
	}
	handler = cmd->handler;
	BUG_ON(!handler);
	XBUS_COUNTER(xbus, RX_BYTES) += pack->datalen;
	handler(xbus, xpd_num, cmd, pack);
out:
	xbus->ops->packet_free(xbus, pack);
	return ret;
}


void process_sim_queue(void *data)
{
	xbus_t 		*xbus = data;
	xpacket_t	*pack;

//	DBG("\n");
	BUG_ON(!xbus);
	while((pack = xbus_dequeue_packet(&xbus->sim_packet_queue)) != NULL) {
//		DBG("pack->addr=0x%X pack->opcode=0x%X\n", XPD_NUM(pack->addr), pack->header.opcode);
		packet_receive(xbus, pack);
	}
}

/**
 * processes a packet recieved from the lower-level.
 * @xbus the data bus
 * @pack the handled packet
 * @returns return status (0 for success).
 * 
 * Should not be blocking. 
 * Has separate handling for PCM packets (direct write) and command packets (queued)
 */

EXPORT_SYMBOL(dump_packet);
EXPORT_SYMBOL(packet_receive);
EXPORT_SYMBOL(proc_xpd_slic_read);
EXPORT_SYMBOL(proc_xpd_slic_write);