int IcqProtocol::parseFnac(const Packet &p)
{
FSPacket fs(p);
debug() << "> FNAC ";
if (!fs.ok())
return -1;
switch (fs.family())
{
case FAMILY_GEN:
return parseFnacGen(fs);
break;
case FAMILY_LOC:
return parseFnacLoc(fs);
break;
case FAMILY_BUD:
return parseFnacBud(fs);
break;
case FAMILY_MSG:
return parseFnacMsg(fs);
break;
case FAMILY_BOS:
return parseFnacBos(fs);
case FAMILY_ICQ:
return parseFnacIcq(fs);
default:
debug() << "> Error" << endl;
packetError(&fs, "Unknown family %d in parseFnac()!\n", fs.family());
return -1;
break;
}
}
int IcqProtocol::parseFnacGen(const FSPacket &fs)
{
debug() << "> GEN ";
switch (fs.subtype())
{
case SUB_GEN_SERVERREADY: // Server is ready for normal operation
debug() << "> ServerReady" << endl;
m_net->sendData(PktCGenSetStatus(_seq++, m_status));
m_net->sendData(PktCGenClientVersions(_seq++));
break;
case SUB_GEN_SERVERVERSIONS:
debug() << "> ServerVersions" << endl;
m_net->sendData(FCPacket(_seq++, FAMILY_GEN, SUB_GEN_RATEREQUEST));
break;
case SUB_GEN_RATERESPONSE:
debug() << "> RateResponse" << endl;
m_net->sendData(FCPacket(_seq++, FAMILY_GEN, SUB_GEN_RATERESPONSEACK,
dstring((uint8*)"\0\x1\0\x2\0\x3\0\x4\0\x5", 10)));
m_net->sendData(FCPacket(_seq++, FAMILY_GEN, SUB_GEN_REQSELFINFO));
m_net->sendData(FCPacket(_seq++, FAMILY_LOC, SUB_LOC_REQRIGHTS));
m_net->sendData(FCPacket(_seq++, FAMILY_BUD, SUB_BUD_REQRIGHTS));
m_net->sendData(FCPacket(_seq++, FAMILY_MSG, SUB_MSG_REQPARMINFO));
m_net->sendData(FCPacket(_seq++, FAMILY_BOS, SUB_BOS_REQRIGHTS));
//Continue in SUB_GEN_SELFINFO
break;
case SUB_GEN_SELFINFO:
debug() << "> SelfInfo" << endl;
m_net->sendData(FCPacket(_seq++, FAMILY_MSG, SUB_MSG_ADDPARAM,
dstring((uint8*)ICBM_PARAM_STRING, 16)));
m_net->sendData(PktCLocCapinfo(_seq++));
if (m_buddies.size() > 0)
m_net->sendData(PktCBudAdd(_seq++,m_buddies));
m_net->sendData(FCPacket(_seq++, FAMILY_GEN, SUB_GEN_CLIENTREADY,
dstring((uint8*)CLIENT_RDY_STRING, 64)));
break;
case SUB_GEN_MOTD:
debug() << "> MOTD" << endl;
debug() << "-------------------------" << endl;
debug() << "And we're up and running!" << endl;
debug() << "-------------------------" << endl;
m_state = S_online;
eventStateChange(S_online);
eventLoggedIn(); //Call virtual function
{ // Added braces because VC++ complains, needed to change scope
for (buddy_t::const_iterator it=m_buddies.begin();it!=m_buddies.end();it++)
m_net->sendData(PktCBudAdd(_seq++, it->second));
}
break;
default:
debug() << "> Error" << endl;
packetError(&fs, "Unknown GEN subtype %d\n", fs.subtype());
return -1;
break;
}
return -100;
}
/* Request Process packet of function 5,6,15,16
* @param: none
* @return: none
* @private
* @comment: just confirm received packet with requested packet
*/
void Modbus::process_F5_F6_F15_F16()
{
//Serial.println("process_F5_F6");
// The repsonse of functions 5,6,15 & 16 are just an echo of the query
unsigned int recieved_address = ((frame[2] << 8) | frame[3]);
unsigned int recieved_data = ((frame[4] << 8) | frame[5]);
if ((recieved_address == _packet->address) && (recieved_data == _packet->data))
packetSuccess();
else
packetError();
}
/* Request Process packet of function 3 and function 4
* @param: none
* @return: Holding register in 2 bytes
* @private
*/
void Modbus::process_F3_F4()
{
//Frame[2] is number of bytes returned in uint16_t = 2 bytes
if ( frame[2] == ( _packet->data * 2) )
{
uint8_t index = 3; //3nd bytes
for (uint8_t i=0;i < _packet->data; i++ )
{
_register_array[i] = ( frame[index] << 8) | frame[index+1];
index += 2; //increase 2 bytes
}
packetSuccess();
}
else
packetError();
}
/* Request Process packet of function 1 and function 2
* @param: none
* @return: Holding register in 2 bytes
* @private
*/
void Modbus::process_F1_F2()
{
//Serial.println("process_F1_F2");
// packet->data for function 1 & 2 is actually the number of boolean points
unsigned char no_of_registers = _packet->data / 16;
unsigned char number_of_bytes = no_of_registers * 2;
// if the number of points dont fit in even 2byte amounts (one register) then use another register and pad
if (_packet->data % 16 > 0)
{
no_of_registers++;
number_of_bytes++;
}
if (frame[2] == number_of_bytes) // check number of bytes returned
{
unsigned char bytes_processed = 0;
unsigned char index = 3; // start at the 4th element in the frame and combine the Lo byte
unsigned int temp;
for (unsigned char i = 0; i < no_of_registers; i++)
{
temp = frame[index];
bytes_processed++;
if (bytes_processed < number_of_bytes)
{
temp = (frame[index + 1] << 8) | temp;
bytes_processed++;
index += 2;
}
_register_array[_packet->register_start_address + i] = temp;
}
packetSuccess();
}
else // incorrect number of bytes returned
packetError();
}
/*---------------------------------------------------------------------*
* *
* getPacket - receive a packet from the camera. *
* Operation: *
* 1. Receive bytes until we get 'start transmission' *
* SIO_STX) *
* 2. Read the length of the packet, in the next 2 bytes. *
* 3. Read the packet bytes, calculating a checksum as *
* we go. If we get an un-escaped control character *
* during this phase, we've experienced a data error, *
* so restart the process. *
* 4. Read the end transmission byte, either SIO_ETB or *
* SIO_ETX. If we read anything else, we've got a *
* data error, so restart the process. *
* 5. Read the checksum byte. If it does not match our *
* calculated value, restart the process. *
* 6. Send positive acknowledgement to the camera *
* (SIO_ACK). *
* *
* When the camera finishes sending a packet, it expects us *
* to send SIO_ACK; if we do not in a reasonable amount of *
* time (???), it will attempt to send it again, an unknown *
* number of times. Unless our system is totally incapable *
* of handling the current baud-rate, we should eventually *
* recover from the error. *
* *
*---------------------------------------------------------------------*/
int qm100_getPacket(int serialdev, qm100_packet_block *packet)
{
unsigned char c, qm100_sum=0, sum=0;
short len, pos=0;
int retries=0;
static int pktcnt = 0;
++pktcnt;
restart:
++retries;
/*------------------------------------------------------------------*
* *
* Read the packet header, consisting of SIO_STX and length. *
* *
*------------------------------------------------------------------*/
while (c != SIO_STX)
c = qm100_readByte(serialdev);
qm100_sum = 0;
sum = 0;
pos = 0;
c=qm100_readCodedByte(serialdev);
len=c;
sum=c;
c=qm100_readCodedByte(serialdev);
len += (c<<8);
sum += c;
packet->packet_len=len;
/*------------------------------------------------------------------*
* *
* Read the data bytes for the packet *
* *
*------------------------------------------------------------------*/
while (len--)
{
c=qm100_readCodedByte(serialdev);
if (c == SIO_STX && !qm100_escapeCode)
{
packetError("Transmission data error", retries, pktcnt);
goto restart;
}
packet->packet[pos]=c;
pos++;
sum+=c;
}
if (qm100_trace)
dump(qm100_trace, "Receive Packet", packet->packet, packet->packet_len);
/*------------------------------------------------------------------*
* *
* Read the packet trailer, consisting of SIO_ETB or SIO_ETX, *
* followed by the checksum *
* *
*------------------------------------------------------------------*/
c=qm100_readByte(serialdev);
if (c==SIO_ETX)
packet->transmission_continues=0;
else if (c==SIO_ETB)
packet->transmission_continues=1;
else
{
packetError("Transmission trailer error", retries, pktcnt);
goto restart;
}
sum+=c;
sum=(sum & 0xff);
qm100_sum=qm100_readCodedByte(serialdev);
if (qm100_sum != sum)
{
packetError("Transmission checksum error", retries, pktcnt);
goto restart;
}
/*------------------------------------------------------------------*
* *
* Everything is AOK, so send the SIO_ACK *
* *
*------------------------------------------------------------------*/
qm100_writeByte(serialdev, SIO_ACK);
return 0;
}
int IcqProtocol::parsePacket(const Packet &p)
{
int ret = 0;
// Login?
debug() << "Packet ";
switch (p.hdr().channel())
{
case 1:
//if (SPacket::isType<PktSConnAck>(p))
//{
debug() << "> ConnAck" << endl;
if (_istate==AUTHORIZER)
{
// New connection Negotiation
// Request cookie
debug() << "Requesting cookie" << endl;
m_net->sendData(PktCCookieRequest(_seq++,
m_conf.child("user")["username"],
m_conf.child("user")["password"]));
}
else if (_istate == BOS)
{
m_net->sendData(PktCSignon(_seq++, _cookie));
}
//}
//else
//{
// packetError(&p, "Unknown packet on channel 1\n");
//}
break;
case 2:
// FNAC data
ret = parseFnac(p);
break;
case 3:
// FLAP level errors
break;
case 4:
// Close Connection Negotiation
if (_istate == AUTHORIZER)
{
PktSCookieReply r(p);
if (r.ok())
{
debug() << "> CookieReply" << endl;
_cookie = r.cookie();
// DEBUG_PRINT(8, "connecting to %s:%d", r.addr().c_str(), r.port());
m_net->disconnect();
m_net->connectTo(r.addr(), r.port());
m_state = S_connecting;
_istate=BOS;
break;
}
else
{
debug() << "> Error" << endl;
TLV t;
string uin, strerror;
uint16 interror=0;
Packet tmp = p;
while(tmp.getTLV(t))
{
if (t.type() == TLV::TLV_UIN)
uin = t.value();
else if (t.type() == TLV::TLV_ERRORCODE)
interror = charToU16(t.value());
else if (t.type() == TLV::TLV_ERRORMSG)
strerror = t.value();
}
switch (interror)
{
case 0x0005:
// Wrong password
m_state = S_offline;
m_net->disconnect();
eventError(interror, "Bad password");
eventLoggedOut();
break;
case 0x0018:
//Reconnecting too fast or something...
m_state = S_offline;
m_net->disconnect();
eventError(interror, "Reconnecting too fast: " + strerror);
eventLoggedOut();
//icq_loggedOut("0x0018: Reconnecting too fast: " + string(msg.value()));
break;
default:
cerr << "Error in cookie reply" << endl;
cerr << "Uin: " << uin << endl;
cerr << "Error: 0x" << interror << endl;
cerr << "Msg: " << strerror << endl;
packetError(&p, "");
m_state = S_offline;
m_net->disconnect();
eventError(interror, "Authorizer error:" + strerror);
eventLoggedOut();
break;
}
ret = -1;
break;
}
}
m_state = S_offline;
m_net->disconnect();
eventLoggedOut();
break;
default:
debug() << "> Unknown" << endl;
packetError(&p, "Unknown channel ID: %d\n", p.hdr().channel());
break;
}
return ret;
}
/* Request Check validity of packet and process the result
* @param: none
* @return: none
* @private
*/
void Modbus::checkPacket()
{
//Check packet response
uint8_t buffer = getPacket();
//if there is nothing received -> return
if ( buffer == 0 )
return;
#if DEBUG_UART
print(F("Response: "));
for (uint8_t i=0;i<buffer;i++)
{
print(frame[i]);
}
print('\t');
#endif
uint8_t stt = 0;
//Check exception response. Slave with OR with 0x80 if exception exists
if ( (frame[1] & 0x80) == 0x80 )
{
println(F("Packet errors"));
switch(frame[2]) //3rd is the exception code
{
case ILLEGAL_FUNCTION:
println(F("Illegal function or not support"));
break;
case ILLEGAL_DATA_VALUE:
println(F("Illegal Value"));
break;
case ILLEGAL_DATA_ADDRESS:
println(F("Illegal data address"));
break;
default:
println(F("Misc exception"));
}
packetError();
return;
}//check exception
uint16_t received_crc = ((frame[buffer - 2] << 8) | frame[buffer - 1]);
uint16_t calculated_crc = calculateCRC(buffer - 2);
if ( calculated_crc == received_crc ) //verify checksum
{
#if DEBUG_UART
print("CRC! ");
#endif
//Check packet functions
switch( frame[1] )
{
case READ_COIL_STATUS:
case READ_INPUT_STATUS:
process_F1_F2();
break;
case READ_INPUT_REGISTERS:
case READ_HOLDING_REGISTERS:
process_F3_F4();
break;
case FORCE_SINGLE_COIL:
case PRESET_SINGLE_REGISTER:
case FORCE_MULTIPLE_COILS:
case PRESET_MULTIPLE_REGISTERS:
process_F5_F6_F15_F16();
break;
default: // illegal function returned
packetError();
break;
}
return;
}//CRC check
else
{
packetError();
println(F("CRC errors"));
return;
}
}
