/*
* Process a program configuration message
*/
boolean ProgramManager::handle_msg(msg_program_t *msg) {
DEBUG4_VALUE("handle_msg: program=", msg->type);
DEBUG4_VALUELN(" output=", msg->hdr.output);
/* Find the program to be executed */
hmtl_program_t *program = lookup_function(msg->type);
if (program == NULL) {
DEBUG1_VALUELN("handle_msg: invalid type: ",
msg->type);
return false;
}
/* Setup the tracker */
if (msg->hdr.output > num_outputs) {
DEBUG1_VALUELN("handle_msg: invalid output: ",
msg->hdr.output);
return false;
}
if (outputs[msg->hdr.output] == NULL) {
DEBUG1_VALUELN("handle_msg: NULL output: ",
msg->hdr.output);
return false;
}
program_tracker_t *tracker = trackers[msg->hdr.output];
if (program->type == HMTL_PROGRAM_NONE) {
/* This is a message to clear the existing program so free the tracker */
free_tracker(msg->hdr.output);
return true;
}
if (tracker != NULL) {
DEBUG5_PRINTLN("handle_msg: reusing old tracker");
if (tracker->state) {
DEBUG5_PRINTLN("handle_msg: deleting old state");
free(tracker->state);
}
} else {
tracker = (program_tracker_t *)malloc(sizeof (program_tracker_t));
trackers[msg->hdr.output] = tracker;
}
tracker->program = program;
tracker->flags = 0x0;
tracker->program->setup(msg, tracker);
return true;
}
/*
* Return the next sensor structure from a sensor message
*/
msg_sensor_data_t* hmtl_next_sensor(msg_hdr_t *msg, msg_sensor_data_t *current) {
byte* next = NULL;
if (current) {
next = (byte *)current + sizeof (msg_sensor_data_t) + current->data_len;
if (next >= (byte *)msg + msg->length) {
next = NULL;
}
} else {
if (msg->length >= sizeof (msg_hdr_t) + sizeof (msg_sensor_data_t)) {
next = (byte *)(msg + 1);
}
}
DEBUG1_COMMAND(
if (msg->type != MSG_TYPE_SENSOR) {
DEBUG1_VALUELN("Not a sensor msg:", msg->type);
next = NULL;
}
if (next) {
if (next + sizeof (msg_sensor_data_t) + ((msg_sensor_data_t*)next)->data_len > (byte *)msg + msg->length) {
DEBUG1_PRINTLN("Invalid sensor message");
next = NULL;
}
}
);
void set_mode_to(uint8_t place, uint8_t mode) {
if (mode == MODE_NONE) {
set_mode(place, MODE_NONE);
return;
}
for (byte i = 0; i < VALID_MODES; i++) {
if (validModes[i] == mode) {
set_mode(place, i);
return;
}
}
DEBUG1_VALUELN("Attempted to set invalid mode:", mode);
}
/*
* Read in the HMTL config, returning the EEProm address following
* what was read.
*/
int hmtl_read_config(config_hdr_t *hdr, config_max_t outputs[],
int max_outputs)
{
int addr;
/* Zero the outputs */
for (int i = 0; i < max_outputs; i++) {
outputs[i].hdr.type = HMTL_OUTPUT_NONE;
}
addr = EEPROM_safe_read(HMTL_CONFIG_ADDR,
(uint8_t *)hdr, sizeof (config_hdr_t));
if (addr < 0) {
DEBUG_ERR("hmtl_read_config: error reading config from eeprom");
return -1;
}
if (hdr->magic != HMTL_CONFIG_MAGIC) {
DEBUG_ERR("hmtl_read_config: read config with invalid magic");
return -2;
}
if (hdr->protocol_version != HMTL_CONFIG_VERSION) {
DEBUG1_VALUELN("hmtl_read_config: hdr has wrong protocol version:", hdr->protocol_version);
return -3;
}
if ((hdr->num_outputs > 0) && (max_outputs != 0)) {
/* Read in the outputs if any were indicated and a buffer was provided */
if (max_outputs < hdr->num_outputs) {
DEBUG_ERR("hmtl_read_config: not enough outputs");
return -4;
}
for (int i = 0; i < hdr->num_outputs; i++) {
addr = EEPROM_safe_read(addr,
(uint8_t *)&outputs[i], sizeof (config_max_t));
if (addr <= 0) {
DEBUG_ERR("hmtl_read_config: error reading outputs");
return -5;
}
}
}
DEBUG2_VALUE("hmtl_read_config: size=", addr - HMTL_CONFIG_ADDR);
DEBUG2_VALUE(" end=", addr);
DEBUG2_VALUELN(" module address=", hdr->address);
return addr;
}
void TimeSync::sendSyncMsg(Socket *socket, socket_addr_t target, byte phase, int adjustment = 0) {
if (socket->send_data_size < HMTL_MSG_SIZE(msg_time_sync_t)) {
DEBUG1_VALUELN("sync buf too small: ", socket->send_data_size);
// DEBUG_ERR("startsync to small");
return;
}
msg_hdr_t *msg_hdr = (msg_hdr_t *)socket->send_buffer;
msg_time_sync_t *msg_time = (msg_time_sync_t *)(msg_hdr + 1);
msg_time->sync_phase = phase;
msg_time->timestamp = ms() + adjustment;
hmtl_msg_fmt(msg_hdr, target, HMTL_MSG_SIZE(msg_time_sync_t), MSG_TYPE_TIMESYNC);
socket->sendMsgTo(target, socket->send_buffer, HMTL_MSG_SIZE(msg_time_sync_t));
DEBUG3_VALUE(" phase:", phase);
DEBUG3_VALUE(" time:", msg_time->timestamp);
}
/* Check for HMTL formatted msg over a socket interface */
msg_hdr_t *
hmtl_socket_getmsg(Socket *socket, unsigned int *msglen, uint16_t address) {
const byte *data;
if (address == SOCKET_ADDR_INVALID) data = socket->getMsg(msglen);
else data = socket->getMsg(address, msglen);
if (data != NULL) {
msg_hdr_t *msg_hdr = (msg_hdr_t *)data;
if (*msglen < sizeof (msg_hdr_t)) {
DEBUG1_VALUE("hmtl_socket_getmsg: msg length ", *msglen);
DEBUG1_VALUELN(" short for header ", sizeof (msg_hdr_t));
goto ERROR_OUT;
}
if (msg_hdr->length < sizeof (msg_hdr_t)) {
DEBUG_ERR("hmtl_socket_getmsg: msg length is too short");
goto ERROR_OUT;
}
if (msg_hdr->length != *msglen) {
DEBUG1_VALUE("hmtl_socket_getmsg: msg->length ", msg_hdr->length);
DEBUG1_VALUE(" != msglen ", *msglen);
DEBUG1_COMMAND(
print_hex_string(data, *msglen)
);
goto ERROR_OUT;
}
// TODO: Check the CRC! Check the version!
return msg_hdr;
}
ERROR_OUT:
*msglen = 0;
return NULL;
}
/*
* Process a program configuration message
*/
boolean ProgramManager::handle_msg(msg_program_t *msg) {
DEBUG4_VALUE("handle_msg: program=", msg->type);
DEBUG4_VALUELN(" output=", msg->hdr.output);
/* Find the program to be executed */
byte program = lookup_function(msg->type);
if (program == NO_PROGRAM) {
DEBUG1_VALUELN("handle_msg: invalid type: ", msg->type);
return false;
}
/* Setup the tracker */
int starting_output, stop_output;
if (msg->hdr.output == HMTL_ALL_OUTPUTS) {
/* This should be applied to all outputs that can handle the message type */
starting_output = 0;
stop_output = num_outputs;
} else if (msg->hdr.output > num_outputs) {
DEBUG1_VALUELN("handle_msg: invalid output: ",
msg->hdr.output);
return false;
} else if (outputs[msg->hdr.output] == NULL) {
DEBUG1_VALUELN("handle_msg: NULL output: ", msg->hdr.output);
return false;
} else {
/* Only apply to the specified output */
starting_output = msg->hdr.output;
stop_output = starting_output + 1;
}
for (int output = starting_output; output < stop_output; output++) {
if (outputs[output] == NULL)
continue;
if (msg->type == HMTL_PROGRAM_NONE) {
/* This is a message to clear the existing program so free the tracker */
DEBUG3_VALUELN("handle_msg: clear ", output);
free_tracker(output);
continue;
}
program_tracker_t *tracker;
if (functions[program].program == NULL) {
/*
* This is an initialization-only command, set tracker to null
*/
DEBUG4_PRINTLN("handle_msg: trackerless")
tracker = NULL;
} else {
/* If there was an active program on this output then clear the tracker */
free_tracker(output);
/* Setup a tracker for this program */
tracker = get_tracker(output);
tracker->program_index = program;
tracker->flags = 0x0;
}
if (tracker) {
// Record the output and object in the tracker
tracker->output = outputs[output];
tracker->object = objects[output];
}
/* Attempt to setup the program */
boolean success = functions[program].setup(msg, tracker, outputs[output],
objects[output], this);
if (!success) {
if (tracker) {
DEBUG4_VALUELN("handle_msg: NA on ", output);
free_tracker(output);
}
continue;
}
DEBUG4_VALUELN("handle_msg: setup on ", output);
}
return true;
}
