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;
}
static int serialize_nodes(struct kowhai_node_t *root, char *dst, int dst_len, struct kowhai_node_t *src_node, void **src_data,
union kowhai_symbol_t *path, int ipath, int path_len, void* get_name_param, kowhai_get_symbol_name_t get_name,
int is_union, int level)
{
int r, i, n = 0;
int count = 0;
int node_size;
int node_count;
int max_union_size = 0;
int rmax = 0;
// print opening array brace only if we are level 0
if (level == 0)
STARTEND("[");
while (1)
{
struct kowhai_node_t *node = &src_node[n];
switch(node->type)
{
case KOW_BRANCH_U_START:
if (kowhai_get_node_size(node, &node_size) != KOW_STATUS_OK)
return -1;
// for each complex array type recurse into each array element
for (i = 0; i < node->count; i++)
{
// append this branch to the path
if (ipath >= path_len)
return -2;
path[ipath].symbol = KOWHAI_SYMBOL(node->symbol, i);
// recurse into branch
r = serialize_nodes(root, dst, dst_len, node + 1, src_data, path, ipath + 1, path_len, get_name_param, get_name, 1, level + 1);
if (r < 0)
return r;
if (r >= dst_len)
return count + r;
}
// union type over
dst += r;
count += r;
dst_len -= r;
*(char **)src_data += node_size;
// move past this branch in the descriptor
if (kowhai_get_node_count(node, &node_count) != KOW_STATUS_OK)
return -1;
node_count -= 1;
n += node_count;
// ending criteria
if (level == 0)
{
STARTEND("]");
return count;
}
break;
case KOW_BRANCH_START:
if (kowhai_get_node_size(node, &node_size) != KOW_STATUS_OK)
return -1;
// for each complex array type recurse into each array element
if ((is_union && (node_size >= max_union_size)) || !is_union)
{
for (i = 0; i < node->count; i++)
{
// append this branch to the path
if (ipath >= path_len)
return -2;
path[ipath].symbol = KOWHAI_SYMBOL(node->symbol, i);
// recurse into branch
r = serialize_nodes(root, dst, dst_len, node + 1, src_data, path, ipath + 1, path_len, get_name_param, get_name, 0, level + 1);
if (r < 0)
return r;
if (r >= dst_len)
return count + r;
rmax = r;
// push pointer on if this is not a union
// if it is we do this at the end for the
// biggest field
if (!is_union)
{
dst += r;
count += r;
dst_len -= r;
}
}
}
// move past this branch in the descriptor
if (kowhai_get_node_count(node, &node_count) != KOW_STATUS_OK)
return -1;
node_count -= 1;
n += node_count;
// ending criteria
if (level == 0)
{
STARTEND("]");
return count;
}
break;
case KOW_BRANCH_END:
if (is_union)
{
dst += rmax;
count += rmax;
dst_len -= rmax;
}
return count;
default:
// get this node size in bytes
node_size = kowhai_get_node_type_size(node->type) * node->count;
if (node_size < 0)
return -1;
// handle unions
if (is_union)
{
if (node_size <= max_union_size)
break;
max_union_size = node_size;
}
// update the path scratch buffer for this item
if (ipath >= path_len)
return -2;
path[ipath].symbol = KOWHAI_SYMBOL(node->symbol, 0);
// print this item
r = print_node_type(root, dst, dst_len, node, src_data, path, ipath, get_name_param, get_name);
if (r < 0)
return r;
if (r > dst_len)
return count + r;
rmax = r;
if (!is_union)
{
dst += r;
count += r;
dst_len -= r;
// move the data pointer
*(char **)src_data += node_size;
}
break;
}
n++;
}
}
/**
* @brief find a item in the tree given its path
* @param node to start searching from for the given item
* @param num_symbols number of items in the path (@todo should we just terminate the path instead)
* @param symbols the path of the item to seek
* @param offset set to number of bytes from the current node to the requested symbol
* @param target_node placeholder for the result of the node search
* @param num_nodes_processed how many nodes were iterated over during this function call
* @return < 0 on failure
* @todo find the correct index (always 0 atm)
*/
int get_node(const struct kowhai_node_t *node, int num_symbols, const union kowhai_symbol_t *symbols, int *offset, struct kowhai_node_t **target_node, int initial_branch, int branch_union)
{
int i = 0;
int _offset = 0;
int ret;
// look through all the items in the node list
while (1)
{
int skip_size;
int skip_nodes;
switch (node[i].type)
{
case KOW_BRANCH_END:
// if we got a branch end then we didn't find it on this path
return KOW_STATUS_INVALID_SYMBOL_PATH;
default:
// if the path symbols match and the node array count is large enough to contain our index this could be the target node
if ((symbols->parts.name == node[i].symbol) && (node[i].count > symbols->parts.array_index))
{
if (num_symbols == 1)
{
// the symbol paths fully match in values and length so this is the node we are looking for
ret = KOW_STATUS_OK;
if (target_node != NULL)
*target_node = (struct kowhai_node_t*)node + i;
goto done;
}
if (node[i].type == KOW_BRANCH_START || node[i].type == KOW_BRANCH_U_START)
{
int branch_offset = 0;
// this is not the target node but it is possibly in this branch so drill baby drill
ret = get_node(node + i + 1, num_symbols - 1, symbols + 1, &branch_offset, target_node, 0, node[i].type == KOW_BRANCH_U_START);
if (ret == KOW_STATUS_INVALID_SYMBOL_PATH)
// branch ended without finding our target node so goto next
break;
// add the branch offset to current total
_offset += branch_offset;
goto done;
}
}
break;
}
if (initial_branch)
return KOW_STATUS_INVALID_SYMBOL_PATH;
// this item is not a match so skip it (find out how many bytes and nodes to skip)
ret = get_node_size(node + i, &skip_size, &skip_nodes);
if (ret != KOW_STATUS_OK)
// propagate the error
return ret;
if (!branch_union)
_offset += skip_size;
i += skip_nodes + 1;
}
done:
// update offset return parameter
if (offset != NULL)
{
// we have the index of the target node
// the offset is: (the size of this node / its count) * symbol array index
int size;
ret = kowhai_get_node_size(node + i, &size);
if (ret != KOW_STATUS_OK)
return ret;
_offset += (size / node[i].count) * symbols->parts.array_index;
*offset = _offset;
}
return ret;
}
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;
}
