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
120
121
122
123
124
125
126
127
|
#!/usr/bin/env python3
from threading import Timer
from struct import unpack,pack
import gi
gi.require_versions({'GLib': '2.0', 'Hinawa': '4.0', 'Hinoko': '1.0'})
from gi.repository import GLib, Hinawa, Hinoko
class IsoIt(Hinoko.FwIsoIt):
def new(self, path, channel, maximum_bytes_per_payload, packets_per_buffer):
# Use 8 byte header for CIP header.
cip_header_size = 8
self.cip_payload = [i for i in range(maximum_bytes_per_payload)]
self.allocate(path, Hinoko.FwScode.S400, channel, cip_header_size)
self.map_buffer(maximum_bytes_per_payload, packets_per_buffer)
self.channel = channel
self.packets_per_buffer = packets_per_buffer
def destroy(self):
self.unmap_buffer()
self.release()
def queue_packet(self, cip_header, cip_payload):
self.accumulated_packet_count += 1
if self.accumulated_packet_count % self.packets_per_interrupt == 0:
schedule_interrupt = True
else:
schedule_interrupt = False
self.register_packet(self.tag, self.sy, cip_header, cip_payload, schedule_interrupt)
def begin(self, dispatcher, packets_per_interrupt, 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,))
self.tag = Hinoko.FwIsoCtxMatchFlag.TAG1
self.sy = 0
# Start 100 msec 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, 800)
cycle_match = (sec & 0x3, cycle)
self.accumulated_packet_count = 0
self.packets_per_interrupt = packets_per_interrupt
# Schedule some isochronous cycles to skip.
for i in range(packets_per_interrupt * 2):
self.queue_packet(None, None)
self.__timer.start()
self.start(cycle_match)
@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):
tstamps = unpack('>{0}I'.format(header_length // 4), header)
for i in range(count):
sec, cycle = self.__ohci1394_tstamp_to_isoc_cycle(tstamps[i])
# Schedule packet at the completed isochronous cycle plus packets per buffer. The header
# of packet has data for the completed isochronous cycle.
next_sec, next_cycle = self.__increment_cycle(sec, cycle, self.packets_per_buffer)
cip_header = pack('>2I', next_sec, next_cycle)
self.queue_packet(cip_header, self.cip_payload)
# Print the data of packet already sent.
iso_header = (len(self.cip_payload) << 16) | \
(self.tag << 14) | \
(self.channel << 8) | \
(0xa << 4) | \
self.sy
print('{0:2d},{1:4d},{2:08x},{3}'.format(sec, cycle, iso_header, i))
channel = 30
maximum_bytes_per_payload = 32
packets_per_buffer = 32
packets_per_interrupt = 4
duration = 4
ctx = IsoIt()
ctx.new('/dev/fw0', channel, maximum_bytes_per_payload, packets_per_buffer)
dispatcher = GLib.MainLoop.new(None, False)
ctx.begin(dispatcher, packets_per_interrupt, duration)
dispatcher.run()
ctx.finish()
ctx.destroy()
del dispatcher
del ctx
|
