X-Git-Url: http://cloudbase.mooo.com/gitweb/z180-stamp.git/blobdiff_plain/209025e847108189669989a3715667aae4f17b54..18d57acbf4ccd7220e59fe1f2ac1556837c5da83:/avr/z180-serv.c?ds=inline

diff --git a/avr/z180-serv.c b/avr/z180-serv.c
index ac57645..6973908 100644
--- a/avr/z180-serv.c
+++ b/avr/z180-serv.c
@@ -6,9 +6,6 @@
 
 #include "z180-serv.h"
 #include "common.h"
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
 #include <util/atomic.h>
 
 #include "config.h"
@@ -109,6 +106,45 @@ void do_msg_get_timer(uint8_t subf, int len, uint8_t * msg)
 
 /* ---------------------------------------------------------------------------*/
 
+/* measure f_cpu */
+void do_msg_m_fcpu(uint8_t subf, int len UNUSED, uint8_t * msg)
+{
+	uint32_t freq;
+	uint8_t eimsk_save;
+
+	/* Save state and disable INT5/INT6 */
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		eimsk_save = EIMSK;
+		EIMSK &= ~_BV(INT6);
+		EIMSK &= ~_BV(INT5);
+	}
+	EIFR = _BV(INTF5);				/* Reset pending int */
+	z80_bus_cmd(Request);
+	z80_write(0x3f, 0xff);
+	z80_bus_cmd(Release);
+
+	freq = z80_measure_phi(*msg);
+
+	z80_bus_cmd(Request);
+	z80_write(0x3f, 0xff);
+	z80_bus_cmd(Release);
+
+	/* Restore INT5/INT6 */
+	ATOMIC_BLOCK(ATOMIC_FORCEON) {
+		if ((eimsk_save & _BV(INT5)) != 0)
+			EIMSK |= _BV(INT5);
+		if ((eimsk_save & _BV(INT6)) != 0)
+			EIMSK |= _BV(INT6);
+		/* Reset pending int */
+		EIFR = _BV(INTF5);
+		EIFR = _BV(INTF6);
+	}
+
+	msg_xmit(4, subf, sizeof(freq), (uint8_t *) &freq);
+}
+
+/* ---------------------------------------------------------------------------*/
+
 #define CPM_DAY_OFFSET ((1978-1900) * 365 + 19)		/* 19 leap years */
 
 /*
@@ -292,17 +328,17 @@ int drv_attach(uint8_t unit, const char *filename, drv_opt_t options)
 
 	drv = unit;
 	if (drv >= CONFIG_CPM_MAX_DRIVE)
-		return AT_RANGE;
+		return EATRANGE;
 
 	struct cpm_drive_s *p = &drv_table[drv];
 
 	if (options & DRV_OPT_REATTATCH) {
 		if (filename) {
-			return AT_ERROR;
+			return EUNEXPARG;
 		}
 
 		if (!p->img_name) {
-			return AT_NOT;
+			return EATNOT;
 		}
 
 		/* change options */
@@ -317,16 +353,16 @@ int drv_attach(uint8_t unit, const char *filename, drv_opt_t options)
 	} else {
 
 		if (p->img_name)
-			return AT_ALREADY;
+			return EATALRDY;
 		if (drv_find_file_attached(filename) >= 0)
-			return AT_OTHER;
+			return EATOTHER;
 
 		p->opt = options;
 
 		/* new attachment */
 
 		if ((p->img_name = strdup(filename)) == NULL)
-			return AT_NOMEM;
+			return ENOMEM;
 
 		res = f_open(&p->fd, p->img_name,
 				FA_READ | (options&DRV_OPT_RO ? 0 : FA_WRITE));
@@ -355,11 +391,11 @@ int drv_attach(uint8_t unit, const char *filename, drv_opt_t options)
 		}
 		if (res) {
 			drv_detach(drv);
-			return AT_OPEN;
+			return EATOPEN;
 		}
 	}
 
-	return AT_OK;
+	return ESUCCESS;
 }
 
 
@@ -417,7 +453,7 @@ void msg_cpm_result(uint8_t subf, uint8_t rc, int res)
 	msg_xmit(2, subf, sizeof(result_msg), result_msg);
 
 	if (rc) {
-#if GCC_BUG_61443
+#if defined(GCC_BUG_61443)
 		char msg[40];
 		strncpy_P(msg, rc_messages[rc & 0x7f], sizeof msg -1);
 		drv_debug(END, PSTR(" rc: %.02x/%d, '%s'"),
@@ -625,6 +661,9 @@ const FLASH struct msg_item z80_messages[] =
 	{ 3,
 	  2, 3,				/* 2: get, 3: set time and date */
 	  do_msg_get_set_time},
+	{ 4,
+	  1, 1,
+	  do_msg_m_fcpu},
 	{ 0xff,				/* end mark */
 	  0, 0,
 	  0},
@@ -632,8 +671,6 @@ const FLASH struct msg_item z80_messages[] =
 };
 
 
-
-
 void do_message(int len, uint8_t *msg)
 {
 	uint8_t fct, sub_fct;