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));
}
