#!/usr/bin/python
import os
import socket
import math
import sys
import time
import sys
import string
import tinyos
import tinyosSF

AM_SYNAPSENP_SERIAL_PING 	 = 131
AM_SYNAPSENP_REPLY 		     = 87
AM_SYNAPSENP_SERIALCOMMAND  	 = 127

AM_FLASHMAN_CMD 	         = 140
AM_FLASHMAN_DATA 	         = 141

SPE_USED			         = (1)
SPE_WORKING			         = (1<<3)
PING_REPLY_FLAG 		     = (1)
PING_PREPARED_FLAG 		     = (1<<1)
PING_OVERFLOW_FLAG		     = (1<<2)
PAYLOAD_LEN			         = 16

CMD_NOOP			= 0
CMD_READTABLE		= 1
CMD_READAPP			= 2
CMD_ADDENTRY		= 3
CMD_FORMAT			= 4
CMD_ENTRYREADY      = 5
CMD_ACK 			= 9

TYPE_NOTHING		= 0
TYPE_TABLE_ENTRY	= 1
TYPE_DATA_CHUNK		= 2

#COMMANDS
transferCmd		= 1
netFormatCmd	= 2
netResetCmd		= 3
netLoadCmd		= 4
tagCmd			= 9
advProcedure	= 5
prepareCmd		= 6
pingReqMsg		= 7
pingRepMsg		= 8

serialPort      = "/dev/ttyUSB0"
platform        = "telosb"
port            = 0
cmdFound        = 0

class CommandPacket(tinyos.GenericPacket):
	def __init__(self, packet = None):
		tinyos.GenericPacket.__init__(self,
		[('cmd','int',  1),
		('param8_1', 'int',  1),
		('param8_2', 'int',  1),
		('param16_1', 'int',  2),
		('param16_2', 'int',  2),],
		packet)
	
class AckPacket(tinyos.GenericPacket):
	def __init__(self, packet = None):
		tinyos.GenericPacket.__init__(self,
		[('cmd','int',  1),
		('end', 'int',  1),
		('param8_2', 'int',  1),
		('param16_1', 'int',  2),
		('param16_2', 'int',  2),],
		packet)
	
class EntryReadyPacket(tinyos.GenericPacket):
	def __init__(self, packet = None):
		tinyos.GenericPacket.__init__(self,
		[('cmd','int',  1),
		('pos', 'int',  1),
		('error', 'int',  1),
		('id', 'int',  2),
		('size', 'int',  2),],
		packet)	
		
class TablePacket(tinyos.GenericPacket):
	def __init__(self, packet = None):
		tinyos.GenericPacket.__init__(self,
		[('type','int',1),
		('end',  'int',  4),
		('size',  'int',  4),
		('flags1', 'int', 2),
		('hash1', 'int', 2),
		('startAddr1', 'int', 4),
		('size1', 'int', 4),
		('pOffs1', 'int', 2),
		('flags2', 'int', 2),
		('hash2', 'int', 2),
		('startAddr2', 'int', 4),
		('size2', 'int', 4),
		('pOffs2', 'int', 2),],
		packet)

class ChunkPacket(tinyos.GenericPacket):
	def __init__(self, packet = None):
		tinyos.GenericPacket.__init__(self,
		[('type','int',1),
		('end',  'int',  4),
		('size',  'int',  4),
		('data',  'int',  40),],
		packet)

class DataPacket(tinyos.GenericPacket):
	def __init__(self, packet = None):
		tinyos.GenericPacket.__init__(self,
		[('type','int',1),
		('address',  'int',  4),
		('size',  'int',  4),
		('data',  '[]',  40),],
		packet)


class SerialPingPacket(tinyos.GenericPacket):
	def __init__(self, packet = None):
		tinyos.GenericPacket.__init__(self,
		[('dest','int',  2),
		('partNum', 'int',  1),
		('pingFlags',   'int',  1),
		('status',   'int',  1),
		('imgSize',   'int',  4),
		('imgOffset',   'int',  4),
		('imgId',   'int',  2),
		('imgFlags',   'int',  2)],
		packet)

class ReplyPacket(tinyos.GenericPacket):
	def __init__(self, packet = None):
		tinyos.GenericPacket.__init__(self,
		[('type', 'int',  1),
		('size',  'int',  4),
		('p32_1',    'int',  4),
		('appID', 'int',  2),
		('flags', 'int',  2),
		('pingFlags',  'int',  1),
		('status','int',  1),],
		packet)
		
#class CommandPacket(tinyos.GenericPacket):
#	def __init__(self, packet = None):
#		tinyos.GenericPacket.__init__(self,
#		[('cmd', 'int',  1),
#		('arg',  'int',  2),],
#		packet)

def parseStatus(s):
	if (s==0):
		return "Initializing"
	if (s==1):
		return "Idle"
	if (s==2):
		return "Preparing to execute a command"
	if (s==3):
		return "Adv Phase"
	if (s==4):
		return "Req Phase"
	if (s==5):
		return "Data Phase"


def int16ToStr(n):
	return chr(int(math.floor(n/256)))+chr(int(n-(math.floor(n/256))*256))

def sendCmd(cmd,arg,port=0):
	if (port==0):
		comm=tinyos.Serial(serialPort, platform)
	else:
		comm=tinyosSF.Forwarder(port)
	sCmdPacket = CommandPacket((cmd, arg>>8,arg&0xFF,0,0))
	comm.write_packet(0, AM_SYNAPSENP_SERIALCOMMAND, sCmdPacket.payload())

def sendFMCmd(cmd,p1,p2,p3,p4,port=0):
	if (port==0):
		comm=tinyos.Serial(serialPort, platform)
	else:
		comm=tinyosSF.Forwarder(port)
	sCmdPacket = CommandPacket((cmd,p1,p2,p3,p4))
	comm.write_packet(0, AM_FLASHMAN_CMD , sCmdPacket.payload())

def ping(nodeID,appNum,port=9000):
	if (port!=0):
		comm=tinyosSF.Forwarder(port)
	else:
		comm=tinyos.Serial(serialPort, platform)
	pe="";
	spingpkt = SerialPingPacket((nodeID, appNum, 0,0,0,0,0,0))
	comm.write_packet(0, AM_SYNAPSENP_SERIAL_PING, spingpkt.payload())
	print "Ping req sent to node %d" %nodeID  + " for partition #%d" %appNum
	while True:
		packet=comm.read_packet(0,AM_SYNAPSENP_REPLY)
		replyPkt=ReplyPacket(packet[1]);
		if(replyPkt['type']==8):
			status = parseStatus(replyPkt['status'])
			print "Node %d is alive" %nodeID + " and " + status
			if((replyPkt['pingFlags'])&PING_PREPARED_FLAG):
				prepared="set"
			else:
				prepared="not set"
			print "Prepared flag is " + prepared
			if((replyPkt['pingFlags'])&PING_OVERFLOW_FLAG):
				print "Slot %d does not exist " %appNum
				print "%d slots are available" %replyPkt['appID']
				print "%d slots are used" %replyPkt['size']
				break
			else:
				if(replyPkt['appID']!=0xFFFF):
					if ((replyPkt['flags']^0xFFFF)&SPE_WORKING):
						print "Application in slot %d has ID " %appNum + " %X" %replyPkt['appID']
					else:
						print "Slot %d occupied by NON WORKING Application" %appNum
						print "Application stored is %X" %replyPkt['appID'] + " (Non Working)"
					print "Application size is %d bytes" %replyPkt['size']
				else:
					print "Slot %d is free" %appNum
				if ((replyPkt['flags']^0xFFFF)&SPE_USED):
					print "Slot %d is marked USED" %appNum
				else:
					print "Slot %d is marked UNUSED" %appNum
				break
def readTable(port=0):
	curEntry = 0
	if (port!=0):
		comm=tinyosSF.Forwarder(port)
	else:
		comm=tinyos.Serial(serialPort, platform)
	while True:
		packet=comm.read_packet(0,AM_FLASHMAN_DATA)
		replyPkt=TablePacket(packet[1])	
		print "Partition Entry: %d" %curEntry
		curEntry = curEntry +1
		print "\tappID: %04X hex -" %(((replyPkt.hash1 & 0x00FF )<<8)|(replyPkt.hash1 &0xFF00 )>>8),
		print "%d dec"  %(((replyPkt.hash1 & 0x00FF )<<8)|(replyPkt.hash1 &0xFF00 )>>8)
		print "\tflags: %04X" %replyPkt.flags1
		print "\tstartAddr: %04X" %((((replyPkt.startAddr1 & 0x00FF0000 )<<8)|(replyPkt.startAddr1 &0xFF000000 )>>8)>>16)
		print "\tappSize: %d" %((((replyPkt.size1 & 0x00FF0000 )<<8)|(replyPkt.size1 &0xFF000000 )>>8)>>16)


		print "Partition Entry: %d" %curEntry 
		curEntry = curEntry +1
		print "\tappID: %04X hex -" %(((replyPkt.hash2 & 0x00FF )<<8)|(replyPkt.hash2 &0xFF00 )>>8),
		print "%d dec"  %(((replyPkt.hash2 & 0x00FF )<<8)|(replyPkt.hash2 &0xFF00 )>>8)
		print "\tflags: %04X" %replyPkt.flags2
		print "\tstartAddr: %04X" %((((replyPkt.startAddr2 & 0x00FF0000 )<<8)|(replyPkt.startAddr2 &0xFF000000 )>>8)>>16)
		print "\tappSize: %d" %((((replyPkt.size2 & 0x00FF0000 )<<8)|(replyPkt.size2 &0xFF000000 )>>8)>>16)
		if replyPkt.end == 1:
			print "END"
			break
		
def readApp(port=0):
	if (port!=0):
		comm=tinyosSF.Forwarder(port)
	else:
		comm=tinyos.Serial(serialPort, platform)

	while True:
		packet=comm.read_packet(0,AM_FLASHMAN_DATA)
		replyPkt=ChunkPacket(packet[1])
		txtdata = "%080X" %replyPkt.data
		if (replyPkt.size == 40):
			print txtdata[0:40]
			print txtdata[40:80]
		elif (replyPkt.size < 20):
			print txtdata[0:(replyPkt.size*2)]
		elif (replyPkt.size < 40):
			print txtdata[0:40]
			print txtdata[40:((replyPkt.size)*2)]
		if replyPkt.end == 1:
			print "END"
			break

def writeApp(appID,filename,port=0):
	if (port!=0):
		comm=tinyosSF.Forwarder(port)
	else:
		comm=tinyos.Serial(serialPort, platform)
		
	segmentdata = []
	data = [40]
	file = open(filename,"rb")
	lines = file.readlines()
	for l in lines:
		if (l[0] != ':'): 
			sys.stderr.write("Ignored unknown field (type 0x%02x) in ihex file.\n" % type)
		l = l.strip()       #fix CR-LF issues...
		length  = int(l[1:3],16)
		address = int(l[3:7],16)
		type    = int(l[7:9],16)
		check   = int(l[-2:],16)
		if type == 0x00:
			for i in range(length):
				segmentdata.append( (int(l[9+2*i:11+2*i],16)) )
		elif type in (0x01, 0x02, 0x03, 0x04, 0x05):
			pass		
		else:
			sys.stderr.write("Ignored unknown field (type 0x%02x) in ihex file.\n" % type)

	appSize = len(segmentdata)
	sCmdPacket = CommandPacket((CMD_ADDENTRY,0,0,appID,appSize))
	comm.write_packet(0, AM_FLASHMAN_CMD , sCmdPacket.payload())
	packet=comm.read_packet(0,AM_FLASHMAN_CMD)
	replyPkt=EntryReadyPacket(packet[1])
	if (replyPkt.error!=0):
		print "error"
	else:
		print "starting application upload"
		type = TYPE_DATA_CHUNK
		address = 0
		size = 40
		keepGoing = True
		lastSeg = 0
		sys.stdout.write("loading [")
		for i in range(0,appSize,size):
			if (address + size >= appSize ):
				size = appSize-address
			if ((((address + size)*40)/appSize )> lastSeg):
				lastSeg = lastSeg + 1
			 	sys.stdout.write("#")
				sys.stdout.flush()
			data = segmentdata[address:address+size]
			sDataPacket = DataPacket((type,address,size,data))
			comm.write_packet(0, AM_FLASHMAN_DATA , sDataPacket.payload())
			address = address + size
			packet=comm.read_packet(0,AM_FLASHMAN_CMD)
		sys.stdout.write("]\n")
		print "done"
def printUsage():
	print "SUINO (Synapse User Interface Network Operation) "
	print "usage: suino [options] --ping nodeAddr PartNum"
	print "\t suino transfer <id>"
	print "\t suino load <id>"
	print "\t suino prepare "
	print "\t suino format "
	print "\t suino reset "
	print "\t suino tag <tag>"
	print "\t suino get_table"
	print "\t suino read_app <appID>"
	print "\t suino write_app <appID> <ihex file>"
	print "\t suino formatBS"
	print ""
	print "options are: -c serial_Port , --port TCP_port, -p baud_rate"

for j in range(len(sys.argv)):
	if (sys.argv[j]=='-c'):
		serialPort=sys.argv[j+1]
	if (sys.argv[j]=='-p'):
		platform=sys.argv[j+1]
	if (sys.argv[j]=='--port'):
		port=int(sys.argv[j+1])
	if (sys.argv[j]=='--help'):
		printUsage()
for j in range(len(sys.argv)):
	if (sys.argv[j]=='prepare'):
		cmdFound=1
		sendCmd(prepareCmd,0,port)
	if (sys.argv[j]=='format'):
		cmdFound=1
		sendCmd(netFormatCmd,0,port)
	if (sys.argv[j]=='reset'):
		cmdFound=1
		sendCmd(netResetCmd,0,port)
	if (sys.argv[j]=='load'):
		cmdFound=1
		sendCmd(netLoadCmd,string.atoi(sys.argv[j+1],16),port)
	if (sys.argv[j]=='tag'):
		cmdFound=1
		sendCmd(tagCmd,string.atoi(sys.argv[j+1],16),port)
	if (sys.argv[j]=='transfer'):
		cmdFound=1
		sendCmd(transferCmd,string.atoi(sys.argv[j+1],16),port)
	if (sys.argv[j]=='ping'):
		cmdFound=1
		ping(int(sys.argv[j+1]),int(sys.argv[j+2]),port)
	if (sys.argv[j]=='get_table'):
		cmdFound=1
		sendFMCmd(CMD_READTABLE,0,0,0,0,port)
		readTable()
	if (sys.argv[j]=='read_app'):
		cmdFound=1
		sendFMCmd(CMD_READAPP,0,0,int(sys.argv[j+1],16),0,port)
		readApp()
		j = j + 1
	if (sys.argv[j]=='write_app'):
		cmdFound=1
		writeApp(int(sys.argv[j+1],16),sys.argv[j+2])
		j = j + 2
	if (sys.argv[j]=='formatBS'):
		cmdFound=1
		sendFMCmd(CMD_FORMAT,0,0,0,0,port)
if(cmdFound==0):
	printUsage()
