1.1 --- a/src/io.h	Sat Jun 24 00:33:10 2017 +0300
     1.2 +++ b/src/io.h	Sat Jun 24 02:33:52 2017 +0300
     1.3 @@ -43,6 +43,8 @@
     1.4  	IO_PAD_A		= 64,
     1.5  	IO_PAD_START	= 128
     1.6  };
     1.7 +#define IO_PAD_LOW_MASK		0x3f
     1.8 +#define IO_PAD_HIGH_MASK	(IO_PAD_A | IO_PAD_START)
     1.9  
    1.10  static inline void io_setdir(int port, uint8_t dir)
    1.11  {
    1.12 @@ -53,12 +55,18 @@
    1.13  {
    1.14  	uint16_t bnstate;
    1.15  
    1.16 -	io_setdir(port, 0x40);		/* pin 6 (multiplexer) output, 0-5 inputs */
    1.17 +	bnstate = ~(IO_REG_DATA(port) << 2) & IO_PAD_HIGH_MASK;
    1.18 +
    1.19  	IO_REG_DATA(port) = 0x40;	/* select mode 1 */
    1.20 -	bnstate = IO_REG_DATA(port);
    1.21 +	asm volatile("nop");
    1.22 +	asm volatile("nop");
    1.23 +	bnstate |= ~IO_REG_DATA(port) & IO_PAD_LOW_MASK;
    1.24 +
    1.25  	IO_REG_DATA(port) = 0;		/* select mode 0 */
    1.26 -	bnstate |= (IO_REG_DATA(port) << 2) & (IO_PAD_A | IO_PAD_START);
    1.27 -	return ~bnstate;
    1.28 +	return bnstate;
    1.29  }
    1.30  
    1.31 +void io_init(void);
    1.32 +/*uint16_t io_readpad(int port);*/
    1.33 +
    1.34  #endif	/* MEGADRIVE_IO_H_ */