Q_parser.c

 
#include <stdio.h>
#include <string.h>
#include "cmsis_os.h"
#include "Q_parser.h"
 
 
/***============================================================================================================================
 * Private functions definitions
 * These functions are only accessible from within the file's scope
 *=============================================================================================================================*/
 
static uint8_t _Q_parser_check_validity(Q_packet_s *packet){
    if(strncmp(packet->raw_data, Q_PACKETID_STRING, Q_PACKETID_BYTELENGTH) == 0){
        if(packet->opcode >= Q_CMD_TRIGGER_START && packet->opcode <= Q_CMD_REPLY_DIAGNOSTIC){
            return 1;
        }
    }
    return 0;
}
 
 
/***============================================================================================================================
 * Public functions definitions
 * These functions can be accessed outside of the file's scope
 * @see Q_parser.h for declarations
 *=============================================================================================================================*/
 
Q_packet_s *Q_parser_parse(void *data, uint8_t len){
 
    Q_packet_s *packet = pvPortMalloc(sizeof(Q_packet_s));
 
    packet->raw_data = pvPortMalloc(sizeof(char) * len);
    memcpy(packet->raw_data, data, len);
 
    packet->opcode  = packet->raw_data[Q_OPCODE_INDEX];
 
    if(_Q_parser_check_validity(packet)){
        packet->payload  = pvPortMalloc(sizeof(char) * (len - Q_PAYLOAD_INDEX));
        memcpy(packet->payload, packet->raw_data + Q_PAYLOAD_INDEX, len - Q_PAYLOAD_INDEX);
        return packet;
    }else{
        vPortFree(packet->raw_data);
        vPortFree(packet);
    }
 
    return NULL;
 
}
 
void Q_parser_free(Q_packet_s *packet){
    if(packet != NULL){
        vPortFree(packet->raw_data);
        vPortFree(packet->payload);
        vPortFree(packet);
    }
}
 
char *Q_parser_forge_discover_reply(uint8_t node_id){
    char *raw_data = pvPortMalloc(sizeof(uint8_t) * Q_DISCOVERREPLY_BYTELENGTH);
    sprintf(raw_data, "%s%c%c", Q_PACKETID_STRING, Q_CMD_REPLY_DISCOVER, node_id);
    return raw_data;
}
 
char *Q_parser_forge_poll_reply(uint8_t node_id){
    char *raw_data = pvPortMalloc(sizeof(uint8_t) * Q_POLLREPLY_BYTELENGTH);
    sprintf(raw_data, "%s%c%c", Q_PACKETID_STRING, Q_CMD_REPLY_POLL, node_id);
    return raw_data;
}