1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
#!/usr/bin/env python3
from threading import Timer
from struct import unpack
import gi
gi.require_versions({'GLib': '2.0', 'Hinawa': '4.0', 'Hinoko': '1.0'})
from gi.repository import GLib, Hinawa, Hinoko
class IsoIrSingle(Hinoko.FwIsoIrSingle):
def new(self, path, channel, maximum_bytes_per_payload):
self.allocate(path, channel, 8)
# Use 10msec interval for I/O operation.
self.map_buffer(maximum_bytes_per_payload, 240)
def destroy(self):
self.unmap_buffer()
self.release()
def queue_packet(self):
schedule_interrupt = False
self.accumulated_packet_count += 1
if self.accumulated_packet_count % self.packets_per_interrupt == 0:
schedule_interrupt = True
self.register_packet(schedule_interrupt)
def begin(self, dispatcher, duration):
_, self.__src = self.create_source()
self.__src.attach(dispatcher.get_context())
self.__dispatcher = dispatcher
# Roughly finish event loop.
self.__timer = Timer(duration, lambda dispatcher: dispatcher.quit(),
args=(dispatcher,))
tags = Hinoko.FwIsoCtxMatchFlag.TAG0 | \
Hinoko.FwIsoCtxMatchFlag.TAG1 | \
Hinoko.FwIsoCtxMatchFlag.TAG2 | \
Hinoko.FwIsoCtxMatchFlag.TAG3
# Start one half of second later. Scheduling is available with lower 2 bits of second field.
cycle_time = Hinawa.CycleTime.new()
clock_id = 4 #CLOCK_MONOTONIC_RAW
_, cycle_time = self.read_cycle_time(clock_id, cycle_time)
(sec, cycle, tick) = cycle_time.get_fields()
sec, cycle = self.__increment_cycle(sec, cycle, 4000)
cycle_match = (sec & 0x3, cycle)
self.accumulated_packet_count = 0
self.packets_per_interrupt = 80
# Schedule for some isochronous cycles.
for i in range(self.packets_per_interrupt * 2):
self.queue_packet()
self.__timer.start()
self.start(cycle_match, 0, tags)
@staticmethod
def __increment_cycle(sec, cycle, addend):
cycle += addend
if cycle >= 8000:
cycle %= 8000
sec += 1
if sec >= 128:
sec %= 128
return (sec, cycle)
def finish(self):
self.__src.destroy()
self.stop()
def do_stopped(self, error):
self.__dispatcher.quit()
if error:
self.__timer.cancel()
print(error)
@staticmethod
def __ohci1394_tstamp_to_isoc_cycle(tstamp):
sec = (tstamp & 0x0000e000) >> 13
cycle = tstamp & 0x00001fff
return (sec, cycle)
def do_interrupted(self, sec, cycle, header, header_length, count):
headers = unpack('>{0}I'.format(header_length // 4), header)
for i in range(count):
iso_header, tstamp = headers[i * 2:(i + 1) * 2]
sec, cycle = self.__ohci1394_tstamp_to_isoc_cycle(tstamp)
data = self.get_payload(i)
frames = unpack('>{0}I'.format(len(data) // 4), data)
print('{0:d},{1:d},{2:08x},{3}'.format(sec, cycle, iso_header, i))
for i, frame in enumerate(frames):
print(' {0:2d}: {1:08x}'.format(i, frame))
self.queue_packet()
# 2 CIP headers + 3 quadlet per data block * 8 data block
channel = 1
maximum_bytes_per_payload = (2 + 3 * 8) * 4
ctx = IsoIrSingle()
ctx.new('/dev/fw0', channel, maximum_bytes_per_payload)
dispatcher = GLib.MainLoop.new(None, False)
ctx.begin(dispatcher, 4)
dispatcher.run()
ctx.finish()
ctx.destroy()
del dispatcher
del ctx
|
