|
|
|
@ -431,42 +431,51 @@ uint32_t ureg_get_num_units(void) |
|
|
|
|
return (uint32_t) unit_count; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
extern osMutexId mutScratchBufferHandle; |
|
|
|
|
|
|
|
|
|
/** Deliver message to it's destination unit */ |
|
|
|
|
void ureg_deliver_unit_request(TF_Msg *msg) |
|
|
|
|
{ |
|
|
|
|
PayloadParser pp = pp_start(msg->data, msg->len, NULL); |
|
|
|
|
uint8_t callsign = pp_u8(&pp); |
|
|
|
|
uint8_t command = pp_u8(&pp); |
|
|
|
|
// we must claim the scratch buffer because it's used by many units internally
|
|
|
|
|
assert_param(osOK == osMutexWait(mutScratchBufferHandle, 5000)); |
|
|
|
|
{ |
|
|
|
|
PayloadParser pp = pp_start(msg->data, msg->len, NULL); |
|
|
|
|
uint8_t callsign = pp_u8(&pp); |
|
|
|
|
uint8_t command = pp_u8(&pp); |
|
|
|
|
|
|
|
|
|
// highest bit indicates user wants an extra confirmation on success
|
|
|
|
|
bool confirmed = (bool) (command & 0x80); |
|
|
|
|
command &= 0x7F; |
|
|
|
|
// highest bit indicates user wants an extra confirmation on success
|
|
|
|
|
bool confirmed = (bool) (command & 0x80); |
|
|
|
|
command &= 0x7F; |
|
|
|
|
|
|
|
|
|
if (callsign == 0 || !pp.ok) { |
|
|
|
|
com_respond_error(msg->frame_id, E_MALFORMED_COMMAND); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
if (callsign == 0 || !pp.ok) { |
|
|
|
|
com_respond_error(msg->frame_id, E_MALFORMED_COMMAND); |
|
|
|
|
goto quit; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
UlistEntry *li = ulist_head; |
|
|
|
|
while (li != NULL) { |
|
|
|
|
Unit *const pUnit = &li->unit; |
|
|
|
|
if (pUnit->callsign == callsign && pUnit->status == E_SUCCESS) { |
|
|
|
|
error_t rv = pUnit->driver->handleRequest(pUnit, msg->frame_id, command, &pp); |
|
|
|
|
|
|
|
|
|
// send extra SUCCESS confirmation message.
|
|
|
|
|
// error is expected to have already been reported.
|
|
|
|
|
if (rv == E_SUCCESS) { |
|
|
|
|
if (confirmed) com_respond_ok(msg->frame_id); |
|
|
|
|
} else { |
|
|
|
|
com_respond_error(msg->frame_id, rv); |
|
|
|
|
UlistEntry *li = ulist_head; |
|
|
|
|
while (li != NULL) { |
|
|
|
|
Unit *const pUnit = &li->unit; |
|
|
|
|
if (pUnit->callsign == callsign && pUnit->status == E_SUCCESS) { |
|
|
|
|
error_t rv = pUnit->driver->handleRequest(pUnit, msg->frame_id, command, &pp); |
|
|
|
|
|
|
|
|
|
// send extra SUCCESS confirmation message.
|
|
|
|
|
// error is expected to have already been reported.
|
|
|
|
|
if (rv == E_SUCCESS) { |
|
|
|
|
if (confirmed) com_respond_ok(msg->frame_id); |
|
|
|
|
} |
|
|
|
|
else { |
|
|
|
|
com_respond_error(msg->frame_id, rv); |
|
|
|
|
} |
|
|
|
|
goto quit; |
|
|
|
|
} |
|
|
|
|
return; |
|
|
|
|
li = li->next; |
|
|
|
|
} |
|
|
|
|
li = li->next; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Not found
|
|
|
|
|
com_respond_error(msg->frame_id, E_NO_SUCH_UNIT); |
|
|
|
|
// Not found
|
|
|
|
|
com_respond_error(msg->frame_id, E_NO_SUCH_UNIT); |
|
|
|
|
} |
|
|
|
|
quit: |
|
|
|
|
assert_param(osOK == osMutexRelease(mutScratchBufferHandle)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/** Send a response for a unit-list request */ |
|
|
|
|