static void oki_play_sample(int sample_no)
{
	UINT16 table_start = (sample_no & 0x80) ? read16(SAMPLE_TABLE_1) : read16(SAMPLE_TABLE_0);
	UINT8 byte1 = read8(table_start + 2 * (sample_no & 0x7f) + 0);
	UINT8 byte2 = read8(table_start + 2 * (sample_no & 0x7f) + 1);
	int chip = (byte1 & 0x80) >> 7;
	running_device *okidevice = (chip) ? NMK004_state.oki2device : NMK004_state.oki1device;

	if ((byte1 & 0x7f) == 0)
	{
		// stop all channels
		okim6295_w(okidevice, 0, 0x78 );
	}
	else
	{
		int sample = byte1 & 0x7f;
		int ch = byte2 & 0x03;
		int force = (byte2 & 0x80) >> 7;

		if (!force && (NMK004_state.oki_playing & (1 << (ch + 4*chip))))
			return;

		NMK004_state.oki_playing |= 1 << (ch + 4*chip);

		// stop channel
		okim6295_w(okidevice, 0, (0x08 << ch) );

		if (sample != 0)
		{
			UINT8 *rom = memory_region(NMK004_state.machine, (chip == 0) ? "oki1" : "oki2");
			int bank = (byte2 & 0x0c) >> 2;
			int vol = (byte2 & 0x70) >> 4;

			if (bank != 3)
				memcpy(rom + 0x20000,rom + 0x40000 + bank * 0x20000,0x20000);

			okim6295_w(okidevice, 0, 0x80 | sample );
			okim6295_w(okidevice, 0, (0x10 << ch) | vol );
		}
	}

uint16_t read16(uint8_t address) {
	uint8_t err, pec;
	uint16_t ret = 0;

	i2c_start_wait(MLX90614_I2CADDR << 1);

	err = i2c_write(address); // send register address to read from
	if (err > 0) {
		return 0;
	}

	err = i2c_start((MLX90614_I2CADDR << 1) + I2C_READ);
	if (err > 0) {
		return 0;
	}
	
	ret = i2c_read_ack();
	ret |= i2c_read_ack() << 8;
	pec = i2c_read_nack();
	
	i2c_stop();

	uint8_t commArray[5] = {MLX90614_I2CADDR << 1,
		address,
		(MLX90614_I2CADDR << 1) + I2C_READ,
		ret & 0xFF,
		(ret >> 8) & 0xFF
	};

	uint8_t crc = crc8_ccitt(commArray, 5);

	if (pec != crc) { // PEC error
		return 0;
	}

	return ret;
}

int32_t readTemp(uint8_t reg) {
	uint16_t reading = read16(reg);
	
	if (reading == 0) {
		return -1;
	}
	
    int32_t temp = ((int32_t) reading) * 100;

    temp /= 50;
    temp -= 27315;

    return temp;
}

void
test_writesame16_unmap_until_end(void)
{
    int i, ret;
    unsigned int j;
    unsigned char *buf = alloca(256 * block_size);

    CHECK_FOR_DATALOSS;
    CHECK_FOR_THIN_PROVISIONING;
    CHECK_FOR_LBPWS;
    CHECK_FOR_SBC;

    logging(LOG_VERBOSE, LOG_BLANK_LINE);
    logging(LOG_VERBOSE, "Test WRITESAME16 of 1-256 blocks at the end of the LUN by setting number-of-blocks==0");
    for (i = 1; i <= 256; i++) {
        logging(LOG_VERBOSE, "Write %d blocks of 0xFF", i);
        memset(buf, 0xff, block_size * i);
        ret = write16(iscsic, tgt_lun, num_blocks - i,
                      i * block_size, block_size,
                      0, 0, 0, 0, 0, buf);

        logging(LOG_VERBOSE, "Unmap %d blocks using WRITESAME16", i);
        ret = writesame16(iscsic, tgt_lun, num_blocks - i,
                          0, i,
                          0, 1, 0, 0, NULL);
        if (ret == -2) {
            logging(LOG_NORMAL, "[SKIPPED] WRITESAME16 is not implemented.");
            CU_PASS("[SKIPPED] Target does not support WRITESAME16. Skipping test");
            return;
        }
        CU_ASSERT_EQUAL(ret, 0);

        if (rc16->lbprz) {
            logging(LOG_VERBOSE, "LBPRZ is set. Read the unmapped "
                    "blocks back and verify they are all zero");

            logging(LOG_VERBOSE, "Read %d blocks and verify they "
                    "are now zero", i);
            ret = read16(iscsic, tgt_lun, num_blocks - i,
                         i * block_size, block_size,
                         0, 0, 0, 0, 0, buf);
            for (j = 0; j < block_size * i; j++) {
                if (buf[j] != 0) {
                    CU_ASSERT_EQUAL(buf[j], 0);
                }
            }
        } else {
            logging(LOG_VERBOSE, "LBPRZ is clear. Skip the read "
                    "and verify zero test");
        }
    }
}

/*  to be called from 'bspstart.c' */
void bsp_hw_init (void)
{
  uint16_t   temp16;

#ifdef STANDALONE_EVB
  /* STANDALONE_EVB: sets up PFC */
  /* no STANDALONE_EVB: accepts defaults, adds RESET */

  /* FIXME: replace 'magic numbers' */

  write16(0x5000, PFC_PACRH);  /* Pin function controller - WRHH, WRHL */
  write16(0x1550, PFC_PACRL1); /* Pin fun. controller - WRH,WRL,RD,CS1 */
  write16(0x0000, PFC_PBCR1);  /* Pin function controller - default */
  write16(0x2005, PFC_PBCR2);  /* Pin fcn. controller - A18,A17,A16 */
  write16(0xFFFF, PFC_PCCR);   /* Pin function controller - A15-A0  */
  write16(0x5555, PFC_PDCRH1); /* Pin function controller - D31-D24 */
  write16(0x5555, PFC_PDCRH2); /* Pin function controller - D23-D16 */
  write16(0xFFFF, PFC_PDCRL);  /* Pin function controller - D15-D0  */
  write16(0x0000, PFC_IFCR);   /* Pin function controller - default */
  write16(0x0000, PFC_PACRL2); /* default disconnects all I/O pins;*/
	                       /*  [re-connected by DEVICE_open()] */
#endif

  /* default hardware setup for SH7045F EVB */

  /* PFC: General I/O except pin 13 (reset): */
  temp16 = read16(PFC_PECR1);
  temp16 |= 0x0800;
  write16(temp16, PFC_PECR1);

  /* All I/O lines bits 7-0: */
  write16(0x00, PFC_PECR2);

  /* P5 (LED) out, all other pins in: */
  temp16 = read16(PFC_PEIOR);
  temp16 |= 0x0020;
  write16(temp16, PFC_PEIOR);

}

void PX4Flow::update()
{
  //send 0x0 to PX4FLOW module and receive back 22 Bytes data 
  Wire.beginTransmission(PX4FLOW_ADDRESS);
  Wire.write(0x0);  
  Wire.endTransmission();  
  
  // request 22 bytes from the module
  Wire.requestFrom(PX4FLOW_ADDRESS, 22);    

  // wait for all data to be available
  // TODO we could manage a timeout in order not to block
  // the loop when no component is connected
  while(Wire.available() < 22);
  
  frame.frame_count       = read16();
  frame.pixel_flow_x_sum  = read16();
  frame.pixel_flow_y_sum  = read16();
  frame.flow_comp_m_x     = read16();
  frame.flow_comp_m_y     = read16();
  frame.qual              = read16();
  frame.gyro_x_rate       = read16();
  frame.gyro_y_rate       = read16();
  frame.gyro_z_rate       = read16();
  frame.gyro_range        = read8();
  frame.sonar_timestamp   = read8();
  frame.ground_distance   = read16();
  
  // if too many bytes are available, we drain in order to be synched
  // on next read
  if(Wire.available()) {
    #if PX4FLOW_DEBUG == true
    {
      Serial.println("ERROR [PX4Flow] : Too many bytes available.");
    }
    #endif
    while(Wire.available()) {Wire.read();}
  }
}

void
test_read16_0blocks(void)
{
	int ret;

	CHECK_FOR_SBC;

	logging(LOG_VERBOSE, LOG_BLANK_LINE);
	logging(LOG_VERBOSE, "Test READ16 0-blocks at LBA==0");
	ret = read16(sd, NULL, 0, 0, block_size,
		     0, 0, 0, 0, 0, NULL,
		     EXPECT_STATUS_GOOD);
	if (ret == -2) {
		logging(LOG_NORMAL, "[SKIPPED] READ16 is not implemented on this target and it does not claim SBC-3 support.");
		CU_PASS("READ16 is not implemented and no SBC-3 support claimed.");
		return;
	}	
	CU_ASSERT_EQUAL(ret, 0);

	logging(LOG_VERBOSE, "Test READ16 0-blocks one block past end-of-LUN");
	ret = read16(sd, NULL, num_blocks + 1, 0,
		     block_size, 0, 0, 0, 0, 0, NULL,
		     EXPECT_LBA_OOB);
	CU_ASSERT_EQUAL(ret, 0);


	logging(LOG_VERBOSE, "Test READ16 0-blocks at LBA==2^63");
	ret = read16(sd, NULL, 0x8000000000000000ULL, 0,
		     block_size, 0, 0, 0, 0, 0, NULL,
		     EXPECT_LBA_OOB);
	CU_ASSERT_EQUAL(ret, 0);


	logging(LOG_VERBOSE, "Test READ16 0-blocks at LBA==-1");
	ret = read16(sd, NULL, -1, 0, block_size,
		     0, 0, 0, 0, 0, NULL,
		     EXPECT_LBA_OOB);
	CU_ASSERT_EQUAL(ret, 0);
}

/**
    \fn    readHeader
    \brief read one track
*/
uint8_t amvHeader::readTrack(int nb)
{
  uint32_t tail,sz;
  AVIStreamHeader stream;
  if(read32()!=MKFCC('s','t','r','h'))
  {
    printf("[AMV]Wrong header (strh)\n"); 
    return 0;
  }
  // First is strh
  sz=read32();
  printf("\t\t[AMV] strh size : %u,sizeof :%u\n",sz,sizeof(AVIStreamHeader));
  fseeko(_fd,sz,SEEK_CUR);
  
  // The strf
  if(read32()!=MKFCC('s','t','r','f'))
  {
    printf("[AMV]Wrong header (strf)\n"); 
    return 0;
  }
  sz=read32();
  uint32_t pos=ftell(_fd);
  printf("\t\t[AMV] strf size : %u,sizeof :%u\n",sz,sizeof(AVIStreamHeader));
  if(nb==1) // Second track=Audio
  {
                                   read16(); // Tag
      wavHeader. 	channels  =read16();
      wavHeader. 	frequency =read32();
      wavHeader. 	byterate  =read32();
      wavHeader. 	blockalign=read16();
      wavHeader. 	encoding  =WAV_AMV_ADPCM;       
      wavHeader. 	bitspersample=16;	 
 
  }
  fseeko(_fd,sz+pos,SEEK_SET);
  
  return 1;
}

void
test_read16_simple(void)
{
	int i, ret;

	CHECK_FOR_SBC;

	logging(LOG_VERBOSE, LOG_BLANK_LINE);
	logging(LOG_VERBOSE, "Test READ16 of 1-256 blocks at the start of the LUN");
	for (i = 1; i <= 256; i++) {
		if (maximum_transfer_length && maximum_transfer_length < i) {
			break;
		}

		ret = read16(sd, 0, i * block_size,
			     block_size, 0, 0, 0, 0, 0, NULL,
			     EXPECT_STATUS_GOOD);
		if (ret == -2) {
			logging(LOG_NORMAL, "[SKIPPED] READ16 is not implemented on this target and it does not claim SBC-3 support.");
			CU_PASS("READ16 is not implemented and no SBC-3 support claimed.");
			return;
		}	
		CU_ASSERT_EQUAL(ret, 0);
	}


	logging(LOG_VERBOSE, "Test READ16 of 1-256 blocks at the end of the LUN");
	for (i = 1; i <= 256; i++) {
		if (maximum_transfer_length && maximum_transfer_length < i) {
			break;
		}

		ret = read16(sd, num_blocks - i,
			     i * block_size, block_size, 0, 0, 0, 0, 0, NULL,
			     EXPECT_STATUS_GOOD);
		CU_ASSERT_EQUAL(ret, 0);
	}
}

static uint32_t reg_script_read_mmio(struct reg_script_context *ctx)
{
	const struct reg_script *step = reg_script_get_step(ctx);

	switch (step->size) {
	case REG_SCRIPT_SIZE_8:
		return read8((u8 *)step->reg);
	case REG_SCRIPT_SIZE_16:
		return read16((u16 *)step->reg);
	case REG_SCRIPT_SIZE_32:
		return read32((u32 *)step->reg);
	}
	return 0;
}

/*
 * Clock_isr
 *
 * Clock interrupt handling routine.
 */
static rtems_isr Clock_isr(rtems_vector_number vector)
{
  uint16_t   tcr;

  /* reset the timer underflow flag */
  tcr = read16(SH7750_TCR0);
  write16(tcr & ~SH7750_TCR_UNF, SH7750_TCR0);

  /* Increment the clock interrupt counter */
  Clock_driver_ticks++ ;

  /* Invoke rtems clock service routine */
    rtems_clock_tick();
}

attribute_info* read_attribute(FILE* stream){
    attribute_info* a = malloc(sizeof(attribute_info));
    memcheck(a);
    a->name_index = read16(stream);
    a->attribute_length = read32(stream);
    assert(a->attribute_length > 0);
    a->info = malloc(a->attribute_length + 1);
    memcheck(a->info);
    a->info[a->attribute_length] = 0;
    for(int i = 0; i < a->attribute_length; i++){
	a->info[i] = read8(stream);
    }
    return a;
}

static u32
read_2338 (u32 edx)
{
	u32 ret;

	write32 (DEFAULT_RCBA + 0x2330, edx);
	write16 (DEFAULT_RCBA + 0x2338, (read16 (DEFAULT_RCBA + 0x2338)
					 & 0x1ff) | 0x600);
	wait_2338 ();
	ret = read32 (DEFAULT_RCBA + 0x2334);
	wait_2338 ();
	read8 (DEFAULT_RCBA + 0x2338);
	return ret;
}

s32 sys_spu_thread_read_ls(u32 id, u32 address, vm::ptr<u64> value, u32 type)
{
    sys_spu.Log("sys_spu_thread_read_ls(id=0x%x, address=0x%x, value=*0x%x, type=%d)", id, address, value, type);

    LV2_LOCK;

    const auto thread = Emu.GetIdManager().get<SPUThread>(id);

    if (!thread)
    {
        return CELL_ESRCH;
    }

    if (address >= 0x40000 || address + type > 0x40000 || address % type) // check range and alignment
    {
        return CELL_EINVAL;
    }

    const auto group = thread->tg.lock();

    if (!group)
    {
        throw EXCEPTION("Invalid SPU thread group");
    }

    if ((group->state < SPU_THREAD_GROUP_STATUS_WAITING) || (group->state > SPU_THREAD_GROUP_STATUS_RUNNING))
    {
        return CELL_ESTAT;
    }

    switch (type)
    {
    case 1:
        *value = thread->read8(address);
        break;
    case 2:
        *value = thread->read16(address);
        break;
    case 4:
        *value = thread->read32(address);
        break;
    case 8:
        *value = thread->read64(address);
        break;
    default:
        return CELL_EINVAL;
    }

    return CELL_OK;
}

static u32
read_iobp(u32 address)
{
	u32 ret;

	write32(DEFAULT_RCBA + IOBPIRI, address);
	write16(DEFAULT_RCBA + IOBPS, (read16(DEFAULT_RCBA + IOBPS)
					 & 0x1ff) | 0x600);
	wait_iobp();
	ret = read32(DEFAULT_RCBA + IOBPD);
	wait_iobp();
	read8(DEFAULT_RCBA + IOBPS); // call wait_iobp() instead here?
	return ret;
}

static
int read_tag(uint8_t* arr, int pos,  int swapBytes, void* dest)
{
        // Format should be 5 over here (rational)
    uint32_t format = read16(arr, pos + 2, swapBytes);
    // Components should be 1
    uint32_t components = read32(arr, pos + 4, swapBytes);
    // Points to the value
    uint32_t offset;

    // sanity
    if (components != 1) return 0;

    if (format == 3)
        offset = pos + 8;
    else
        offset =  read32(arr, pos + 8, swapBytes);

    switch (format) {

    case 5: // Rational
          {
          double num = read32(arr, offset, swapBytes);
          double den = read32(arr, offset + 4, swapBytes);
          *(double *) dest = num / den;
          }
          break;

    case 3: // uint 16
        *(int*) dest = read16(arr, offset, swapBytes);
        break;

    default:  return 0;
    }

    return 1;
}

bool PX4Flow::update_integral()
{
  //send 0x16 to PX4FLOW module and receive back 25 Bytes data 
  Wire.beginTransmission(PX4FLOW_ADDRESS);
  Wire.write(0x16);  
  Wire.endTransmission();  
  
  // request 25 bytes from the module
  Wire.requestFrom(PX4FLOW_ADDRESS, 26);    

  // wait for all data to be available
  // TODO we could manage a timeout in order not to block
  // the loop when no component is connected
  if (!wait(26)) {
      return false;
  }
  
  // read the data
  iframe.frame_count_since_last_readout = read16();
  iframe.pixel_flow_x_integral  = read16();
  iframe.pixel_flow_y_integral  = read16();
  iframe.gyro_x_rate_integral   = read16();
  iframe.gyro_y_rate_integral   = read16();
  iframe.gyro_z_rate_integral   = read16();
  iframe.integration_timespan   = read32();
  iframe.sonar_timestamp        = read32();
  iframe.ground_distance        = read16();
  iframe.gyro_temperature       = read16();
  iframe.quality                = read8();

  // This is due to the lack of structure packing
  // in the PX4Flow code.
  read8();
  
  // if too many bytes are available, we drain in order to be synched
  // on next read
  if(Wire.available()) {
    #if PX4FLOW_DEBUG == true
    {
      Serial.println("ERROR [PX4Flow] : Too many bytes available.");
    }
    #endif
    while(Wire.available()) {Wire.read();}
  }

  return true;
}

/*
 * Initialization of serial port
 */
void sh_sci_init(int minor)
{
    uint16_t   temp16;

    /*
     * set PFC registers to enable I/O pins
     */
    if ((minor == 0)) {
        temp16 = read16(PFC_PACRL2);         /* disable SCK0, DMA, IRQ */
        temp16 &= ~(PA2MD1 | PA2MD0);
        temp16 |= (PA_TXD0 | PA_RXD0);       /* enable pins for Tx0, Rx0 */
        write16(temp16, PFC_PACRL2);

    } else if (minor == 1) {
        temp16 = read16(PFC_PACRL2);          /* disable SCK1, DMA, IRQ */
        temp16 &= ~(PA5MD1 | PA5MD0);
        temp16 |= (PA_TXD1 | PA_RXD1);        /* enable pins for Tx1, Rx1 */
        write16(temp16, PFC_PACRL2);
    }

    /*
     * Non-default hardware setup occurs in sh_sci_first_open
     */
}

int read_relocation_bitmask()
{
	uint32_t first;
	uint32_t lower;
	uint32_t upper;

	first = read_byte();
	
	if ((first & 0x80) != 0x80)
		return first;
	
	if ((first & 0xC0) != 0xC0)
		return ((first & 0x7F) << 8) + read_byte();
	
	if ((first & 0xE0) != 0xE0) {
		upper = ((first & 0xFF3F) << 8) + read_byte();
		lower = read16();
	} else {
		upper = read16();
		lower = read16();
	}
	uint32_t ret = lower + (upper << 16);
	return ret;
}

/**
 * \brief Conversion of unsigned int
 */
const char *Translator::toUnsigned(uint16_t *length, uint8_t *data_record, uint16_t offset, const ipfix_element_t * element, struct json_conf * config)
{
	if(*length == BYTE1) {
		// 1 byte
		if(element->en == 0 && element->id == 6 && config->tcpFlags) {
			// Formated TCP flags
			return formatFlags8(read8(data_record + offset));
		} else if (element->en == 0 && element->id == 4 && !config->protocol) {
			// Formated protocol identification (TCP, UDP, ICMP,...)
			return (formatProtocol(read8(data_record + offset)));
		} else {
			// Other elements
			snprintf(buffer, BUFF_SIZE, "%" PRIu8, read8(data_record + offset));
		}
	} else if(*length == BYTE2) {
		// 2 bytes
		if (element->en == 0 && element->id == 6 && config->tcpFlags) {
			// Formated TCP flags
			return formatFlags16(read16(data_record + offset));
		} else {
			// Other elements
			snprintf(buffer, BUFF_SIZE, "%" PRIu16, ntohs(read16(data_record + offset)));
		}
	} else if(*length == BYTE4) {
		// 4 bytes
		snprintf(buffer, BUFF_SIZE, "%" PRIu32, ntohl(read32(data_record + offset)));
	} else if(*length == BYTE8) {
		// 8 bytes
		snprintf(buffer, BUFF_SIZE, "%" PRIu64, be64toh(read64(data_record + offset)));
	} else {
		// Other sizes
		snprintf(buffer, BUFF_SIZE, "%s", "\"unknown\"");
	}

	return buffer;
}

void
test_mandatory_sbc(void)
{
	int ret;
	//unsigned char buf[4096];
	//struct unmap_list list[1];

	logging(LOG_VERBOSE, LOG_BLANK_LINE);
	logging(LOG_VERBOSE, "Test support for all mandatory opcodes on SBC devices");

	CHECK_FOR_SBC;

	logging(LOG_VERBOSE, "Test INQUIRY.");
	ret = inquiry(sd, NULL, 0, 0, 255,
		      EXPECT_STATUS_GOOD);
	CU_ASSERT_EQUAL(ret, 0);

	logging(LOG_VERBOSE, "Test READCAPACITY10.");
	ret = readcapacity10(sd, NULL, 0, 0,
			     EXPECT_STATUS_GOOD);
	CU_ASSERT_EQUAL(ret, 0);

	if (sbc3_support) {
		logging(LOG_VERBOSE, "Test READCAPACITY16. The device claims SBC-3 support.");
		ret = readcapacity16(sd, NULL, 15,
				     EXPECT_STATUS_GOOD);
		CU_ASSERT_EQUAL(ret, 0);
	}

	logging(LOG_VERBOSE, "Test READ10.");
	ret = read10(sd, NULL, 0, block_size, block_size,
		     0, 0, 0, 0, 0, NULL,
		     EXPECT_STATUS_GOOD);
	CU_ASSERT_EQUAL(ret, 0);

	if (sbc3_support) {
		logging(LOG_VERBOSE, "Test READ16. the device claims SBC-3 support.");
		ret = read16(sd, 0, block_size, block_size,
			     0, 0, 0, 0, 0, NULL,
			     EXPECT_STATUS_GOOD);
		CU_ASSERT_EQUAL(ret, 0);
	}

	logging(LOG_VERBOSE, "Test TESTUNITREADY.");
	ret = testunitready(sd,
			    EXPECT_STATUS_GOOD);
	CU_ASSERT_EQUAL(ret, 0);
}