#include #include GomspaceDeviceHandler::GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie, uint16_t maxConfigTableAddress, uint16_t maxHkTableAddress) : DeviceHandlerBase(objectId, comIF, comCookie), maxConfigTableAddress( maxConfigTableAddress), maxHkTableAddress(maxHkTableAddress) { mode = MODE_NORMAL; if (comCookie == NULL) { sif::error << "GomspaceDeviceHandler invalid com cookie" << std::endl; } } GomspaceDeviceHandler::~GomspaceDeviceHandler() { } void GomspaceDeviceHandler::doStartUp(){ } void GomspaceDeviceHandler::doShutDown(){ } ReturnValue_t GomspaceDeviceHandler::buildNormalDeviceCommand( DeviceCommandId_t * id) { return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GomspaceDeviceHandler::buildTransitionDeviceCommand( DeviceCommandId_t * id){ return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t * commandData, size_t commandDataLen) { ReturnValue_t result; switch(deviceCommand) { case(PING): { result = generatePingCommand(commandData, commandDataLen); if(result != HasReturnvaluesIF::RETURN_OK) { return result; } break; } case(REBOOT): { generateRebootCommand(); break; } case(PARAM_SET):{ result = generateSetParamCommand(commandData, commandDataLen); if(result != HasReturnvaluesIF::RETURN_OK){ return result; } break; } case(PARAM_GET):{ result = generateGetParamCommand(commandData, commandDataLen); if(result != HasReturnvaluesIF::RETURN_OK){ return result; } break; } case(GNDWDT_RESET): { result = generateResetWatchdogCmd(); if(result != HasReturnvaluesIF::RETURN_OK){ return result; } } default: break; } return HasReturnvaluesIF::RETURN_OK; } void GomspaceDeviceHandler::fillCommandAndReplyMap(){ this->insertInCommandAndReplyMap(PING, 3); this->insertInCommandMap(REBOOT); this->insertInCommandAndReplyMap(PARAM_SET, 3); this->insertInCommandAndReplyMap(PARAM_GET, 3); this->insertInCommandMap(GNDWDT_RESET); } ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t *start, size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { switch(rememberCommandId) { case(PING): *foundId = PING; *foundLen = PING_REPLY_SIZE; rememberCommandId = NONE; break; case(PARAM_GET): { *foundId = PARAM_GET; *foundLen = rememberRequestedSize + CspGetParamReply::GS_HDR_LENGTH; rememberCommandId = NONE; break; } case(PARAM_SET): { *foundId = PARAM_SET; *foundLen = rememberRequestedSize; rememberCommandId = NONE; break; } default: return IGNORE_REPLY_DATA; } return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch(id) { case(PING): { SerializeElement replyTime = *packet; handleDeviceTM(&replyTime, id, true); break; } case(PARAM_GET): { // -2 to subtract address size from gomspace parameter reply packet uint16_t payloadLength = (*(packet + 2) << 8 | *(packet + 3)) - 2; if(payloadLength > sizeof(uint32_t)){ sif::error << "GomspaceDeviceHandler: PARAM_GET: Invalid payload " << "size in reply" << std::endl; return INVALID_PAYLOAD_SIZE; } uint8_t tempPayloadBuffer[payloadLength]; /* Extract information from received data */ CspGetParamReply cspGetParamReply(tempPayloadBuffer, payloadLength); size_t size = CspGetParamReply::GS_HDR_LENGTH + payloadLength; ReturnValue_t result = cspGetParamReply.deSerialize(&packet, &size, SerializeIF::Endianness::BIG); if(result != HasReturnvaluesIF::RETURN_OK){ sif::error << "GomspaceDeviceHandler: Failed to deserialize get parameter" << "reply" << std::endl; return result; } uint8_t action = cspGetParamReply.getAction(); uint8_t tableId = cspGetParamReply.getTableId(); uint16_t address = cspGetParamReply.getAddress(); /* Pack relevant information into a tm packet */ ParamReply paramReply(action, tableId, address, payloadLength, tempPayloadBuffer); handleDeviceTM(¶mReply, id, true); break; } case(PARAM_SET): { /* When setting a parameter, the p60dock sends back the state of the * operation */ if(*packet != PARAM_SET_OK){ return HasReturnvaluesIF::RETURN_FAILED; } break; } default: break; } return HasReturnvaluesIF::RETURN_OK; } void GomspaceDeviceHandler::setNormalDatapoolEntriesInvalid(){ } ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand( const uint8_t * commandData, size_t commandDataLen) { SetParamMessageUnpacker setParamMessageUnpacker; ReturnValue_t result = setParamMessageUnpacker.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG); if(result != HasReturnvaluesIF::RETURN_OK){ sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter " "message" << std::endl; return result; } /* Get and check address */ uint16_t address = setParamMessageUnpacker.getAddress(); if(address > maxConfigTableAddress){ sif::error << "GomspaceDeviceHandler: Invalid address for set parameter " << "action" << std::endl; return INVALID_ADDRESS; } uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM; uint16_t seq = 0; uint16_t total = 0; /* CSP reply only contains the transaction state */ uint16_t querySize = 1; const uint8_t* parameterPtr = setParamMessageUnpacker.getParameter(); uint8_t parameterSize = setParamMessageUnpacker.getParameterSize(); uint16_t payloadlength = sizeof(address) + parameterSize; /* Generate command for CspComIF */ CspSetParamCommand setParamCmd(querySize, payloadlength, checksum, seq, total, address, parameterPtr, parameterSize); size_t cspPacketLen = 0; uint8_t* buffer = cspPacket; result = setParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); if(result != HasReturnvaluesIF::RETURN_OK){ sif::error << "GomspaceDeviceHandler: Failed to serialize command for " << "CspComIF" << std::endl; return result; } if(cspPacketLen > MAX_PACKET_LEN){ sif::error << "GomspaceDeviceHandler: Invalid length of set parameter " "command" << std::endl; return PACKET_TOO_LONG; } rawPacket = cspPacket; rawPacketLen = cspPacketLen; rememberRequestedSize = querySize; rememberCommandId = PARAM_SET; return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand( const uint8_t * commandData, size_t commandDataLen){ ReturnValue_t result; /* Unpack the received action message */ GetParamMessageUnpacker getParamMessage; result = getParamMessage.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG); if(result != HasReturnvaluesIF::RETURN_OK) { sif::error << "Failed to deserialize message to extract information " "from get parameter message" << std::endl; return result; } /* Get an check table id to read from */ uint8_t tableId = getParamMessage.getTableId(); if(tableId != CONFIG_TABLE_ID && tableId != HK_TABLE_ID){ sif::error << "GomspaceDeviceHandler: Invalid table id in get parameter" " message" << std::endl; return INVALID_TABLE_ID; } /* Get and check address */ uint16_t address = getParamMessage.getAddress(); if(address > maxHkTableAddress && tableId == HK_TABLE_ID){ sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from " << "housekeeping table" << std::endl; return INVALID_ADDRESS; } if(address > maxConfigTableAddress && tableId == CONFIG_TABLE_ID){ sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from " << "configuration table" << std::endl; return INVALID_ADDRESS; } uint16_t length = sizeof(address); uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM; uint16_t seq = 0; uint16_t total = 0; uint8_t parameterSize = getParamMessage.getParameterSize(); if(parameterSize > sizeof(uint32_t)) { sif::error << "GomspaceDeviceHandler: GET_PARAM: Invalid parameter " << "size" << std::endl; return INVALID_PARAM_SIZE; } uint16_t querySize = parameterSize + CspGetParamCommand::GS_HDR_LENGTH; /* Generate the CSP command to send to the P60 Dock */ CspGetParamCommand getParamCmd(querySize, tableId, length, checksum, seq, total, address); size_t cspPacketLen = 0; uint8_t* buffer = cspPacket; result = getParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); if(result != HasReturnvaluesIF::RETURN_OK){ sif::error << "GomspaceDeviceHandler: Failed to serialize command to " << "get parameter" << std::endl; } if(cspPacketLen > MAX_PACKET_LEN){ sif::error << "GomspaceDeviceHandler: Received invalid get parameter " "command" << std::endl; return PACKET_TOO_LONG; } rawPacket = cspPacket; rawPacketLen = cspPacketLen; rememberRequestedSize = querySize; rememberCommandId = PARAM_GET; return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GomspaceDeviceHandler::generatePingCommand( const uint8_t * commandData, size_t commandDataLen) { CspPingCommand cspPingCommand(commandData, commandDataLen); size_t cspPacketLen = 0; uint8_t* buffer = cspPacket; ReturnValue_t result = cspPingCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); if(result != HasReturnvaluesIF::RETURN_OK){ sif::error << "GomspaceDeviceHandler: Failed to serialize ping command" << std::endl; return result; } if(cspPacketLen > MAX_PACKET_LEN){ sif::error << "GomspaceDeviceHandler: Received invalid ping message" << std::endl; return PACKET_TOO_LONG; } rawPacket = cspPacket; rawPacketLen = cspPacketLen; rememberCommandId = PING; return HasReturnvaluesIF::RETURN_OK; } void GomspaceDeviceHandler::generateRebootCommand(){ uint8_t cspPort = GOMSPACE::REBOOT_PORT; uint16_t querySize = 0; *cspPacket = GOMSPACE::REBOOT_PORT; *(cspPacket + 1) = querySize; size_t cspPacketLen = sizeof(cspPort) + sizeof(cspPacketLen); rawPacket = cspPacket; rawPacketLen = cspPacketLen; } ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd(){ WatchdogResetCommand watchdogResetCommand; size_t cspPacketLen = 0; uint8_t* buffer = cspPacket; ReturnValue_t result = watchdogResetCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG); if(result != HasReturnvaluesIF::RETURN_OK){ sif::error << "GomspaceDeviceHandler: Failed to serialize watchdog reset " << "command" << std::endl; return result; } rawPacket = cspPacket; rawPacketLen = cspPacketLen; rememberRequestedSize = 0; // No bytes will be queried with the ground // watchdog command. rememberCommandId = GNDWDT_RESET; return HasReturnvaluesIF::RETURN_OK; } uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 0; }