RValue CHR_6dm::_sending_command(byte *buffer) { int count = 0; int wr, rd; byte *local_buffer = new byte[BUF_MAX]; RValue ret_val; while(1) { count++; if (count >= 100) { ret_val = CHR_ErrorCommand; break; } wr = serial.WriteData(buffer, buffer[4] + 7); if (wr <= 0) { perror("Write Data"); ret_val = CHR_Error; break; } rd = serial.ReadData(local_buffer, BUF_MAX); if (rd == 0) { ret_val = CHR_Timeout; break; } else if (rd < 0) { ret_val = CHR_Error; break; } /* DEBUG */ printf("Command Type:%X, Received data:\n", buffer[3]); for (int i=0; i<rd; i++) { printf("%X ", local_buffer[i]); } printf("\n\n"); if (verifyPacket(local_buffer, COMMAND_COMPLETE)) { ret_val = CHR_OK; break; } /* Special case, prevent unnecessary loop */ if (buffer[3] == SET_BROADCAST_MODE) { if (verifyPacket(local_buffer, SENSOR_DATA)) { ret_val = CHR_OK; break; } } } delete local_buffer; return ret_val; }
void eraseBaseband(int fd, unsigned int begin, unsigned int end) { // correct the end address as the boot loader does, but still give it the // 'wrong' value unsigned int end2 = end; if (begin == 0xa0020000) end = 0xa0310000; LOG(LOGLEVEL_INFO, "Erasing flash range 0x%08x-0x%08x...\n", begin, end); unsigned int base = end - begin; if (base == 0) base = 1; // no div by 0, rather wrong values ErasePacket packet = { .begin = begin, .end = end2 }; writePacket(fd, 0x805, &packet, ERASE_PACKET_SIZE); char buffer[PACKET_SIZE(sizeof(unsigned short))]; size_t length = readPacket(fd, WRITE_TIMEOUT, buffer, sizeof(buffer)); unsigned short *job = verifyPacket(buffer, length); LOG(LOGLEVEL_DEBUG, "Erase returns: %d\n", job ? *job : 0); if (job) { EraseStatusReplyPacket *reply; int previous = -1; do { writePacket(fd, 0x806, job, sizeof(*job)); char buffer2[PACKET_SIZE(ERASE_STATUS_REPLY_PACKET_SIZE)]; length = readPacket(fd, DEFAULT_TIMEOUT, buffer2, sizeof(buffer2)); reply = verifyPacket(buffer2, length); if (reply) { LOG(LOGLEVEL_DEBUG, "Erase status returns: done=%s current=0x%08x w00=%d\n", reply->done ? "yes" : "no", reply->current, reply->w00); if (reply->current >= begin && reply->current <= end) { int percent = (reply->current - begin) * 100 / base; if (percent != previous) { LOG(LOGLEVEL_INFO, "Current progress: %u%%\n", percent); previous = percent; } } else if (reply->current == 0xa0000000 && reply->done) { LOG(LOGLEVEL_ERROR, "Looks like the erase command failed due to an invalid secpack.\n"); } else { LOG(LOGLEVEL_INFO, "Current position: 0x%08x\n", reply->current); } } } while (reply && !reply->done); } else { LOG(LOGLEVEL_ERROR, "Erase command failed!\n"); } }
size_t readBaseband(int fd, void *buffer, unsigned short size) { LOG(LOGLEVEL_STATUS, "Reading %u bytes from flash...\n", size); writePacket(fd, 0x803, &size, sizeof(size)); void *temp = malloc(PACKET_SIZE(size)); size_t length = readPacket(fd, DEFAULT_TIMEOUT, temp, PACKET_SIZE(size)); void *ret = verifyPacket(temp, length); if (ret) { CmdPacket *packet = (CmdPacket *) temp; LOG(LOGLEVEL_DEBUG, "Read returns %d bytes:\n", packet->dataSize); LOGDO(LOGLEVEL_DEBUG, printBuffer(ret, packet->dataSize)); if (packet->dataSize <= size) { memcpy(buffer, ret, packet->dataSize); } else { LOG(LOGLEVEL_WARN, "Warning: The returned data does not fit into the buffer!\n"); memcpy(buffer, ret, size); } free(temp); return packet->dataSize; } else { LOG(LOGLEVEL_DEBUG, "Read returns: ERROR!\n"); free(temp); return 0; } }
void prepareFlash(int fd) { LOG(LOGLEVEL_INFO, "Preparing flash access...\n"); short param = 0; writePacket(fd, 0x84, ¶m, sizeof(param)); char buffer[PACKET_SIZE(CFI1_PACKET_SIZE)]; size_t length = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer)); LOG(LOGLEVEL_DEBUG, "CFI Stage 1 returns:\n"); LOGDO(LOGLEVEL_DEBUG, printBuffer(verifyPacket(buffer, length), CFI1_PACKET_SIZE)); writePacket(fd, 0x85, NULL, 0); length = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer)); short *unknown = verifyPacket(buffer, length); LOG(LOGLEVEL_DEBUG, "CFI Stage 2 returns: %d\n", unknown ? *unknown : 0); //LOGDO(LOGLEVEL_DEBUG, printBuffer(buffer, length)); }
/****************************************************************************** * * @brief On packet reception, process the data * BSL-based protocol expects: * HEADER = 0x80 * Lenght = lenght of CMD + [ADDR] + [DATA] * CMD = 1 byte with the corresponding command * ADDR = optional address depending on command * DATA = optional data depending on command * CHKSUM = 2 bytes (L:H) with CRC checksum of CMD + [ADDR] + [DATA] * * @return RET_OK: Communication protocol in progress * RET_JUMP_TO_APP: Last byte received, request jump to application *****************************************************************************/ uint8_t TI_MSPBoot_CI_Process(void) { uint8_t ret = RET_NO_DATA; uint8_t sendAck; if (readPacket()) // On complete packet reception { if ((gPacket.length > PACKET_DESTINATION) && (gPacket.data[PACKET_PROTOCOL] == MSP430_BSL_PROTOCOL) && (gPacket.data[PACKET_DESTINATION] == gPacket.myAddr)) { sendAck = verifyPacket(); ackPacket(sendAck); if (sendAck == ACK) { /* CI_CMD_Intepreter will set up the packet response */ ret = CI_CMD_Intepreter(); updatePacket(); writePacket(); } } packetReset(); } return ret; }
void seekBaseband(int fd, unsigned int offset) { LOG(LOGLEVEL_INFO, "Seeking to 0x%08x...\n", offset); writePacket(fd, 0x802, &offset, sizeof(offset)); char buffer[PACKET_SIZE(SEEK_REPLY_PACKET_SIZE)]; size_t length = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer)); LOG(LOGLEVEL_DEBUG, "Seek returns:\n"); LOGDO(LOGLEVEL_DEBUG, printBuffer(verifyPacket(buffer, length), SEEK_REPLY_PACKET_SIZE)); }
void secPack(int fd, void *secpack) { LOG(LOGLEVEL_INFO, "Sending secpack...\n"); writePacket(fd, 0x204, secpack, 0x800); char buffer[PACKET_SIZE(SECPACK_REPLY_PACKET_SIZE)]; size_t rlength = readPacket(fd, WRITE_TIMEOUT, buffer, sizeof(buffer)); SecpackReplyPacket *reply = verifyPacket(buffer, rlength); LOG(LOGLEVEL_DEBUG, "Secpack returns: unknown1=%d unknown2=0x%x\n", reply ? reply->unknown1 : 0, reply ? reply->unknown2 : 0); }
void endSecPack(int fd) { LOG(LOGLEVEL_INFO, "Ending secpack...\n"); unsigned short unknown = 0; writePacket(fd, 0x205, &unknown, sizeof(unknown)); char buffer[PACKET_SIZE(sizeof(unsigned short))]; size_t rlength = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer)); unsigned short *ret = verifyPacket(buffer, rlength); LOG(LOGLEVEL_DEBUG, "End secpack returns: %d\n", ret ? *ret : 0); }
void writeBaseband(int fd, void *data, size_t length) { LOG(LOGLEVEL_STATUS, "Writing %lu bytes to flash...\n", length); writePacket(fd, 0x804, data, length); char buffer[PACKET_SIZE(sizeof(unsigned short))]; size_t rlength = readPacket(fd, WRITE_TIMEOUT, buffer, sizeof(buffer)); unsigned short *ret = verifyPacket(buffer, rlength); if (ret && *ret) { LOG(LOGLEVEL_ERROR, "Write returns error: %d\n", *ret); } }
void setBaudRate(int fd, unsigned int speed) { LOG(LOGLEVEL_INFO, "Increasing baud rate to %dbps...\n", speed); writePacket(fd, 0x82, &speed, 4); char buffer[PACKET_SIZE(sizeof(unsigned int))]; int length = readPacket(fd, DEFAULT_TIMEOUT, buffer, sizeof(buffer)); unsigned int *setSpeed = (unsigned int *) verifyPacket(buffer, length); LOG(LOGLEVEL_DEBUG, "Set baudrate returns: %d, %s\n", setSpeed ? *setSpeed : 0, (speed == (setSpeed ? *setSpeed : 0)) ? "ok" : "NOT ok"); //LOGDO(LOGLEVEL_DEBUG, printBuffer(buffer, length)); struct termios options; tcgetattr(fd, &options); cfsetspeed(&options, speed); tcsetattr(fd, TCSANOW, &options); }
VersionPacket getBootVersion(int fd) { LOG(LOGLEVEL_INFO, "Getting bootloader version...\n"); writePacket(fd, 0x801, NULL, 0); char buffer[PACKET_SIZE(VERSION_PACKET_SIZE)]; size_t length = readPacket(fd, 5, buffer, sizeof(buffer)); VersionPacket *packet = (VersionPacket *) verifyPacket(buffer, length); if (packet) { LOG(LOGLEVEL_DEBUG, "Got bootloader version: %d.%d\n", packet->major, packet->minor); //LOGDO(LOGLEVEL_DEBUG, printBuffer(buffer, length)); return *packet; } VersionPacket zeroBoot; memset(&zeroBoot, 0, VERSION_PACKET_SIZE); return zeroBoot; }
void *CHR_6dm::_communicator(void *ptr) { struct SharedData *s = (struct SharedData *)ptr; class SerialPort serial(s->path); serial.OpenPort(); serial.SetAttr(s->baudrate, 1, s->dataNumber); int rd, wr; byte local_buffer[BUF_MAX]; pthread_cleanup_push(CHR_6dm::_cleanup_function, ptr); int error_cnt = 0; while (1) { rd = serial.ReadData(local_buffer, s->dataNumber); if (verifyPacket(local_buffer)) { /* DEBUG printf("Data Received:"); for (int i=0; i<rd; i++) { printf("%X ", local_buffer[i]); } printf("\n"); */ pthread_mutex_lock(&s->mutex); //s->updated = true; memcpy(s->data, local_buffer, s->dataNumber); // Sending signal pthread_cond_signal(&s->condvar); pthread_mutex_unlock(&s->mutex); } else { error_cnt++; if (error_cnt >= 10) { rd = serial.ReadData(local_buffer, BUF_MAX); error_cnt = 0; } } } pthread_cleanup_pop(0); }
void test_verifyPacket_given_data_packet_should_return_0_if_packet_is_incorect(void) { char data[] = "qL1160000000000000000#55"; TEST_ASSERT_EQUAL(0, verifyPacket(data)); }
void test_verifyPacket_given_data_packet_should_return_1_if_checksum_corect(void) { char data[] = "$qL1160000000000000000#55"; TEST_ASSERT_EQUAL(1, verifyPacket(data)); }
int main(int argc, char *argv[]) { #ifdef DEBUG signal(SIGSEGV,debug_backtrace); #endif printf("begin!\n"); int tap_fd, tap_fd1, option; int dummy = 0; int flags = IFF_TUN; char if_name0[IFNAMSIZ] = "tap0"; char if_name1[IFNAMSIZ] = "tap1"; int maxfd; uint16_t nread, nwrite, plength; char buffer[BUFSIZE]; struct sockaddr_in local, remote; char remote_ip[16] = ""; /* dotted quad IP string */ unsigned short int port = PORT; int sock_fd, net_fd, optval = 1; socklen_t remotelen; int cliserv = -1; /* must be specified on cmd line */ unsigned long int tap2net = 0, net2tap = 0; struct in_addr addr; struct host twohopneis[10]; inet_aton(twohopnei_ip_addr,&addr); twohopneis[0].ip_addr = addr.s_addr; memcpy(twohopneis[0].mac_addr,gate_mac_addr, ETHER_ADDR_LEN); struct host localhost; inet_aton(local_ip_addr,&addr); localhost.ip_addr = addr.s_addr; memcpy(localhost.mac_addr, local_mac_addr, ETHER_ADDR_LEN); progname = argv[0]; /* Check command line options */ while((option = getopt(argc, argv, "i:sc:p:uahd")) > 0) { switch(option) { case 'd': printf("Enter dummy mode\n"); dummy = 1; break; case 'h': usage(); break; case 'i': strncpy(if_name0,optarg, IFNAMSIZ-1); break; case 's': cliserv = SERVER; break; case 'c': cliserv = CLIENT; strncpy(remote_ip,optarg,15); break; case 'p': port = atoi(optarg); break; case 'u': flags = IFF_TUN; break; case 'a': flags = IFF_TAP; break; default: my_err("Unknown option %c\n", option); usage(); } } argv += optind; argc -= optind; if(argc > 0) { my_err("Too many options!\n"); usage(); } if(*if_name0 == '\0') { my_err("Must specify interface name!\n"); usage(); } else if(cliserv < 0) { my_err("Must specify client or server mode!\n"); usage(); } else if((cliserv == CLIENT)&&(*remote_ip == '\0')) { my_err("Must specify server address!\n"); usage(); } printf("allocate tap device!\n"); /* initialize tun/tap interface */ if ( (tap_fd = tun_alloc(if_name0, flags | IFF_NO_PI)) < 0 ) { my_err("Error connecting to tun/tap interface %s!\n", if_name0); exit(1); } if ( (tap_fd1 = tun_alloc(if_name1, flags | IFF_NO_PI)) < 0 ) { my_err("Error connecting to tun1/tap1 interface %s!\n", if_name1); exit(1); } printf("allocate tap device success!\n"); //do_debug("Successfully connected to interface %s\n", if_name0); /* use select() to handle two descriptors at once */ maxfd = (tap_fd > tap_fd1)?tap_fd:tap_fd1; while(1) { int ret; fd_set rd_set; FD_ZERO(&rd_set); FD_SET(tap_fd, &rd_set); FD_SET(tap_fd1, &rd_set); ret = select(maxfd + 1, &rd_set, NULL, NULL, NULL); if (ret < 0 && errno == EINTR){ do_debug("ret error"); continue; } if (ret < 0) { perror("select()"); exit(1); } if(FD_ISSET(tap_fd, &rd_set)) { /* data from tun/tap: just read it and write it to the network */ DEBUGMSG(2,"\n--Send a packet--\n"); nread = cread(tap_fd, buffer, BUFSIZE); //append the trailer if(!dummy){ if(isIP((unsigned char*)buffer) && isTCP((unsigned char*)buffer)){ ++send_counter; if(send_counter >= UP_LIMIT){ const int len = 14 + sizeof(struct ip) + 4; unsigned char signal[14 + sizeof(struct ip) + 4] = {0}; ConstructSignal(signal, &(twohopneis[0]), &localhost); DEBUGMSG(1,"Epoch end and send a signal\n"); nwrite = write(tap_fd1, signal, len); send_counter = 0; } int newLen = extendPacket(TCP, (unsigned char*)buffer); nread = newLen; } } //DEBUGMSG(2,"route a packet(len: %i) to tap1\n", nread); nwrite = write(tap_fd1, buffer, nread); //DEBUGMSG(2,"Written %d bytes to the network\n", nwrite); } if(FD_ISSET(tap_fd1, &rd_set)) { /* data from tun/tap: just read it and write it to the network */ nread = cread(tap_fd1, buffer, BUFSIZE); /* write length + packet */ plength = htons(nread); if(!dummy){ if(isEpochEnd((unsigned char*)buffer)){ DEBUGMSG(1,"Receive a Epoch end signal\n"); const int len = 14 + sizeof(struct ip) + 40; unsigned char echobuffer [14 + sizeof(struct ip) + 4] = {0}; ConstructSignal(echobuffer, (unsigned char*)buffer); nwrite = write(tap_fd1, echobuffer, len); continue; } if(isIP((unsigned char*)buffer) && isTCP((unsigned char*)buffer)){ int newLen = 0; verifyPacket(TCP, (unsigned char*)buffer, &newLen); nread = newLen; } DEBUGMSG(2,"packet len: %i\n", nread); } nwrite = write(tap_fd, buffer, nread); do_debug("TAP2NET %lu: Written %d bytes to the network\n", tap2net, nwrite); } //if(FD_ISSET(net_fd, &rd_set)) { if(0) { /* data from the network: read it, and write it to the tun/tap interface. * We need to read the length first, and then the packet */ /* Read length */ nread = read_n(net_fd, (char *)&plength, sizeof(plength)); if(nread == 0) { /* ctrl-c at the other end */ break; } net2tap++; /* read packet */ nread = read_n(net_fd, buffer, ntohs(plength)); do_debug("NET2TAP %lu: Read %d bytes from the network\n", net2tap, nread); /* now buffer[] contains a full packet or frame, write it into the tun/tap interface */ nwrite = cwrite(tap_fd, buffer, nread); do_debug("NET2TAP %lu: Written %d bytes to the tap interface\n", net2tap, nwrite); } } return(0); }
void handlePacket(Packet *pkt) { if(verifyPacket(pkt)) { int type = getPacketType(pkt); switch(type) { case 0: { // WHOHAS fprintf(stderr,"->WHOHAS\n"); Packet *pktIHAVE = newPacketIHAVE(pkt); enqueue(nonCongestQueue, (void *)pktIHAVE); break; } case 1: { // IHAVE fprintf(stderr,"->IHAVE\n"); int peerIndex = searchPeer(&(pkt->src)); int peerID = peerInfo.peerList[peerIndex].peerID; newPacketGET(pkt, downloadPool[peerID].getQueue); idle = 0; break; } case 2: { // GET fprintf(stderr,"->GET\n"); if(numConnUp == maxConn){ fprintf(stderr,"->GET request denied.\n"); freePacket(pkt); break; } numConnUp++; int peerIndex = searchPeer(&(pkt->src)); int peerID = peerInfo.peerList[peerIndex].peerID; if(downloadPool[peerID].connected == 0){ clearQueue(uploadPool[peerID].ackWaitQueue); initWindows(&(downloadPool[peerID].rw), &(uploadPool[peerID].sw)); newPacketDATA(pkt, uploadPool[peerID].dataQueue); uploadPool[peerID].connID++; gettimeofday(&(uploadPool[peerID].startTime), NULL); } else { fprintf(stderr,"Only one-way connection is allowed.\n"); freePacket(pkt); } break; } case 3: { // DATA fprintf(stderr,"->DATA"); int peerIndex = searchPeer(&(pkt->src)); int peerID = peerInfo.peerList[peerIndex].peerID; if(1 == updateGetSingleChunk(pkt, peerID)) { downloadPool[peerID].connected = 0; numConnDown--; updateGetChunk(); } break; } case 4: { // ACK fprintf(stderr,"->ACK\n"); int peerIndex = searchPeer(&(pkt->src)); int peerID = peerInfo.peerList[peerIndex].peerID; updateACKQueue(pkt, peerID); break; } case 5: // DENIED default: fprintf(stderr,"DENIED\n"); } } else { fprintf(stderr,"Invalid packet!\n"); } freePacket(pkt); return; }
size_t readBraillePacket ( BrailleDisplay *brl, GioEndpoint *endpoint, void *packet, size_t size, BraillePacketVerifier *verifyPacket, void *data ) { unsigned char *bytes = packet; size_t count = 0; size_t length = 1; if (!endpoint) endpoint = brl->gioEndpoint; while (1) { unsigned char byte; { int started = count > 0; if (!gioReadByte(endpoint, &byte, started)) { if (started) logPartialPacket(bytes, count); return 0; } } gotByte: if (count < size) { bytes[count++] = byte; { BraillePacketVerifierResult result = verifyPacket(brl, bytes, count, &length, data); switch (result) { case BRL_PVR_EXCLUDE: count -= 1; case BRL_PVR_INCLUDE: break; default: logMessage(LOG_WARNING, "unimplemented braille packet verifier result: %u", result); case BRL_PVR_INVALID: if (--count) { logShortPacket(bytes, count); count = 0; length = 1; goto gotByte; } logIgnoredByte(byte); continue; } } if (count == length) { logInputPacket(bytes, length); return length; } } else { if (count++ == size) logTruncatedPacket(bytes, size); logDiscardedByte(byte); } } }
void test_verifyPacket_given_data_packet_should_return_0_if_packet_contain_$$(void) { char data[] = "$$qL1160000000000000000#55"; TEST_ASSERT_EQUAL(0, verifyPacket(data)); }