Index: common/a8core/sio_device.vhdl
===================================================================
--- common/a8core/sio_device.vhdl	(revision 373)
+++ common/a8core/sio_device.vhdl	(revision 374)
@@ -76,12 +76,14 @@
 	constant state_wait_high_low : std_logic_vector(3 downto 0) := "0000";
 	constant state_command_clear_irq : std_logic_vector(3 downto 0) := "0001";
 	constant state_command_enable_irq : std_logic_vector(3 downto 0) := "0010";
-	constant state_command_wait_for_byte : std_logic_vector(3 downto 0) := "0011";
-	constant state_command_clear_irq2 : std_logic_vector(3 downto 0) := "0100";
-	constant state_command_enable_irq2 : std_logic_vector(3 downto 0) := "0101";
-	constant state_command_read_data : std_logic_vector(3 downto 0) := "0110";
-	constant state_command_ready_on_command_high : std_logic_vector(3 downto 0) := "0111";
+	constant state_command_reset_skstat : std_logic_vector(3 downto 0) := "1011";
+	constant state_command_wait_for_byte : std_logic_vector(3 downto 0) := "0100";
+	constant state_command_clear_irq2 : std_logic_vector(3 downto 0) := "0101";
+	constant state_command_enable_irq2 : std_logic_vector(3 downto 0) := "0110";
+	constant state_command_read_data : std_logic_vector(3 downto 0) := "0111";
 	constant state_command_ready : std_logic_vector(3 downto 0) := "1000";
+	constant state_command_error : std_logic_vector(3 downto 0) := "1001";
+	constant state_command_stop : std_logic_vector(3 downto 0) := "1111";
 	signal clear_request : std_logic;
 
 	signal state_wr_en : std_logic;
@@ -95,6 +97,7 @@
 
 	signal pokey_data_out : std_logic_vector(7 downto 0);
 
+	constant pokey_skrest : std_logic_vector(3 downto 0) := x"a";
 	constant pokey_serout : std_logic_vector(3 downto 0) := x"d";
 	constant pokey_irqen : std_logic_vector(3 downto 0) := x"e";
 
@@ -113,7 +116,8 @@
 	process(clk,reset_n)
 	begin
 		if (reset_n = '0') then
-			state_reg <= state_wait_high_low;	
+			-- wait for explicit initialisation via clear
+			state_reg <= state_command_stop;
 			target_reg <= (others=>'0');
 			command_reg <= '1';	
 			en_delay_reg <= (others=>'0');
@@ -169,6 +173,12 @@
 				if (state_reg = state_command_ready) then
 					data_out(0) <= '1';
 				end if;
+				if (state_reg = state_command_stop) then
+					data_out(6) <= '1';
+				end if;
+				if (state_reg = state_command_error) then
+					data_out(7) <= '1';
+				end if;
 			end if;
 		else
 			data_out <= pokey_data_out;
@@ -187,7 +197,7 @@
 	end process;
 
 	-- state machine
-	process(state_reg, command_next, command_reg, bus_free, pokey_data_out, target_reg, clear_request, sio_command)
+	process(state_reg, command_reg, bus_free, pokey_data_out, target_reg, clear_request, sio_command)
 	begin
 		state_next <= state_reg;
 		state_wr_en <= '0';
@@ -216,11 +226,24 @@
 				state_addr<=pokey_irqen;
 				state_wr_en<='1';
 				state_data<=x"38";
-				state_next <= state_command_wait_for_byte;				
+				state_next <= state_command_reset_skstat;
+			when state_command_reset_skstat =>
+				state_addr<=pokey_skrest;
+				state_wr_en<='1';
+				state_data<=x"00";
+				state_next <= state_command_wait_for_byte;
 			when state_command_wait_for_byte =>
 				state_addr<=pokey_irqen;
 				if (pokey_data_out(5)='0') then
 					state_next <= state_command_clear_irq2;
+				else
+					if (sio_command = '1') then
+						if (target_reg="101") then
+							state_next <= state_command_ready;
+						else
+							state_next <= state_command_error;
+						end if;
+					end if;
 				end if;
 			when state_command_clear_irq2 =>
 				state_addr<=pokey_irqen;
@@ -231,23 +254,27 @@
 				state_addr<=pokey_irqen;
 				state_wr_en<='1';
 				state_data<=x"38";
-				state_next <= state_command_read_data;								
+				state_next <= state_command_read_data;
 			when state_command_read_data =>
 				state_addr<=pokey_serout;
-				target_wr_en<='1';
-				if (target_reg="100") then
-					state_next <= state_command_ready_on_command_high;
-				else
-					state_next <= state_command_wait_for_byte;
+				state_next <= state_command_wait_for_byte;
+				case target_reg is
+				when "000"|"001"|"010"|"011"|"100" =>
 					target_next <= std_logic_vector(unsigned(target_reg)+1);
+					target_wr_en<='1';
+				when others =>
+					-- indicate error, too many command bytes
+					target_next <= "111";
+				end case;
+			when state_command_ready =>
+				if (sio_command = '0') then
+					-- invalidate command frame when command goes low
+					state_next <= state_command_error;
 				end if;
-			when state_command_ready_on_command_high =>
-				if (command_reg='1') then
-					state_next <= state_command_ready;
-				end if;
-			when state_command_ready =>
+			when state_command_error => null;
+			when state_command_stop => null;
 			when others =>
-				state_next <= state_wait_high_low;
+				state_next <= state_command_stop;
 			end case;
 		end if;
 
Index: de1/atari800core_de1.vhd
===================================================================
--- de1/atari800core_de1.vhd	(revision 373)
+++ de1/atari800core_de1.vhd	(revision 374)
@@ -358,7 +358,9 @@
 		 );
 
 -- PIA mapping
-SIO_COMMAND <= CB2_OUT;
+-- emulate pull-up on command line
+SIO_COMMAND <= CB2_OUT when CB2_DIR_OUT='1' else '1';
+-- SIO_COMMAND <= CB2_OUT;
 --PORTA_IN <= ((JOY2_n(3)&JOY2_n(2)&JOY2_n(1)&JOY2_n(0)&JOY1_n(3)&JOY1_n(2)&JOY1_n(1)&JOY1_n(0)) and not (porta_dir_out)) or (porta_dir_out and porta_out);
 --PORTA_IN <= (not (porta_dir_out)) or (porta_dir_out and porta_out);
 PORTB_IN <= PORTB_OUT;
Index: de1/gpiov2.vhd
===================================================================
--- de1/gpiov2.vhd	(revision 373)
+++ de1/gpiov2.vhd	(revision 374)
@@ -112,8 +112,10 @@
 -- OUTPUTS TO GPIO
 	-- unused
 	--GPIO_0_DIR_OUT(35 downto 4) <= (others=>'0');
-	GPIO_0_OUT(35 downto 0) <= (others=>'0');
-	
+	GPIO_0_OUT(35 downto 2) <= (others=>'0');
+	GPIO_0_OUT(1) <= '0' when gpio_enable='1' else CB2_OUT;
+	GPIO_0_OUT(0) <= '0';
+
 	-- sio
 	--CB1=SIO_IRQ
 	--CB2=SIO_COMMAND
@@ -121,7 +123,7 @@
 	--CA2=SIO_MOTOR_RAW
 
 	GPIO_0_DIR_OUT(0) <= '0';
-	GPIO_0_DIR_OUT(1) <= CB2_dir_out and not(CB2_OUT) and gpio_enable;
+	GPIO_0_DIR_OUT(1) <= CB2_dir_out and not(CB2_OUT) when gpio_enable='1' else '1';
 	GPIO_0_DIR_OUT(2) <= '0';
 	GPIO_0_DIR_OUT(3) <= CA2_dir_out and not(CA2_OUT) and gpio_enable;
 	GPIO_0_DIR_OUT(4) <= not(SIO_OUT) and gpio_enable;
@@ -129,12 +131,12 @@
 	GPIO_0_DIR_OUT(6) <= not(SIO_CLOCKOUT) and gpio_enable;
 	GPIO_0_DIR_OUT(7) <= '0';
 	
-	CB1_in <= GPIO_0_IN(0);
-	CB2_in <= GPIO_0_IN(1);
-	CA1_in <= GPIO_0_IN(2);
-	CA2_in <= GPIO_0_IN(3);
-	SIO_IN <= GPIO_0_IN(5);
-	SIO_CLOCKIN <= GPIO_0_IN(7);
+	CB1_in <= GPIO_0_IN(0) when gpio_enable='1' else '1';
+	CB2_in <= GPIO_0_IN(1) when gpio_enable='1' else '1';
+	CA1_in <= GPIO_0_IN(2) when gpio_enable='1' else '1';
+	CA2_in <= GPIO_0_IN(3) when gpio_enable='1' else '1';
+	SIO_IN <= GPIO_0_IN(5) when gpio_enable='1' else '1';
+	SIO_CLOCKIN <= GPIO_0_IN(7) when gpio_enable='1' else '1';
 	
 	-- sticks
 	-- PORTA7,6,5,4,TRIG1,POT3,2,1,0,PORTA3,2,1,0,TRIG0
Index: firmware/atari_drive_emulator.c
===================================================================
--- firmware/atari_drive_emulator.c	(revision 373)
+++ firmware/atari_drive_emulator.c	(revision 374)
@@ -124,90 +124,68 @@
 	u08 aux2;
 	u08 chksum;
 } __attribute__((packed));
+
+static void switch_speed()
+{
+	if (speed == speedslow)
+	{
+		speed = speedfast;
+	}
+	else
+	{
+		speed = speedslow;
+	}
+	USART_Init(speed+6);
+}
+
 void getCommand(struct command * cmd)
 {
 	int expchk;
 	int i;
-
-	//printf("\n%f:WaitCMD\n",when());
-	while (0 == USART_Command_Line());
-	//printf("%f:CMDHigh\n",when());
-	USART_Init(speed+6);
-	//printf("%f:Clear:%d\n",when(),*zpu_siocommand_ready);
-	*zpu_siocommand_ready = 1; // clear
-
+	unsigned char cmdstat;
 	while (1)
 	{
-		unsigned char x = *zpu_siocommand_ready;
-		if (x != 0) break;
-		actions();
+		while (0 == USART_Command_Line())
+		{
+			actions();
+			wait_us(3);
+		}
+		USART_Init(speed+6);
+		*zpu_siocommand_ready = 1; // clear
 
-//#ifdef SOCKIT
-//		double a = when();
-//		while ((when()-a)<3e-6);
-//#endif
-		wait_us(3);
-	} // usb poll can take some ms, but we have 16 ms to reply. We have hardware now to capture the command frame.
-	for (i=0;i!=5;++i)
-		((unsigned char *)cmd)[i] = zpu_siocommand_data[i<<2];
-	*zpu_siocommand_ready = 1; // clear
+		do {
+			actions();
+			wait_us(3);
+			cmdstat = *zpu_siocommand_ready;
+		} while (0 == cmdstat);
 
-	/*cmd->deviceId = USART_Receive_Byte();
-	cmd->command = USART_Receive_Byte();
-	cmd->aux1 = USART_Receive_Byte();
-	cmd->aux2 = USART_Receive_Byte();
-	cmd->chksum = USART_Receive_Byte();*/
-	//while (0 == USART_Command_Line());
-	//printf("cmd:");
-	//printf("Gone high\n");
-	atari_sector_buffer[0] = cmd->deviceId;
-	atari_sector_buffer[1] = cmd->command;
-	atari_sector_buffer[2] = cmd->aux1;
-	atari_sector_buffer[3] = cmd->aux2;
-	expchk = get_checksum(&atari_sector_buffer[0],4);
+		if (cmdstat & 0x40) {
+			// state machine is stopped, just (re-)start it
+			continue;
+		}
+		if ((cmdstat & 0x80) || USART_Framing_Error()) {
+			// state machine reported error or we got a framing error:
+			// switch speed and retry
+			switch_speed();
+			continue;
+		}
+		for (i=0;i!=5;++i)
+			((unsigned char *)cmd)[i] = zpu_siocommand_data[i<<2];
 
-/*unsigned char * temp = baseaddr;
-baseaddr = (unsigned char volatile *)(40000 + atari_regbase);
-debug_pos=160;
-*/
-	//printf("%f:CMD: %02x %02x %02x %02x %02x/%02x %s\n",when(),cmd->deviceId,cmd->command,cmd->aux1,cmd->aux2,cmd->chksum,expchk, cmd->chksum!=expchk ? "BAD":"");
-/*
-baseaddr = temp;*/
+		atari_sector_buffer[0] = cmd->deviceId;
+		atari_sector_buffer[1] = cmd->command;
+		atari_sector_buffer[2] = cmd->aux1;
+		atari_sector_buffer[3] = cmd->aux2;
+		expchk = get_checksum(&atari_sector_buffer[0],4);
 
-	//printf("Device id:");
-	//printf("%d",cmd->deviceId);
-	//printf("\n");
-	//printf("command:");
-	//printf("%d",cmd->command);
-	//printf("\n");
-	//printf("aux1:");
-	//printf("%d",cmd->aux1);
-	//printf("\n");
-	//printf("aux2:");
-	//printf("%d",cmd->aux2);
-	//printf("\n");
-	//printf("chksum:");
-	//printf("%x ",cmd->chksum);
-	//printf("%x ",expchk);
-
-	if (expchk!=cmd->chksum || USART_Framing_Error())
-	{
-		//printf("ERR ");
-		//wait_us(1000000);
-		if (speed == speedslow)
-		{
-			speed = speedfast;
-			//printf("SPDF");
-			//printf("%d",speed);
+		if (expchk==cmd->chksum) {
+			// got a command frame
+			break;
+		} else {
+			// just an invalid checksum, switch speed anyways
+			switch_speed();
 		}
-		else
-		{
-			speed = speedslow;
-			//printf("SPDS");
-			//printf("%d",speed);
-		}
 	}
-	//printf("\n");
 
 	DELAY_T2_MIN;
 }
Index: firmware/pokey/uart.c
===================================================================
--- firmware/pokey/uart.c	(revision 373)
+++ firmware/pokey/uart.c	(revision 374)
@@ -77,7 +77,7 @@
 
 int USART_Framing_Error()
 {
-	if (0xc0&(*zpu_pokey_skctl))
+	if (0x80&(*zpu_pokey_skctl))
 	{
 		return 0;
 	}
