Skip to content

Commit 9072a57

Browse files
raidozraidoz
committed
Initial version.
1 parent 488f273 commit 9072a57

4 files changed

Lines changed: 554 additions & 0 deletions

File tree

‎deviceparameters.go‎

Lines changed: 393 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,393 @@
1+
// Author Raido Pahtma
2+
// License MIT
3+
4+
package deviceparameters
5+
6+
import "fmt"
7+
import "time"
8+
import "errors"
9+
import "bytes"
10+
import "encoding/binary"
11+
12+
import "github.com/proactivity-lab/go-loggers"
13+
import "github.com/proactivity-lab/go-sfconnection"
14+
15+
type DeviceParameter struct {
16+
Name string
17+
Type uint8
18+
Seqnum uint8
19+
Value []byte
20+
Timestamp time.Time
21+
Error error
22+
}
23+
24+
const (
25+
DP_TYPE_RAW = 0x00
26+
DP_TYPE_UINT8 = 0x01
27+
DP_TYPE_UINT16 = 0x02
28+
DP_TYPE_UINT32 = 0x04
29+
DP_TYPE_UINT64 = 0x08
30+
31+
DP_TYPE_STRING = 0x80
32+
DP_TYPE_INT8 = 0x81
33+
DP_TYPE_INT16 = 0x82
34+
DP_TYPE_INT32 = 0x84
35+
DP_TYPE_INT64 = 0x88
36+
)
37+
38+
const TOS_SERIAL_DEVICE_PARAMETERS_ID = 0x80
39+
40+
type DeviceParameterManager struct {
41+
loggers.DIWEloggers
42+
sfc *sfconnection.SfConnection
43+
dsp *sfconnection.PacketDispatcher
44+
45+
values map[string]*DeviceParameter
46+
devstart time.Time
47+
heartbeat time.Time
48+
49+
timeout time.Duration
50+
retries int
51+
52+
receive chan sfconnection.Packet
53+
54+
done chan bool
55+
closed bool
56+
}
57+
58+
type ParameterError struct{ s string }
59+
type TimeoutError struct{ s string }
60+
61+
func (self ParameterError) Error() string { return self.s }
62+
func NewParameterError(text string) error { return &ParameterError{text} }
63+
func (self TimeoutError) Error() string { return self.s }
64+
func NewTimeoutError(text string) error { return &TimeoutError{text} }
65+
66+
func NewDeviceParameterManager(sfc *sfconnection.SfConnection) *DeviceParameterManager {
67+
dpm := new(DeviceParameterManager)
68+
dpm.InitLoggers()
69+
dpm.values = make(map[string]*DeviceParameter)
70+
dpm.done = make(chan bool)
71+
dpm.closed = false
72+
dpm.receive = make(chan sfconnection.Packet)
73+
dpm.timeout = time.Second
74+
dpm.retries = 3
75+
76+
dpm.dsp = sfconnection.NewPacketDispatcher(sfconnection.NewRawPacket(TOS_SERIAL_DEVICE_PARAMETERS_ID))
77+
dpm.dsp.RegisterReceiver(dpm.receive)
78+
79+
dpm.sfc = sfc
80+
dpm.sfc.AddDispatcher(dpm.dsp)
81+
82+
go dpm.run()
83+
return dpm
84+
}
85+
86+
func (self *DeviceParameterManager) SetTimeout(timeout time.Duration) {
87+
self.timeout = timeout
88+
}
89+
90+
func (self *DeviceParameterManager) SetRetries(retries int) {
91+
self.retries = retries
92+
}
93+
94+
func (self *DeviceParameterManager) GetValue(name string) ([]byte, error) {
95+
// Interrupt the run goroutine
96+
self.done <- true
97+
98+
var result error = errors.New("disabled")
99+
100+
for retries := 0; retries <= self.retries; retries++ {
101+
// Send get request
102+
msg := self.dsp.NewPacket().(*sfconnection.RawPacket)
103+
payload := new(DpGetParameterId)
104+
payload.Header = DP_GET_PARAMETER_WITH_ID
105+
payload.Id = name
106+
msg.Payload = sfconnection.SerializePacket(payload)
107+
self.sfc.Send(msg)
108+
109+
// Wait for value
110+
dp, err := self.waitValueId(name)
111+
if err == nil {
112+
go self.run()
113+
return dp.Value, nil
114+
} else {
115+
result = err
116+
if _, ok := err.(*ParameterError); ok {
117+
break
118+
}
119+
}
120+
}
121+
122+
go self.run()
123+
return nil, result
124+
}
125+
126+
func (self *DeviceParameterManager) SetValue(name string, value []byte) error {
127+
// Interrupt the run goroutine
128+
self.done <- true
129+
130+
var result error = errors.New("disabled")
131+
132+
for retries := 0; retries <= self.retries; retries++ {
133+
// Send set request
134+
msg := self.dsp.NewPacket().(*sfconnection.RawPacket)
135+
payload := new(DpSetParameterId)
136+
payload.Header = DP_SET_PARAMETER_WITH_ID
137+
payload.Id = name
138+
payload.Value = value
139+
msg.Payload = sfconnection.SerializePacket(payload)
140+
self.sfc.Send(msg)
141+
142+
// Wait for value
143+
dp, err := self.waitValueId(name)
144+
if err == nil {
145+
if bytes.Compare(dp.Value, value) == 0 {
146+
// store in values table
147+
self.values[name] = dp
148+
go self.run()
149+
return nil
150+
} else {
151+
result = errors.New(fmt.Sprintf("Returned value %X does not match set value %X!", dp.Value, value))
152+
}
153+
} else {
154+
result = err
155+
if _, ok := err.(*ParameterError); ok {
156+
break
157+
}
158+
}
159+
}
160+
161+
go self.run()
162+
return result
163+
}
164+
165+
func (self *DeviceParameterManager) GetList() (chan *DeviceParameter, error) {
166+
// Interrupt the run goroutine
167+
self.done <- true
168+
169+
delivery := make(chan *DeviceParameter)
170+
go self.getList(delivery)
171+
172+
return delivery, nil
173+
}
174+
175+
func (self *DeviceParameterManager) receivedPacket(msg *sfconnection.RawPacket) {
176+
self.Debug.Printf("%s\n", msg)
177+
if len(msg.Payload) > 0 {
178+
if msg.Payload[0] == DP_HEARTBEAT {
179+
p := new(DpHeartbeat)
180+
if err := sfconnection.DeserializePacket(p, msg.Payload); err == nil {
181+
self.heartbeat = time.Now()
182+
self.devstart = self.heartbeat.Add(-time.Duration(p.Uptime) * time.Second)
183+
// TODO check stuff
184+
}
185+
}
186+
}
187+
}
188+
189+
func (self *DeviceParameterManager) waitValueId(name string) (*DeviceParameter, error) {
190+
start := time.Now()
191+
for {
192+
select {
193+
case packet := <-self.receive:
194+
msg := packet.(*sfconnection.RawPacket)
195+
if len(msg.Payload) > 0 {
196+
if msg.Payload[0] == DP_PARAMETER {
197+
p := new(DpParameter)
198+
if err := sfconnection.DeserializePacket(p, msg.Payload); err == nil {
199+
if p.Id == name {
200+
return &DeviceParameter{name, p.Type, p.Seqnum, p.Value, time.Now(), nil}, nil
201+
}
202+
} else {
203+
self.Error.Printf("Deserialize error %s %s\n", err, msg)
204+
}
205+
} else if msg.Payload[0] == DP_ERROR_PARAMETER_ID {
206+
p := new(DpErrorParameterId)
207+
if err := sfconnection.DeserializePacket(p, msg.Payload); err == nil {
208+
if p.Id == name {
209+
if p.Exists {
210+
return nil, errors.New(fmt.Sprintf("Something went wrong with parameter \"%s\", error %d!", name, p.Err))
211+
} else {
212+
return nil, NewParameterError(fmt.Sprintf("No parameter \"%s\" on device!", name))
213+
}
214+
} else {
215+
self.Warning.Printf("Received unexpected error for parameter %s\n", p.Id)
216+
}
217+
} else {
218+
self.Error.Printf("Deserialize error %s %s\n", err, msg)
219+
}
220+
} else {
221+
self.receivedPacket(msg)
222+
}
223+
}
224+
case <-time.After(remaining(start, self.timeout)):
225+
return nil, NewTimeoutError(fmt.Sprintf("Timeout for parameter \"%s\"!", name))
226+
}
227+
}
228+
}
229+
230+
func (self *DeviceParameterManager) waitValueSeqnum(seqnum uint8) (*DeviceParameter, error) {
231+
start := time.Now()
232+
for {
233+
select {
234+
case packet := <-self.receive:
235+
msg := packet.(*sfconnection.RawPacket)
236+
if len(msg.Payload) > 0 {
237+
if msg.Payload[0] == DP_PARAMETER {
238+
p := new(DpParameter)
239+
if err := sfconnection.DeserializePacket(p, msg.Payload); err == nil {
240+
if p.Seqnum == seqnum {
241+
return &DeviceParameter{p.Id, p.Type, p.Seqnum, p.Value, time.Now(), nil}, nil
242+
}
243+
} else {
244+
self.Error.Printf("Deserialize error %s %s\n", err, msg)
245+
}
246+
} else if msg.Payload[0] == DP_ERROR_PARAMETER_SEQNUM {
247+
p := new(DpErrorParameterSeqnum)
248+
if err := sfconnection.DeserializePacket(p, msg.Payload); err == nil {
249+
if p.Seqnum == seqnum {
250+
if p.Exists {
251+
return nil, errors.New(fmt.Sprintf("Something went wrong with parameter %d, error %d!", seqnum, p.Err))
252+
} else {
253+
return nil, NewParameterError(fmt.Sprintf("No parameter %d on device!", seqnum))
254+
}
255+
} else {
256+
self.Warning.Printf("Received unexpected error for parameter %d\n", p.Seqnum)
257+
}
258+
} else {
259+
self.Error.Printf("Deserialize error %s %s\n", err, msg)
260+
}
261+
} else {
262+
self.receivedPacket(msg)
263+
}
264+
}
265+
case <-time.After(remaining(start, self.timeout)):
266+
return nil, NewTimeoutError(fmt.Sprintf("Timeout for parameter %d!", seqnum))
267+
}
268+
}
269+
}
270+
271+
func (self *DeviceParameterManager) getList(delivery chan *DeviceParameter) {
272+
for i := 0; i < 256; i++ {
273+
for retries := 0; retries <= self.retries; retries++ {
274+
self.Debug.Printf("Get %d %d/%d\n", i, retries, self.retries)
275+
// Send get request
276+
msg := self.dsp.NewPacket().(*sfconnection.RawPacket)
277+
payload := new(DpGetParameterSeqnum)
278+
payload.Header = DP_GET_PARAMETER_WITH_SEQNUM
279+
payload.Seqnum = uint8(i)
280+
msg.Payload = sfconnection.SerializePacket(payload)
281+
self.sfc.Send(msg)
282+
283+
// Wait for value
284+
dp, err := self.waitValueSeqnum(uint8(i))
285+
if err == nil {
286+
delivery <- dp
287+
break
288+
} else {
289+
self.Debug.Printf("Got %s\n", err)
290+
if _, ok := err.(*ParameterError); ok { // This parameter does not exist and therefore the list is complete
291+
self.Debug.Printf("closing")
292+
close(delivery)
293+
go self.run()
294+
return
295+
} else if retries == self.retries {
296+
delivery <- &DeviceParameter{"", 0, uint8(i), nil, time.Now(), err}
297+
break
298+
}
299+
}
300+
}
301+
}
302+
303+
close(delivery)
304+
go self.run()
305+
}
306+
307+
func (self *DeviceParameterManager) run() {
308+
self.Debug.Printf("DPM running\n")
309+
for {
310+
select {
311+
case packet := <-self.receive:
312+
msg := packet.(*sfconnection.RawPacket)
313+
self.receivedPacket(msg)
314+
case done := <-self.done:
315+
if done {
316+
self.Debug.Printf("DPM interrupted\n")
317+
} else {
318+
self.Debug.Printf("DPM closed\n")
319+
}
320+
return
321+
}
322+
}
323+
}
324+
325+
func (self *DeviceParameterManager) Close() error {
326+
if !self.closed {
327+
self.closed = true
328+
close(self.done)
329+
return nil
330+
}
331+
return errors.New("Close has already been called!")
332+
}
333+
334+
func (self *DeviceParameter) String() string {
335+
if self.Type == DP_TYPE_RAW {
336+
return fmt.Sprintf("%X", self.Value)
337+
} else if self.Type == DP_TYPE_STRING {
338+
return string(self.Value)
339+
} else {
340+
s := fmt.Sprintf("%v", self.Value)
341+
buf := bytes.NewBuffer(self.Value)
342+
if self.Type == DP_TYPE_UINT8 {
343+
var v uint8
344+
if err := binary.Read(buf, binary.BigEndian, &v); err == nil {
345+
s = fmt.Sprintf("%d", v)
346+
}
347+
} else if self.Type == DP_TYPE_UINT16 {
348+
var v uint16
349+
if err := binary.Read(buf, binary.BigEndian, &v); err == nil {
350+
s = fmt.Sprintf("%d", v)
351+
}
352+
} else if self.Type == DP_TYPE_UINT32 {
353+
var v uint32
354+
if err := binary.Read(buf, binary.BigEndian, &v); err == nil {
355+
s = fmt.Sprintf("%d", v)
356+
}
357+
} else if self.Type == DP_TYPE_UINT64 {
358+
var v uint64
359+
if err := binary.Read(buf, binary.BigEndian, &v); err == nil {
360+
s = fmt.Sprintf("%d", v)
361+
}
362+
} else if self.Type == DP_TYPE_INT8 {
363+
var v int8
364+
if err := binary.Read(buf, binary.BigEndian, &v); err == nil {
365+
s = fmt.Sprintf("%d", v)
366+
}
367+
} else if self.Type == DP_TYPE_INT16 {
368+
var v int16
369+
if err := binary.Read(buf, binary.BigEndian, &v); err == nil {
370+
s = fmt.Sprintf("%d", v)
371+
}
372+
} else if self.Type == DP_TYPE_INT32 {
373+
var v int32
374+
if err := binary.Read(buf, binary.BigEndian, &v); err == nil {
375+
s = fmt.Sprintf("%d", v)
376+
}
377+
} else if self.Type == DP_TYPE_INT64 {
378+
var v int64
379+
if err := binary.Read(buf, binary.BigEndian, &v); err == nil {
380+
s = fmt.Sprintf("%d", v)
381+
}
382+
}
383+
return s
384+
}
385+
}
386+
387+
func remaining(start time.Time, timeout time.Duration) time.Duration {
388+
elapsed := time.Since(start)
389+
if elapsed < timeout {
390+
return timeout - elapsed
391+
}
392+
return 0
393+
}

0 commit comments

Comments
 (0)