def _ofs_from_idx(self, idx):
     ofs = struct.unpack('!I', str(buffer(self.ofstable, idx*4, 4)))[0]
     if ofs & 0x80000000:
         idx64 = ofs & 0x7fffffff
         ofs = struct.unpack('!Q',
                             str(buffer(self.ofs64table, idx64*8, 8)))[0]
     return ofs

    def _recv_dict(self, pipe):
        """Receive a serialized dict on a pipe

        Returns the dictionary.
        """
        # Wire format:
        #   // Pipe sees (all numbers are longs, except for the first):
        #   // - num bytes in a long (sent as a single unsigned char!)
        #   // - num elements of the MIME dictionary; Jupyter selects one to display.
        #   // For each MIME dictionary element:
        #   //   - length of MIME type key
        #   //   - MIME type key
        #   //   - size of MIME data buffer (including the terminating 0 for
        #   //     0-terminated strings)
        #   //   - MIME data buffer
        data = {}
        b1 = os.read(pipe, 1)
        sizeof_long = struct.unpack('B', b1)[0]
        if sizeof_long == 8:
            fmt = 'Q'
        else:
            fmt = 'L'
        buf = os.read(pipe, sizeof_long)
        num_elements = struct.unpack(fmt, buf)[0]
        for i in range(num_elements):
            buf = os.read(pipe, sizeof_long)
            len_key = struct.unpack(fmt, buf)[0]
            key = os.read(pipe, len_key).decode('utf8')
            buf = os.read(pipe, sizeof_long)
            len_value = struct.unpack(fmt, buf)[0]
            value = os.read(pipe, len_value).decode('utf8')
            data[key] = value
        return data

    def saveScreenshot(self, filename):
        """
        Takes a screenshot of what's being display on the device. Uses
        "screencap" on newer (Android 3.0+) devices (and some older ones with
        the functionality backported). This function also works on B2G.

        Throws an exception on failure. This will always fail on devices
        without the screencap utility.
        """
        screencap = '/system/bin/screencap'
        if not self.fileExists(screencap):
            raise DMError("Unable to capture screenshot on device: no screencap utility")

        with open(filename, 'w') as pngfile:
            # newer versions of screencap can write directly to a png, but some
            # older versions can't
            tempScreenshotFile = self.getDeviceRoot() + "/ss-dm.tmp"
            self.shellCheckOutput(["sh", "-c", "%s > %s" %
                                   (screencap, tempScreenshotFile)],
                                  root=True)
            buf = self.pullFile(tempScreenshotFile)
            width = int(struct.unpack("I", buf[0:4])[0])
            height = int(struct.unpack("I", buf[4:8])[0])
            with open(filename, 'w') as pngfile:
                pngfile.write(self._writePNG(buf[12:], width, height))
            self.removeFile(tempScreenshotFile)

 def _decode_one(buf):
     self = navio_imu_t()
     self.timestamp = struct.unpack(">q", buf.read(8))[0]
     self.imu_pos = struct.unpack('>3q', buf.read(24))
     self.imu_vel = struct.unpack('>3q', buf.read(24))
     self.imu_acc = struct.unpack('>3q', buf.read(24))
     return self

    def do_checksum_buffer(self, buf, checksum):
        self._discFrameCounter += 1

        # on first track ...
        if self._trackNumber == 1:
            # ... skip first 4 CD frames
            if self._discFrameCounter <= 4:
                gst.debug('skipping frame %d' % self._discFrameCounter)
                return checksum
            # ... on 5th frame, only use last value
            elif self._discFrameCounter == 5:
                values = struct.unpack("<I", buf[-4:])
                checksum += common.SAMPLES_PER_FRAME * 5 * values[0]
                checksum &= 0xFFFFFFFF
                return checksum

        # on last track, skip last 5 CD frames
        if self._trackNumber == self._trackCount:
            discFrameLength = self._sampleLength / common.SAMPLES_PER_FRAME
            if self._discFrameCounter > discFrameLength - 5:
                self.debug('skipping frame %d', self._discFrameCounter)
                return checksum

        values = struct.unpack("<%dI" % (len(buf) / 4), buf)
        for i, value in enumerate(values):
            # self._bytes is updated after do_checksum_buffer
            checksum += (self._bytes / 4 + i + 1) * value
            checksum &= 0xFFFFFFFF
            # offset = self._bytes / 4 + i + 1
            # if offset % common.SAMPLES_PER_FRAME == 0:
            #   print 'frame %d, ends before %d, last value %08x, CRC %08x' % (
            #     offset / common.SAMPLES_PER_FRAME, offset, value, sum)

        return checksum

    def getRSSI(self):
        """Detects whether the device is near by or not"""
        addr = self.address
		
        # Open hci socket
        hci_sock = bt.hci_open_dev()
        hci_fd = hci_sock.fileno()

        # Connect to device (to whatever you like)
        bt_sock = bluetooth.BluetoothSocket(bluetooth.L2CAP)
        bt_sock.settimeout(10)
        result = bt_sock.connect_ex((addr, 1))	# PSM 1 - Service Discovery

        try:
            # Get ConnInfo
            reqstr = struct.pack("6sB17s", bt.str2ba(addr), bt.ACL_LINK, "\0" * 17)
            request = array.array("c", reqstr )
            handle = fcntl.ioctl(hci_fd, bt.HCIGETCONNINFO, request, 1)
            handle = struct.unpack("8xH14x", request.tostring())[0]

            # Get RSSI
            cmd_pkt=struct.pack('H', handle)
            rssi = bt.hci_send_req(hci_sock, bt.OGF_STATUS_PARAM,
                         bt.OCF_READ_RSSI, bt.EVT_CMD_COMPLETE, 4, cmd_pkt)
            rssi = struct.unpack('b', rssi[3])[0]

            # Close sockets
            bt_sock.close()
            hci_sock.close()

            return rssi

        except Exception, e:
            return None

	def _version_response(self, endpoint, data):
		fw_names = {
			0: "normal_fw",
			1: "recovery_fw"
		}

		resp = {}
		for i in xrange(2):
			fwver_size = 47
			offset = i*fwver_size+1
			fw = {}
			fw["timestamp"],fw["version"],fw["commit"],fw["is_recovery"], \
				fw["hardware_platform"],fw["metadata_ver"] = \
				unpack("!i32s8s?bb", data[offset:offset+fwver_size])

			fw["version"] = fw["version"].replace("\x00", "")
			fw["commit"] = fw["commit"].replace("\x00", "")

			fw_name = fw_names[i]
			resp[fw_name] = fw

		resp["bootloader_timestamp"],resp["hw_version"],resp["serial"] = \
			unpack("!L9s12s", data[95:120])

		resp["hw_version"] = resp["hw_version"].replace("\x00","")

		btmac_hex = binascii.hexlify(data[120:126])
		resp["btmac"] = ":".join([btmac_hex[i:i+2].upper() for i in reversed(xrange(0, 12, 2))])

		return resp

def getInfo(dbfile):
    header_part = HEADER_SIZE/4
    ftree = open(dbfile, "r")

    ftree.seek(header_part)
    buf = ftree.read(header_part)

    key_len = struct.unpack("!L", buf.decode('hex'))[0]

    buf = ftree.read(header_part)
    max_user_id = struct.unpack("!L", buf.decode('hex'))[0]

    buf = ftree.read(header_part)
    next_user_id = struct.unpack("!L", buf.decode('hex'))[0]

    info = DBInfo()

    info.key_len = key_len
    info.max_user_id = max_user_id
    info.min_user_id = max_user_id>>1
    if(next_user_id == info.min_user_id):
        info.current_user_id = 0 # no new user exists
    else:
        info.current_user_id = next_user_id-1

    ftree.close()
    return info

def gen_subkeys(K,cipher):
    """Generate subkeys of cipher"""
    from struct import pack, unpack
    
    L = cipher.encrypt("00000000000000000000000000000000".decode("hex"))

    LHigh = unpack(">Q",L[:8])[0]
    LLow  = unpack(">Q",L[8:])[0]

    K1High = ((LHigh << 1) | ( LLow >> 63 )) & 0xFFFFFFFFFFFFFFFF
    K1Low  = (LLow << 1) & 0xFFFFFFFFFFFFFFFF

    if (LHigh >> 63):
        K1Low ^= 0x87

    K2High = ((K1High << 1) | (K1Low >> 63)) & 0xFFFFFFFFFFFFFFFF
    K2Low  = ((K1Low << 1)) & 0xFFFFFFFFFFFFFFFF

    if (K1High >> 63):
        K2Low ^= 0x87

    K1 = pack(">QQ", K1High, K1Low)
    K2 = pack(">QQ", K2High, K2Low)

    return K1, K2

def read_xbs_chunk(stream):
    """Read one chunk from the underlying xbstream file object

    :param stream: a file-like object
    :returns: XBSChunk instance
    """
    header = stream.read(XBS_HEADER_SIZE)
    if not header:
        # end of stream
        return None

    magic, flags, _type, pathlen = struct.unpack(b'<8sBcI', header)

    if magic != XBS_MAGIC:
        raise common.UnpackError("Incorrect magic '%s' in chunk" % magic)

    path = stream.read(pathlen)

    if _type == b'E':
        return XBSChunk(flags, _type, path, b'', None)
    elif _type != b'P':
        raise common.UnpackError("Unknown chunk type '%r'" % _type)

    payload_length, payload_offset = struct.unpack(b'<QQ', stream.read(16))
    checksum, = struct.unpack(b'<I', stream.read(4))
    payload = stream.read(payload_length)

    computed_checksum = zlib.crc32(payload) & 0xffffffff

    if checksum != computed_checksum:
        raise common.UnpackError("Invalid checksum(offset=%d path=%s)" %
                                 (payload_offset, path))

    return XBSChunk(flags, _type, path, payload, payload_offset)

def getUserConfig(dbfile, user_id):
    header_part = HEADER_SIZE/4
    ftree = open(dbfile, "r")
    ftree.seek(header_part)
    buf = ftree.read(header_part)
    key_len = struct.unpack("!L", buf.decode('hex'))[0]

    buf = ftree.read(header_part)
    max_user_id = struct.unpack("!L", buf.decode('hex'))[0]

    if( (user_id < max_user_id/2) or (user_id > max_user_id -1)):
        print "user id ", user_id, " not found"
        return

    print "#user key configuration"
    print "user_id=", user_id
    print "key_len=", key_len
    while(user_id != 0):
        ftree.seek(HEADER_SIZE + (user_id * key_len))
        key = ftree.read(key_len)
        print user_id, "=", key
        user_id = user_id >> 1


    ftree.close()

    def read(self, fp, length, endian, param):
        """
        read data from fp
        :param fp: file pointer
        :param length: length to be readed
        :param endian: endian type in datafile
        :type param: list
        :param param: sampling rate,sample size, block time, channels
        :rtype: list of list
        :return: list of data
        """

        buff = fp.read(length)
        samplrate = param[0]
        numbyte = param[1]
        numchan = param[3].count(1)
        num = (samplrate/10)*numbyte*numchan
        data = [[] for _ in range(numchan)]
        if (length != num):
            raise EVTBadDataError("Bad data lenght")
        for j in range(samplrate/10):
            for k in range(numchan):
                i = (j*numchan)+k
                if numbyte == 2:
                    val = unpack(">i", buff[i*2:(i*2)+2] + '\0\0')[0] >> 8
                elif numbyte == 3:
                    val = unpack(">i", buff[i*3:(i*3)+3] + '\0')[0] >> 8
                elif numbyte == 4:
                    val = unpack(">i", buff[i*4:(i*4)+4])[0]
                else:
                    raise EVTBadDataError("Bad data format")
                data[k].append(val)
        return data

 def read_binary_int(self, fname,fp,length):
     '''
     read a binary int value
     '''
     if length > 3:
         raise CFFormatError('Integer greater than 8 bytes: %s' % length)
 
     nbytes = 1 << length
     val = None
     buff = fp.read(nbytes)
 
     if length == 0:
         val = unpack('>B', buff)
         val = val[0]
     elif length == 1:
         val = unpack('>H', buff)
         val = val[0]
     elif length == 2:
         val = unpack('>L', buff)
         val = val[0]
     elif length == 3:
         (hiword,loword) = unpack('>LL', buff)
         if not (hiword & 0x80000000) == 0:
             # 8 byte integers are always signed, and are negative when bit
             # 63 is set. Decoding into either a Fixnum or Bignum is tricky,
             # however, because the size of a Fixnum varies among systems,
             # and Ruby doesn't consider the number to be negative, and
             # won't sign extend.
             val = -(2**63 - ((hiword & 0x7fffffff) << 32 | loword))
         else:
             val = hiword << 32 | loword
     
     return CFInteger(val)

def init_cam():
    global PASSWORD
    try:
        bcv = httpopen('http://%s/bacpac/cv' % CAM_IP).read()
        print('bacpac CV:', repr(bcv))
        bacpac_version = struct.unpack('BBB', bcv[9:12])
        bacpac_version = '.'.join([str(x) for x in bacpac_version])
        print('   bacpac version:', bacpac_version)
        bacpac_mac = bcv[12:18]
        bacpac_mac = ':'.join(['%02x' % x for x in bacpac_mac])
        print('bacpac mac:', bacpac_mac)
        bacpac_name = bcv[19:].decode('utf-8')
        print('bacpac name:', bacpac_name)
        
        bsd = httpopen('http://%s/bacpac/sd' % CAM_IP).read()
        print('bacpac SD:', repr(bsd))
        PASSWORD = bsd[2:].decode('utf-8')
        print('bacpac password', PASSWORD)
     
        ccv = httpopen('http://%s/camera/cv' % CAM_IP).read()
        print('camera CV:', repr(ccv))
        # b'\x00\x00\x01\x13HD2.08.12.198.47.00\x05HERO2'
        dlen = struct.unpack('B', ccv[3:4])[0]
        camera_version = ccv[4:4+dlen].decode('UTF-8')
        print('camera version', camera_version)
        ipos = 4+dlen
        dlen = struct.unpack('B', ccv[ipos:ipos+1])[0]
        ipos += 1
        camera_model = ccv[ipos:ipos+dlen].decode('UTF-8')
        print('camera_model', camera_model) #FIXME this is the CN parameter
        return True
    except (urllib.error.HTTPError, urllib.error.URLError, socket.error):
        print('Error communicating with bacpac/camera')
        return False

    def recieve_data(self):
        buffer = ''

        # recieve junk
        prev_byte = '\x00'
        while 1:
            cur_byte = self.ser.read(1)
            if prev_byte + cur_byte == HEADER:
                # header found, stop
                break
            prev_byte  = cur_byte

        length = struct.unpack('<H', self.get_n_bytes(2)) [0]

        packet = self.get_n_bytes(length, True)

        reserved, command = struct.unpack('<HH', packet[:4])
        data = packet[4:-1]
        checksum = ord(packet[-1])

        #~ print self.tohex(packet[:-1])

        packet_int = map(ord, packet[:-1])
        checksum_calc = reduce( lambda x,y:  x^y,  packet_int  )

        if data[0] == '\x00':
            if checksum != checksum_calc:
                raise Exception, "bad checksum"


        return command, data

def ExtPak(fs,dirname):
    fs.seek(0)
    magic, desc, minor_ver, major_ver, index_len, flags, index_entries,\
           data_offset, index_offset=unpack('16s32sHHIIIII',fs.read(72))

    if(magic.rstrip(b'\0')!=b'DataPack5'):
        print("Format Error!")
        return 0
    fs.seek(index_offset)
    idx=bytearray(fs.read(index_len))
    if(flags&1):
        Decrypt(idx)
    idx=LzssUnc((bytes)(idx),index_entries*0x68)
    #int3()
    idxstm=io.BytesIO(idx)
    for i in range(index_entries):
        fname,offset,length=unpack('64sII32x',idxstm.read(0x68))
        fname=fname.rstrip(b'\0').decode('932')
        if(fname==''):
            xxx=0
        newf=open(os.path.join(dirname,fname),'wb')
        fs.seek(offset+data_offset)
        buff=fs.read(length)
        newf.write(buff)
        newf.close()

 def sendCMDreceiveATT(self, data_length, code, data):
     checksum = 0
     total_data = ['$', 'M', '<', data_length, code] + data
     for i in struct.pack('<2B%dh' % len(data), *total_data[3:len(total_data)]):
         checksum = checksum ^ ord(i)
     total_data.append(checksum)
     try:
         start = time.time()
         b = None
         b = self.ser.write(struct.pack('<3c2B%dhB' % len(data), *total_data))
         while True:
             header = self.ser.read()
             if header == '$':
                 header = header+self.ser.read(2)
                 break
         datalength = struct.unpack('<b', self.ser.read())[0]
         code = struct.unpack('<b', self.ser.read())
         data = self.ser.read(datalength)
         temp = struct.unpack('<'+'h'*(datalength/2),data)
         self.ser.flushInput()
         self.ser.flushOutput()
         elapsed = time.time() - start
         self.attitude['angx']=float(temp[0]/10.0)
         self.attitude['angy']=float(temp[1]/10.0)
         self.attitude['heading']=float(temp[2])
         self.attitude['elapsed']=round(elapsed,3)
         self.attitude['timestamp']="%0.2f" % (time.time(),) 
         return self.attitude
     except Exception, error:
         #print "\n\nError in sendCMDreceiveATT."
         #print "("+str(error)+")\n\n"
         pass

def decode_pair(s, pos=0):
    """
    Decodes a name/value pair.

    The number of bytes decoded as well as the name/value pair
    are returned.
    """
    nameLength = ord(s[pos])
    if nameLength & 128:
        nameLength = struct.unpack('!L', s[pos:pos+4])[0] & 0x7fffffff
        pos += 4
    else:
        pos += 1

    valueLength = ord(s[pos])
    if valueLength & 128:
        valueLength = struct.unpack('!L', s[pos:pos+4])[0] & 0x7fffffff
        pos += 4
    else:
        pos += 1

    name = s[pos:pos+nameLength]
    pos += nameLength
    value = s[pos:pos+valueLength]
    pos += valueLength

    return (pos, (name, value))

def receive_one_ping(my_socket, ID, timeout):
    """
    receive the ping from the socket.
    """
    timeLeft = timeout
    while True:
        startedSelect = time.time()
        whatReady = select.select([my_socket], [], [], timeLeft)
        howLongInSelect = (time.time() - startedSelect)
        if whatReady[0] == []: # Timeout
            return
 
        timeReceived = time.time()
        recPacket, addr = my_socket.recvfrom(1024)
        icmpHeader = recPacket[20:28]
        type, code, checksum, packetID, sequence = struct.unpack(
            "bbHHh", icmpHeader
        )
        if packetID == ID:
            bytesInDouble = struct.calcsize("d")
            timeSent = struct.unpack("d", recPacket[28:28 + bytesInDouble])[0]
            return timeReceived - timeSent
 
        timeLeft = timeLeft - howLongInSelect
        if timeLeft <= 0:
            return

    def __init__(self, raw_packet=None):
        if raw_packet is None:
            self.ethernet = Ethernet()
            self.hardware_type = None
            self.protocol_type = None
            self.hardware_size = None
            self.protocol_size = None
            self.operation = None
            self.sender_hardware_address = None
            self.sender_protocol_address = None
            self.target_hardware_address = None
            self.target_protocol_address = None
        else:
            self.ethernet = Ethernet(raw_packet)
            raw_arp = self.ethernet.data

            self.hardware_type = struct.unpack('!H', raw_arp[:2])[0]
            self.protocol_type = struct.unpack('!H', raw_arp[2:4])[0]
            self.hardware_size = struct.unpack('!B', raw_arp[4])[0]
            self.protocol_size = struct.unpack('!B', raw_arp[5])[0]
            self.operation = struct.unpack('!H', raw_arp[6:8])[0]
            self.sender_hardware_address = raw_arp[8:14]
            self.sender_protocol_address = raw_arp[14:18]
            self.target_hardware_address = raw_arp[18:24]
            self.target_protocol_address = raw_arp[24:28]