void readExtFlash(void)
{
RadioInfoPacket_t packet;
bool prevMissed = false;
while (extFlashAddress < EXT_FLASH_SIZE) {
extFlashRead(extFlashAddress, &packet, sizeof(packet));
if (packet.crc == crc16((uint8_t *)&packet, sizeof(packet) - sizeof(uint16_t))) {
if (prevMissed) {
PRINT("corrupt packet\n");
}
prevMissed = false;
printPacket(&packet);
} else {
// XXX: this is supposed to find first non-packet, but it will find first two invalid packets!!
if (!prevMissed) {
prevMissed = true;
} else {
break;
}
}
extFlashAddress += sizeof(packet);
}
// PRINTF("flash packet offset=%lu\n", extFlashAddress - EXT_FLASH_RESERVED);
}
void readExtFlash(void)
{
DataPacket_t packet;
bool prevMissed = false;
while (extFlashAddress < EXT_FLASH_SIZE) {
extFlashRead(extFlashAddress, &packet, sizeof(packet));
bool valid = true;
if (packet.crc != crc16((uint8_t *)&packet, sizeof(packet) - sizeof(uint16_t))) {
valid = false;
} else {
if (packet.dataSeqnum == 0) valid = false;
}
if (valid) {
if (prevMissed) {
PRINT("corrupt packet\n");
}
prevMissed = false;
printPacket(&packet);
} else {
// XXX: this is supposed to find first non-packet, but it will find first two invalid packets!!
if (!prevMissed) {
prevMissed = true;
} else {
// break;
PRINT("bad!\n");
}
}
extFlashAddress += sizeof(packet);
}
}
int ModbusClientV1::readPacket(RSPacket::PacketStruct * p)
{
if (verbose > 2)
{
printf("READING PACKET\n");
}
uint16_t buffer[RS_MAX_PACKET_SIZE] =
{ 0 };
if (!isInitialized)
{
fprintf(stderr, "readPacket failed! Modbus is not initialized!\n");
return -1;
}
int rc = modbus_read_registers(ctx, 0, RS_MAX_PACKET_SIZE, /*buffer*/
(uint16_t*) p); // Read all registers
if (rc == -1)
{
fprintf(stderr, "Failed to read data [%s]\n", modbus_strerror(errno));
}
else
{
// uint8_t * b = (uint8_t*)b;
// uint8_t * data = &b[3];
// uint8_t size = b[2];
// assert(size % 2 == 0);
// RSPacket::swapWords(data, size/2);
if (verbose > 3)
printPacket(*p);
}
return rc;
}
static void canRead(void) {
scheduleMsg(&logger, "waiting for CAN");
canReceive(&EFI_CAN_DEVICE, CAN_ANY_MAILBOX, &rxBuffer, TIME_INFINITE);
canReadCounter++;
printPacket(&rxBuffer);
obdOnCanPacketRx(&rxBuffer);
}
int main(int argc, char **argv) {
packet test_pack;
STAT_packet *data = (STAT_packet*) malloc( sizeof(STAT_packet) );
data->mount = 1234;
strcpy(data->filereq, "test.txt");
test_pack.request = eSTAT;
test_pack.data = data;
printPacket(&test_pack);
char *serialized = serializePacket( &test_pack );
packet *dsPack;
dsPack = deserializePacket( serialized );
printPacket(dsPack);
return 0;
}
void pretendSend(Packet *p) {
uint8_t *buffer;
size_t length = serializePacket(p, &buffer);
Packet rec;
parsePacket(buffer, length, &rec);
printf("\nSent:\n");
printPacket(p);
printf("Received:\n");
printPacket(p);
printf("\n");
// Fail the test if the packets are different after being serialized
assert(comparePackets(p, &rec));
freePacket(&rec);
free(buffer);
}
//-------------------------------------------
// Entry point for the application
//-------------------------------------------
void appMain(void)
{
PRINTF("\n\nAccelerometer data gathering app\n");
extFlashWake();
// start with clean and new...
cleanFlash();
accelOn();
userButtonEnable(onUserButton);
mdelay(START_DELAY);
uint16_t lastSecond;
uint16_t samplesPerSecond;
for (;;) {
while (!isActiveMode);
redLedOn();
lastSecond = getTimeSec();
samplesPerSecond = 0;
uint16_t i;
for (i = 0; isActiveMode; i++) {
if (i % 10 == 0) redLedToggle();
uint32_t now = getTimeMs();
uint32_t endTime = now + MAIN_LOOP_LENGTH;
Packet_t p;
p.timestamp = now;
p.acc_x = accelReadX();
p.acc_y = accelReadY();
p.acc_z = accelReadZ();
printPacket(&p);
samplesPerSecond++;
uint16_t currentSecond = getTimeSec();
if (currentSecond != lastSecond) {
PRINTF("samples per second = %u\n", samplesPerSecond);
lastSecond = currentSecond;
samplesPerSecond = 0;
}
while (timeAfter32(endTime, getTimeMs()));
}
redLedOff();
}
}
void appMain(void)
{
uint16_t i;
// ------------------------- serial number
DPRINTF("Mote %#04x starting...\n", localAddress);
// ------------------------- external flash
#if WRITE_TO_FLASH
prepareExtFlash();
#endif
// ------------------------- light sensors
if (localAddress != 0x0796) {
PRINT("init isl\n");
islInit();
islOn();
} else {
PRINT("init ads\n");
adsInit();
adsSelectInput(0);
}
// ------------------------- main loop
mdelay(3000);
DPRINTF("starting main loop...\n");
for (i = 0; i < 6; ++i) {
redLedToggle();
mdelay(100);
}
ledOff();
uint32_t nextDataReadTime = 0;
uint32_t nextBlinkTime = 0;
for (;;) {
uint32_t now = getRealTime();
if (timeAfter32(now, nextDataReadTime)) {
if (getJiffies() < 300 * 1000ul ) {
nextDataReadTime = now + DATA_INTERVAL_SMALL;
} else {
nextDataReadTime = now + DATA_INTERVAL;
}
DataPacket_t packet;
readSensors(&packet);
#if PRINT_PACKET
printPacket(&packet);
#endif
}
if (timeAfter32(now, nextBlinkTime)) {
nextBlinkTime = now + BLINK_INTERVAL;
ledOn();
msleep(100);
ledOff();
}
msleep(1000);
}
}
void readANT(){
unsigned char packet[MAXPACKETLEN];
int packetsRead;
unsigned char msgId, msgSize;
unsigned char *msgData;
packetsRead = readPacket(packet, MAXPACKETLEN, PACKETREADTIMEOUT);
if (packetsRead > 0) {
msgId = packet[2];
msgSize = packet[1];
msgData = &packet[3];
switch (msgId) {
case MESG_RESPONSE_EVENT_ID:
printHeader("MESG_RESPONSE_EVENT_ID: ");
printPacket(packet);
break;
case MESG_CAPABILITIES_ID:
printHeader("MESG_CAPABILITIES_ID: ");
printPacket(packet);
break;
case MESG_BROADCAST_DATA_ID:
if (oldHeartRate != msgData[msgSize-1]) {
oldHeartRate = msgData[msgSize-1];
#ifdef DEBUG
printHeader("New Heart Rate: ");
printf("%i \n", oldHeartRate);
#endif
IOWR(HEARTRATEOUT_BASE, 0, oldHeartRate); //write to parallel out
IOWR(WDT_BASE, 2, 0xFF); //reset watchdog timer
}
break;
default:
printHeader("MESG_ID_UKNOWN: ");
printPacket(packet);
break;
}
}
}
void RegisterList::loadPacket(int nReg, byte *b, int nBytes, int nRepeat, int printFlag) volatile {
nReg=nReg%((maxNumRegs+1)); // force nReg to be between 0 and maxNumRegs, inclusive
while(nextReg!=NULL); // pause while there is a Register already waiting to be updated -- nextReg will be reset to NULL by interrupt when prior Register updated fully processed
if(regMap[nReg]==NULL) // first time this Register Number has been called
regMap[nReg]=maxLoadedReg+1; // set Register Pointer for this Register Number to next available Register
Register *r=regMap[nReg]; // set Register to be updated
Packet *p=r->updatePacket; // set Packet in the Register to be updated
byte *buf=p->buf; // set byte buffer in the Packet to be updated
b[nBytes]=b[0]; // copy first byte into what will become the checksum byte
for(int i=1;i<nBytes;i++) // XOR remaining bytes into checksum byte
b[nBytes]^=b[i];
nBytes++; // increment number of bytes in packet to include checksum byte
buf[0]=0xFF; // first 8 bytes of 22-byte preamble
buf[1]=0xFF; // second 8 bytes of 22-byte preamble
buf[2]=0xFC + bitRead(b[0],7); // last 6 bytes of 22-byte preamble + data start bit + b[0], bit 7
buf[3]=b[0]<<1; // b[0], bits 6-0 + data start bit
buf[4]=b[1]; // b[1], all bits
buf[5]=b[2]>>1; // b[2], bits 7-1
buf[6]=b[2]<<7; // b[2], bit 0
if(nBytes==3){
p->nBits=49;
} else{
buf[6]+=b[3]>>2; // b[3], bits 7-2
buf[7]=b[3]<<6; // b[3], bit 1-0
if(nBytes==4){
p->nBits=58;
} else{
buf[7]+=b[4]>>3; // b[4], bits 7-3
buf[8]=b[4]<<5; // b[4], bits 2-0
if(nBytes==5){
p->nBits=67;
} else{
buf[8]+=b[5]>>4; // b[5], bits 7-4
buf[9]=b[5]<<4; // b[5], bits 3-0
p->nBits=76;
} // >5 bytes
} // >4 bytes
} // >3 bytes
nextReg=r;
this->nRepeat=nRepeat;
maxLoadedReg=max(maxLoadedReg,nextReg);
if(printFlag && SHOW_PACKETS) // for debugging purposes
printPacket(nReg,b,nBytes,nRepeat);
} // RegisterList::loadPacket
SerialPacket* turnPacket(char direction, int degrees) {
SerialPacket* packet = newPacket();
packet->type = TURN;
packet->data1 = (uint8)direction;
packet->data2 = (uint8)degrees;
printPacket(packet);
return packet;
}
bool PacketManager::broadcastPacket(uint8 *data, uint32 length, uint8 channelNo, uint32 flag)
{
PDEBUG_LOG_LINE(Log::getMainInstance()," Broadcast packet:\n");
printPacket(data, length);
if(length >= 8)
_blowfish->Encrypt(data, length-(length%8)); //Encrypt everything minus the last bytes that overflow the 8 byte boundary
ENetPacket *packet = enet_packet_create(data, length, flag);
enet_host_broadcast(_server, channelNo, packet);
return true;
}
uint8_t Bowler_Server_Local(BowlerPacket * Packet){
Print_Level l = getPrintLevel();
//setPrintLevelNoPrint();
if (GetBowlerPacket_arch(Packet)){
//setLed(1,1,1);
if(Packet->use.head.RPC != _PNG){
println_I("Got:");printPacket(Packet,INFO_PRINT);
}
if ( (CheckAddress(MyMAC.v,Packet->use.head.MAC.v) == true) || ((CheckAddress((uint8_t *)Broadcast.v,(uint8_t *)Packet->use.head.MAC.v) == true) )) {
float start=getMs();
Process_Self_Packet(Packet);
if(getMs()-start>5){
println_E("Process too long: ");p_fl_E(getMs()-start);
}
for (i=0;i<6;i++){
Packet->use.head.MAC.v[i]=MyMAC.v[i];
}
SetCRC(Packet);
start=getMs();
PutBowlerPacket(Packet);
if(getMs()-start>5){
println_E("Return too long: ");p_fl_E(getMs()-start);
}
if(Packet->use.head.RPC != _PNG){
println_I("Response:");printPacket(Packet,INFO_PRINT);
}
}else{
//println_I("Packet not addressed to me: ");printByteArray(Packet->use.head.MAC.v,6); print_I(" is not mine: ");printByteArray(MyMAC.v,6);
}
//setLed(0,0,1);
setPrintLevel(l);
return true;
}//Have a packet
setPrintLevel(l);
return false;
}
void appMain(void)
{
// ------------------------- serial number
PRINTF("Mote %#04x starting...\n", localAddress);
// ------------------------- light sensors
islInit();
islOn();
for (;;) {
DataPacket_t packet;
readSensors(&packet);
printPacket(&packet);
mdelay(2000);
}
}
void printUdp(const struct udphdr* udp, int verbosite)
{
printf("**********UDP**********\n");
printf("Source port: %u\n",ntohs(udp->source));
printf("Destination port: %u\n", ntohs(udp->dest));
if(verbosite > 1)
{
printf("Header size: %d\n", ntohs(udp->len));
printf("Checksum: %d\n", ntohs(udp->check));
if(verbosite > 2)
{
printPacket((const u_char*) udp, ntohs(udp->len));
printf("\n");
}
}
printf("\n");
}
bool PacketManager::sendPacket(ENetPeer *peer, uint8 *data, uint32 length, uint8 channelNo, uint32 flag)
{
PDEBUG_LOG_LINE(Log::getMainInstance()," Sending packet:\n");
if(length < 300)
printPacket(data, length);
if(length >= 8)
_blowfish->Encrypt(data, length-(length%8)); //Encrypt everything minus the last bytes that overflow the 8 byte boundary
ENetPacket *packet = enet_packet_create(data, length, flag);
if(enet_peer_send(peer, channelNo, packet) < 0)
{
PDEBUG_LOG_LINE(Log::getMainInstance(),"Warning fail, send!");
return false;
}
return true;
}
void printTcp(const struct tcphdr* tcp, int verbosite)
{
printf("***********TCP*********\n");
printf("Source port: %u\n", ntohs(tcp->source));
printf("Destination port: %u\n", ntohs(tcp->dest));
if(verbosite ==1)
{
printf("Flag:");
if(tcp->urg != 0 )
printf("URGENT ");
if(tcp->ack != 0 )
printf("ACK ");
if(tcp->psh != 0 )
printf("PUSH ");
if(tcp->rst != 0 )
printf("RESET ");
if(tcp->syn != 0 )
printf("SYN ");
if(tcp->fin != 0 )
printf("FINISH ");
printf("\n");
}
else
{
printf("Flags URGENT : %d\n",tcp->urg);
printf("Flags ACK : %d\n",tcp->ack);
printf("Flags PUSH : %d\n",tcp->psh);
printf("Flags RESET : %d\n",tcp->rst);
printf("Flags SYN : %d\n",tcp->syn);
printf("Flags FINISH : %d\n",tcp->fin);
}
if(verbosite > 2)
{
//printf("Data Offset:%d\n", ntohs(tcp->th_off));
printf("Window: %d\n", ntohs(tcp->window));
printf("Checksum: %d\n",ntohs(tcp->check));
printf("Urgent Pointer: %d\n", ntohs(tcp->urg_ptr));
printPacket((const u_char*) tcp, tcp->doff*4);
printf("\n");
}
printf("\n");
}
/*FUNC: handles incoming OKTF:s*/
int rwg_handle_oktf(rwg_header *rwghdr, packetBuffer *packetBuff)
{
if(TRACE){printf("rwg_handle_oktf: This packet is of type: OKTF\n");}
int match;
unsigned char packet_id[8];
rwg_header tmp;
memcpy(packet_id,rwghdr->origin,sizeof(rwghdr->origin));
memcpy(packet_id+sizeof(rwghdr->origin),&rwghdr->sequenceNumber,sizeof(rwghdr->sequenceNumber));
match = rwg_match_packetid(packet_id,packetBuff);
//update the visted and recent visited vector if the REQF exists in the REQF buffer
if(match >= 0){
rwg_bitvector_update(((rwg_header *)packetBuff->reqf[match].reqf)->visited,rwghdr->visited);
rwg_bitvector_update(((rwg_header *)packetBuff->reqf[match].reqf)->recentVisited,rwghdr->recentVisited);
}else{
if(TRACE){printf("rwg_handle_oktf: OKTF no match in reqf buffer\n");}
return 0;
}
//check oktf target
if(memcmp(rwghdr->target,sender,sizeof(tmp.target)) == 0){
if(TRACE){
printf("rwg_handle_oktf: Target match\n");
printPacket((unsigned char *)rwghdr,rwghdr->packetLength);
}
packetBuff->active_reqf.reqf = packetBuff->reqf[match].reqf;
packetBuff->active_reqf.reqf_pos = match;
}else{
if(TRACE){printf("rwg_handle_oktf: OKTF target does not match, visited/recentVisited updated\n");}
return 0;
}
//check if the visited if the visited list >= groupSize
if((rwg_bitvector_count(((rwg_header *)packetBuff->reqf[match].reqf)->visited) >= ((rwg_header *)packetBuff->reqf[match].reqf)->groupSize)){
if(TRACE){printf("rwg_handle_oktf, will not forward");}
return 0;
}
//set flag to forward REQF
SEND_REQF_F = 1;
return 1;
}
int checkReturn() {
unsigned char packet[MAXPACKETLEN];
int packetsRead;
packetsRead = readPacket(packet, MAXPACKETLEN, PACKETREADTIMEOUT);
// Data <sync> <len> <msg id> <channel> <msg id being responded to> <msg code> <chksum>
// <sync> always 0xa4
// <msg id> always 0x40 denoting a channel response / event
// <msg code? success is 0. See page 84 of ANT MPaU for other codes
if (packetsRead > 0) {
#ifdef DEBUG
printf("RX: ");
printPacket(packet);
#endif
}
return packetsRead;
}
void SetName(char * name) {
//WORD_VAL raw;
uint8_t i = 0;
println_E(name);
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_POST;
downstreamPacketTemp.use.head.RPC = GetRPCValue(eepd);
downstreamPacketTemp.use.data[0] = NAMESTART;
downstreamPacketTemp.use.data[1] = LOCKSTART;
while (name[i] != '\0') {
downstreamPacketTemp.use.data[2 + i] = name[i];
i++;
buttonCheck(9);
if (i == NAMESIZE)
break;
}
downstreamPacketTemp.use.data[2 + i] = '\0';
downstreamPacketTemp.use.head.DataLegnth = 6 + i + 1;
printPacket(&downstreamPacketTemp,ERROR_PRINT);
SendPacketToCoProc(&downstreamPacketTemp);
}
__interrupt void PORT2 (void) {
char res ; // CRC Check
if (P2IFG & 0x01) // Check P2IFG Bit P2.0 - CC1100 Rx Packet
{
CLEAR(P2IFG, 0x01);
LEDTOGGLE;
res = receivePacket(); // CRC Rückgabe
if (res) // wenn Packet OK ...
{
printPacket(); // Packet auf Terminal ausgeben
}
else
{
spiStrobe(CC1100_SIDLE); // Switch to IDLE
spiStrobe(CC1100_SFRX); // Flush the RX FIFO
}
}
else
{
CLEAR(P2IFG, 0xFF); // Clear all flags
}
spiStrobe(CC1100_SRX); // Switch to RX Mode
}
void CServerDevice::onReceive(const int nSocketFD, const void *pData)
{
int nRet = -1;
int nPacketLen = 0;
CMP_HEADER cmpHeader;
char *pPacket;
pPacket = (char*) const_cast<void*>(pData);
memset(&cmpHeader, 0, sizeof(CMP_HEADER));
cmpHeader.command_id = cmpParser->getCommand(pPacket);
cmpHeader.command_length = cmpParser->getLength(pPacket);
cmpHeader.command_status = cmpParser->getStatus(pPacket);
cmpHeader.sequence_number = cmpParser->getSequence(pPacket);
printPacket(cmpHeader.command_id, cmpHeader.command_status, cmpHeader.sequence_number, cmpHeader.command_length,
"[CServerDevice] Recv ", nSocketFD);
if (cmpParser->isAckPacket(cmpHeader.command_id))
{
return;
}
map<int, MemFn>::iterator iter;
iter = mapFunc.find(cmpHeader.command_id);
if (0x000000FF < cmpHeader.command_id || 0x00000000 >= cmpHeader.command_id || mapFunc.end() == iter)
{
sendPacket(dynamic_cast<CSocket*>(serverDevice), nSocketFD, generic_nack | cmpHeader.command_id,
STATUS_RINVCMDID, cmpHeader.sequence_number, 0);
return;
}
(this->*this->mapFunc[cmpHeader.command_id])(nSocketFD, cmpHeader.command_id, cmpHeader.sequence_number, pPacket);
}
boolean GetName(char * name) {
//WORD_VAL raw;
uint8_t i = 0;
LoadCorePacket(&downstreamPacketTemp);
downstreamPacketTemp.use.head.Method = BOWLER_GET;
downstreamPacketTemp.use.head.RPC = GetRPCValue(eepd);
downstreamPacketTemp.use.data[0] = NAMESTART;
downstreamPacketTemp.use.data[1] = LOCKSTART;
downstreamPacketTemp.use.head.DataLegnth = 6;
SendPacketToCoProc(&downstreamPacketTemp);
printPacket(&downstreamPacketTemp,WARN_PRINT);
while (downstreamPacketTemp.use.data[i] != '\0') {
name[i] = downstreamPacketTemp.use.data[i];
i++;
buttonCheck(11);
if (i == NAMESIZE)
break;
}
name[i] = '\0';
println_W(name);
return isAscii(name);
}
int ModbusClientV1::writePacket(RSPacket::PacketStruct * p, int size)
{
if (verbose > 2)
{
printf("SENDING PACKET\n");
}
if (verbose > 3)
printPacket(*p);
if (!isInitialized)
{
fprintf(stderr, "writePacket failed! Modbus is not initialized!\n");
return -1;
}
int rc = modbus_write_registers(ctx, 0, size, (uint16_t*) p);
if (rc == -1)
{
fprintf(stderr, "Failed to write BallsCount data [%s]\n",
modbus_strerror(errno));
}
return rc;
}
bool PacketHandler::handlePacket(ENetPeer *peer, ENetPacket *packet, uint8 channelID)
{
m_CurrPeer = peer;
if(packet->dataLength >= 8)
{
if(peerInfo(peer)->keyChecked)
_blowfish->Decrypt(packet->data, packet->dataLength-(packet->dataLength%8)); //Encrypt everything minus the last bytes that overflow the 8 byte boundary
}
PacketHeader *header = reinterpret_cast<PacketHeader*>(packet->data);
printf("Handling OpCode %02X\n", header->cmd);
bool (PacketHandler::*handler)(HANDLE_ARGS) = _handlerTable[header->cmd][channelID];
if(handler)
{
return (*this.*handler)(packet);
}
else
{
PDEBUG_LOG_LINE(Logging,"Unknown packet: CMD %X(%i) CHANNEL %X(%i)\n", header->cmd, header->cmd,channelID,channelID);
printPacket(packet->data, packet->dataLength);
}
return false;
}
void printIP(const struct ip* ip, int verbosite)
{
char *aux = inet_ntoa(ip->ip_src);
char *ab = strcpy(malloc(strlen(aux)+1), aux);
char *bux = inet_ntoa(ip->ip_dst);
char *cd = strcpy(malloc(strlen(aux)+1), bux);
printf("**********IP**********\n");
printf("From IP: %s\nTo: %s\n",ab,cd);
if(verbosite>1)
{
printf("Version: %d\n", IP_V(ip));
printf("Length: %d\n", ip->ip_len);
printf("Type de service : %d\n", ip->ip_tos);
printf("Identification : %d\n", ip->ip_id);
if(verbosite>2)
{
printf("Fragment offset: %d\n", ip->ip_off);
printf("Time to live: %d\n", ip->ip_ttl);
printf("Checksum: %d\n", ip->ip_sum);
printPacket((u_char *) ip, sizeof(struct ip));
}
}
printf("\n");
}
void ProcessAsyncData(BowlerPacket * Packet){
//println_I("**Got Async Packet**");
//printPacket(Packet,INFO_PRINT);
Print_Level l = getPrintLevel();
setPrintLevelInfoPrint();
if (Packet->use.head.RPC==GCHV){
BYTE pin = Packet->use.data[0];
BYTE mode = GetChannelMode(pin);
if(mode == IS_ANALOG_IN ){
UINT16_UNION ana;
ana.byte.SB = Packet->use.data[1];
ana.byte.LB = Packet->use.data[2];
//ADC_val[pin-8]=ana.Val;
if(ana.Val>=0 && ana.Val<1024)
SetValFromAsync(pin,ana.Val);//asyncData[pin].currentVal=ana.Val;
println_I("***Setting analog value: ");p_int_I(pin);print_I(", ");p_int_I(ana.Val);
}
else if((mode == IS_DI) || (mode == IS_COUNTER_INPUT_HOME)|| (mode == IS_COUNTER_OUTPUT_HOME) || mode == IS_SERVO){
//DIG_val[pin]=Packet->use.data[1];
SetValFromAsync(pin,Packet->use.data[1]);//asyncData[pin].currentVal=Packet->use.data[1];
println_I("***Setting digital value: ");p_int_I(pin);print_I(", ");p_int_I(Packet->use.data[1]);//printStream(DIG_val,NUM_PINS);
}else {
if(IsAsync(pin)){
println_I("Sending async packet, not digital or analog");
PutBowlerPacket(Packet);
}
}
}else if (Packet->use.head.RPC==AASN){
int i;
for(i=0;i<8;i++){
BYTE pin = i+8;
BYTE mode = GetChannelMode(pin);
if(mode == IS_ANALOG_IN ){
UINT16_UNION ana;
ana.byte.SB = Packet->use.data[i*2];
ana.byte.LB = Packet->use.data[(i*2)+1];
//ADC_val[pin-8]=ana.Val
if(ana.Val>=0 && ana.Val<1024);
SetValFromAsync(pin,ana.Val);//asyncData[pin].currentVal=ana.Val;
}
}
}else if (Packet->use.head.RPC==DASN){
int i;
for(i=0;i<GetNumberOfIOChannels();i++){
BYTE mode = GetChannelMode(i);
if((mode == IS_DI) || (mode == IS_COUNTER_INPUT_HOME)|| (mode == IS_COUNTER_OUTPUT_HOME)|| (mode == IS_SERVO)){
SetValFromAsync(i,Packet->use.data[i]);//asyncData[i].currentVal=Packet->use.data[i];
}
}
//println_I("***Setting All Digital value: ");
}if (Packet->use.head.RPC==GetRPCValue("gacv")){
int i;
int val;
for(i=0;i<GetNumberOfIOChannels();i++){
val = get32bit(Packet, i*4);
if(getBcsIoDataTable()[i].PIN.asyncDataCurrentVal!=val){
println_I("Data on Pin ");p_int_I(i);print_I(" to val ");p_int_I(val);
SetValFromAsync(i,val);//
}
}
}else{
println_W("***Async packet not UNKNOWN***");
printPacket(Packet,WARN_PRINT);
}
// println_I("***Setting All value: [");
// int i;
// for(i=0;i<NUM_PINS;i++){
// p_int_I(asyncData[i].currentVal);print_I(" ");
// }
// print_I("]");
setPrintLevel(l);
}
int my_main(const char* str_latero_ip, int print_response, int npack,
unsigned int* dacval, int dacval_cnt,
char rd, char wr, int addr, int value, char dst_main, char dst_io, int testpattern )
{
latero_conn latero;
/*
struct sockaddr_in si_server, si_other;
int s, ii, slen=sizeof(si_other);
char pktbuff[BUFLEN];
char rspbuff[BUFLEN];
*/
int ii;
uint8_t blades[64];
uint16_t dio_out = 0x0000;
// For raw address commands
uint16_t saddr = 0x0000;
uint16_t sdata = 0x0000;
latero_dst_device destination;
int numbytes = 0;
latero_pkt_t response;
printf("Init Connection: ");
if ( ii = init_connection( &latero, str_latero_ip ) < 0 ) {
printf("Error (%d)!\n",ii );
return(-1);
} else {
printf("OK.\n");
}
// Sanitize the dac values
for(ii = 0 ; ii < 4 ; ii++ ) {
if (ii < dacval_cnt ) {
setDAC( &latero, ii, dacval[ii] );
}
}
/*
for(ii = 0 ; ii<64 ; ii++ ) {
blades[ii] = ii;
}
setBlades( &latero, blades );
*/
// Run a few test cases here
TestInit();
ii = 0;
switch(testpattern) {
case 1:
TestSplit1(&latero);
sleep(1);
TestSplit2(&latero);
break;
case 2:
TestAllpin(&latero);
break;
case 3:
TestRow(&latero);
sleep(1);
TestCol(&latero);
break;
case 4:
// Test continuously without stopping
while(1) {
printf("%d ", ii);
TestRow(&latero);
printf("%d ", ii);
TestCol(&latero);
ii++;
}
break;
}
/* Just on led is turned on... */
setDIO( &latero, 0xDEAC );
// setDAC( &latero, 0x0, 0x1234 );
// setDAC( &latero, 0x1, 0x5678 );
// setDAC( &latero, 0x2, 0x9ABC );
// setDAC( &latero, 0x3, 0xDEF0 );
printf("Sending\n");
sendNormalPacket( &latero, &response );
printf("printing\n");
printPacket( &response );
/*
printf("Test Connection\n");
if( test_connection( &latero ) < 0 ) {
fprintf(stderr,"The test_connection() failed\n");
} else {
printf("Connection with Latero is OK.\n");
}
*/
if( rd > 0 || wr > 0 ) {
saddr = addr & 0xFFFF;
sdata = value & 0xFFFF;
if ( dst_main > 0 ) {
destination = LATERO_CONTROLLER;
} else if ( dst_io > 0 ) {
destination = LATERO_IO;
}
if( rd > 0 ) {
rawRead( &latero, destination, saddr, &sdata );
printf("Data Read at address 0x%4.4X = 0x%4.4X\n", saddr,sdata);
} else { /* Write */
rawWrite(&latero, destination, saddr, sdata );
printf("0x%4.4X written at address 0x%4.4X\n", sdata,saddr );
}
} else {
// Normal Latero Packets
//fillNormalPacket(PKT_TYPE_FULL, dac, blades, dio_out, &packetSend);
}
return( close_connection( &latero ) );
}
/*FUNC: handles incoming REQF:s*/
int rwg_handle_reqf(rwg_header *rwghdr, packetBuffer *packetBuff, int outPipe)
{
if(TRACE){printf("rwg_handle_refq: RWG packet is of type REQF\n");}
//will be used for checking if incoming REQF matches REQF in buffer
unsigned char packet_id[8];
int match;
memcpy(packet_id,rwghdr->origin,sizeof(unsigned char)*6);
memcpy(packet_id+sizeof(unsigned char)*6,&rwghdr->sequenceNumber,sizeof(unsigned short int));
unsigned short int* rVisited = rwghdr->recentVisited;
unsigned short int* visited = rwghdr->visited;
t_stamp stamp;
//Checks if this packet already exists in the REQF buffer, and updates the visited list.
// Also sets this node to the visited list(probably redundant)
if((match = rwg_match_packetid(packet_id,packetBuff)) >= 0){
if(TRACE){printPacket((unsigned char *)packetBuff->reqf[match].reqf,((rwg_header *)packetBuff->reqf[match].reqf)->packetLength);}
rwg_bitvector_update(((rwg_header *)packetBuff->reqf[match].reqf)->visited,rwghdr->visited);
rwg_bitvector_update(((rwg_header *)packetBuff->reqf[match].reqf)->recentVisited,rwghdr->recentVisited);
}
//checks if this node have messages that the sender of the incoming REQF do not have
unsigned char incomingSender[6];
memcpy(&incomingSender,rwghdr->sender,sizeof(unsigned char)*6);
rwg_wake(incomingSender, packetBuff);
//if already visited but not recent vistited (a message will empty recentVisited when hops > hopLimit)
if(rwg_bitvector_lookup(visited, hashedAddr) && !rwg_bitvector_lookup(rVisited, hashedAddr)){
if(TRACE){ printf("rwg_handle_reqf: Is already visited but not recentVisited");}
if(match >= 0){
//changes to the new recentVisited vector and changes the sender of the stored packet (so the ACK will be sent to)
//the correct node)
memcpy(((rwg_header *)packetBuff->reqf[match].reqf)->recentVisited,rwghdr->recentVisited,sizeof(((rwg_header *)packetBuff->reqf[match].reqf)->recentVisited));
memcpy(((rwg_header *)packetBuff->reqf[match].reqf)->sender, rwghdr->sender,sizeof(((rwg_header *)packetBuff->reqf[match].reqf)->sender));
rwg_bitvector_setbit(((rwg_header *)packetBuff->reqf[match].reqf)->recentVisited,hashedAddr);
packetBuff->active_reqf.reqf = packetBuff->reqf[match].reqf;
packetBuff->active_reqf.reqf_pos = match;
SEND_ACK = 1;
return 1;
}else{
if(TRACE){printf("rwg_handle_refq: REQF does not exist in buffer (perhaps restarted node)\n");}
}
//if already visited
}else if(rwg_bitvector_lookup(visited, hashedAddr)){
if(TRACE){ printf("rwg_handle_refq: Is already visited\n");}
//Find buffered REFQ with matching packetid and schedule a send BS if groupSize < visted nodes
if(match >= 0){
//check size of visited list, save packet id and schedule send bs
if(rwg_bitvector_count(((rwg_header *)packetBuff->reqf[match].reqf)->visited) >= ((rwg_header *)packetBuff->reqf[match].reqf)->groupSize){
packetBuff->active_reqf.reqf = packetBuff->reqf[match].reqf;
packetBuff->active_reqf.reqf_pos = match;
SEND_BS = 1;
return 1;
}
return 0;
}else{
if(TRACE){printf("rwg_handle_refq: REQF does not exist in buffer (perhaps restarted node)\n");}
}
//if not already recent visited
}else if(!rwg_bitvector_lookup(rVisited, hashedAddr)){
//new reqf have arrived
if(TRACE){printf("rwg_handle_reqf: Not recent visited\n");}
if(match < 0){
//update recentVisited and visited
rwg_bitvector_setbit(rwghdr->recentVisited,hashedAddr);
rwg_bitvector_setbit(rwghdr->visited,hashedAddr);
//writes the payload to the output buffer
// if(!rwg_write_output((unsigned char *)rwghdr + sizeof(rwg_header),outPipe,rwghdr->packetLength-sizeof(rwg_header))){perror("rwg_handle_refq, failed to write to buffer\n");}
if(!rwg_write_output((unsigned char *)rwghdr,outPipe,rwghdr->packetLength)){
perror("rwg_handle_refq, failed to write to buffer\n");
}
//copy packet and add a pointer of the copy to (reqf) packetBuffer buffer, add lenght n' increase counter
unsigned char *reqfCopy = (unsigned char*)malloc(rwghdr->packetLength);
memcpy(reqfCopy,rwghdr,rwghdr->packetLength);
packetBuff->reqf[packetBuff->reqf_counter].reqf = reqfCopy;
packetBuff->reqf[packetBuff->reqf_counter].wake = 0;
//set the time stamp arrived_at in buffer
set_time_stamp(&stamp);
packetBuff->reqf[packetBuff->reqf_counter].arrived_at.seconds = stamp.seconds;
packetBuff->reqf[packetBuff->reqf_counter].arrived_at.u_seconds = stamp.u_seconds;
packetBuff->reqf[packetBuff->reqf_counter].reqf_pos = packetBuff->reqf_counter;
packetBuff->active_reqf.reqf = packetBuff->reqf[packetBuff->reqf_counter].reqf;
packetBuff->active_reqf.reqf_pos = packetBuff->reqf_counter;
packetBuff->reqf_counter++;
//check if the groupSize have been reached, if so send a BS
if(rwg_bitvector_count(((rwg_header *)rwghdr)->visited) >= ((rwg_header *)rwghdr)->groupSize){
SEND_BS = 1;
return 1;
}
}else{
//this have to be done due to possible missed acks
if(TRACE){printf("rwg_handle_reqf: REQF exists in buffer but not recentVisited/visited, updates recentVisited\n");}
rwg_bitvector_setbit(((rwg_header *)packetBuff->reqf[match].reqf)->recentVisited, hashedAddr);
rwg_bitvector_setbit(((rwg_header *)packetBuff->reqf[match].reqf)->visited, hashedAddr);
memcpy(((rwg_header *)packetBuff->reqf[match].reqf)->sender,rwghdr->sender,sizeof(char)*6);
packetBuff->active_reqf.reqf = packetBuff->reqf[match].reqf;
packetBuff->active_reqf.reqf_pos = match;
//Sends out a BS so it the other node will update its bitvectors
SEND_BS = 1;
return 1;
}
//schedule a send ACK
SEND_ACK = 1;
return 1;
}
if(TRACE){printf("rwg_handle_reqf, recent visited REQF\n");}
return 0;
}
/*FUNC: Handles incoming packets*/
int rwg_handle_incoming(unsigned char *packet, packetBuffer *packetBuff, int outPipe, int length)
{
int type;
unsigned char *prot_type;
unsigned char* srcaddr;
//unsigned char* packet;
rwg_header *rwghdr;
struct ether_header *eptr;
eptr = (struct ether_header *) packet;
/*check the type on the etherheader, RWG type is 0x1111*/
if(eptr->ether_type == RWG_ETHER_TYPE){ // ethernet headern kastas om (bytevis)
if(TRACE){
printf("rwg_handle_incoming: Ethernet packet contains RWG packet!\n");
printPacket(packet,length);
}
/*checking the type of the rwg packet*/
if(length >= 14){
rwghdr = (rwg_header *) (packet+14);
type = (int)rwghdr->type;
}else{
if(TRACE){printf("rwg_handle_incoming: It's just an ethernet header\n");}
return 0;
}
/*Switch Case over rwg protocol types : REQF (1), ACK(2), OKTF(3), BS(4)*/
switch(type)
{
case 1:
if(rwg_handle_reqf(rwghdr,packetBuff,outPipe))
return 1;
else
break;
case 2:
if(packetBuff->w_tail != packetBuff->w_front){
if(TRACE){printf("rwg_handle_incoming: Type ACK has arrived\n");}
rwg_handle_ack(rwghdr,packetBuff);
}else{
if(TRACE){printf("rwg_handle_incoming: Type ACK, but not waiting for one\n");}
}
break;
case 3:
if(rwg_handle_oktf(rwghdr,packetBuff))
return 1;
else
break;
case 4: rwg_handle_bs(rwghdr,packetBuff); break;
default: if(TRACE){printf("rwg_handle_incoming: Incorrect RWG protocol type\n");}
break;
}
}else{
if(TRACE){printf("rwg_handle_incoming: Ethernet packet does not contain RWG (type 1111): %i\n", eptr->ether_type);}
}
return 0;
}
