C++ Msg类代码示例

bool Usb::dispatch(Msg& msg)
{

    uint8_t b;
    uint32_t i;
    uint32_t count;

    if ( msg.is(os,SIG_ERC,fd(),0))
    {
        logger.level(Logger::WARN) << " error occured. Reconnecting.";
        logger.flush();
        disconnect();
//        connect();
        return 0;
    }
    PT_BEGIN ( );
    while(true)
    {
        PT_YIELD_UNTIL ( msg.is(this,SIG_CONNECTED));
        while( true )
        {
            PT_YIELD_UNTIL(msg.is(os,SIG_RXD,fd(),0)|| msg.is(os,SIG_ERC,fd(),0));//event->is(RXD) || event->is(FREE) || ( inBuffer.hasData() && (_isComplete==false)) );
            if ( msg.is(os,SIG_RXD,fd(),0)  &&  hasData())
            {
                count =hasData();
                for(i=0; i<count; i++)
                {
                    b=read();
                    inBuffer.write(b);
                }
                logger.level(Logger::DEBUG)<< "recvd: " << inBuffer.size() << " bytes.";
                logger.flush();
                while( inBuffer.hasData() )
                {
                    if ( _inBytes.Feed(inBuffer.read()))
                    {
                        Str l(256);
                        _inBytes.toString(l);
                        logger.level(Logger::DEBUG)<< "recv : " << l;
                        logger.flush();
                        _inBytes.Decode();
                        if ( _inBytes.isGoodCrc() )
                        {
                            _inBytes.RemoveCrc();
                            Str l(256);
                            _inBytes.toString(l);
                            logger.level(Logger::INFO)<<" recv clean : " <<l;
                            logger.flush();

                            MqttIn* _mqttIn=new MqttIn(256);
                            _inBytes.offset(0);
                            while(_inBytes.hasData())
                                _mqttIn->Feed(_inBytes.read());

                            if ( _mqttIn->parse())
                            {
                                MsgQueue::publish(this,SIG_RXD,_mqttIn->type(),_mqttIn); // _mqttIn will be deleted by msg process
                            }
                            else
                            {
                                Sys::warn(EINVAL, "MQTT");
                                delete _mqttIn;
                            }
                        }
                        else
                        {
                            logger.level(Logger::WARN)<<"Bad CRC. Dropped packet. ";
                            logger.flush();
                            _inBytes.clear(); // throw away bad data
                        }
                        _inBytes.clear();
                    }
                }
            }
            else if ( msg.is(os,SIG_ERC,fd(),0) )
            {
                _inBytes.clear();
                break;
            }

            PT_YIELD ( );
        }

    }

    PT_END (  );
}

//-----------------------------------------------------------------------------
// <Security::EncryptMessage>
// Encrypt and send a Z-Wave message securely.
//-----------------------------------------------------------------------------
bool Security::EncryptMessage
(
	uint8 const* _nonce
)
{
#if 1
	if( m_nonceTimer.GetMilliseconds() > 10000 )
	{
		// The nonce was  not received within 10 seconds
		// of us sending the nonce request.  Send it again
		RequestNonce();
		return false;
	}

	// Fetch the next payload from the queue and encapsulate it
	m_queueMutex->Lock();
	if( m_queue.empty() )
	{
		// Nothing to do
		m_queueMutex->Release();
		return false;
	}

	SecurityPayload * payload = m_queue.front();
	m_queue.pop_front();
	//uint32 queueSize = m_queue.size();
	m_queueMutex->Unlock();
#else
	uint32 queueSize = m_queue.size();
	struct SecurityPayload payload;
	payload.m_length = 7;
	payload.m_part = 0;
	uint8 tmpdata[7] = {0x62, 0x03, 0x00, 0x10, 0x02, 0xfe, 0xfe};
	for (int i = 0; i < payload.m_length; i++)
		payload.m_data[i] = tmpdata[i];
#endif
	// Encapsulate the message fragment
	/* MessageEncapNonceGet doesn't seem to work */
	//Msg* msg = new Msg( (queueSize>1) ? "SecurityCmd_MessageEncapNonceGet" : "SecurityCmd_MessageEncap", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true );
	string LogMessage("SecurityCmd_MessageEncap (");
	LogMessage.append(payload->logmsg);
	LogMessage.append(")");
	Msg* msg = new Msg( LogMessage, GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
	msg->Append( GetNodeId() );
	msg->Append( payload->m_length + 20 );
	msg->Append( GetCommandClassId() );
	//msg->Append( (queueSize>1) ? SecurityCmd_MessageEncapNonceGet : SecurityCmd_MessageEncap );
	msg->Append( SecurityCmd_MessageEncap );
	/* create the iv
	 *
	 */
	uint8 initializationVector[16];
	/* the first 8 bytes of a outgoing IV are random */
	for (int i = 0; i < 8; i++) {
		//initializationVector[i] = (rand()%0xFF)+1;
		initializationVector[i] = 0xAA;
	}
	/* the remaining 8 bytes are the NONCE we got from the device */
	for (int i = 0; i < 8; i++) {
		initializationVector[8+i] = _nonce[i];
	}

	/* Append the first 8 bytes of the initialization vector
	 * to the message. The remaining 8 bytes are the NONCE we recieved from
	 * the node, and is ommitted from sending back to the Node. But we use the full 16 bytes to
	 * as the IV to encrypt out message.
	 */
	for(int i=0; i<8; ++i )
	{
		msg->Append( initializationVector[i] );
	}

	// Append the sequence data
	uint8 sequence = 0;
	if( payload->m_part == 0 )
	{
		sequence = 0;
	}
	else if( payload->m_part == 1 )
	{
		sequence = (++m_sequenceCounter) & 0x0f;
		sequence |= 0x10;		// Sequenced, first frame
	}
	if( payload->m_part == 2 )
	{
		sequence = m_sequenceCounter & 0x0f;
		sequence |= 0x30;		// Sequenced, second frame
	}
	/* at most, the payload will be 28 bytes + 1 byte for the Sequence. */
	uint8 plaintextmsg[32];
	plaintextmsg[0] = sequence;
	for (int i = 0; i < payload->m_length; i++)
		plaintextmsg[i+1] = payload->m_data[i];

	/* Append the message payload after encrypting it with AES-OFB (key is EncryptPassword,
	 * full IV (16 bytes - 8 Random and 8 NONCE) and payload.m_data
//.........这里部分代码省略.........

bool  Wifi::dispatch(Msg& msg) {
//	INFO("line : %d ",_ptLine);
// INFO("msg : %d:%d",msg.src(),msg.signal());
PT_BEGIN();
INIT : {
	PT_WAIT_UNTIL(msg.is(0,SIG_INIT));
	struct station_config stationConf;

	INFO("WIFI_INIT");
	if ( wifi_set_opmode(STATION_MODE) ){
		; // STATIONAP_MODE was STATION_MODE
		INFO("line : %d",__LINE__);
		if ( wifi_set_phy_mode(PHY_MODE_11B)) {
			os_memset(&stationConf, 0, sizeof(struct station_config));
			ets_strncpy((char*)stationConf.ssid,_ssid,sizeof(stationConf.ssid));
			ets_strncpy((char*)stationConf.password,_pswd,sizeof(stationConf.password));
			stationConf.bssid_set=0;
			INFO("line : %d",__LINE__);
			if ( wifi_station_set_config(&stationConf) ){
				if ( wifi_station_connect() ){
					INFO("line : %d",__LINE__);
					goto DISCONNECTED;//	wifi_station_set_auto_connect(TRUE);
				}
			}
		}
	}
	//	wifi_station_set_auto_connect(FALSE);
	INFO(" WIFI INIT failed , retrying... ");
	goto INIT;
};
DISCONNECTED: {
	while(true) {
		timeout(1000);
		PT_YIELD_UNTIL(timeout());
		struct ip_info ipConfig;
		wifi_get_ip_info(STATION_IF, &ipConfig);
		wifiStatus = wifi_station_get_connect_status();
		if ( wifi_station_get_connect_status()== STATION_NO_AP_FOUND || wifi_station_get_connect_status()==STATION_WRONG_PASSWORD || wifi_station_get_connect_status()==STATION_CONNECT_FAIL)
		{
			INFO(" NOT CONNECTED ");
			wifi_station_connect();
		} else if (wifiStatus == STATION_GOT_IP && ipConfig.ip.addr != 0) {
			_connections++;
			union {
				uint32_t addr;
				uint8_t ip[4];
			} v;
			v.addr = ipConfig.ip.addr;
			INFO("  IP Address : %d.%d.%d.%d ",v.ip[0],v.ip[1],v.ip[2],v.ip[3]);
			INFO(" CONNECTED ");
			Msg::publish(this,SIG_CONNECTED);
			_connected=true;
			timeout(2000);
			goto CONNECTED;
		} else {
			INFO(" STATION_IDLE ");
		}
		timeout(500);
	}
};
CONNECTED : {
	while(true) {
		PT_YIELD_UNTIL(timeout());
		struct ip_info ipConfig;
		wifi_get_ip_info(STATION_IF, &ipConfig);
		wifiStatus = wifi_station_get_connect_status();
		if (wifiStatus != STATION_GOT_IP ) {
			Msg::publish(this,SIG_DISCONNECTED);
			timeout(500);
			_connected=false;
			goto DISCONNECTED;
		}
		timeout(2000);
	}

};
PT_END();

}

void ABTMessageTest::test_accessors(void)
{
    using namespace maxmm::ma;
    {
        typedef ABTMessage<uint32_t> Msg;
    
        Msg msg;
        msg.make_ok();
        msg.ok().agent_assignment().value() = 2;
        msg.ok().agent_assignment().agent_id() = ma::AgentId(1);
        CPPUNIT_ASSERT_EQUAL(uint32_t(2), msg.ok().agent_assignment().value());
        CPPUNIT_ASSERT_EQUAL(uint32_t(1), msg.ok().agent_assignment().agent_id().id());
        
        Msg msg2;
        msg2 = msg;
    
        Msg msg3;
    }
    
    {
        typedef ABTMessage<uint32_t> Msg;
    
        Msg msg;
        msg.make_nogood().nogoods().push_back(AgentAssignment<uint32_t>());
        CPPUNIT_ASSERT_EQUAL(std::size_t(1),  msg.nogood().nogoods().size());
        
        msg.nogood().nogoods().push_back(AgentAssignment<uint32_t>());
        CPPUNIT_ASSERT_EQUAL(std::size_t(2),  msg.nogood().nogoods().size());

        msg.nogood().nogoods().at(0).value() = 2;
        msg.nogood().nogoods().at(1).value() = 3;

        CPPUNIT_ASSERT_EQUAL(uint32_t(2), msg.nogood().nogoods().at(0).value());
        CPPUNIT_ASSERT_EQUAL(uint32_t(3), msg.nogood().nogoods().at(1).value());
    }

}

bool Security::Init
(
)
{
	/* if we are adding this node, then instead to a SchemeGet Command instead - This
	 * will start the Network Key Exchange
	 */
	if (GetNodeUnsafe()->IsAddingNode()) {
		Msg * msg = new Msg ("SecurityCmd_SchemeGet", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
		msg->Append( GetNodeId() );
		msg->Append( 3 );
		msg->Append( GetCommandClassId() );
		msg->Append( SecurityCmd_SchemeGet );
		msg->Append( 0 );
		msg->Append( GetDriver()->GetTransmitOptions() );
		/* SchemeGet is unencrypted */
		GetDriver()->SendMsg(msg, Driver::MsgQueue_Security);
	} else {
		Msg* msg = new Msg( "SecurityCmd_SupportedGet", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
		msg->Append( GetNodeId() );
		msg->Append( 2 );
		msg->Append( GetCommandClassId() );
		msg->Append( SecurityCmd_SupportedGet );
		msg->Append( GetDriver()->GetTransmitOptions() );
		this->SendMsg( msg);
	}

	return true;
}

//
// routine: receive and send on incoming messages
//
// Notes:
//		No analysis of the message is performed - that is left
//		to the command processor task.  Instead, the local copy
//		of the header
//		is demarshalled to determined whether or not there
//		is associated data; if there is, space is allocated to
//		contain it.
void *
SseInputTask::routine()
{
    extractArgs();

    Timer timer;
    if (cmdArgs->noSSE()) {
        processIpFromFile();
        while (1)
            timer.sleep(3000);
    };

    // run forever, waiting for messages from the SSE
    bool stopIssued = false;
    bool done = false;
    uint32_t lastCode = MESSAGE_CODE_UNINIT;
    int32_t lastLen = 0;
    uint32_t lastTime = 0;
    while (!done) {
        // if there's no connection, request that it be
        // established, then wait for that to happen
        if (!sse->isConnected()) {
            requestConnection();
            while (!sse->isConnected())
                timer.sleep(3000);
        }
        stopIssued = false;

        // got a connection - wait for data to come in
        SseInterfaceHeader hdr;
        Error err = sse->recv((void *) &hdr, sizeof(hdr));
        if (err) {
            switch (err) {
            case EAGAIN:
            case EINTR:
            case ENOTCONN:
            case ECONNRESET:
                stopAllActivities(stopIssued);
                continue;
            default:
                Fatal(err);
                break;
            }
        }
        // demarshall the header
        hdr.demarshall();

        if (cmdArgs->logSseMessages()) {
            LogWarning(ERR_NE, hdr.activityId,
                       "bad msg from Sse, code = %d, len = %d", hdr.code,
                       hdr.dataLength);
        }

        // allocate a message to hold the incoming message
        Msg *msg = msgList->alloc();
        msg->setHeader(hdr);
        msg->setUnit((sonata_lib::Unit) UnitSse);

        // if there's data associated with the message,
        // allocate space and retrieve it, demarshall it
        // based on the message type,
        // then send it on to the command processor
        void *data = 0;
        int32_t len = hdr.dataLength;
        timeval tv;
        gettimeofday(&tv, NULL);
        if (len > 10000) {
            LogWarning(ERR_NE, hdr.activityId,
                       "msg code = %d, len = %d, t = %u, last msg = %d, last len = %d, last t = %u",
                       hdr.code, len, tv.tv_sec, lastCode, lastLen, lastTime);
            Timer t;
            t.sleep(100);
        }
        else {
            lastCode = hdr.code;
            lastLen = len;
            lastTime = tv.tv_sec;
        }
        if (len) {
            MemBlk *blk = partitionSet->alloc(len);
            Assert(blk);
            if (!blk)
                Fatal(ERR_MAF);
            data = blk->getData();
            err = sse->recv(data, len);
            if (err) {
                switch (err) {
                case EAGAIN:
                case EINTR:
                case ENOTCONN:
//.........这里部分代码省略.........

bool HControlPointPrivate::processDeviceDiscovery(
    const Msg& msg, const HEndpoint& source, HControlPointSsdpHandler*)
{
    HLOG2(H_AT, H_FUN, m_loggingIdentifier);

    const HUdn& resourceUdn = msg.usn().udn();

    HDefaultClientDevice* device =
        static_cast<HDefaultClientDevice*>(
            m_deviceStorage.searchDeviceByUdn(msg.usn().udn(), AllDevices));

    if (device)
    {
        // according to the UDA v1.1 spec, if a control point receives an
        // alive announcement of any type for a device tree, the control point
        // can assume that all devices and services are available.
        // ==> reset timeouts for entire device tree and all services.

        device = static_cast<HDefaultClientDevice*>(device->rootDevice());
        device->startStatusNotifier(HDefaultClientDevice::All);

        // it cannot be that only some embedded device is available at certain
        // interface, since the device description is always fetched from the
        // the location that the root device specifies ==> the entire device
        // tree has to be available at that location.
        if (device->addLocation(msg.location()))
        {
            HLOG_DBG(QString("Existing device [%1] now available at [%2]").arg(
                         resourceUdn.toString(), msg.location().toString()));
        }

        if (!device->deviceStatus()->online())
        {
            device->deviceStatus()->setOnline(true);
            emit q_ptr->rootDeviceOnline(device);
            processDeviceOnline(device, false);
        }

        return true;
    }

    // it does not matter if the device is an embedded device, since the
    // location of the device always points to the root device's description
    // and the internal device model is built of that. Hence, any advertisement
    // will do to build the entire model correctly.

    DeviceBuildTask* dbp = m_deviceBuildTasks.get(msg);
    if (dbp)
    {
        if (!dbp->m_locations.contains(msg.location()))
        {
            dbp->m_locations.push_back(msg.location());
        }

        return true;
    }

    if (!q_ptr->acceptResource(msg.usn(), source))
    {
        HLOG_DBG(QString("Resource advertisement [%1] rejected").arg(
                     msg.usn().toString()));

        return true;
    }

    DeviceBuildTask* newBuildTask = new DeviceBuildTask(this, msg);

    newBuildTask->setAutoDelete(false);

    m_deviceBuildTasks.add(newBuildTask);

    bool ok = connect(
                  newBuildTask, SIGNAL(done(Herqq::Upnp::HUdn)),
                  this, SLOT(deviceModelBuildDone(Herqq::Upnp::HUdn)));

    Q_ASSERT(ok);
    Q_UNUSED(ok)

    HLOG_INFO(QString(
                  "New resource [%1] is available @ [%2]. "
                  "Attempting to build the device model.").arg(
                  msg.usn().toString(), msg.location().toString()));

    m_threadPool->start(newBuildTask);

    return true;
}

//-----------------------------------------------------------------------------
// <BasicWindowCovering::SetValue>
// Set a value on the Z-Wave device
//-----------------------------------------------------------------------------
bool BasicWindowCovering::SetValue
(
    Value const& _value
)
{
    if( ValueID::ValueType_Button == _value.GetID().GetType() )
    {
        ValueButton const* button = static_cast<ValueButton const*>(&_value);

        uint8 action = 0x40;
        if( button->GetID().GetIndex() )	// Open is index zero, so non-zero is close.
        {
            // Close
            action = 0;
        }

        if( button && button->IsPressed() )
        {
            Log::Write( LogLevel_Info, GetNodeId(), "BasicWindowCovering - Start Level Change (%s)", action ? "Open" : "Close" );
            Msg* msg = new Msg( "Basic Set", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true );
            msg->SetInstance( this, _value.GetID().GetInstance() );
            msg->Append( GetNodeId() );
            msg->Append( 3 );
            msg->Append( GetCommandClassId() );
            msg->Append( BasicWindowCoveringCmd_StartLevelChange );
            msg->Append( action );
            msg->Append( GetDriver()->GetTransmitOptions() );
            GetDriver()->SendMsg( msg, Driver::MsgQueue_Send );
            return true;
        }
        else
        {
            Log::Write( LogLevel_Info, GetNodeId(), "BasicWindowCovering - Stop Level Change" );
            Msg* msg = new Msg( "Basic Set", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true );
            msg->SetInstance( this, _value.GetID().GetInstance() );
            msg->Append( GetNodeId() );
            msg->Append( 2 );
            msg->Append( GetCommandClassId() );
            msg->Append( BasicWindowCoveringCmd_StopLevelChange );
            msg->Append( GetDriver()->GetTransmitOptions() );
            GetDriver()->SendMsg( msg, Driver::MsgQueue_Send );
            return true;
        }
    }

    return false;
}

//-----------------------------------------------------------------------------
// <ManufacturerProprietary::SetValue>
// Set the lock's state
//-----------------------------------------------------------------------------
bool ManufacturerProprietary::SetValue
(
    Value const& _value
)
{
    uint64 value_id = _value.GetID().GetIndex();
    Msg* msg = new Msg( "ManufacturerProprietary_SetValue", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );

    if (FibaroVenetianBlindsValueIds_Blinds == value_id || FibaroVenetianBlindsValueIds_Tilt == value_id){
        ValueByte const* value = static_cast<ValueByte const*>(&_value);

        msg->SetInstance( this, _value.GetID().GetInstance() );
        msg->Append( GetNodeId() );
        msg->Append( 2 + // length of data
                     sizeof(MANUFACTURER_ID_FIBARO) +
                     sizeof(FIBARO_VENETIAN_BLINDS_GET_POSITION_TILT) );
        msg->Append( GetCommandClassId() );
        msg->AppendArray( MANUFACTURER_ID_FIBARO, sizeof(MANUFACTURER_ID_FIBARO) );
        if (FibaroVenetianBlindsValueIds_Blinds == value_id) {
            msg->AppendArray( FIBARO_VENETIAN_BLINDS_SET_POSITION, sizeof(FIBARO_VENETIAN_BLINDS_SET_POSITION) );
            msg->Append( value->GetValue() );
            msg->Append( 0x00 );
        } else if (FibaroVenetianBlindsValueIds_Tilt == value_id) {
            msg->AppendArray( FIBARO_VENETIAN_BLINDS_SET_TILT, sizeof(FIBARO_VENETIAN_BLINDS_SET_TILT) );
            msg->Append( value->GetValue() );
        }
        msg->Append( GetDriver()->GetTransmitOptions() );
        GetDriver()->SendMsg( msg, Driver::MsgQueue_Send );
        return true;
    } else {
        Log::Write( LogLevel_Info, GetNodeId(), "ManufacturerProprietary_SetValue %d not supported on node %d", value_id, GetNodeId());
        return false;
    }
}

//-----------------------------------------------------------------------------
// <ClimateControlSchedule::SetValue>
// Set a value in the device
//-----------------------------------------------------------------------------
bool ClimateControlSchedule::SetValue
(
	Value const& _value
)
{
//	bool res = false;
	uint8 instance = _value.GetID().GetInstance();
	uint8 idx = _value.GetID().GetIndex();

	if( idx < 8 )
	{
		// Set a schedule
		ValueSchedule const* value = static_cast<ValueSchedule const*>(&_value);

		Log::Write( LogLevel_Info, GetNodeId(), instance, "Set the climate control schedule for %s", c_dayNames[idx]);

		Msg* msg = new Msg( "ClimateControlScheduleCmd_Set", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
		msg->SetInstance( this, instance );
		msg->Append( GetNodeId() );
		msg->Append( 30 );
		msg->Append( GetCommandClassId() );
		msg->Append( ClimateControlScheduleCmd_Set );
		msg->Append( idx );	// Day of week

		for( uint8 i=0; i<9; ++i )
		{
			uint8 hours;
			uint8 minutes;
			int8 setback;
			if( value->GetSwitchPoint( i, &hours, &minutes, &setback ) )
			{
				msg->Append( hours );
				msg->Append( minutes );
				msg->Append( setback );
			}
			else
			{
				// Unused switch point
				msg->Append( 0 );
				msg->Append( 0 );
				msg->Append( 0x7f );
			}
		}

		msg->Append( GetDriver()->GetTransmitOptions() );
		GetDriver()->SendMsg( msg, Driver::MsgQueue_Send );
	}
	else
	{
		// Set an override
		ValueList* state = static_cast<ValueList*>( GetValue( instance, ClimateControlScheduleIndex_OverrideState ) );
		ValueByte* setback = static_cast<ValueByte*>( GetValue( instance, ClimateControlScheduleIndex_OverrideSetback ) );

		if( state && setback )
		{
			ValueList::Item const *item = state->GetItem();
			if (item == NULL) {
			        return false;
            }

			Msg* msg = new Msg( "ClimateControlScheduleCmd_OverrideSet", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
			msg->SetInstance( this, instance );
			msg->Append( GetNodeId() );
			msg->Append( 4 );
			msg->Append( GetCommandClassId() );
			msg->Append( ClimateControlScheduleCmd_OverrideSet );
			msg->Append( (uint8)item->m_value );
			msg->Append( setback->GetValue() );
			msg->Append( GetDriver()->GetTransmitOptions() );
			GetDriver()->SendMsg( msg, Driver::MsgQueue_Send );
		}
	}

	return true;
}

//-----------------------------------------------------------------------------
// <ThermostatMode::RequestValue>
// Get the static thermostat mode details from the device
//-----------------------------------------------------------------------------
bool ThermostatMode::RequestValue
(
	uint32 const _requestFlags,
	uint8 const _getTypeEnum,
	uint8 const _instance,
	Driver::MsgQueue const _queue
)
{
	if( _getTypeEnum == ThermostatModeCmd_SupportedGet )
	{
		// Request the supported modes
		Msg* msg = new Msg( "Request Supported Thermostat Modes", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
		msg->SetInstance( this, _instance );
		msg->Append( GetNodeId() );
		msg->Append( 2 );
		msg->Append( GetCommandClassId() );
		msg->Append( ThermostatModeCmd_SupportedGet );
		msg->Append( GetDriver()->GetTransmitOptions() );
		GetDriver()->SendMsg( msg, _queue );
		return true;
	}

	if( _getTypeEnum == 0 )		// get current mode
	{
		// Request the current mode
		Msg* msg = new Msg( "Request Current Thermostat Mode", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
		msg->SetInstance( this, _instance );
		msg->Append( GetNodeId() );
		msg->Append( 2 );
		msg->Append( GetCommandClassId() );
		msg->Append( ThermostatModeCmd_Get );
		msg->Append( GetDriver()->GetTransmitOptions() );
		GetDriver()->SendMsg( msg, _queue );
		return true;
	}
	return false;
}

void Stub::OnRead(ssize_t nread, uv_buf_t buff)
{
	GTimer timer;

	m_uInputBufferPos += nread;

	if (STUB_INPUTBUFFER_SIZE - m_uInputBufferPos < STUB_INPUTBUFFER_SIZE/4) {
		g_Log.SaveLog(LOG_LV_ERROR, "Stub alloc error! used too much<%d, %d>", STUB_INPUTBUFFER_SIZE, m_uInputBufferPos);
		Close();
		return;
	}

	size_t nStartPos = 0;

	MSG_ID_t uMsgID;
	MSG_LEN_t uMsgLen;

	size_t  uIDLen = sizeof(uMsgID);
	size_t	uLenLen = sizeof(uMsgLen);
	size_t	uMsgTotalLen = 0;

	while (nStartPos + uIDLen + uLenLen <= m_uInputBufferPos) {
		memcpy(&uMsgID, m_pInputBuffers + nStartPos, uIDLen);
		memcpy(&uMsgLen, m_pInputBuffers + nStartPos + uIDLen, uLenLen);

		uMsgTotalLen = uIDLen + uLenLen + uMsgLen;

		if (uMsgLen > STUB_INPUTBUFFER_SIZE / 2) {
			g_Log.SaveLog(LOG_LV_ERROR, "Stub read Error! illegality msg<%d, %d> from<%s,>", uMsgID, uMsgLen, m_sIP.c_str());
			Close();
			break;
		}

		if (nStartPos + uMsgTotalLen > m_uInputBufferPos) {
			break;
		}

		IMsgFactory* pFactory = g_MsgFactoryManager.GetFactory(uMsgID);
		if (pFactory == NULL) {
			g_Log.SaveLog(LOG_LV_WARNING, "Stub Read Warning! can not find the factory<id:%d, len:%d>", uMsgID, uMsgLen);
		} else {
			Msg* pMsg = pFactory->GetMsg();
			if (pMsg == NULL) {
				g_Log.SaveLog(LOG_LV_ERROR, "Stub Read Warning! create msg<id:%d, len:%d> failed!", uMsgID, uMsgLen);
			} else {
				pMsg->Len(uMsgLen);
				if (!pFactory->Read(pMsg, m_pInputBuffers + nStartPos + uIDLen + uLenLen, uMsgLen)) {
					g_Log.SaveLog(LOG_LV_ERROR, "Stub Read Warning! read<id:%d, len:%d> failed!", uMsgID, uMsgLen);
				} else {
					pFactory->HandleMsg(pMsg, this);
				}
			}
		}

		nStartPos += uMsgTotalLen;
	}

	assert(m_uInputBufferPos >= nStartPos);

	size_t uLeft = m_uInputBufferPos - nStartPos;
	if (uLeft > 0 && nStartPos > 0) {
		memcpy(m_pInputBuffers, m_pInputBuffers + nStartPos, uLeft);
	}
	m_uInputBufferPos = uLeft;

	//g_Log.SaveLog(LOG_LV_NORMAL, "cost time:%f", timer.Seconds());
}

bool DWM1000_Tag::dispatch(Msg& msg) {
	PT_BEGIN()
	PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
	init();

	POLL_SEND: {
		while (true) {
			timeout(1000);				// delay  between POLL
			PT_YIELD_UNTIL(timeout());
			/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
			tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
			dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);
			dwt_writetxfctrl(sizeof(tx_poll_msg), 0);

			/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
			 * set by dwt_setrxaftertxdelay() has elapsed. */
			LOG<< " Start TXF " << FLUSH;
			dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);// SEND POLL MSG
			dwt_setinterrupt(DWT_INT_TFRS, 0);
			dwt_setinterrupt(DWT_INT_RFCG, 1);	// enable
			clearInterrupt();
			_timeoutCounter = 0;

			/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
			timeout(10);
			PT_YIELD_UNTIL(timeout() || isInterruptDetected());	// WAIT RESP MSG

			if (isInterruptDetected())
				LOG<< " INTERRUPT DETECTED " << FLUSH;

			status_reg = dwt_read32bitreg(SYS_STATUS_ID);
			LOG<< HEX <<" SYS_STATUS " << status_reg << FLUSH;
			if (status_reg == 0xDEADDEAD) {
				init();
			} else if (status_reg & SYS_STATUS_RXFCG)
				goto RESP_RECEIVED;
			else if (status_reg & SYS_STATUS_ALL_RX_ERR) {

				if (status_reg & SYS_STATUS_RXRFTO)
					INFO(" RX Timeout");
				else
					INFO(" RX error ");
				dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); /* Clear RX error events in the DW1000 status register. */

			}
		}

	}
	RESP_RECEIVED: {

		LOG<< " Received " <<FLUSH;

		frame_seq_nb++; /* Increment frame sequence number after transmission of the poll message (modulo 256). */
		uint32 frame_len;

		/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
		dwt_write32bitreg(SYS_STATUS_ID,
				SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);

		/* A frame has been received, read iCHANGEt into the local buffer. */
		frame_len =
		dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
		if (frame_len <= RX_BUF_LEN) {
			dwt_readrxdata(rx_buffer, frame_len, 0);
		}

		/* Check that the frame is the expected response from the companion "DS TWR responder" example.
		 * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
		rx_buffer[ALL_MSG_SN_IDX] = 0;
		if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0) {	// CHECK RESP MSG
			uint32 final_tx_time;

			/* Retrieve poll transmission and response reception timestamp. */
			poll_tx_ts = get_tx_timestamp_u64();
			resp_rx_ts = get_rx_timestamp_u64();

			/* Compute final message transmission time. See NOTE 9 below. */
			final_tx_time = (resp_rx_ts
					+ (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME))
			>> 8;
			dwt_setdelayedtrxtime(final_tx_time);

			/* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
			final_tx_ts = (((uint64) (final_tx_time & 0xFFFFFFFE)) << 8)
			+ TX_ANT_DLY;

			/* Write all timestamps in the final message. See NOTE 10 below. */
			final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX],
					poll_tx_ts);
			final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX],
					resp_rx_ts);
			final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX],
					final_tx_ts);

			/* Write and send final message. See NOTE 7 below. */
			tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
			dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);
			dwt_writetxfctrl(sizeof(tx_final_msg), 0);
			dwt_starttx(DWT_START_TX_DELAYED);				// SEND FINAL MSG

//.........这里部分代码省略.........

/* static */
void
Server :: receiveHandler(NetworkClient* aClient, uint8_t* aBuf, size_t aLen)
{
    if (aClient != nullptr && aBuf != nullptr)
    {
        if (aLen < sizeof(Msg::Header))
            return;

        uint8_t* received = new uint8_t[aLen];
        memcpy(received, aBuf, aLen);

        // TODO? clean this line and add some checks
        Client* client = (Client*)aClient->getOwner();
        client->getCipher().decrypt(received, aLen);

        size_t size = 0;
        for (size_t i = 0; i < aLen; i += size)
        {
            #if BYTE_ORDER == BIG_ENDIAN
            size = bswap16(((Msg::Header*)(received + i))->Length);
            #else
            size = ((Msg::Header*)(received + i))->Length;
            #endif

            ASSERT(size <= 1024); // invalid msg size...
            if (size < aLen)
            {
                uint8_t* packet = new uint8_t[size];
                memcpy(packet, received + i, size);

                #if BYTE_ORDER == BIG_ENDIAN
                Msg::Header* header = (Msg::Header*)packet;
                header->Length = bswap16(header->Length);
                header->Type = bswap16(header->Type);
                #endif

                Msg* msg = nullptr;
                Msg::create(&msg, &packet, size);
                msg->process(client);

                SAFE_DELETE(msg);
                SAFE_DELETE_ARRAY(packet);
            }
            else
            {
                #if BYTE_ORDER == BIG_ENDIAN
                Msg::Header* header = (Msg::Header*)received;
                header->Length = bswap16(header->Length);
                header->Type = bswap16(header->Type);
                #endif

                Msg* msg = nullptr;
                Msg::create(&msg, &received, size);
                msg->process(client);

                SAFE_DELETE(msg);
            }
        }

        SAFE_DELETE_ARRAY(received);
    }
}

//-----------------------------------------------------------------------------
// <MultiInstance::HandleMultiChannelEndPointReport>
// Handle a message from the Z-Wave network
//-----------------------------------------------------------------------------
void MultiInstance::HandleMultiChannelEndPointReport
(	
	uint8 const* _data,
	uint32 const _length
)
{
	if( m_numEndpoints != 0 )
	{
		return;
	}

	m_numEndPointsCanChange = (( _data[1] & 0x80 ) != 0 );	// Number of endpoints can change.
	m_endPointsAreSameClass = (( _data[1] & 0x40 ) != 0 );	// All endpoints are the same command class.
	m_numEndpoints = _data[2] & 0x7f;

	if( m_endPointsAreSameClass )
	{
		Log::Write( LogLevel_Info, GetNodeId(), "Received MultiChannelEndPointReport from node %d. All %d endpoints are the same.", GetNodeId(), m_numEndpoints );
	
		// Send a single capability request to endpoint 1 (since all classes are the same)
		char str[128];
		snprintf( str, sizeof( str ), "MultiChannelCmd_CapabilityGet for endpoint 1" );
		Msg* msg = new Msg( str, GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
		msg->Append( GetNodeId() );
		msg->Append( 3 );
		msg->Append( GetCommandClassId() );
		msg->Append( MultiChannelCmd_CapabilityGet );
		msg->Append( 1 );
		msg->Append( TRANSMIT_OPTION_ACK | TRANSMIT_OPTION_AUTO_ROUTE );
		GetDriver()->SendMsg( msg, Driver::MsgQueue_Send );
	}
	else
	{
		Log::Write( LogLevel_Info, GetNodeId(), "Received MultiChannelEndPointReport from node %d. Endpoints are not all the same.", GetNodeId() );
		Log::Write( LogLevel_Info, GetNodeId(), "    Starting search for endpoints by generic class..." );

		// This is where things get really ugly.  We need to get the capabilities of each
		// endpoint, but we only know how many there are, not which indices they
		// are at.  We will first try to use MultiChannelCmd_EndPointFind to get
		// lists of indices.  We have to specify a generic device class in the find,
		// so we try generic classes in an order likely to find the endpoints the quickest.
		m_endPointFindIndex = 0;
		m_numEndPointsFound = 0;

		char str[128];
		snprintf( str, 128, "MultiChannelCmd_EndPointFind for generic device class 0x%.2x (%s)", c_genericClass[m_endPointFindIndex], c_genericClassName[m_endPointFindIndex] );
		Msg* msg = new Msg( str, GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
		msg->Append( GetNodeId() );
		msg->Append( 4 );
		msg->Append( GetCommandClassId() );
		msg->Append( MultiChannelCmd_EndPointFind );
		msg->Append( c_genericClass[m_endPointFindIndex] );		// Generic device class
		msg->Append( 0xff );									// Any specific device class
		msg->Append( TRANSMIT_OPTION_ACK | TRANSMIT_OPTION_AUTO_ROUTE );
		GetDriver()->SendMsg( msg, Driver::MsgQueue_Send );
	}
}