int SensorHAL::getHeight() {
	/*
	 Bohrung oben	3506,3524,3528
	 Bohrung unten	2470,2478,2480
	 zu flach	2731,2737
	 Bohrung oben oM	3492
	 */
	int hoehe = -1;
	int i;

	out8(AIO_PORT_A, AIO_GET_VAL);

	for (i = 0; i < 50; i++) {
		//Bit 7 goes HIGH when an A/D conversion completes
		if ((in8(AIO_BASE) & (1 << 7))) { // == (1<<7)
			hoehe = in16(AIO_PORT_A);
			break;
		}
	}
	return hoehe;
}

/*
 * IOControl constructor - initializes I/O settings and controls
 */
IOControl::IOControl( std::queue<Event>* qin, pthread_mutex_t *aQ ) {
	// Initialize variables
	accessQ = aQ;
	q = qin;

	// Initialize HW I/O control handlers
	if( ThreadCtl(_NTO_TCTL_IO, NULL) == -1 )
	{
		std::perror("Error - could not access I/O registers");
	}

	CONTROL_HANDLE = mmap_device_io(IO_PORT_SIZE, IO_CONTROL_REGISTER);
	A_HANDLE = mmap_device_io(IO_PORT_SIZE, IO_A_REGISTER);
	B_HANDLE = mmap_device_io(IO_PORT_SIZE, IO_B_REGISTER);
	C_HANDLE = mmap_device_io(IO_PORT_SIZE, IO_C_REGISTER);

    if(CONTROL_HANDLE == MAP_DEVICE_FAILED)
    {
        std::cout << "Failed to map control register";
    }

    if(A_HANDLE == MAP_DEVICE_FAILED)
    {
        std::cout << "Failed to map register A";
    }

    if(B_HANDLE == MAP_DEVICE_FAILED)
    {
        std::cout << "Failed to map register B";
    }

    if(C_HANDLE == MAP_DEVICE_FAILED)
    {
        std::cout << "Failed to map register C";
    }
    // Initialize control register
    // A is input
    // B,C are output
    out8(CONTROL_HANDLE,0x90);
}

static int __read_sectors_lba48(void *data, unsigned long long sector,
			size_t count)
{
	const unsigned long slo = sector & 0xfffffffful;
	const unsigned long shi = sector >> 32;

	const unsigned clo = count & 0xffu;
	const unsigned chi = count >> 8;

	ide_wait_ready();

	out8(IDE_DEVCTL_REG, IDE_NIEN);

	out8(IDE_SC_REG, chi);
	out8(IDE_LBA0_REG, shi & 0xff);
	out8(IDE_LBA1_REG, (shi >> 8) & 0xff);
	out8(IDE_LBA2_REG, (shi >> 16) & 0xff);

	out8(IDE_SC_REG, clo);
	out8(IDE_LBA0_REG, slo & 0xff);
	out8(IDE_LBA1_REG, (slo >> 8) & 0xff);
	out8(IDE_LBA2_REG, (slo >> 16) & 0xff);

	out8(IDE_DEV_REG, IDE_LBA);
	out8(IDE_CMD_REG, IDE_CMD_READ_LBA48);

	ide_wait_ready();

	uint16_t *wptr = (uint16_t *)data;
	const size_t words = IDE_SECTOR_SIZE / sizeof(*wptr);

	for (size_t i = 0; i != count; ++i) {
		for (size_t j = 0; j != words; ++j)
			*wptr++ = in16(IDE_DATA_REG);
		in8(IDE_ASTATUS_REG); // wait one PIO transfer cycle

		if (ide_wait_ready() != 0)
			return -1;
	}

	return 0;
}

///// 制御ボードイニシャライズ関数//////////////////////////////////////////////
void Board_Open(void)
{
	int ch;
	ThreadCtl(_NTO_TCTL_IO,0);
	mmap_device_io(BIO_SIZE,0x300);

///// カウンタ初期化 /////
	out8(Count_com,0x13);		//A-2-phase x4
	out8(Count_com,0x1b);		//B-2-phase x4
	out8(Count_com,0x14);		//A-count enable set
	out8(Count_com,0x1c);		//B-count enable set
	out8(Count_com,0x16);		//separate mode
	out8(Count_com,0x0e);		//level CLR
	out8(Count_com,0x0f);		//edge CLR
	
	for(ch=1; ch<=Channel; ch++){
		M[ch].AD_Init = 2048;										//とりあえずのADの初期値
		DA_Write(ch,2048);											//2.5V出力
	}
}

/** Handler for a PCI ATA IRQ.
 * @param num		IRQ number.
 * @param _channel	Pointer to channel structure.
 * @return		Whether the IRQ was handled. */
static irq_status_t pci_ata_irq_handler(unsigned num, void *_channel) {
	pci_ata_channel_t *data = _channel;
	uint8_t status;

	if(!data->channel) {
		return IRQ_UNHANDLED;
	}

	/* Check whether this device has raised an interrupt. */
	status = in8(data->bus_master_base + PCI_ATA_BM_REG_STATUS);
	if(!(status & PCI_ATA_BM_STATUS_INTERRUPT)) {
		return IRQ_UNHANDLED;
	}

	/* Clear interrupt flag. */
	out8(data->bus_master_base + PCI_ATA_BM_REG_STATUS, (status & 0xF8) | PCI_ATA_BM_STATUS_INTERRUPT);

	/* Clear INTRQ. */
	in8(data->cmd_base + ATA_CMD_REG_STATUS);

	/* Pass the interrupt to the ATA bus manager. */
	return ata_channel_interrupt(data->channel);
}

/**
 * Hauptschleife des geerbten HAW-Thread.
 * Die oberklasse HAW-Thread erzwingt die Implementierung der execute Methode.
 * Der Thread endet nach Ende dieser Methode.
 */
void Blink_Thread::execute(void*){
    /* Klassenweiten Mutex, locken. */
    Lock lock(&mtx_);
    cout << "Blink_Thread executing" << endl;

    /* Zugriffsrechte von QNX fuer diesen Thread, auf die Hardware erbitten. */
    if( ThreadCtl(_NTO_TCTL_IO_PRIV,0) == -1 ){
        cout << "Can't get Hardware access, therefore can't do anything." << endl;
    }

    /* IO Register als Eingänge bzw. Ausgänge definieren. */
    out8(ioControlAddress_, ioControlBitmask_);

    /* Gruenes Licht blinken lassen inkl. Pruefung ob der Thread extern gestoppt wurde. */
    for(int i = 0; i < times_; i++){
        /* Pruefen ob der Thread durch stop() beendet wurde. */
        if( !isStopped() ){
            turnGreenOn();
            usleep(500000);
            turnGreenOff();
            usleep(500000);
        }
    }
}

const struct sigevent* ISR(void *arg, int id) {
	struct sigevent *event = (struct sigevent *) arg;

	int portB = 0;
	int portC = 0;

	int iir = in8(DIO_INTERRUPT_READ_CLEAR); //Status auslesen und IRQ zurück setzen

	event->sigev_value.sival_int = 0;
	event->sigev_notify = SIGEV_PULSE;

	if (iir == PORTB_INTERRUPT || iir == PORTC_INTERRUPT) {
		portB = in8(PORT_B);
		portC = in8(PORT_C) & 0xF0;
		portC = portC << 4;
		event->sigev_value.sival_int = portC | portB;
	} else {
		return NULL;
	}

	out8(DIO_INTERRUPT_CLEAR_REG, 0x00);

	return event;
}

int write_RXFE_EEPROM_address(int  base, int cardnum, unsigned int  address, int Adata){

        int temp;
        int WE_on=0x01, RW_on=0x40;

        if ( (cardnum > 32) || (cardnum < 0)){
                printf("ERROR: Card numnber must be between 0 and 32\n");
        }
        cardnum=cardnum & 0x1f;
        cardnum=cardnum << 1;
#ifdef __QNX__
        // set all groups as output
        out8(base+CNTRL_GRP_4,0x80);
        set_RXFE_EEPROM_address(base, address);
        // set R/W bit to write
        out8(base+PC_GRP_4,RW_on | cardnum);
        // set data to write
        out8(base+PA_GRP_4,Adata);
        // set WE bit high
        out8(base+PC_GRP_4,RW_on | WE_on | cardnum);
        // set WE bit low
        out8(base+PC_GRP_4,RW_on | cardnum);
        delay(10);
        // set port A as input
        out8(base+CNTRL_GRP_4,0x90);
        set_RXFE_EEPROM_address(base, address);
        // set R/W bit low
        out8(base+PC_GRP_4,cardnum);
        //delay(1000);
        delay(4);
        temp=in8(base+PA_GRP_4);

        if (temp != Adata){
                printf("WRITE ERROR\n");
        }
        printf("%d was read on port A\n", temp);
#endif
}

void kbd_write_output(unsigned char data)
{
	out8(CONFIG_SYS_ISA_IO_BASE_ADDRESS + KDB_DATA_PORT, data);
}

int flash_erase (flash_info_t *info, int s_first, int s_last)
{
	volatile ulong addr = info->start[0];
	int flag, prot, sect, l_sect;
	ulong start, now, last;

	flash_to_xd();

	if (s_first < 0 || s_first > s_last) {
		if (info->flash_id == FLASH_UNKNOWN) {
			printf ("- missing\n");
		} else {
			printf ("- no sectors to erase\n");
		}
		flash_to_mem();
		return 1;
	}

	if (info->flash_id == FLASH_UNKNOWN) {
		printf ("Can't erase unknown flash type %08lx - aborted\n",
			info->flash_id);
		flash_to_mem();
		return 1;
	}

	prot = 0;
	for (sect=s_first; sect<=s_last; ++sect) {
		if (info->protect[sect]) {
			prot++;
		}
	}

	if (prot) {
		printf ("- Warning: %d protected sectors will not be erased!\n",
			prot);
	} else {
		printf ("\n");
	}

	l_sect = -1;

	/* Disable interrupts which might cause a timeout here */
	flag = disable_interrupts();

	out8(addr + 0x555, 0xAA);
	iobarrier_rw();
	out8(addr + 0x2AA, 0x55);
	iobarrier_rw();
	out8(addr + 0x555, 0x80);
	iobarrier_rw();
	out8(addr + 0x555, 0xAA);
	iobarrier_rw();
	out8(addr + 0x2AA, 0x55);
	iobarrier_rw();

	/* Start erase on unprotected sectors */
	for (sect = s_first; sect<=s_last; sect++) {
		if (info->protect[sect] == 0) {	/* not protected */
			addr = info->start[sect];
			out8(addr, 0x30);
			iobarrier_rw();
			l_sect = sect;
		}
	}

	/* re-enable interrupts if necessary */
	if (flag)
		enable_interrupts();

	/* wait at least 80us - let's wait 1 ms */
	udelay (1000);

	/*
	 * We wait for the last triggered sector
	 */
	if (l_sect < 0)
		goto DONE;

	start = get_timer (0);
	last  = start;
	addr = info->start[l_sect];

	DEBUGF ("Start erase timeout: %d\n", CONFIG_SYS_FLASH_ERASE_TOUT);

	while ((in8(addr) & 0x80) != 0x80) {
		if ((now = get_timer(start)) > CONFIG_SYS_FLASH_ERASE_TOUT) {
			printf ("Timeout\n");
			flash_reset (info->start[0]);
			flash_to_mem();
			return 1;
		}
		/* show that we're waiting */
		if ((now - last) > 1000) {	/* every second */
			putc ('.');
			last = now;
		}
		iobarrier_rw();
	}

DONE:
//.........这里部分代码省略.........

/*
 * pic_init()
 *	Initialise the 8259 interrupt controllers
 */
void pic_init(void)
{
	/*
	 * initialise the 8259's
	 */
	out8(PIC0_ICW1, 0x11);		/* Reset */
	out8(PIC0_ICW2, IRQ_BASE);	/* Vectors served */
	out8(PIC0_ICW3, 1 << 2);	/* Chain to PIC1 */
	out8(PIC0_ICW4, 1);		/* 8086 mode */
	out8(PIC0_OCW1, 0xff);		/* No interrupts for now */
	out8(PIC0_OCW2, 2);		/* ISR mode */

	out8(PIC1_ICW1, 0x11);		/* Reset */
	out8(PIC1_ICW2, IRQ_BASE + 8);  /* Vectors served */
	out8(PIC1_ICW3, 2);		/* We are a slave unit */
	out8(PIC1_ICW4, 1);		/* 8086 mode */
	out8(PIC1_OCW1, 0xff);		/* No interrupts for now */
	out8(PIC1_OCW2, 2);		/* ISR mode */
}

void kbd_write_command(unsigned char cmd)
{
	out8(CONFIG_SYS_ISA_IO_BASE_ADDRESS + KDB_COMMAND_PORT,cmd);
}

void write_ec_sc(ec_dev_t* dev, uint8_t data)
{
    out8(EC_SC, data);
}

status_t
pci_controller_init(void)
{
	bool search_mech1 = true;
	bool search_mech2 = true;
	bool search_bios = true;
	void *config = NULL;
	status_t status;

	status = pci_x86_irq_init();
	if (status != B_OK)
		return status;

	config = load_driver_settings("pci");
	if (config) {
		const char *mech = get_driver_parameter(config, "mechanism",
			NULL, NULL);
		if (mech) {
			search_mech1 = search_mech2 = search_bios = false;
			if (strcmp(mech, "1") == 0)
				search_mech1 = true;
			else if (strcmp(mech, "2") == 0)
				search_mech2 = true;
			else if (strcmp(mech, "bios") == 0)
				search_bios = true;
			else
				panic("Unknown pci config mechanism setting %s\n", mech);
		}
		unload_driver_settings(config);
	}

	// TODO: check safemode "don't call the BIOS" setting and unset search_bios!

	// PCI configuration mechanism 1 is the preferred one.
	// If it doesn't work, try mechanism 2.
	// Finally, try to fallback to PCI BIOS

	if (search_mech1) {
		// check for mechanism 1
		out32(0x80000000, PCI_MECH1_REQ_PORT);
		if (0x80000000 == in32(PCI_MECH1_REQ_PORT)) {
			dprintf("PCI: mechanism 1 controller found\n");
			return pci_controller_add(&pci_controller_x86_mech1, NULL);
		}
	}

	if (search_mech2) {
		// check for mechanism 2
		out8(0x00, 0xCFB);
		out8(0x00, 0xCF8);
		out8(0x00, 0xCFA);
		if (in8(0xCF8) == 0x00 && in8(0xCFA) == 0x00) {
			dprintf("PCI: mechanism 2 controller found\n");
			return pci_controller_add(&pci_controller_x86_mech2, NULL);
		}
	}

	if (search_bios) {
		// check for PCI BIOS
		if (pci_bios_init() == B_OK) {
			dprintf("PCI: BIOS support found\n");
			return pci_controller_add(&pci_controller_x86_bios, NULL);
		}
	}

	dprintf("PCI: no configuration mechanism found\n");
	return B_ERROR;
}

void i2c_init (int speed, int slaveadd)
{
	sys_info_t sysInfo;
	unsigned long freqOPB;
	int val, divisor;

#ifdef CFG_I2C_INIT_BOARD
	/* call board specific i2c bus reset routine before accessing the   */
	/* environment, which might be in a chip on that bus. For details   */
	/* about this problem see doc/I2C_Edge_Conditions.                  */
	i2c_init_board();
#endif

	/* Handle possible failed I2C state */
	/* FIXME: put this into i2c_init_board()? */
	_i2c_bus_reset ();

	/* clear lo master address */
	out8 (IIC_LMADR, 0);

	/* clear hi master address */
	out8 (IIC_HMADR, 0);

	/* clear lo slave address */
	out8 (IIC_LSADR, 0);

	/* clear hi slave address */
	out8 (IIC_HSADR, 0);

	/* Clock divide Register */
	/* get OPB frequency */
	get_sys_info (&sysInfo);
	freqOPB = sysInfo.freqPLB / sysInfo.pllOpbDiv;
	/* set divisor according to freqOPB */
	divisor = (freqOPB - 1) / 10000000;
	if (divisor == 0)
		divisor = 1;
	out8 (IIC_CLKDIV, divisor);

	/* no interrupts */
	out8 (IIC_INTRMSK, 0);

	/* clear transfer count */
	out8 (IIC_XFRCNT, 0);

	/* clear extended control & stat */
	/* write 1 in SRC SRS SWC SWS to clear these fields */
	out8 (IIC_XTCNTLSS, 0xF0);

	/* Mode Control Register
	   Flush Slave/Master data buffer */
	out8 (IIC_MDCNTL, IIC_MDCNTL_FSDB | IIC_MDCNTL_FMDB);
	__asm__ volatile ("eieio");


	val = in8(IIC_MDCNTL);
	__asm__ volatile ("eieio");

	/* Ignore General Call, slave transfers are ignored,
	   disable interrupts, exit unknown bus state, enable hold
	   SCL
	   100kHz normaly or FastMode for 400kHz and above
	*/

	val |= IIC_MDCNTL_EUBS|IIC_MDCNTL_HSCL;
	if( speed >= 400000 ){
		val |= IIC_MDCNTL_FSM;
	}
	out8 (IIC_MDCNTL, val);

	/* clear control reg */
	out8 (IIC_CNTL, 0x00);
	__asm__ volatile ("eieio");

}

static void rtc_write (short reg, uchar val)
{
	out8(RTC(reg),val);
}

/*
  This code tries to use the features of the 405GP i2c
  controller. It will transfer up to 4 bytes in one pass
  on the loop. It only does out8(lbz) to the buffer when it
  is possible to do out16(lhz) transfers.

  cmd_type is 0 for write 1 for read.

  addr_len can take any value from 0-255, it is only limited
  by the char, we could make it larger if needed. If it is
  0 we skip the address write cycle.

  Typical case is a Write of an addr followd by a Read. The
  IBM FAQ does not cover this. On the last byte of the write
  we don't set the creg CHT bit, and on the first bytes of the
  read we set the RPST bit.

  It does not support address only transfers, there must be
  a data part. If you want to write the address yourself, put
  it in the data pointer.

  It does not support transfer to/from address 0.

  It does not check XFRCNT.
*/
static
int i2c_transfer(unsigned char cmd_type,
		 unsigned char chip,
		 unsigned char addr[],
		 unsigned char addr_len,
		 unsigned char data[],
		 unsigned short data_len )
{
	unsigned char* ptr;
	int reading;
	int tran,cnt;
	int result;
	int status;
	int i;
	uchar creg;

	if( data == 0 || data_len == 0 ){
		/*Don't support data transfer of no length or to address 0*/
		printf( "i2c_transfer: bad call\n" );
		return IIC_NOK;
	}
	if( addr && addr_len ){
		ptr = addr;
		cnt = addr_len;
		reading = 0;
	}else{
		ptr = data;
		cnt = data_len;
		reading = cmd_type;
	}

	/*Clear Stop Complete Bit*/
	out8(IIC_STS,IIC_STS_SCMP);
	/* Check init */
	i=10;
	do {
		/* Get status */
		status = in8(IIC_STS);
		__asm__ volatile("eieio");
		i--;
	} while ((status & IIC_STS_PT) && (i>0));

	if (status & IIC_STS_PT) {
		result = IIC_NOK_TOUT;
		return(result);
	}
	/*flush the Master/Slave Databuffers*/
	out8(IIC_MDCNTL, ((in8(IIC_MDCNTL))|IIC_MDCNTL_FMDB|IIC_MDCNTL_FSDB));
	/*need to wait 4 OPB clocks? code below should take that long*/

	/* 7-bit adressing */
	out8(IIC_HMADR,0);
	out8(IIC_LMADR, chip);
	__asm__ volatile("eieio");

	tran = 0;
	result = IIC_OK;
	creg = 0;

	while ( tran != cnt && (result == IIC_OK)) {
		int  bc,j;

		/* Control register =
		   Normal transfer, 7-bits adressing, Transfer up to bc bytes, Normal start,
		   Transfer is a sequence of transfers
		*/
		creg |= IIC_CNTL_PT;

		bc = (cnt - tran) > 4 ? 4 :
			cnt - tran;
		creg |= (bc-1)<<4;
		/* if the real cmd type is write continue trans*/
		if ( (!cmd_type && (ptr == addr)) || ((tran+bc) != cnt) )
			creg |= IIC_CNTL_CHT;

//.........这里部分代码省略.........

/*
 * Function     - operation_thread()
 *
 * Arguments    - <args> Pointer to this thread's arguments
 *
 * Return Value - Pointer to the return value
 */
void * operation_thread(void * args)
{
   uint8_t port_val = 0;
   uint32_t total_time = 0;
   uint32_t count = 0, sec = 0;
   sint32_t rcv_id;

#if defined (__ARM__)
   sint32_t temp_high = 0, temp_low = 0;
#endif   // #if defined (__ARM__)

   // Setup the timer for PWM
   __pwm_setup();

   // Wait for timer pulse
   for (;;)
   {
      rcv_id = MsgReceive(channel_id, &timer_msg, sizeof(timer_msg), NULL);
      if(rcv_id == 0)
      {
         if(timer_msg.pulse.code == __TIMER_PULSE_CODE)
         {
            if(__OP_IDLE == state)
            {
               sec = 0;
               count = 0;
               total_time = 0;
            }
            else
            {
               if(0 == local_vol)
               {
                  state = __OP_IDLE;

                  if(0 == pthread_mutex_lock(&operation_mutex))
                  {
                     operation = __OP_COMPLETE;
                     pthread_mutex_unlock(&operation_mutex);
                  }
               }

               if(1000 == ++count)
               {
                  count = 0;
                  sec++;
               }

               if(60 == sec)
               {
                  sec = 0;
                  total_time++;

                  if(local_vol >= local_flowrate)
                     local_vol -= local_flowrate;
                  else
                     local_vol = 0;

                  if(0 == pthread_mutex_lock(&operation_mutex))
                  {
                     op_percentage = 100 - ((local_vol * 100) / start_vol);
                     pthread_mutex_unlock(&operation_mutex);
                  }
               }
            }

#if defined (__ARM__)
            // PWM pulse
            if(0 < --temp_high)
            {
               port_val = in8(gpio_base + 0);
               port_val |= GPIO_SERVO_PIN;
               out8(gpio_base + 0, port_val);
            }
            else if(0 < --temp_low)
            {
               port_val = in8(gpio_base + 0);
               port_val &= (uint8_t)(~(GPIO_SERVO_PIN));
               out8(gpio_base + 0, port_val);
            }
            else if(__OP_IDLE == state)
            {
               temp_high = __DEFALT_DUTY_CYCLE;
               temp_low = __DEFALT_DUTY_CYCLE;
            }
            else
            {
               temp_high = steps_high;
               temp_low = steps_low;
            }
#endif   // #if defined (__ARM__)
         }
      }
   }
//.........这里部分代码省略.........

// ------------------------------------------------------------------------------------------------
void pci_out8(uint id, uint reg, u8 data)
{
    u32 address = 0x80000000 | id | (reg & 0xfc);
    out32(PCI_CONFIG_ADDR, address);
    out8(PCI_CONFIG_DATA + (reg & 0x03), data);
}

static uchar rtc_read (short reg)
{
	out8(RTC_PORT_ADDR0, reg & 0xFF);
	out8(RTC_PORT_ADDR1, (reg>>8) & 0xFF);
	return in8(RTC_PORT_DATA);
}