int kowhai_write(struct kowhai_tree_t *tree, int num_symbols, union kowhai_symbol_t* symbols, int write_offset, void* value, int write_size)
{
struct kowhai_node_t* node;
int offset;
int status;
int size;
// find this node
status = kowhai_get_node(tree->desc, num_symbols, symbols, &offset, &node);
if (status != KOW_STATUS_OK)
return status;
if (write_offset < 0)
return KOW_STATUS_INVALID_OFFSET;
// check the write wont overrun the item
status = kowhai_get_node_size(node, &size);
if (status != KOW_STATUS_OK)
return status;
if (write_size + write_offset > size)
return KOW_STATUS_NODE_DATA_TOO_SMALL;
// do write
memcpy((char*)tree->data + offset + write_offset, value, write_size);
return status;
}
int kowhai_read(struct kowhai_tree_t *tree, int num_symbols, union kowhai_symbol_t* symbols, int read_offset, void* result, int read_size)
{
struct kowhai_node_t* node;
int offset;
int status;
int size;
// find this node
status = kowhai_get_node(tree->desc, num_symbols, symbols, &offset, &node);
if (status != KOW_STATUS_OK)
return status;
if (read_offset < 0)
return KOW_STATUS_INVALID_OFFSET;
// check the read wont overrun the item
status = kowhai_get_node_size(node, &size);
if (status != KOW_STATUS_OK)
return status;
if (read_size + read_offset > size)
return KOW_STATUS_NODE_DATA_TOO_SMALL;
// do read
memcpy(result, (char*)tree->data + offset + read_offset, read_size);
return status;
}
int kowhai_get_float(struct kowhai_tree_t *tree, int num_symbols, union kowhai_symbol_t* symbols, float* result)
{
struct kowhai_node_t* node;
int offset;
int status;
status = kowhai_get_node(tree->desc, num_symbols, symbols, &offset, &node);
if (status != KOW_STATUS_OK)
return status;
if (node->type == KOW_FLOAT)
{
*result = *((float*)((char*)tree->data + offset));
return status;
}
return KOW_STATUS_INVALID_NODE_TYPE;
}
int kowhai_set_int32(struct kowhai_tree_t *tree, int num_symbols, union kowhai_symbol_t* symbols, int32_t value)
{
struct kowhai_node_t* node;
int offset;
int status;
status = kowhai_get_node(tree->desc, num_symbols, symbols, &offset, &node);
if (status != KOW_STATUS_OK)
return status;
if (node->type == KOW_INT32 || node->type == KOW_UINT32)
{
uint32_t* target_address = (uint32_t*)((char*)tree->data + offset);
*target_address = value;
return status;
}
return KOW_STATUS_INVALID_NODE_TYPE;
}
static int path_to_str(union kowhai_symbol_t *path, int path_len, char *dst, int dst_len, const char *separator, int hide_last_index, struct kowhai_node_t *root, void* get_name_param, kowhai_get_symbol_name_t get_name)
{
int i, r, e;
int count = 0;
for (i = 0; i < path_len; i++)
{
struct kowhai_node_t *tnode;
// if a root is given we can use this to hide ugly [0] on each node since we can look-up the
// node count for each path item using kowhai_get_node(), then we only add the index if it
// is not 0
if (root)
if ((e = kowhai_get_node(root, i+1, path, 0, &tnode)) != KOW_STATUS_OK)
return -1;
// display the index at the end of this node if we can know the count of this node (needs root so we can find this node from
// the path), and if the count > 1 ie is an array, and if this is the last item and hide_last_index == true
if (root == NULL || tnode->count == 1 || ((i == (path_len - 1)) && hide_last_index))
r = snprintf(dst, dst_len, "%s", get_name(get_name_param, path[i].parts.name));
else
r = snprintf(dst, dst_len, "%s[%d]", get_name(get_name_param, path[i].parts.name), path[i].parts.array_index);
if (r < 0)
return r; // real error
if (r > dst_len)
return count + r; // dst not long enough
dst += r;
count += r;
dst_len -= r;
// put separator unless this is the last item
if (i != path_len - 1)
{
r = snprintf(dst, dst_len, "%s", separator);
if (r < 0)
return r; // real error
if (r > dst_len)
return count + r; // dst not long enough
dst += r;
count += r;
dst_len -= r;
}
}
return count;
}
int kowhai_server_process_packet(struct kowhai_protocol_server_t* server, void* packet, size_t packet_size)
{
struct kowhai_protocol_t prot;
int bytes_required, status;
if (packet_size > server->max_packet_size)
{
printf(" error: packet size too large\n");
return KOW_STATUS_PACKET_BUFFER_TOO_BIG;
}
status = kowhai_protocol_parse(packet, packet_size, &prot);
if (status != KOW_STATUS_OK && status != KOW_STATUS_INVALID_PROTOCOL_COMMAND)
return status;
if (prot.header.tree_id >= 0 && prot.header.tree_id < server->tree_count)
{
struct kowhai_tree_t tree;
tree.desc = *(server->tree_descriptors + prot.header.tree_id);
tree.data = *(server->tree_data_buffers + prot.header.tree_id);
switch (prot.header.command)
{
case KOW_CMD_WRITE_DATA:
printf(" CMD write data\n");
status = kowhai_write(&tree, prot.payload.spec.data.symbols.count, prot.payload.spec.data.symbols.array_, prot.payload.spec.data.memory.offset, prot.payload.buffer, prot.payload.spec.data.memory.size);
if (status == KOW_STATUS_OK)
{
// call node_written callback
struct kowhai_node_t* node;
uint16_t offset;
kowhai_get_node(tree.desc, prot.payload.spec.data.symbols.count, prot.payload.spec.data.symbols.array_, &offset, &node);
server->node_written(server->node_written_param, node);
// send response
prot.header.command = KOW_CMD_WRITE_DATA_ACK;
kowhai_read(&tree, prot.payload.spec.data.symbols.count, prot.payload.spec.data.symbols.array_, prot.payload.spec.data.memory.offset, prot.payload.buffer, prot.payload.spec.data.memory.size);
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
}
else
{
switch (status)
{
case KOW_STATUS_INVALID_SYMBOL_PATH:
printf(" invalid symbol path\n");
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_INVALID_SYMBOL_PATH);
break;
case KOW_STATUS_INVALID_OFFSET:
printf(" invalid payload offset\n");
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_INVALID_PAYLOAD_OFFSET);
break;
case KOW_STATUS_NODE_DATA_TOO_SMALL:
printf(" invalid payload size\n");
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_INVALID_PAYLOAD_SIZE);
break;
default:
printf(" unkown error\n");
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_UNKNOWN);
break;
}
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
}
break;
case KOW_CMD_READ_DATA:
{
uint16_t node_offset;
int size, overhead, max_payload_size;
struct kowhai_node_t* node;
struct kowhai_protocol_symbol_spec_t symbols = prot.payload.spec.data.symbols;
printf(" CMD read data\n");
// get node information
status = kowhai_get_node(tree.desc, prot.payload.spec.data.symbols.count, prot.payload.spec.data.symbols.array_, &node_offset, &node);
if (status == KOW_STATUS_OK)
{
union kowhai_symbol_t last_sym = symbols.array_[symbols.count-1];
kowhai_get_node_size(node, &size);
if (node->count > 1)
size = size - size / node->count * last_sym.parts.array_index;
// get protocol overhead
prot.header.command = KOW_CMD_READ_DATA_ACK;
kowhai_protocol_get_overhead(&prot, &overhead);
// setup max payload size and payload offset
max_payload_size = server->max_packet_size - overhead;
prot.payload.spec.data.memory.offset = 0;
prot.payload.spec.data.memory.type = node->type;
// allocate payload buffer
prot.payload.buffer = malloc(server->max_packet_size - overhead);
// send packets
while (size > max_payload_size)
{
prot.payload.spec.data.memory.size = max_payload_size;
kowhai_read(&tree, prot.payload.spec.data.symbols.count, prot.payload.spec.data.symbols.array_, prot.payload.spec.data.memory.offset, prot.payload.buffer, prot.payload.spec.data.memory.size);
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
// increment payload offset and decrement remaining payload size
prot.payload.spec.data.memory.offset += max_payload_size;
size -= max_payload_size;
}
// send final packet
prot.header.command = KOW_CMD_READ_DATA_ACK_END;
prot.payload.spec.data.memory.size = size;
kowhai_read(&tree, prot.payload.spec.data.symbols.count, prot.payload.spec.data.symbols.array_, prot.payload.spec.data.memory.offset, prot.payload.buffer, prot.payload.spec.data.memory.size);
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
// free payload buffer
free(prot.payload.buffer);
}
else
{
switch (status)
{
case KOW_STATUS_INVALID_SYMBOL_PATH:
printf(" invalid symbol path\n");
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_INVALID_SYMBOL_PATH);
break;
case KOW_STATUS_INVALID_OFFSET:
printf(" invalid payload offset\n");
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_INVALID_PAYLOAD_OFFSET);
break;
case KOW_STATUS_NODE_DATA_TOO_SMALL:
printf(" invalid payload size\n");
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_INVALID_PAYLOAD_SIZE);
break;
default:
printf(" unkown error\n");
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_UNKNOWN);
break;
}
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
}
break;
}
case KOW_CMD_READ_DESCRIPTOR:
{
int size, overhead, max_payload_size;
printf(" CMD read descriptor\n");
// get descriptor size
size = *(server->tree_descriptor_sizes + prot.header.tree_id);
// get protocol overhead
prot.header.command = KOW_CMD_READ_DESCRIPTOR_ACK;
kowhai_protocol_get_overhead(&prot, &overhead);
// setup max payload size and payload offset
max_payload_size = server->max_packet_size - overhead;
prot.payload.spec.descriptor.offset = 0;
prot.payload.spec.descriptor.node_count = size / sizeof(struct kowhai_node_t);
// allocate payload buffer
prot.payload.buffer = malloc(server->max_packet_size - overhead);
// send packets
while (size > max_payload_size)
{
prot.payload.spec.descriptor.size = max_payload_size;
memcpy(prot.payload.buffer, (char*)tree.desc + prot.payload.spec.descriptor.offset, prot.payload.spec.descriptor.size);
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
// increment payload offset and decrement remaining payload size
prot.payload.spec.descriptor.offset += max_payload_size;
size -= max_payload_size;
}
// send final packet
prot.header.command = KOW_CMD_READ_DESCRIPTOR_ACK_END;
prot.payload.spec.descriptor.size = size;
memcpy(prot.payload.buffer, (char*)tree.desc + prot.payload.spec.descriptor.offset, prot.payload.spec.descriptor.size);
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
// free payload buffer
free(prot.payload.buffer);
break;
}
default:
printf(" invalid command (%d)\n", prot.header.command);
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_INVALID_COMMAND);
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
break;
}
}
else
{
printf(" invalid tree id (%d)\n", prot.header.tree_id);
POPULATE_PROTOCOL_CMD(prot, prot.header.tree_id, KOW_CMD_ERROR_INVALID_TREE_ID);
kowhai_protocol_create(server->packet_buffer, server->max_packet_size, &prot, &bytes_required);
server->send_packet(server->send_packet_param, server->packet_buffer, bytes_required);
}
return KOW_STATUS_OK;
}
