[z180-stamp.git] / avr / z180-serv.c

/*
 * (C) Copyright 2014 Leo C. <erbl259-lmu@yahoo.de>
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include "common.h"
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <util/atomic.h>

#include "background.h"
#include "env.h"
#include "ff.h"
#include "serial.h"
#include "z80-if.h"
#include "debug.h"
#include "print-utils.h"
#include "z180-serv.h"
#include "timer.h"

/*--------------------------------------------------------------------------*/


uint8_t z80_get_byte(uint32_t adr)
{
	uint8_t data;

	z80_bus_cmd(Request);
	data = z80_read(adr);
	z80_bus_cmd(Release);

	return data;
}


/*--------------------------------------------------------------------------*/

struct msg_item {
	uint8_t fct;
	uint8_t sub_min, sub_max;
	void (*func)(uint8_t, int, uint8_t *);
};

uint32_t msg_to_addr(uint8_t *msg)
{
	union {
		uint32_t as32;
		uint8_t as8[4];
	} addr;

	addr.as8[0] = msg[0];
	addr.as8[1] = msg[1];
	addr.as8[2] = msg[2];
	addr.as8[3] = 0;

	return addr.as32;
}


static int msg_xmit_header(uint8_t func, uint8_t subf, int len)
{
	z80_memfifo_putc(fifo_msgout, 0xAE);
	z80_memfifo_putc(fifo_msgout, len+2);
	z80_memfifo_putc(fifo_msgout, func);
	z80_memfifo_putc(fifo_msgout, subf);

	return 0;
}

int msg_xmit(uint8_t func, uint8_t subf, int len, uint8_t *msg)
{
	msg_xmit_header(func, subf, len);
	while (len--)
		z80_memfifo_putc(fifo_msgout, *msg++);

	return 0;
}

void do_msg_ini_memfifo(uint8_t subf, int len, uint8_t * msg)
{
	(void)len;

	z80_memfifo_init(subf, msg_to_addr(msg));
}


void do_msg_char_out(uint8_t subf, int len, uint8_t * msg)
{
	(void)subf;

	while (len--)
		putchar(*msg++);
}

/* echo message */
void do_msg_echo(uint8_t subf, int len, uint8_t * msg)
{
	(void)subf;

	/* send re-echo */
	msg_xmit(1, 3, len, msg);
}

/* ---------------------------------------------------------------------------*/

#define MAX_DRIVE	4
#define BLOCK_SIZE	512
#define TPA_BASE	0x10000
#define COMMON_BASE	0xC000

struct cpm_drive_s {
	uint8_t drv;
	uint8_t device;
	char *img_name;
	FIL fd;
};

static uint8_t disk_buffer[BLOCK_SIZE];
static struct cpm_drive_s drv_table[MAX_DRIVE];

/*
 	db	2	; disk command
	ds	1	; subcommand (login/read/write)
	ds	1	; @adrv (8 bits)	+0
	ds	1	; @rdrv (8 bits)	+1
	ds	3	; @xdph (24 bits)	+2
*/

void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
{

	FRESULT res = 0;
	uint8_t rc = 0;
	uint8_t drv;
	char *np;
	uint8_t result_msg[3];

	(void)subf;

	if (len != 5) {     /* TODO: check adrv, rdrv */
		rc = 0x01;
		goto out;
	}

	debug("\n## %7lu  login: %c:\n", get_timer(0), msg[0]+'A');


	drv = msg[0];
	if ( drv>= MAX_DRIVE) {
		rc = 0x02;
		goto out;
	}

/*
	uint32_t dph = ((uint32_t)msg[4] << 16) + ((uint16_t)msg[3] << 8) + msg[2];
*/

	if (drv_table[drv].img_name != NULL) {
		debug("## %7lu  close: '%s'\n", get_timer(0), drv_table[drv].img_name);
		f_close(&drv_table[drv].fd);
		free(drv_table[drv].img_name);
		drv_table[drv].img_name = NULL;
	}

	strcpy_P((char *)disk_buffer, PSTR("dsk0"));
	disk_buffer[3] = msg[0] + '0';
	if (((np = getenv((char*)disk_buffer)) == NULL) ||
			((drv_table[drv].img_name = strdup(np)) == NULL)) {
		rc = 0x03;
		goto out;
	}


	res = f_open(&drv_table[drv].fd, drv_table[drv].img_name,
			FA_WRITE | FA_READ);

    debug("## %7lu   open: '%s', (env: '%s'), res: %d\n", get_timer(0),
			drv_table[drv].img_name, disk_buffer, res);

out:

	if (res)
		rc |= 0x80;

	result_msg[0] = rc;
	result_msg[1] = res;
	result_msg[2] = res >> 8;

	if (rc) {
		debug("## %7lu   error    rc: %.02x, res: %d\n", get_timer(0), rc, res);
	};

	/* send  result*/
	msg_xmit(2, subf, sizeof(result_msg), result_msg);
}


/*
 	db	2	; disk command
	ds	1	; subcommand (login/read/write)
	ds	1	; @adrv (8 bits)		+0
	ds	1	; @rdrv (8 bits)		+1
	ds	2	; @trk (16 bits)		+2
	ds	2	; @sect(16 bits)		+4
	ds	1	; @cnt  (8 bits)		+6
	ds	3	; phys. transfer addr	+7
*/

#define ADRV	0
#define RDRV	1
#define TRK		2
#define SEC		4
#define CNT		6
#define ADDR	7

void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
{
	uint8_t drv;
	uint32_t addr;
	uint32_t pos;
	unsigned int bytes;
	bool dowrite = (subf == 2);
	bool hightpa = false;
	FRESULT res = 0;
	uint8_t rc = 0;
	bool buserr = 0;
	uint8_t result_msg[3];

	if (len != 10) {     /* TODO: check adrv, rdrv */
		rc = 0x01;
		goto out;
	}

	drv = msg[ADRV];
	if ( drv>= MAX_DRIVE) {
		rc = 0x02;
		goto out;
	}

	bytes = msg[CNT] * BLOCK_SIZE;
	addr = ((uint32_t)msg[ADDR+2] << 16) + ((uint16_t)msg[ADDR+1] << 8) + msg[ADDR];


	/* TODO: tracks per sector from dpb */
	pos = (((uint16_t)(msg[TRK+1] << 8) + msg[TRK]) * 8
			+ ((uint32_t)(msg[SEC+1] << 8) + msg[SEC])) * BLOCK_SIZE;

	debug("## %7lu cpm_rw: %s %c: trk: %4d, sec: %d, pos: 0x%.8lx, secs: %2d\n",
			get_timer(0), dowrite ? "write" : " read", msg[ADRV]+'A',
			((uint16_t)(msg[TRK+1] << 8) + msg[TRK]), msg[SEC], pos, msg[CNT]);


	/* TODO: check bank boundary crossing */

	res = f_lseek(&drv_table[drv].fd, pos);
	while (!res && bytes) {
		unsigned int cnt, br;

		if (bytes >= BLOCK_SIZE)
			cnt = BLOCK_SIZE;
		else
			cnt = bytes;

		if (addr < (TPA_BASE + COMMON_BASE) &&
					(addr + cnt) > (TPA_BASE + COMMON_BASE)) {
			cnt = (TPA_BASE + COMMON_BASE) - addr;
			hightpa = true;
		}

		bytes -= cnt;

		debug("## %7lu                                                      "
				" addr: 0x%.5lx, cnt: %3d\n",
				get_timer(0), addr, cnt);

		if (dowrite) {
			if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
				buserr = 1;
			} else {
				z80_read_block(disk_buffer, addr, cnt);
				z80_bus_cmd(Release);
				res = f_write(&drv_table[drv].fd, disk_buffer, cnt, &br);
			}
		} else {
			res = f_read(&drv_table[drv].fd, disk_buffer, cnt, &br);
			if (res == FR_OK) {
				if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
					buserr = 1;
				} else {
					z80_write_block(disk_buffer, addr, br);
					z80_bus_cmd(Release);
				}
			}
		}

		addr += br;
		if (hightpa) {
			addr -= TPA_BASE;
			hightpa = false;
		}

		if (br != cnt) {
			debug("## %7lu f_read res: %d, bytes rd/wr: %u\n", get_timer(0), res, br);
			dump_ram(disk_buffer, 0, 64, "Read Data");
			res = -1;
		}

	}

	if (dowrite && !res)
		res = f_sync(&drv_table[drv].fd);

out:
	if (buserr) {
		debug("Bus timeout\n");
		rc = 0x03;
	}
	if (res)
		rc |= 0x80;

	result_msg[0] = rc;
	result_msg[1] = res;
	result_msg[2] = res >> 8;

	if (rc) {
		debug("###%7lu  error rc: %.02x, res: %d\n", get_timer(0), rc, res);
	}

	/* send  result*/
	msg_xmit(2, subf, sizeof(result_msg), result_msg);
}


const FLASH struct msg_item z80_messages[] =
{
	{ 0,			/* fct nr. */
	  1, 3,			/* sub fct nr. from, to */
	  do_msg_ini_memfifo},
	{ 1,
	  1, 1,
	  do_msg_char_out},
	{ 1,
	  2, 2,
	  do_msg_echo},
	{ 2,
	  0, 0,
	  do_msg_cpm_login},
	{ 2,
	  1, 2,
	  do_msg_cpm_rw},
	{ 0xff,				/* end mark */
	  0, 0,
	  0},

};


void do_message(int len, uint8_t *msg)
{
	uint8_t fct, sub_fct;
	int_fast8_t i = 0;

	if (len >= 2) {
		fct = *msg++;
		sub_fct = *msg++;
		len -= 2;

		while (fct != z80_messages[i].fct) {
			if (z80_messages[i].fct == 0xff) {
				DBG_P(1, "do_message: Unknown function: %i, %i\n",
						fct, sub_fct);
				return; /* TODO: unknown message # */
			}

			++i;
		}

		while (fct == z80_messages[i].fct) {
			if (sub_fct >= z80_messages[i].sub_min &&
					sub_fct <= z80_messages[i].sub_max )
				break;
			++i;
		}

		if (z80_messages[i].fct != fct) {
			DBG_P(1, "do_message: Unknown sub function: %i, %i\n",
					fct, sub_fct);
			return; /* TODO: unknown message sub# */
		}

		(z80_messages[i].func)(sub_fct, len, msg);


	} else {
		/* TODO: error */
		DBG_P(1, "do_message: to few arguments (%i); this shouldn't happen!\n", len);
	}
}


#define CTRBUF_LEN 256

void check_msg_fifo(void)
{
	int ch;
	static int_fast8_t state;
	static int msglen,idx;
	static uint8_t buffer[CTRBUF_LEN];

	while ((ch = z80_memfifo_getc(fifo_msgin)) >= 0) {
		switch (state) {
		case 0:		/* wait for start of message */
			if (ch == 0xAE)	{ /* TODO: magic number */
				msglen = 0;
				idx = 0;
				state = 1;
			}
			break;
		case 1:		/* get msg len */
			if (ch > 0 && ch <= CTRBUF_LEN) {
				msglen = ch;
				state = 2;
			} else
				state = 0;
			break;
		case 2: 	/* get message */
			buffer[idx++] = ch;
			if (idx == msglen) {
				do_message(msglen, buffer);
				state = 0;
			}
			break;
		}
	}
}


int msg_handling(int state)
{
	uint8_t pending;

	ATOMIC_BLOCK(ATOMIC_FORCEON) {
		pending = (Stat & S_MSG_PENDING) != 0;
		Stat &= ~S_MSG_PENDING;
	}

	if (pending) {
		switch (state) {
		case 0:			/* need init */
			/* Get address of fifo_list */
			z80_bus_cmd(Request);
			uint32_t fifo_list = z80_read(0x40) +
				((uint16_t) z80_read(0x41) << 8) +
				((uint32_t) z80_read(0x42) << 16);
			z80_bus_cmd(Release);
			if (fifo_list != 0) {
				/* Get address of fifo 0 */
				z80_bus_cmd(Request);
				uint32_t fifo_addr = z80_read(fifo_list) +
					((uint16_t) z80_read(fifo_list+1) << 8) +
					((uint32_t) z80_read(fifo_list+2) << 16);
				z80_bus_cmd(Release);
				if (fifo_addr != 0) {
					z80_memfifo_init(fifo_msgin, fifo_addr);
					state = 1;
				}
			}
			break;
		case 1:			/* awaiting messages */
			check_msg_fifo();
			break;
		}
	}

	return state;
}


static int handle_msg_handling;

void setup_z180_serv(void)
{

	handle_msg_handling = bg_register(msg_handling, 0);
}

void restart_z180_serv(void)
{
	z80_bus_cmd(Request);
	z80_write(0x40, 0);
	z80_write(0x41, 0);
	z80_write(0x42, 0);
	z80_bus_cmd(Release);

	for (int i = 0; i < NUM_FIFOS; i++)
		z80_memfifo_init(i, 0);
	bg_setstat(handle_msg_handling, 0);

}

/*--------------------------------------------------------------------------*/

const FLASH uint8_t iniprog[] = {
	0xAF,			// xor     a
	0xED, 0x39, 0x36,	// out0    (rcr),a         ;disable  DRAM refresh
	0x3E, 0x30,		// ld      a,030h
	0xED, 0x39, 0x32	//out0    (dcntl),a       ;0 mem, max i/0 wait states
};

const FLASH uint8_t sertest[] = {
	0xAF,              //  	xor	a
	0xED, 0x39, 0x36,  //  	out0	(rcr),a		;disable  DRAM refresh
	0x3E, 0x30,        //  	ld	a,030h
	0xED, 0x39, 0x32,  //  	out0	(dcntl),a	;0 mem, max i/0 wait states
	0x3E, 0x80,        //  	ld	a,M_MPBT		;no MP, PS=10, DR=16, SS=0
	0xED, 0x39, 0x03,  //  	out0	(cntlb1),a
	0x3E, 0x64,        //  	ld	a,M_RE + M_TE + M_MOD2	;
	0xED, 0x39, 0x01,  //  	out0	(cntla1),a
	0x3E, 0x00,        //  	ld	a,0
	0xED, 0x39, 0x05,  //  	out0	(stat1),a	;Enable rx interrupts
	0xED, 0x38, 0x05,  //l0:in0	a,(stat1)
	0xE6, 0x80,        //  	and	80h
	0x28, 0xF9,        //  	jr	z,l0
	0xED, 0x00, 0x09,  //  	in0	b,(rdr1)
	0xED, 0x38, 0x05,  //l1:in0	a,(stat1)
	0xE6, 0x02,        //  	and	02h
	0x28, 0xF9,        //  	jr	z,l1
	0xED, 0x01, 0x07,  //  	out0	(tdr1),b
	0x18, 0xEA,        //  	jr	l0
};

const FLASH uint8_t test1[] = {
	0xAF,              //	xor	a
	0xED, 0x39, 0x36,  //	out0	(rcr),a		;disable  DRAM refresh
	0x3E, 0x30,        //	ld	a,030h
	0xED, 0x39, 0x32,  //	out0	(dcntl),a	;0 mem, max i/0 wait states
	0x21, 0x1E, 0x00,  // 	ld	hl,dmclrt	;load DMA registers
	0x06, 0x08,        //	ld	b,dmct_e-dmclrt
	0x0E, 0x20,        //	ld	c,sar0l
	0xED, 0x93,        //	otimr
	0x3E, 0xC3,        //	ld	a,0c3h		;dst +1, src +1, burst
	0xED, 0x39, 0x31,  //	out0	(dmode),a	;
	0x3E, 0x62,        //	ld	a,062h		;enable dma0,
	0xED, 0x39, 0x30,  //cl_1:	out0	(dstat),a	;copy 64k
	0x18, 0xFB,        //	jr	cl_1		;
	0x00, 0x00,        //dmclrt:	dw	0		;src (inc)
	0x00,              //	db 	0		;src
	0x00, 0x00,        //	dw	0		;dst (inc),
	0x00,              //	db	0		;dst
	0x00, 0x00,        //	dw 	0		;count (64k)
};
Commit	Line	Data
35edb766 L	1	/*
	2	* (C) Copyright 2014 Leo C. <erbl259-lmu@yahoo.de>
	3	*
	4	* SPDX-License-Identifier: GPL-2.0+
	5	*/
	6
72f58822	7	#include "common.h"
5f7f3586 L	8	#include <stdlib.h>
	9	#include <string.h>
	10	#include <stdbool.h>
89adce76	11	#include <util/atomic.h>
72f58822	12
89adce76	13	#include "background.h"
5f7f3586 L	14	#include "env.h"
5f7f3586 L	15	#include "ff.h"
72f58822 L	16	#include "serial.h"
72f58822 L	17	#include "z80-if.h"
889202c4	18	#include "debug.h"
5f7f3586	19	#include "print-utils.h"
89adce76	20	#include "z180-serv.h"
daacc6f9	21	#include "timer.h"
72f58822	22
72f58822 L	23	/--------------------------------------------------------------------------/
72f58822 L	24
72f58822 L	25
	26	uint8_t z80_get_byte(uint32_t adr)
	27	{
	28	uint8_t data;
8a7decea	29
62f624d3	30	z80_bus_cmd(Request);
89adce76	31	data = z80_read(adr);
62f624d3	32	z80_bus_cmd(Release);
8a7decea	33
72f58822 L	34	return data;
	35	}
	36
	37
	38	/--------------------------------------------------------------------------/
	39
	40	struct msg_item {
	41	uint8_t fct;
	42	uint8_t sub_min, sub_max;
	43	void (func)(uint8_t, int, uint8_t );
	44	};
	45
	46	uint32_t msg_to_addr(uint8_t *msg)
	47	{
	48	union {
	49	uint32_t as32;
	50	uint8_t as8[4];
	51	} addr;
	52
	53	addr.as8[0] = msg[0];
	54	addr.as8[1] = msg[1];
	55	addr.as8[2] = msg[2];
	56	addr.as8[3] = 0;
	57
	58	return addr.as32;
	59	}
	60
72f58822	61
1a2460dc L	62	static int msg_xmit_header(uint8_t func, uint8_t subf, int len)
	63	{
	64	z80_memfifo_putc(fifo_msgout, 0xAE);
	65	z80_memfifo_putc(fifo_msgout, len+2);
	66	z80_memfifo_putc(fifo_msgout, func);
	67	z80_memfifo_putc(fifo_msgout, subf);
	68
	69	return 0;
	70	}
	71
	72	int msg_xmit(uint8_t func, uint8_t subf, int len, uint8_t *msg)
	73	{
	74	msg_xmit_header(func, subf, len);
	75	while (len--)
	76	z80_memfifo_putc(fifo_msgout, *msg++);
	77
	78	return 0;
	79	}
	80
72f58822 L	81	void do_msg_ini_memfifo(uint8_t subf, int len, uint8_t * msg)
	82	{
	83	(void)len;
	84
89adce76	85	z80_memfifo_init(subf, msg_to_addr(msg));
72f58822 L	86	}
	87
	88
	89	void do_msg_char_out(uint8_t subf, int len, uint8_t * msg)
	90	{
	91	(void)subf;
	92
	93	while (len--)
	94	putchar(*msg++);
	95	}
	96
1a2460dc L	97	/* echo message */
	98	void do_msg_echo(uint8_t subf, int len, uint8_t * msg)
	99	{
	100	(void)subf;
	101
	102	/* send re-echo */
	103	msg_xmit(1, 3, len, msg);
	104	}
	105
5f7f3586 L	106	/* ---------------------------------------------------------------------------*/
5f7f3586 L	107
01484095	108	#define MAX_DRIVE 4
5f7f3586	109	#define BLOCK_SIZE 512
daacc6f9 L	110	#define TPA_BASE 0x10000
daacc6f9 L	111	#define COMMON_BASE 0xC000
5f7f3586 L	112
	113	struct cpm_drive_s {
	114	uint8_t drv;
	115	uint8_t device;
	116	char *img_name;
	117	FIL fd;
	118	};
	119
	120	static uint8_t disk_buffer[BLOCK_SIZE];
5f7f3586 L	121	static struct cpm_drive_s drv_table[MAX_DRIVE];
	122
	123	/*
	124	db 2 ; disk command
	125	ds 1 ; subcommand (login/read/write)
	126	ds 1 ; @adrv (8 bits) +0
	127	ds 1 ; @rdrv (8 bits) +1
	128	ds 3 ; @xdph (24 bits) +2
	129	*/
	130
	131	void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
	132	{
	133
	134	FRESULT res = 0;
	135	uint8_t rc = 0;
	136	uint8_t drv;
	137	char *np;
	138	uint8_t result_msg[3];
	139
	140	(void)subf;
	141
	142	if (len != 5) { /* TODO: check adrv, rdrv */
	143	rc = 0x01;
	144	goto out;
	145	}
	146
daacc6f9	147	debug("\n## %7lu login: %c:\n", get_timer(0), msg[0]+'A');
5f7f3586 L	148
	149
	150	drv = msg[0];
	151	if ( drv>= MAX_DRIVE) {
	152	rc = 0x02;
	153	goto out;
	154	}
	155
5f7f3586 L	156	/*
	157	uint32_t dph = ((uint32_t)msg[4] << 16) + ((uint16_t)msg[3] << 8) + msg[2];
	158	*/
	159
	160	if (drv_table[drv].img_name != NULL) {
daacc6f9	161	debug("## %7lu close: '%s'\n", get_timer(0), drv_table[drv].img_name);
5f7f3586 L	162	f_close(&drv_table[drv].fd);
	163	free(drv_table[drv].img_name);
	164	drv_table[drv].img_name = NULL;
	165	}
	166
	167	strcpy_P((char *)disk_buffer, PSTR("dsk0"));
	168	disk_buffer[3] = msg[0] + '0';
	169	if (((np = getenv((char*)disk_buffer)) == NULL) \|\|
	170	((drv_table[drv].img_name = strdup(np)) == NULL)) {
	171	rc = 0x03;
	172	goto out;
	173	}
	174
	175
	176	res = f_open(&drv_table[drv].fd, drv_table[drv].img_name,
	177	FA_WRITE \| FA_READ);
	178
daacc6f9	179	debug("## %7lu open: '%s', (env: '%s'), res: %d\n", get_timer(0),
5f7f3586 L	180	drv_table[drv].img_name, disk_buffer, res);
	181
	182	out:
	183
	184	if (res)
	185	rc \|= 0x80;
	186
	187	result_msg[0] = rc;
	188	result_msg[1] = res;
	189	result_msg[2] = res >> 8;
	190
	191	if (rc) {
daacc6f9	192	debug("## %7lu error rc: %.02x, res: %d\n", get_timer(0), rc, res);
5f7f3586 L	193	};
	194
	195	/* send result*/
	196	msg_xmit(2, subf, sizeof(result_msg), result_msg);
	197	}
	198
	199
	200	/*
	201	db 2 ; disk command
	202	ds 1 ; subcommand (login/read/write)
daacc6f9 L	203	ds 1 ; @adrv (8 bits) +0
	204	ds 1 ; @rdrv (8 bits) +1
	205	ds 2 ; @trk (16 bits) +2
	206	ds 2 ; @sect(16 bits) +4
	207	ds 1 ; @cnt (8 bits) +6
5f7f3586 L	208	ds 3 ; phys. transfer addr +7
	209	*/
	210
daacc6f9 L	211	#define ADRV 0
	212	#define RDRV 1
	213	#define TRK 2
	214	#define SEC 4
	215	#define CNT 6
	216	#define ADDR 7
	217
5f7f3586 L	218	void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
	219	{
	220	uint8_t drv;
	221	uint32_t addr;
	222	uint32_t pos;
daacc6f9	223	unsigned int bytes;
5f7f3586	224	bool dowrite = (subf == 2);
daacc6f9	225	bool hightpa = false;
5f7f3586 L	226	FRESULT res = 0;
	227	uint8_t rc = 0;
	228	bool buserr = 0;
	229	uint8_t result_msg[3];
	230
	231	if (len != 10) { /* TODO: check adrv, rdrv */
	232	rc = 0x01;
	233	goto out;
	234	}
	235
daacc6f9	236	drv = msg[ADRV];
5f7f3586 L	237	if ( drv>= MAX_DRIVE) {
	238	rc = 0x02;
	239	goto out;
	240	}
	241
daacc6f9 L	242	bytes = msg[CNT] * BLOCK_SIZE;
daacc6f9 L	243	addr = ((uint32_t)msg[ADDR+2] << 16) + ((uint16_t)msg[ADDR+1] << 8) + msg[ADDR];
5f7f3586	244
5f7f3586	245
daacc6f9 L	246	/* TODO: tracks per sector from dpb */
	247	pos = (((uint16_t)(msg[TRK+1] << 8) + msg[TRK]) * 8
	248	+ ((uint32_t)(msg[SEC+1] << 8) + msg[SEC])) * BLOCK_SIZE;
5f7f3586	249
daacc6f9 L	250	debug("## %7lu cpm_rw: %s %c: trk: %4d, sec: %d, pos: 0x%.8lx, secs: %2d\n",
	251	get_timer(0), dowrite ? "write" : " read", msg[ADRV]+'A',
	252	((uint16_t)(msg[TRK+1] << 8) + msg[TRK]), msg[SEC], pos, msg[CNT]);
5f7f3586 L	253
	254
	255
	256	/* TODO: check bank boundary crossing */
5f7f3586 L	257
5f7f3586 L	258	res = f_lseek(&drv_table[drv].fd, pos);
daacc6f9 L	259	while (!res && bytes) {
	260	unsigned int cnt, br;
	261
	262	if (bytes >= BLOCK_SIZE)
	263	cnt = BLOCK_SIZE;
	264	else
	265	cnt = bytes;
	266
	267	if (addr < (TPA_BASE + COMMON_BASE) &&
	268	(addr + cnt) > (TPA_BASE + COMMON_BASE)) {
	269	cnt = (TPA_BASE + COMMON_BASE) - addr;
	270	hightpa = true;
	271	}
	272
	273	bytes -= cnt;
	274
	275	debug("## %7lu "
	276	" addr: 0x%.5lx, cnt: %3d\n",
	277	get_timer(0), addr, cnt);
5f7f3586 L	278
	279	if (dowrite) {
	280	if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
	281	buserr = 1;
	282	} else {
daacc6f9	283	z80_read_block(disk_buffer, addr, cnt);
5f7f3586	284	z80_bus_cmd(Release);
daacc6f9	285	res = f_write(&drv_table[drv].fd, disk_buffer, cnt, &br);
5f7f3586 L	286	}
5f7f3586 L	287	} else {
daacc6f9	288	res = f_read(&drv_table[drv].fd, disk_buffer, cnt, &br);
5f7f3586 L	289	if (res == FR_OK) {
	290	if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
	291	buserr = 1;
	292	} else {
	293	z80_write_block(disk_buffer, addr, br);
	294	z80_bus_cmd(Release);
	295	}
	296	}
	297	}
	298
daacc6f9 L	299	addr += br;
	300	if (hightpa) {
	301	addr -= TPA_BASE;
	302	hightpa = false;
	303	}
	304
	305	if (br != cnt) {
	306	debug("## %7lu f_read res: %d, bytes rd/wr: %u\n", get_timer(0), res, br);
5f7f3586 L	307	dump_ram(disk_buffer, 0, 64, "Read Data");
	308	res = -1;
	309	}
daacc6f9	310
5f7f3586 L	311	}
5f7f3586 L	312
daacc6f9 L	313	if (dowrite && !res)
	314	res = f_sync(&drv_table[drv].fd);
	315
5f7f3586 L	316	out:
	317	if (buserr) {
	318	debug("Bus timeout\n");
	319	rc = 0x03;
	320	}
	321	if (res)
	322	rc \|= 0x80;
	323
	324	result_msg[0] = rc;
	325	result_msg[1] = res;
	326	result_msg[2] = res >> 8;
	327
	328	if (rc) {
daacc6f9	329	debug("###%7lu error rc: %.02x, res: %d\n", get_timer(0), rc, res);
5f7f3586 L	330	}
	331
	332	/* send result*/
	333	msg_xmit(2, subf, sizeof(result_msg), result_msg);
	334	}
	335
72f58822 L	336
	337	const FLASH struct msg_item z80_messages[] =
	338	{
	339	{ 0, /* fct nr. */
89adce76	340	1, 3, /* sub fct nr. from, to */
72f58822 L	341	do_msg_ini_memfifo},
	342	{ 1,
	343	1, 1,
	344	do_msg_char_out},
1a2460dc L	345	{ 1,
	346	2, 2,
	347	do_msg_echo},
5f7f3586 L	348	{ 2,
	349	0, 0,
	350	do_msg_cpm_login},
	351	{ 2,
	352	1, 2,
	353	do_msg_cpm_rw},
72f58822 L	354	{ 0xff, /* end mark */
	355	0, 0,
	356	0},
	357
	358	};
	359
	360
	361
	362
	363	void do_message(int len, uint8_t *msg)
	364	{
	365	uint8_t fct, sub_fct;
	366	int_fast8_t i = 0;
	367
	368	if (len >= 2) {
	369	fct = *msg++;
	370	sub_fct = *msg++;
	371	len -= 2;
	372
89adce76 L	373	while (fct != z80_messages[i].fct) {
	374	if (z80_messages[i].fct == 0xff) {
	375	DBG_P(1, "do_message: Unknown function: %i, %i\n",
	376	fct, sub_fct);
	377	return; /* TODO: unknown message # */
	378	}
8a7decea	379
72f58822	380	++i;
72f58822 L	381	}
	382
	383	while (fct == z80_messages[i].fct) {
8a7decea	384	if (sub_fct >= z80_messages[i].sub_min &&
89adce76	385	sub_fct <= z80_messages[i].sub_max )
72f58822 L	386	break;
	387	++i;
	388	}
	389
	390	if (z80_messages[i].fct != fct) {
	391	DBG_P(1, "do_message: Unknown sub function: %i, %i\n",
	392	fct, sub_fct);
	393	return; /* TODO: unknown message sub# */
	394	}
	395
	396	(z80_messages[i].func)(sub_fct, len, msg);
	397
	398
	399	} else {
	400	/* TODO: error */
	401	DBG_P(1, "do_message: to few arguments (%i); this shouldn't happen!\n", len);
	402	}
	403	}
	404
	405
	406
	407	#define CTRBUF_LEN 256
	408
	409	void check_msg_fifo(void)
	410	{
	411	int ch;
	412	static int_fast8_t state;
	413	static int msglen,idx;
	414	static uint8_t buffer[CTRBUF_LEN];
	415
89adce76	416	while ((ch = z80_memfifo_getc(fifo_msgin)) >= 0) {
72f58822 L	417	switch (state) {
72f58822 L	418	case 0: /* wait for start of message */
3531528e	419	if (ch == 0xAE) { /* TODO: magic number */
72f58822 L	420	msglen = 0;
	421	idx = 0;
	422	state = 1;
	423	}
	424	break;
	425	case 1: /* get msg len */
	426	if (ch > 0 && ch <= CTRBUF_LEN) {
	427	msglen = ch;
	428	state = 2;
	429	} else
	430	state = 0;
	431	break;
	432	case 2: /* get message */
	433	buffer[idx++] = ch;
89adce76 L	434	if (idx == msglen) {
	435	do_message(msglen, buffer);
	436	state = 0;
	437	}
	438	break;
	439	}
	440	}
	441	}
	442
	443
	444	int msg_handling(int state)
	445	{
	446	uint8_t pending;
8a7decea L	447
8a7decea L	448	ATOMIC_BLOCK(ATOMIC_FORCEON) {
89adce76 L	449	pending = (Stat & S_MSG_PENDING) != 0;
	450	Stat &= ~S_MSG_PENDING;
	451	}
8a7decea	452
89adce76 L	453	if (pending) {
89adce76 L	454	switch (state) {
1a2460dc	455	case 0: /* need init */
cdc4625b	456	/* Get address of fifo_list */
89adce76	457	z80_bus_cmd(Request);
cdc4625b	458	uint32_t fifo_list = z80_read(0x40) +
89adce76 L	459	((uint16_t) z80_read(0x41) << 8) +
	460	((uint32_t) z80_read(0x42) << 16);
	461	z80_bus_cmd(Release);
cdc4625b L	462	if (fifo_list != 0) {
	463	/* Get address of fifo 0 */
	464	z80_bus_cmd(Request);
	465	uint32_t fifo_addr = z80_read(fifo_list) +
	466	((uint16_t) z80_read(fifo_list+1) << 8) +
	467	((uint32_t) z80_read(fifo_list+2) << 16);
	468	z80_bus_cmd(Release);
	469	if (fifo_addr != 0) {
	470	z80_memfifo_init(fifo_msgin, fifo_addr);
	471	state = 1;
	472	}
89adce76 L	473	}
89adce76 L	474	break;
1a2460dc	475	case 1: /* awaiting messages */
89adce76	476	check_msg_fifo();
72f58822 L	477	break;
	478	}
	479	}
	480
89adce76 L	481	return state;
89adce76 L	482	}
89adce76 L	483
	484
	485	static int handle_msg_handling;
	486
	487	void setup_z180_serv(void)
	488	{
8a7decea	489
89adce76	490	handle_msg_handling = bg_register(msg_handling, 0);
72f58822 L	491	}
72f58822 L	492
89adce76 L	493	void restart_z180_serv(void)
	494	{
	495	z80_bus_cmd(Request);
	496	z80_write(0x40, 0);
	497	z80_write(0x41, 0);
	498	z80_write(0x42, 0);
	499	z80_bus_cmd(Release);
8a7decea L	500
	501	for (int i = 0; i < NUM_FIFOS; i++)
	502	z80_memfifo_init(i, 0);
89adce76	503	bg_setstat(handle_msg_handling, 0);
5f7f3586	504
89adce76	505	}
72f58822 L	506
	507	/--------------------------------------------------------------------------/
	508
72f58822 L	509	const FLASH uint8_t iniprog[] = {
	510	0xAF, // xor a
	511	0xED, 0x39, 0x36, // out0 (rcr),a ;disable DRAM refresh
	512	0x3E, 0x30, // ld a,030h
	513	0xED, 0x39, 0x32 //out0 (dcntl),a ;0 mem, max i/0 wait states
	514	};
	515
	516	const FLASH uint8_t sertest[] = {
	517	0xAF, // xor a
	518	0xED, 0x39, 0x36, // out0 (rcr),a ;disable DRAM refresh
	519	0x3E, 0x30, // ld a,030h
	520	0xED, 0x39, 0x32, // out0 (dcntl),a ;0 mem, max i/0 wait states
	521	0x3E, 0x80, // ld a,M_MPBT ;no MP, PS=10, DR=16, SS=0
	522	0xED, 0x39, 0x03, // out0 (cntlb1),a
	523	0x3E, 0x64, // ld a,M_RE + M_TE + M_MOD2 ;
	524	0xED, 0x39, 0x01, // out0 (cntla1),a
	525	0x3E, 0x00, // ld a,0
	526	0xED, 0x39, 0x05, // out0 (stat1),a ;Enable rx interrupts
	527	0xED, 0x38, 0x05, //l0:in0 a,(stat1)
	528	0xE6, 0x80, // and 80h
	529	0x28, 0xF9, // jr z,l0
	530	0xED, 0x00, 0x09, // in0 b,(rdr1)
	531	0xED, 0x38, 0x05, //l1:in0 a,(stat1)
	532	0xE6, 0x02, // and 02h
	533	0x28, 0xF9, // jr z,l1
	534	0xED, 0x01, 0x07, // out0 (tdr1),b
	535	0x18, 0xEA, // jr l0
	536	};
	537
	538	const FLASH uint8_t test1[] = {
	539	0xAF, // xor a
	540	0xED, 0x39, 0x36, // out0 (rcr),a ;disable DRAM refresh
	541	0x3E, 0x30, // ld a,030h
	542	0xED, 0x39, 0x32, // out0 (dcntl),a ;0 mem, max i/0 wait states
	543	0x21, 0x1E, 0x00, // ld hl,dmclrt ;load DMA registers
	544	0x06, 0x08, // ld b,dmct_e-dmclrt
	545	0x0E, 0x20, // ld c,sar0l
8a7decea	546	0xED, 0x93, // otimr
72f58822 L	547	0x3E, 0xC3, // ld a,0c3h ;dst +1, src +1, burst
72f58822 L	548	0xED, 0x39, 0x31, // out0 (dmode),a ;
8a7decea	549	0x3E, 0x62, // ld a,062h ;enable dma0,
72f58822 L	550	0xED, 0x39, 0x30, //cl_1: out0 (dstat),a ;copy 64k
72f58822 L	551	0x18, 0xFB, // jr cl_1 ;
8a7decea	552	0x00, 0x00, //dmclrt: dw 0 ;src (inc)
72f58822 L	553	0x00, // db 0 ;src
	554	0x00, 0x00, // dw 0 ;dst (inc),
	555	0x00, // db 0 ;dst
	556	0x00, 0x00, // dw 0 ;count (64k)
	557	};