param.c

#include <sys/types.h>
#include <inttypes.h>

#include "param.h"
#include "commands.h"
#include "bitwise.h"

enum GetparamData {
	GP_BLOCK_ID_L,
	GP_BLOCK_ID_H,
	GP_PARAM_ID_L,
	GP_PARAM_ID_H,
	GP_DATA_SIZE
};

/* Creates data and metadata for a respcontrol packet
 * Returns data size.
 */
ssize_t EncodeGetParam(
        struct metadata * m,        /* data_len and msg_type will be populated*/
        uint8_t * data,             /* Output buffer */
        size_t data_size,           /* Max buffer size */
        const char * msg)       /* Message to encode */
{
	m->msg_type = GETPARAM_ID;
	m->data_len = GP_DATA_SIZE;

	if (data_size < GP_DATA_SIZE) {
		return -1;
	}

	int16_t block, param;
	sscanf(msg, "getparam %" SCNd16 " %" SCNd16, &block, &param);


	data[GP_BLOCK_ID_L] = LSByte16(block);
	data[GP_BLOCK_ID_H] = MSByte16(block);

	data[GP_PARAM_ID_L] = LSByte16(param);
	data[GP_PARAM_ID_H] = MSByte16(param);

	return GP_DATA_SIZE;
}

enum SetparamData {
	SP_BLOCK_ID_L,
	SP_BLOCK_ID_H,
	SP_PARAM_ID_L,
	SP_PARAM_ID_H,
	SP_VAL_1,
	SP_VAL_2,
	SP_VAL_3,
	SP_VAL_4,
	SP_DATA_SIZE
};

/* Creates data and metadata for a setcontrol packet
 * Returns data size.
 */
ssize_t EncodeSetParam(
        struct metadata * m,        /* data_len and msg_type will be populated*/
        uint8_t * data,             /* Output buffer */
        size_t data_size,           /* Max buffer size */
        const char * msg)       /* Message to encode */
{
	m->msg_type = SETPARAM_ID;
	m->data_len = SP_DATA_SIZE;

	if (data_size < SP_DATA_SIZE) {
		return -1;
	}

	int16_t block, param;
	float val;

	sscanf(msg, "setparam %" SCNd16 " %" SCNd16 " %f", &block, &param, &val);

	data[SP_BLOCK_ID_L] = LSByte16(block);
	data[SP_BLOCK_ID_H] = MSByte16(block);

	data[SP_PARAM_ID_L] = LSByte16(param);
	data[SP_PARAM_ID_H] = MSByte16(param);


	data[SP_VAL_1] = FloatByte1(val);
	data[SP_VAL_2] = FloatByte2(val);
	data[SP_VAL_3] = FloatByte3(val);
	data[SP_VAL_4] = FloatByte4(val);

	return SP_DATA_SIZE;
}



enum ResponseData {
	RESP_BLOCK_ID_L,
	RESP_BLOCK_ID_H,
	RESP_PARAM_ID_L,
	RESP_PARAM_ID_H,
	RESP_VAL_1,
	RESP_VAL_2,
	RESP_VAL_3,
	RESP_VAL_4,
	RESP_DATA_SIZE
};

/* Decode a metadata and data to populate a controller.
 * Returns bytes written to msg, -1 on failure.
 */
int DecodeResponseParam(
        char * msg,     			/* Decoded controller message */
		size_t max_len,				   /* msg buffer max size */
        const struct metadata * m,          /* Metadata to aid in decoding */
        const uint8_t * data)               /* Data to decode */
{
	if (m->data_len < RESP_DATA_SIZE) {
		return -1;
	}
	if (m->msg_type != RESPPARAM_ID) {
		return -1;
	}

	return snprintf(msg, max_len, "getparam %" PRId16 " %" PRId16 " %f\n", 
		BytesTo16(data[RESP_BLOCK_ID_L], data[RESP_BLOCK_ID_H]), 
		BytesTo16(data[RESP_PARAM_ID_L], data[RESP_PARAM_ID_H]), 
		BytesToFloat(data[RESP_VAL_1], data[RESP_VAL_2],
			data[RESP_VAL_3], data[RESP_VAL_4]));
}