X-Git-Url: http://cloudbase.mooo.com/gitweb/z180-stamp.git/blobdiff_plain/4122fe90c2fffbec62a431ac15fb4b373aecb1ea..2ca8d8c70415549571f3e6759683dd33daf1bc42:/avr/z180-serv.c?ds=sidebyside

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index ddb4622..25ffc9c 100644
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -24,7 +24,7 @@
 #include "bcd.h"
 #include "rtc.h"
 
-#define DEBUG_CPM_SDIO	1	/* set to 1 to debug */
+#define DEBUG_CPM_SDIO	0	/* set to 1 to debug */
 
 #define debug_cpmsd(fmt, args...)		                                            \
 	debug_cond(DEBUG_CPM_SDIO, fmt, ##args)
@@ -212,6 +212,24 @@ static struct cpm_drive_s drv_table[CONFIG_CPM_MAX_DRIVE];
 static int handle_cpm_drv_to;
 
 
+void drv_debug(uint8_t drv, const FLASH char *const fmt, ...)		                                            \
+{
+	struct cpm_drive_s *dp = &drv_table[drv];
+
+	if (dp->opt & DRV_OPT_DEBUG) {
+
+		va_list ap;
+
+		printf_P(PSTR("#  %7lu  dsk%d: "), get_timer(0), drv);
+
+		va_start (ap, fmt);
+		vfprintf_P (stdout, fmt, ap);
+		va_end (ap);
+
+		putc('\n', stdout);
+	}
+}
+
 int drv_list(void)
 {
 	for (uint8_t i = 0; i < CONFIG_CPM_MAX_DRIVE; i++) {
@@ -230,8 +248,7 @@ int drv_detach(uint8_t drv)
 	if (drv < CONFIG_CPM_MAX_DRIVE) {
 		struct cpm_drive_s *p = &drv_table[drv];
 
-		debug_cpmsd("##  detach dsk%d: %s\n", drv,
-				p->img_name ? p->img_name : "-");
+		drv_debug(drv, PSTR("detach from '%s'"), p->img_name ? p->img_name : "-");
 
 		if (p->img_name) {
 			f_close(&p->fd);
@@ -239,6 +256,13 @@ int drv_detach(uint8_t drv)
 			p->opt = 0;
 			p->flags &= ~DRV_FLG_DIRTY;
 			p->img_name = NULL;
+
+			uint32_t scb = getenv_ulong(ENV_CPM3_SCB, 16, 0);
+			if (scb && (z80_bus_cmd(Request) & ZST_ACQUIRED)) {
+				z80_write(scb + 0xf0, 0xff);
+				z80_write(p->dph + 11, 0xff);
+				z80_bus_cmd(Release);
+			}
 		}
 	}
 	return 0;
@@ -273,9 +297,14 @@ int drv_attach(uint8_t drv, const char *filename, drv_opt_t options)
 			return AT_NOT;
 		}
 
-		p->opt = options & ~DRV_OPT_REATTATCH;
+		/* change options */
+		if ((p->opt ^ options) & DRV_OPT_RO) {
+			f_close(&p->fd);
+			res = f_open(&p->fd, p->img_name,
+					FA_READ | (options&DRV_OPT_RO ? 0 : FA_WRITE));
+		}
 
-		/* TODO: change options */
+		p->opt = options & ~DRV_OPT_REATTATCH;
 
 	} else {
 
@@ -292,11 +321,11 @@ int drv_attach(uint8_t drv, const char *filename, drv_opt_t options)
 			return AT_NOMEM;
 
 		res = f_open(&p->fd, p->img_name,
-				FA_WRITE | FA_READ);
+				FA_READ | (options&DRV_OPT_RO ? 0 : FA_WRITE));
 
 		if (!res && f_size(&p->fd) < CONFIG_CPM_DISKSIZE) {
+#if 0
 			unsigned int bw;
-
 			debug_cpmsd("            expanding image file from %ld to %ld\n",
 					f_size(&p->fd), CONFIG_CPM_DISKSIZE);
 
@@ -310,6 +339,11 @@ int drv_attach(uint8_t drv, const char *filename, drv_opt_t options)
 				p->flags |= DRV_FLG_DIRTY;
 				bg_setstat(handle_cpm_drv_to, 1);
 			}
+#else
+			drv_debug(drv, PSTR("wrong image file size: %ld, should be %ld"),
+					f_size(&p->fd), CONFIG_CPM_DISKSIZE);
+			res = 64;
+#endif
 		}
 		if (res) {
 			drv_detach(drv);
@@ -338,9 +372,9 @@ int cpm_drv_to(int state)
 		if (get_timer(ts) > 1000) {
 			for (uint_fast8_t i=0; i < CONFIG_CPM_MAX_DRIVE; i++) {
 				if (drv_table[i].flags & DRV_FLG_DIRTY) {
-					f_sync(&drv_table[i].fd);
 					drv_table[i].flags &= ~DRV_FLG_DIRTY;
-					debug_cpmsd("## %7lu f_sync: %c:\n", get_timer(0), i + CONFIG_CPM_BASE_DRIVE);
+					f_sync(&drv_table[i].fd);
+					drv_debug(i, PSTR("f_sync"));
 				}
 			}
 			state = 0;
@@ -379,7 +413,7 @@ void msg_cpm_result(uint8_t subf, uint8_t rc, int res)
 void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 {
 	uint8_t drv;
-	struct cpm_drive_s *p;
+	struct cpm_drive_s *dp;
 	FRESULT res = 0;
 
 	(void)subf;
@@ -388,27 +422,27 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 		return msg_cpm_result(subf, 0x01, res);
 	}
 
-	debug_cpmsd("\n## %7lu  login: %c:\n", get_timer(0), msg[1]+CONFIG_CPM_BASE_DRIVE);
-
 	/* Get relative drive number */
 	drv = msg[1];
 	if ( drv >= CONFIG_CPM_MAX_DRIVE) {
-		debug_cpmsd("##           failed: invalid relative drive number '%d'\n", drv);
+		debug_cpmsd("##           login failed: invalid relative drive number '%d'\n", drv);
 		return msg_cpm_result(subf, 0x02, res);
 	}
 
-	p = &drv_table[drv];
-	p->flags &= ~DRV_FLG_OPEN;
-	p->dph = ((uint32_t)msg[4] << 16) + ((uint16_t)msg[3] << 8) + msg[2];
+	drv_debug(drv, PSTR("login"));
 
+	dp = &drv_table[drv];
+	dp->flags &= ~DRV_FLG_OPEN;
+	dp->dph = ((uint32_t)msg[4] << 16) + ((uint16_t)msg[3] << 8) + msg[2];
 
-	if (p->img_name == NULL) {
+
+	if (dp->img_name == NULL) {
 		/* no file attached */
-		debug_cpmsd("##           failed: no file attached:\n");
+		drv_debug(drv, PSTR("login failed: no file attached"));
 		return msg_cpm_result(subf, 0x03, res);
 	}
 
-	p->flags |= DRV_FLG_OPEN;
+	dp->flags |= DRV_FLG_OPEN;
 
 	/* send  result*/
 	msg_cpm_result(subf, 0x00, res);
@@ -436,10 +470,13 @@ void do_msg_cpm_login(uint8_t subf, int len, uint8_t * msg)
 void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 {
 	uint8_t drv;
+	struct cpm_drive_s *dp;
 	uint32_t addr;
 	uint32_t pos;
+	uint16_t track;
+	uint16_t sec;
 	uint8_t secs;
-	bool dowrite = (subf == 2);
+	bool dowrite;
 	FRESULT res = 0;
 	uint8_t rc = 0;
 	bool buserr = 0;
@@ -453,26 +490,41 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 		return msg_cpm_result(subf, 0x02, res);
 	}
 
+	dp = &drv_table[drv];
+
+	if (dp->img_name == NULL) {
+		/* no media */
+		return msg_cpm_result(subf, 0x06, res);
+	}
+
+	if (subf == 2) {
+		dowrite = true;
+		if (dp->opt & DRV_OPT_RO) {
+			return msg_cpm_result(subf, 0x05, res);
+		}
+	} else {
+		dowrite = false;
+	}
+
+	track = (uint16_t)(msg[TRK+1] << 8) + msg[TRK];
+	sec  = (uint16_t)(msg[SEC+1] << 8) + msg[SEC];
 	secs = msg[CNT];
 	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])) * CONFIG_CPM_BLOCK_SIZE;
+	pos = (track * 8UL + sec) * CONFIG_CPM_BLOCK_SIZE;
 
-	debug_cpmsd("## %7lu cpm_rw: %s %c: trk:%4d, sec: %d, pos: %.8lx, secs: %2d, "
-			"addr: %.5lx\n", get_timer(0), dowrite ? "write" : " read",
-			msg[RDRV]+CONFIG_CPM_BASE_DRIVE,
-			((uint16_t)(msg[TRK+1] << 8) + msg[TRK]), msg[SEC],	pos, msg[CNT], addr);
+	drv_debug(drv, PSTR("%5s trk:%4d, sec: %d, pos: %.8lx, secs: %2d, addr: %.5lx"),
+			dowrite ? "write" : " read", track, sec, pos, secs, addr);
 
 	if (pos + secs * CONFIG_CPM_BLOCK_SIZE > CONFIG_CPM_DISKSIZE) {
-		debug_cpmsd("                   access > DISKSIZE (%.8lx > %.8lx) aborted!\n",
+		drv_debug(drv, PSTR("access > DISKSIZE (%.8lx > %.8lx) aborted!"),
 						pos + secs * CONFIG_CPM_BLOCK_SIZE, CONFIG_CPM_DISKSIZE);
 		return msg_cpm_result(subf, 0x04, res);
 	}
 
-	res = f_lseek(&drv_table[drv].fd, pos);
+	res = f_lseek(&dp->fd, pos);
 
 	while (!res && secs--) {
 		unsigned int brw;
@@ -484,9 +536,9 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 				z80_read_block(disk_buffer, addr, CONFIG_CPM_BLOCK_SIZE);
 				z80_bus_cmd(Release);
 			}
-			res = f_write(&drv_table[drv].fd, disk_buffer, CONFIG_CPM_BLOCK_SIZE, &brw);
+			res = f_write(&dp->fd, disk_buffer, CONFIG_CPM_BLOCK_SIZE, &brw);
 		} else {
-			res = f_read(&drv_table[drv].fd, disk_buffer, CONFIG_CPM_BLOCK_SIZE, &brw);
+			res = f_read(&dp->fd, disk_buffer, CONFIG_CPM_BLOCK_SIZE, &brw);
 			if (res == FR_OK) {
 				if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
 					buserr = 1;
@@ -498,19 +550,19 @@ void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
 			}
 		}
 		if (brw != CONFIG_CPM_BLOCK_SIZE) {
-			debug_cpmsd("           short read or write: res: %d, bytes rd/wr: %u\n", res, brw);
+			drv_debug(drv, PSTR("short read or write: res: %d, bytes rd/wr: %u"), res, brw);
 			res = 64;
 		}
 		addr += CONFIG_CPM_BLOCK_SIZE;
 	}
 
 	if (dowrite && !res) {
-		drv_table[drv].flags |= DRV_FLG_DIRTY;
+		dp->flags |= DRV_FLG_DIRTY;
 		bg_setstat(handle_cpm_drv_to, 1);
 	}
 
 	if (buserr) {
-		debug_cpmsd("Bus timeout\n");
+		drv_debug(drv, PSTR("Bus timeout"));
 		rc = 0x03;
 	}