UART driver compatible with Telemetry (http://bit.ly/1XtKWXS) for board NXP KL25Z using base code from NXP Cup

driver.hpp

int32_t write(void * buf, uint32_t sizeToWrite)
{
  uint8_t * buffer = (uint8_t *) buf;
  uint_32t i = 0;
  for(i = 0 ; i < sizeToWrite ; i++)
  {
    ByteEnqueue(&SDA_SERIAL_OUTGOING_QUEUE, (uint8_t)(buffer[i]));
  }
  return (int32_t)sizeToWrite;
}

int32_t writeable()
{
  return BytesInQueue(&SDA_SERIAL_OUTGOING_QUEUE) < SDA_SERIAL_OUTGOING_QUEUE_SIZE ;
}

int32_t read(void * buf, uint32_t sizeToRead)
{
  uint8_t * buffer = (uint8_t *)buf;
  uint32_t avail = BytesInQueue(&SDA_SERIAL_INCOMING_QUEUE);
  
  uint32_t effectiveSizeToRead = (sizeToRead <= avail) ? sizeToRead : avail;
  uint32_t i = 0;
  for(i = 0 ; i < effectiveSizeToRead ; i ++)
  {
    buffer[i] = (uint8_t)ForcedByteDequeue(&SDA_SERIAL_INCOMING_QUEUE);
  }
  return (int32_t)effectiveSizeToRead;
}

int32_t readable()
{
  return BytesInQueue(&SDA_SERIAL_INCOMING_QUEUE);
}

main.c

#include "Telemetry.h"
#include "driver.hpp"

int main(void)
{
  // Initialize telemetry
  TM_transport transport;
  transport.read = read;
  transport.write = write;
  transport.readable = readable;
  transport.writeable = writable;
  
  init_telemetry(&transport);
 
  while (1) {
      update_telemetry(0);
      publish("Hello","world!");
  }
}