void fs20_send(uint8_t fht, uint16_t housecode, uint8_t address, uint8_t command, uint8_t command2)
{
    for (uint8_t i = 0; i < 3; i++) 
    {
        fs20_send_sync();

        /* magic constant 6 from fs20 protocol definition, 0x0c from fht protocol definition... */
        uint8_t sum = fht ? 0x0C : 0x06; 

        fs20_send_byte(HI8(housecode));
        sum += HI8(housecode);
        fs20_send_byte(LO8(housecode));
        sum += LO8(housecode);
        fs20_send_byte(address);
        sum += address;
        fs20_send_byte(command);
        sum += command;

        if ( command & 0x20 )
        {
            fs20_send_byte(command2);
            sum += command2;
        }

        fs20_send_byte(sum);

        fs20_send_zero();

        _delay_loop_2(FS20_DELAY_CMD);
    }
}

unsigned int enc28j60_receive_packet(unsigned int maxlen, unsigned char *buffer)
{
	unsigned int rxstat;
	unsigned int len;

	//packet in buffer ?	
	if ((enc28j60_read_address(ENC28J60_REG_EIR) & (1<<ENC28J60_BIT_PKTIF)) == 0){
		//double check!	
		//errata says that PKTIF does not work as it should 
		//->check packetcount too:
		if (enc28j60_read_address(ENC28J60_REG_EPKTCNT) == 0)
			return 0;
	}

	//set read pointer to next packet;
	enc28j60_write_address(ENC28J60_REG_ERDPTL, (enc28j60_next_packet_ptr));
	enc28j60_write_address(ENC28J60_REG_ERDPTH, (enc28j60_next_packet_ptr)>>8);

	//now read the transmit status vector
	//read next packet ptr
	enc28j60_next_packet_ptr  = enc28j60_spi_read_byte(ENC28J60_OP_READ_BUF_MEM, 0);
	enc28j60_next_packet_ptr |= enc28j60_spi_read_byte(ENC28J60_OP_READ_BUF_MEM, 0)<<8;

	//read packet length
	len  = enc28j60_spi_read_byte(ENC28J60_OP_READ_BUF_MEM, 0);
	len |= enc28j60_spi_read_byte(ENC28J60_OP_READ_BUF_MEM, 0)<<8;
	
	//read rx stat
	rxstat  = enc28j60_spi_read_byte(ENC28J60_OP_READ_BUF_MEM, 0);
	rxstat |= enc28j60_spi_read_byte(ENC28J60_OP_READ_BUF_MEM, 0)<<8;

	//limit read bytecount
	if (len>maxlen)
		len = maxlen;

	//transfer packet from enc28j60 to our buffer
	enc28j60_read_buffer(buffer,len);

	//mark packet as processed (free mem)

	//ERRATA says we need to check packet pointer:
	if ((enc28j60_next_packet_ptr- 1 < ENC28J60_RX_BUFFER_START) || (enc28j60_next_packet_ptr- 1 > ENC28J60_RX_BUFFER_END)){
		enc28j60_write_address(ENC28J60_REG_ERXRDPTL, LO8(ENC28J60_RX_BUFFER_END));
		enc28j60_write_address(ENC28J60_REG_ERXRDPTH, HI8(ENC28J60_RX_BUFFER_END));
	}else{
		enc28j60_write_address(ENC28J60_REG_ERXRDPTL, LO8(enc28j60_next_packet_ptr- 1));
		enc28j60_write_address(ENC28J60_REG_ERXRDPTH, HI8(enc28j60_next_packet_ptr- 1));
	}

	//decrement packet counter:
	enc28j60_spi_write_word(ENC28J60_OP_BFS|ENC28J60_REG_ECON2, (1<<ENC28J60_BIT_PKTDEC));

	return len;
}

cystatus CySpcGetTemp(uint8 numSamples)
#endif  /* (CY_PSOC5A) */
{
    cystatus status = CYRET_STARTED;

    /* Make sure the SPC is ready to accept command */
    if(CY_SPC_IDLE)
    {
        CY_SPC_CPU_DATA_REG = CY_SPC_KEY_ONE;
        CY_SPC_CPU_DATA_REG = CY_SPC_KEY_TWO(CY_SPC_CMD_GET_TEMP);
        CY_SPC_CPU_DATA_REG = CY_SPC_CMD_GET_TEMP;

        /* Make sure the command was accepted */
        if(CY_SPC_BUSY)
        {
            CY_SPC_CPU_DATA_REG = numSamples;

            #if(CY_PSOC5A)
                CY_SPC_CPU_DATA_REG = HI8(timerPeriod);
                CY_SPC_CPU_DATA_REG = LO8(timerPeriod);
                CY_SPC_CPU_DATA_REG = clkDivSelect;
            #endif  /* (CY_PSOC5A) */
        }
        else
        {
            status = CYRET_CANCELED;
        }
    }
    else
    {
        status = CYRET_LOCKED;
    }

    return(status);
}

/*******************************************************************************
* Function Name: CySpcCreateCmdLoadMultiByte
********************************************************************************
* Summary:
*   Sets up the command to LoadMultiByte.
*
* Parameters:
* array:
*   Id of the array.
*
* address:
*   flash/eeprom addrress
*
* size:
*   number bytes to load.
*   
* Return:
*   CYRET_SUCCESS if the command was created sucessfuly.
*   CYRET_LOCKED if the SPC is in use.
*   CYRET_BAD_PARAM if an invalid parameter was passed.
*
* Theory:
*
*
*******************************************************************************/
cystatus CySpcCreateCmdLoadMultiByte(uint8 array, uint16 address, uint16 number)
{
    cystatus status;


    /* Check if number is correct for array. */
    if((array < LAST_FLASH_ARRAYID && number == 32) ||
       (array > LAST_FLASH_ARRAYID && number == 16))
    {
        /* Create packet command. */
        cyCommand[0] = SPC_KEY_ONE;
        cyCommand[1] = SPC_KEY_TWO(SPC_CMD_LD_MULTI_BYTE);
        cyCommand[2] = SPC_CMD_LD_MULTI_BYTE;
        
        /* Create packet parameters. */
        cyCommand[3] = array;
        cyCommand[4] = 1 & HI8(address);
        cyCommand[5] = LO8(address);
        cyCommand[6] = number - 1;

        cyCommandSize = SIZEOF_CMD_LOAD_MBYTE;

        status = CYRET_SUCCESS;
    }
    else
    {
        status = CYRET_BAD_PARAM;
    }

    return status;
}

uint16_t
rfm12_trans(uint16_t val)
{
  *rfm12_modul->rfm12_port &= (uint8_t) ~rfm12_modul->rfm12_mask;

  /* spi clock down */
#ifdef CONF_RFM12_SLOW_SPI
  _SPCR0 |= (uint8_t) _BV(SPR1);
#else
  _SPCR0 |= (uint8_t) _BV(SPR0);
#endif

  uint16_t retval = (uint16_t) (spi_send(HI8(val)) << 8);
  retval += spi_send(LO8(val));

  /* spi clock high */
#ifdef CONF_RFM12_SLOW_SPI
  _SPCR0 &= (uint8_t) ~ _BV(SPR1);
#else
  _SPCR0 &= (uint8_t) ~ _BV(SPR0);
#endif

  *rfm12_modul->rfm12_port |= rfm12_modul->rfm12_mask;
  return retval;
}

// Low-Level sending of message to bus
uint8_t
bsbport_send(uint8_t * const msg)
{

  msg[SOT] = SOT_BYTE;
  msg[SRC] = 0x80 | BSBPORT_OWNADDRESS;

  {
    uint16_t crc = bsbport_crc(msg, msg[LEN] - 2);
    msg[msg[LEN] - 2] = HI8(crc);
    msg[msg[LEN] - 1] = LO8(crc);
  }

  BSBPORT_DEBUG("Send MSG: %02x%02x%02x%02x%02x%02x%02x%02x%02x \n", msg[SOT],
               msg[SRC], msg[DEST], msg[LEN], msg[TYPE], msg[P1], msg[P2],
               msg[P3], msg[P4]);
  BSBPORT_DEBUG("Data:");
  for (uint8_t i = DATA; i < msg[LEN] - 2; i++)
  {
    BSBPORT_DEBUG("%02x", msg[i]);
  }
  BSBPORT_DEBUG("CRC: %02x%02x \n", msg[msg[LEN] - 2], msg[msg[LEN] - 1]);

  return bsbport_txstart(msg, msg[LEN]);
}

void setupType0Adv()
{
    advPacket0 *ap;
    
    ap = (advPacket0 *)&cyBle_discoveryModeInfo.advData->advData[ADVINDEX];
    
    // packet type + LSM9Setting
  
    
    ap->setup = ADVPACKET0 | (LSM9DS0GetSetting()<<2);
    
    // fix this
    ap->tb0 =LO8(systime);
    ap->tb1 =HI8(LO16(systime));
    ap->tb2 =LO8(HI16(systime));
    
    
    // position // if it is a uint16 it causes some unhandled exception
    uint16 val= QD_ReadCounter();
     
    
    ap->position = val;
       
    
    // acceleration x,y,z
    memcpy(&ap->accel , LSM9DS0GetAccel(), 6); // sizeof(LSM9DS0DATA));
    
    // gyro x,y,z
    memcpy(&ap->gyro , LSM9DS0GetGyro(),6 ); // sizeof(LSM9DS0DATA));
    
    // mag x,y,z
    memcpy(&ap->mag , LSM9DS0GetMag(),6); //sizeof(LSM9DS0DATA));
             
    CyBle_GapUpdateAdvData(cyBle_discoveryModeInfo.advData, cyBle_discoveryModeInfo.scanRspData);
}

/* ----------------------------------------------------------------------------
 * send an ArtDmx packet
 */
void
artnet_sendDmxPacket(void)
{
  static unsigned char sequence = 1;
  /* prepare artnet Dmx packet */
  struct artnet_dmx *msg =
    (struct artnet_dmx *) &uip_buf[UIP_LLH_LEN + UIP_IPUDPH_LEN];
  memset(msg, 0, sizeof(struct artnet_dmx));

  strncpy_P((char *) msg->id, artnet_ID, 8);

  msg->opcode = OP_OUTPUT;

  msg->versionH = 0;
  msg->version = PROTOCOL_VERSION;

  msg->sequence = sequence++;
  if (sequence == 0)
    sequence = 1;

  msg->physical = 1;
  msg->universe = ((artnet_subNet << 4) | artnet_inputUniverse);
  msg->lengthHi = HI8(DMX_STORAGE_CHANNELS);
  msg->length = LO8(DMX_STORAGE_CHANNELS);
  for (uint8_t i = 0; i < DMX_STORAGE_CHANNELS; i++)
    msg->dataStart[i] =
      get_dmx_channel_slot(artnet_inputUniverse, i, artnet_conn_id);
  /* send packet to artnet_outputTarget */
  artnet_send(&artnet_outputTarget, sizeof(struct artnet_dmx) + DMX_STORAGE_CHANNELS);
}

/*******************************************************************************
* Function Name: CySpcReadMultiByte
********************************************************************************
* Summary:
*  Returns 1 to 256 bytes from user space of Flash/EEPROM. Doesn't span row
*  boundaries.
*
* Parameters:
*  uint8 array:
*   Id of the array.
*
*  uint8 ecc:
*   0x80 if reading ecc data.
*   0x00 if user data.
*
*  uint16 address:
*   Flash addrress.
*
*  uint8 size:
*   Number bytes to load.
*
* Return:
*  CYRET_STARTED
*  CYRET_CANCELED
*  CYRET_LOCKED
*
*******************************************************************************/
cystatus CySpcReadMultiByte(uint8 array, uint8 ecc, uint16 address, uint8 size)
{
    cystatus status = CYRET_STARTED;

    /* Make sure the SPC is ready to accept command */
    if(CY_SPC_IDLE)
    {
        CY_SPC_CPU_DATA_REG = CY_SPC_KEY_ONE;
        CY_SPC_CPU_DATA_REG = CY_SPC_KEY_TWO(CY_SPC_CMD_RD_MULTI_BYTE);
        CY_SPC_CPU_DATA_REG = CY_SPC_CMD_RD_MULTI_BYTE;

        /* Make sure the command was accepted */
        if(CY_SPC_BUSY)
        {
            CY_SPC_CPU_DATA_REG = array;
            CY_SPC_CPU_DATA_REG = ecc;
            CY_SPC_CPU_DATA_REG = HI8(address);
            CY_SPC_CPU_DATA_REG = LO8(address);
            CY_SPC_CPU_DATA_REG = size - 1;
        }
        else
        {
            status = CYRET_CANCELED;
        }
    }
    else
    {
        status = CYRET_LOCKED;
    }

    return(status);
}

int16_t
parse_cmd_adc_get(char *cmd, char *output, uint16_t len)
{
  uint16_t adc;
  uint8_t channel;
  uint8_t ret = 0;
  if (cmd[0] && cmd[1])
  {
    channel = cmd[1] - '0';
    if (channel < ADC_CHANNELS)
    {
      adc = adc_get(channel);
      channel = ADC_CHANNELS;
      goto adc_out;
    }
    else
      return ECMD_ERR_PARSE_ERROR;
  }
  for (channel = 0; channel < ADC_CHANNELS; channel++)
  {
    adc = adc_get(channel);
  adc_out:
    output[0] = NIBBLE_TO_HEX(LO4(HI8(adc)));
    output[1] = NIBBLE_TO_HEX(HI4(LO8(adc)));
    output[2] = NIBBLE_TO_HEX(LO4(adc));
    output[3] = ' ';
    output[4] = 0;
    ret += 4;
    output += 4;
  }
  return ECMD_FINAL(ret);
}

/*******************************************************************************
* Function Name: CySpcWriteRow
********************************************************************************
* Summary:
*  Erases then programs a row in Flash/EEPROM with data in row latch.
*
* Parameters:
*  uint8 array:
*   Id of the array.
*
*  uint16 address:
*   flash/eeprom addrress
*
*  uint8 tempPolarity:
*   temperature polarity.
*   1: the Temp Magnitude is interpreted as a positive value
*   0: the Temp Magnitude is interpreted as a negative value
*
*  uint8 tempMagnitude:
*   temperature magnitude.
*
* Return:
*  CYRET_STARTED
*  CYRET_CANCELED
*  CYRET_LOCKED
*
*******************************************************************************/
cystatus CySpcWriteRow(uint8 array, uint16 address, uint8 tempPolarity, uint8 tempMagnitude)
{
    cystatus status = CYRET_STARTED;

    /* Make sure the SPC is ready to accept command */
    if(CY_SPC_IDLE)
    {
        CY_SPC_CPU_DATA_REG = CY_SPC_KEY_ONE;
        CY_SPC_CPU_DATA_REG = CY_SPC_KEY_TWO(CY_SPC_CMD_WR_ROW);
        CY_SPC_CPU_DATA_REG = CY_SPC_CMD_WR_ROW;

        /* Make sure the command was accepted */
        if(CY_SPC_BUSY)
        {
            CY_SPC_CPU_DATA_REG = array;
            CY_SPC_CPU_DATA_REG = HI8(address);
            CY_SPC_CPU_DATA_REG = LO8(address);
            CY_SPC_CPU_DATA_REG = tempPolarity;
            CY_SPC_CPU_DATA_REG = tempMagnitude;
        }
        else
        {
            status = CYRET_CANCELED;
        }
    }
    else
    {
        status = CYRET_LOCKED;
    }

    return(status);
}

/*******************************************************************************
* Function Name: CySpcCreateCmdGetTemp
********************************************************************************
* Summary:
*   Sets up the command to GetTemp.
*   
*
* Parameters:
* numSamples:
*
*
* timerPeriod:
*
*
* clkDivSelect:
*
*   
* Return:
*   CYRET_SUCCESS if the command was created sucessfuly.
*   CYRET_BAD_PARAM if an invalid parameter was passed.
*
* Theory:
*
*
*******************************************************************************/
cystatus CySpcCreateCmdGetTemp(uint8 numSamples, uint16 timerPeriod, uint8 clkDivSelect)
{
    cystatus status;


    /* Check parameters. */
    if(numSamples)
    {
        /* Create packet command. */
        cyCommand[0] = SPC_KEY_ONE;
        cyCommand[1] = SPC_KEY_TWO(SPC_CMD_GET_TEMP);
        cyCommand[2] = SPC_CMD_GET_TEMP;
    
        /* Create packet parameters. */
        cyCommand[3] = numSamples;
        cyCommand[4] = HI8(timerPeriod);
        cyCommand[5] = LO8(timerPeriod);
        cyCommand[6] = clkDivSelect;

        cyCommandSize = SIZEOF_CMD_GET_TEMP;

        status = CYRET_SUCCESS;
    }
    else
    {
        status = CYRET_BAD_PARAM;
    }

    return status;
}

/********************************************************************
 * Set cursor at the cursor_offset directory through VGA-ports.
 *
 * In  : cursor_offset    = absolute offset of cursor
 *
 * Out : -1 on error, 1 on OK
 */
int con_set_vga_cursor (int cursor_offset) {
  // Set VGA Cursor through vga io-ports
  outb (0x3d4, 0x0E);
  outb (0x3d5, HI8(cursor_offset));
  outb (0x3d4, 0x0F);
  outb (0x3d5, LO8(cursor_offset));

  return ERR_OK;
}

uint8_t
encode_short(uint8_t * ptr, uint8_t type, uint16_t val)
{
  ptr[0] = type;
  ptr[1] = 2;
  ptr[2] = HI8(val);
  ptr[3] = LO8(val);
  return 4;
}

int16_t lome6_get_temperature (struct ow_rom_code_t *rom) {

	int16_t retval = 0x7FFF;  // error

	// disable interrupts
	uint8_t sreg = SREG;
	cli();

	struct ow_temp_scratchpad_t sp;
	if (ow_temp_read_scratchpad(rom, &sp) != 1)
		goto out;  // scratchpad read failed

	uint16_t temp = ow_temp_normalize(rom, &sp);
	retval = ((int8_t) HI8(temp)) * 10 + HI8(((temp & 0x00ff) * 10) + 0x80);

out:
	sei();
	SREG = sreg;
	return retval;
	
}

 /*******************************************************************************
 * Function Name: SPI_PutWordInRxBuffer
 ********************************************************************************
 *
 * Summary:
 *  Stores byte/word into the RX buffer.
 *  Only available in Unconfigured operation mode.
 *
 * Parameters:
 *  index:      index to store data byte/word in the RX buffer.
 *  rxDataByte: byte/word to store.
 *
 * Return:
 *  None
 *
 *******************************************************************************/
 void SPI_PutWordInRxBuffer(uint32 idx, uint32 rxDataByte)
 {
     /* Put data in the buffer */
     if(SPI_ONE_BYTE_WIDTH == SPI_rxDataBits)
     {
         SPI_rxBuffer[idx] = ((uint8) rxDataByte);
     }
     else
     {
         SPI_rxBuffer[(uint32)(idx << 1u)]      = LO8(LO16(rxDataByte));
         SPI_rxBuffer[(uint32)(idx << 1u) + 1u] = HI8(LO16(rxDataByte));
     }
 }

 /*******************************************************************************
 * Function Name: ModbusComms_PutWordInTxBuffer
 ********************************************************************************
 *
 * Summary:
 *  Stores byte/word into the TX buffer.
 * Only available in Unconfigured operation mode.
 *
 * Parameters:
 *  idx:        index to store data byte/word in the TX buffer.
 *  txDataByte: byte/word to store.
 *
 * Return:
 *  None
 *
 *******************************************************************************/
 void ModbusComms_PutWordInTxBuffer(uint32 idx, uint32 txDataByte)
 {
     /* Put data in the buffer */
     if(ModbusComms_ONE_BYTE_WIDTH == ModbusComms_txDataBits)
     {
         ModbusComms_txBuffer[idx] = ((uint8) txDataByte);
     }
     else
     {
         ModbusComms_txBuffer[(uint32)(idx << 1u)]      = LO8(LO16(txDataByte));
         ModbusComms_txBuffer[(uint32)(idx << 1u) + 1u] = HI8(LO16(txDataByte));
     }
 }

 /*******************************************************************************
 * Function Name: ESP_UART_PutWordInRxBuffer
 ********************************************************************************
 *
 * Summary:
 *  Stores a byte/word into the RX buffer.
 *  Only available in the Unconfigured operation mode.
 *
 * Parameters:
 *  index:      index to store data byte/word in the RX buffer.
 *  rxDataByte: byte/word to store.
 *
 * Return:
 *  None
 *
 *******************************************************************************/
 void ESP_UART_PutWordInRxBuffer(uint32 idx, uint32 rxDataByte)
 {
     /* Put data in buffer */
     if (ESP_UART_ONE_BYTE_WIDTH == ESP_UART_rxDataBits)
     {
         ESP_UART_rxBuffer[idx] = ((uint8) rxDataByte);
     }
     else
     {
         ESP_UART_rxBuffer[(uint32)(idx << 1u)]      = LO8(LO16(rxDataByte));
         ESP_UART_rxBuffer[(uint32)(idx << 1u) + 1u] = HI8(LO16(rxDataByte));
     }
 }

 /*******************************************************************************
 * Function Name: SPIS_1_PutWordInTxBuffer
 ********************************************************************************
 *
 * Summary:
 *  Stores byte/word into the TX buffer.
 *  Only available in the Unconfigured operation mode.
 *
 * Parameters:
 *  idx:        index to store data byte/word in the TX buffer.
 *  txDataByte: byte/word to store.
 *
 * Return:
 *  None
 *
 *******************************************************************************/
 void SPIS_1_PutWordInTxBuffer(uint32 idx, uint32 txDataByte)
 {
     /* Put data in buffer */
     if(SPIS_1_ONE_BYTE_WIDTH == SPIS_1_txDataBits)
     {
         SPIS_1_txBuffer[idx] = ((uint8) txDataByte);
     }
     else
     {
         SPIS_1_txBuffer[(uint32)(idx << 1u)]      = LO8(LO16(txDataByte));
         SPIS_1_txBuffer[(uint32)(idx << 1u) + 1u] = HI8(LO16(txDataByte));
     }
 }

 /*******************************************************************************
 * Function Name: UART_PutWordInTxBuffer
 ********************************************************************************
 *
 * Summary:
 *  Stores byte/word into the TX buffer.
 * Only available in Unconfigured operation mode.
 *
 * Parameters:
 *  idx:        index to store data byte/word in the TX buffer.
 *  txDataByte: byte/word to store.
 *
 * Return:
 *  None
 *
 *******************************************************************************/
 void UART_PutWordInTxBuffer(uint32 idx, uint32 txDataByte)
 {
     /* Put data in the buffer */
     if(UART_ONE_BYTE_WIDTH == UART_txDataBits)
     {
         UART_txBuffer[idx] = ((uint8) txDataByte);
     }
     else
     {
         UART_txBuffer[(uint32)(idx << 1u)]      = LO8(LO16(txDataByte));
         UART_txBuffer[(uint32)(idx << 1u) + 1u] = HI8(LO16(txDataByte));
     }
 }