androsensor.c

#include "heivs/config.h"
#if (USE_STM32_USB_HOST_MODE || USE_STM32_USB_USE_DEVICE_MODE || USE_STM32_USB_OTG_MODE)
#include "config/usb_conf.h"
#if defined(USE_HOST_MODE)

#include "heivs/androsensor/androsensor.h"
#include "heivs/androsensor/usbh_adk_core.h"
#include <string.h>

static const uint32_t USE_KEEPALIVE = 0;
#define ANDROSENSOR_KEEPALIVE_TIME_MS (500)

static const uint8_t ANDROSENSOR_SOF[] = {0xff,'a','s',0xff,0x00,0x00,0x00,0x01};

enum androsensor_msg_type
{
    MSG_TYPE_KEEPALIVE  = 0x33,
    MSG_TYPE_DATA       = 0x01,
};

struct androsensor_msg_t
{
    uint8_t     sof[sizeof(ANDROSENSOR_SOF)];
    uint8_t     len[sizeof(uint32_t)];
    uint8_t     type[sizeof(uint32_t)];
    uint8_t     data[];
};

static void encode_32(uint8_t *data, uint32_t value)
{
    *data++ = value >> 24;
    *data++ = value >> 16;
    *data++ = value >> 8;
    *data++ = value >> 0;
}

static uint32_t decode_32(uint8_t *data)
{
    uint32_t value = 0;
    value |= *data++;
    value <<= 8;
    value |= *data++;
    value <<= 8;
    value |= *data++;
    value <<= 8;
    value |= *data++;

    return value;
}

static uint32_t buffer[(128+sizeof(struct androsensor_msg_t)+sizeof(uint32_t)-1) / sizeof(uint32_t)];

static status_e msg_send(struct androsensor_t *as, enum androsensor_msg_type type, const uint8_t *data, uint32_t len)
{
    struct androsensor_msg_t *msg  = (struct androsensor_msg_t *)buffer;

    if (!androsensor_active(as))
    {
        return ERROR_BAD_STATE;
    }

    if (len + sizeof(struct androsensor_msg_t) > sizeof(buffer))
    {
        return ERROR_BAD_PARAM;
    }

    memcpy(msg->sof, ANDROSENSOR_SOF, sizeof(ANDROSENSOR_SOF));
    encode_32(msg->len, len);
    encode_32(msg->type, type);
    memcpy(msg->data, data, len);

    USBH_ADK_write(&USB_OTG_Core, (uint8_t*)msg, sizeof(*msg) + len);

    return NO_ERROR;
}

void androsensor_process(struct androsensor_t *as)
{
    status_e status;
    const uint8_t keepalive_msg[] = {0x00,0x00};
    struct androsensor_msg_t *msg = (struct androsensor_msg_t *)&as->inbuff;

    USBH_Process(&USB_OTG_Core , &USB_Host);

    if (USE_KEEPALIVE && time_elapsed(as->keepalive_out_timeout))
    {
        as->keepalive_out_timeout = time_set_timeout_ms(ANDROSENSOR_KEEPALIVE_TIME_MS);
        msg_send(as, MSG_TYPE_KEEPALIVE, keepalive_msg, sizeof(keepalive_msg));
    }

    if (USE_KEEPALIVE && time_elapsed(as->keepalive_in_timeout))
    {
        as->keepalive_in_timeout = time_set_timeout_ms(0);
        as->keepalive_in_timeout = time_set_timeout_ms(ANDROSENSOR_KEEPALIVE_TIME_MS);
        if (as->keepalivefatal)
        {
            as->state = STATE_UNKNOWN;
        }
    }

    if (!USBH_ADK_Connected())
    {
        return;
    }

    status = USBH_ADK_read(&USB_OTG_Core, (uint8_t *)as->inbuff, sizeof(as->inbuff), &as->insize);
    if (status != NO_ERROR)
    {
        return ;
    }

    if (!as->insize)
    {
        return;
    }

    if (memcmp(msg->sof, ANDROSENSOR_SOF, sizeof(msg->sof)) )
    {
        as->insize = 0;
        return ;
    }

    if (decode_32(msg->type) == MSG_TYPE_KEEPALIVE)
    {
        as->keepalive_in_timeout = time_set_timeout_ms(ANDROSENSOR_KEEPALIVE_TIME_MS);
        as->state = STATE_CONNECTED;

        return;
    }

    as->insize = decode_32(msg->len);
    if (decode_32(msg->len) > as->insize)
    {
        as->insize = 0;
        return;
    }

    memcpy(as->inbuff, msg->data, as->insize);
}

status_e androsensor_init_master(struct androsensor_t *as, const char* manufacture, const char* model, const char* description, const char* version, const char* uri, const char* serial)
{
    as->keepalive_out_timeout = time_set_timeout_ms(ANDROSENSOR_KEEPALIVE_TIME_MS);
    as->keepalive_in_timeout = time_set_timeout_ms(ANDROSENSOR_KEEPALIVE_TIME_MS);
    as->insize = 0;
    as->keepalivefatal = 0;

    USBH_ADK_Init
    (
        manufacture,
        model,
        description,
        version,
        uri,
        serial
    );

    return NO_ERROR;
}

uint32_t androsensor_active(struct androsensor_t *as)
{
    if (!USBH_ADK_Connected())
    {
        as->state = STATE_UNKNOWN;
    }
    else
    {
        as->state = STATE_CONNECTED;
    }

    if (as->state != STATE_CONNECTED)
    {
        return 0;
    }

    return 1;
}

status_e androsensor_write(struct androsensor_t *as, const uint8_t *data, size_t len)
{
    return msg_send(as, MSG_TYPE_DATA, data, len);
}

status_e androsensor_read(struct androsensor_t *as, uint8_t *data, size_t len, size_t *rlen)
{
    *rlen = 0;
    if (as->insize)
    {
        memcpy(data, as->inbuff, len);
        // FIXME : remove the memset when debugged
        memset(as->inbuff, 0x00, as->insize);
        *rlen = as->insize;
        as->insize = 0;
    }

    return NO_ERROR;
}

#endif
#endif /* (USE_STM32_USB_HOST_MODE || USE_STM32_USB_USE_DEVICE_MODE || USE_STM32_USB_OTG_MODE) */