/*
* Free a single program tracker
*/
void ProgramManager::free_tracker(int index) {
program_tracker_t *tracker = trackers[index];
if (tracker != NULL) {
DEBUG3_VALUELN("free_tracker: clearing program for ",
index);
if (tracker->state) free(tracker->state);
free(tracker);
}
trackers[index] = NULL;
}
/*
* Free a single program tracker. This
*/
void ProgramManager::free_tracker(int index) {
program_tracker_t *tracker = trackers[index];
if (IS_RUNNING_PROGRAM(tracker)) {
DEBUG3_VALUELN("free_tracker:", index);
if (tracker->flags & PROGRAM_DEALLOC_STATE) {
free_program_state(tracker);
}
/* Clear the tracker and set its program to NO_PROGRAM */
memset(tracker, 0, sizeof (program_tracker_t));
tracker->program_index = NO_PROGRAM;
}
}
/*
* Allocate a new program tracker
*/
program_tracker_t * ProgramManager::get_tracker(int index) {
if (trackers[index] == NULL) {
DEBUG3_VALUELN("get_tracker:", index);
// TODO: Should these be allocated or assigned when manager is initialized?
program_tracker_t *tracker = (program_tracker_t *) malloc(
sizeof(program_tracker_t));
trackers[index] = tracker;
}
memset(trackers[index], 0, sizeof (program_tracker_t));
return trackers[index];
}
void set_mode(uint8_t place, uint8_t new_mode) {
if (current_modes[place] != new_mode) {
previous_modes[place] = current_modes[place];
current_modes[place] = new_mode;
// Set next_time to zero to trigger initialization
modeConfigs[place].next_time = 0;
DEBUG3_VALUE("Set mode ", place);
DEBUG3_VALUELN("=", current_modes[place]);
// Reset the brightness
FastLED.setBrightness(255);
}
}
ProgramManager::ProgramManager(output_hdr_t **_outputs,
program_tracker_t **_trackers,
void **_objects,
byte _num_outputs,
hmtl_program_t *_functions,
byte _num_programs) {
outputs = _outputs;
trackers = _trackers;
objects = _objects;
num_outputs = _num_outputs;
functions = _functions;
num_programs = _num_programs;
for (byte i = 0; i < num_outputs; i++) {
trackers[i] = NULL;
}
DEBUG3_VALUE("ProgramManager: outputs:", num_outputs);
DEBUG3_VALUELN(" programs:", num_programs);
}
/* Process a message if it is for this module */
boolean MessageHandler::process_msg(msg_hdr_t *msg_hdr, Socket *src,
Socket *serial_socket,
config_hdr_t *config) {
if (msg_hdr->version != HMTL_MSG_VERSION) {
DEBUG_ERR("Invalid message version");
return false;
}
/* Test if the message is for this device */
if ((msg_hdr->address == address) ||
(msg_hdr->address == SOCKET_ADDR_ANY)) {
if ((msg_hdr->flags & MSG_FLAG_ACK) &&
(msg_hdr->address != SOCKET_ADDR_ANY)) {
/*
* This is an ack message that is not for us, resend it over serial in
* case that was the original source.
* TODO: Maybe this should check address as well, and serial needs to be
* assigned an address?
*/
DEBUG4_PRINTLN("Forwarding ack to serial");
Serial.write((byte *)msg_hdr, msg_hdr->length);
if (msg_hdr->type != MSG_TYPE_SENSOR) { // Sensor broadcasts are for everyone
return false;
}
}
switch (msg_hdr->type) {
case MSG_TYPE_OUTPUT: {
output_hdr_t *out_hdr = (output_hdr_t *)(msg_hdr + 1);
if (out_hdr->type == HMTL_OUTPUT_PROGRAM) {
manager->handle_msg((msg_program_t *)out_hdr);
} else {
hmtl_handle_output_msg(msg_hdr, manager->num_outputs,
manager->outputs, manager->objects);
}
return true;
}
case MSG_TYPE_POLL: {
// Generate a response to a poll message
uint16_t source_address = 0;
Socket *sock;
if (src != NULL) {
// The response will be going over a socket, get the source address
source_address = src->sourceFromData(msg_hdr);
sock = src;
} else {
// The data will be sent back to the indicated Serial device. A
// socket still needs to be specified in order to have a buffer to
// fill.
sock = serial_socket;
}
DEBUG3_VALUELN("Poll req src:", source_address);
// Format the poll response
uint16_t len = hmtl_poll_fmt(sock->send_buffer,
sock->send_data_size,
source_address,
msg_hdr->flags, OBJECT_TYPE,
config,
manager->outputs,
sock->recvLimit);
// Respond to the appropriate source
if (src != NULL) {
if (msg_hdr->address == SOCKET_ADDR_ANY) {
// If this was a broadcast address then do not respond immediately,
// delay for time based on our address.
int delayMs = address * 2;
DEBUG3_VALUELN("Delay resp: ", delayMs)
delay(delayMs); // TODO: This blocks any running program? Use a timer
}
src->sendMsgTo(source_address, sock->send_buffer, len);
} else {
// Send the response on the serial device
Serial.write(sock->send_buffer, len);
}
break;
}
case MSG_TYPE_SET_ADDR: {
/* Handle an address change message */
msg_set_addr_t *set_addr = (msg_set_addr_t *)(msg_hdr + 1);
if ((set_addr->device_id == 0) ||
(set_addr->device_id == config->device_id)) {
address = set_addr->address;
src->sourceAddress = address;
DEBUG2_VALUELN("Address changed to ", address);
}
break;
}
case MSG_TYPE_SENSOR: {
if (msg_hdr->flags & MSG_FLAG_ACK) {
/*
* This is a sensor response, record relevant values for usage
* elsewhere.
*/
msg_sensor_data_t *sensor = NULL;
while ((sensor = hmtl_next_sensor(msg_hdr, sensor))) {
// Call the ProgramManager's handler for the sensor function
manager->run_program(PROGRAM_SENSOR_DATA, sensor);
}
DEBUG_PRINT_END();
}
break;
}
case MSG_TYPE_TIMESYNC: {
/*
* This is a time synchronization message, send to the ProgramManager's
* TimeSync object.
*/
timesync.synchronize(src, SOCKET_ADDR_INVALID, msg_hdr);
break;
}
case MSG_TYPE_DUMP_CONFIG: {
/*
* This is a request to dump the EEPROM objects to the serial device.
*
* TODO: This could be made to work remotely rather than only to requests
* from the serial device.
*/
uint16_t source_address = 0;
DEBUG3_VALUELN("Dump req src:", source_address);
/*
* The response will be a typical message header followed by the raw data
* from EEPROM.
*/
msg_dumpconfig_response_t *resp = (msg_dumpconfig_response_t *)(serial_socket->send_buffer + sizeof (msg_hdr_t));
int location = HMTL_CONFIG_ADDR; // Starting location in EEPROM
int next_addr = 0;
uint8_t flags;
do {
uint16_t len;
flags = msg_hdr->flags;
next_addr = EEPROM_safe_read(location, resp->data,
serial_socket->send_data_size -
HMTL_MSG_DUMPCONFIG_MIN_LEN);
if (next_addr > 0) {
uint16_t datalen = (uint16_t)EEPROM_DATA_SIZE(next_addr - location);
DEBUG4_VALUELN("Dump config:", location);
/*
* Check if the next address is a valid structure and if so indicate
* that there will be additional messages.
*/
if (EEPROM_check_address(next_addr)) {
flags |= MSG_FLAG_MORE_DATA;
} else {
DEBUG4_PRINTLN("Dump final message")
}
// Now that the length of the data is known construct the message
len = hmtl_dumpconfig_fmt(serial_socket->send_buffer,
serial_socket->send_data_size,
source_address,
flags,
datalen);
location = next_addr;
} else {
/*
* There was an error, respond with an error flag
*/
flags |= MSG_FLAG_ERROR;
len = hmtl_dumpconfig_fmt(serial_socket->send_buffer,
serial_socket->send_data_size,
source_address,
flags,
0);
}
Serial.write(serial_socket->send_buffer, len);
} while (flags & MSG_FLAG_MORE_DATA);
break;
}
}
}
/*
* Set the delta to make the indicated time current
*/
void TimeSync::set(unsigned long time) {
delta = time - millis();
DEBUG3_VALUELN("TimeSet:", delta);
}
// TODO: Static method?
boolean TimeSync::synchronize(Socket *socket,
socket_addr_t target,
msg_hdr_t *msg_hdr) {
if (msg_hdr == NULL) {
/* This was called to synchronize the times to a remote address */
if (state != STATE_IDLE) {
// XXX: What should we do if a sync was already pending? For now
// just continue as if it weren't
}
/* Begin the synchronization process */
DEBUG3_VALUELN("SYNC to:", target);
sendSyncMsg(socket, target, TIMESYNC_SYNC);
state = STATE_AWAITING_ACK;
} else {
if (msg_hdr->type != MSG_TYPE_TIMESYNC) {
// TODO: What to do here?
goto EXIT;
}
unsigned long now = millis();
msg_time_sync_t *msg_time = (msg_time_sync_t *)(msg_hdr + 1);
socket_addr_t source = socket->sourceFromData(msg_hdr);
switch (msg_time->sync_phase) {
case TIMESYNC_CHECK: {
DEBUG3_COMMAND(unsigned long local_now = ms();
DEBUG3_VALUE("CHECK from:", source);
DEBUG3_VALUE(" ts:", msg_time->timestamp);
DEBUG3_VALUE(" ms:", local_now);
DEBUG3_VALUE(" diff:", (long)(msg_time->timestamp + latency - local_now));
);
sendSyncMsg(socket, source, TIMESYNC_ACK, latency);
break;
}
case TIMESYNC_SYNC: {
switch (state) {
case STATE_IDLE:
case STATE_SYNCED: {
// Record the timestamp to compute the latency
DEBUG3_VALUE("SYNC from:", source);
latency = now;
// Reply with ack
sendSyncMsg(socket, source, TIMESYNC_ACK);
state = STATE_AWAITING_SET;
break;
}
}
break;
}
case TIMESYNC_RESYNC: {
if (state == STATE_SYNCED) {
delta = (msg_time->timestamp + latency) - now;
DEBUG3_VALUE("RESYNC from:", source);
DEBUG3_VALUE(" delta:", delta);
}
break;
}
case TIMESYNC_ACK: {
// TODO: Check if this is a response to the message we sent
DEBUG3_VALUE("ACK from:", source);
if (state == STATE_AWAITING_ACK) {
// Send TIMESYNC_SET
sendSyncMsg(socket, source, TIMESYNC_SET);
state = STATE_IDLE;
} else {
DEBUG3_COMMAND(unsigned long local_now = ms();
DEBUG3_VALUE(" ts:", msg_time->timestamp);
DEBUG3_VALUE(" ms:", local_now);
DEBUG3_VALUE(" diff:", (long)(local_now - msg_time->timestamp));
);
}
break;
}
case TIMESYNC_SET: {
if (state == STATE_AWAITING_SET) {
/*
* Set the latecy to 1/2 the time between sending the ack and receiving
* this message, and then use that latecy to compute the delta from
* the sender's time.
*/
DEBUG3_VALUE("SET from:", source);
DEBUG3_VALUE(" ts:", msg_time->timestamp);
latency = (now - latency) / 2;
delta = (msg_time->timestamp + latency) - now;
state = STATE_SYNCED;
DEBUG3_VALUE(" lat:", latency);
DEBUG3_VALUE(" delta:", delta);
}
break;
}
}
/*
* 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;
}
