frow/csafe.go at master · axiom/frow · GitHub

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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
package main

import (
	"bytes"
	"errors"
	"fmt"
	"log"
	"strings"
)

const (
	ExtendedStartFlag = 0xf0
	StandardStartFlag = 0xf1
	StopFlag          = 0xf2
	StuffFlag         = 0xf3
)

type Framer interface {
	Frame() []byte
}

type FrameContents interface {
	Bytes() []byte
	Len() int
}

type rawContents []byte

func (fc rawContents) Bytes() []byte {
	return []byte(fc)
}

func (fc rawContents) Len() int {
	return len([]byte(fc))
}

func (fc rawContents) String() string {
	fancyBytes := make([]string, len(fc))
	for i, b := range []byte(fc) {
		fancyBytes[i] = fmt.Sprintf("0x%x", b)
	}
	return fmt.Sprintf("[%v]", strings.Join(fancyBytes, " "))
}

type Frame struct {
	contents FrameContents
}

func NewFrame(contents FrameContents) Frame {
	return Frame{contents: contents}
}

func (f Frame) String() string {
	return rawContents(f.contents.Bytes()).String()
}

func Checksum(bytes []byte) byte {
	var checksum byte
	for _, b := range bytes {
		checksum ^= b
	}
	return checksum
}

func (f Frame) Bytes() ([]byte, error) {
	var buffer bytes.Buffer

	if err := buffer.WriteByte(StandardStartFlag); err != nil {
		return nil, err
	}

	for _, b := range f.contents.Bytes() {
		switch b {
		case ExtendedStartFlag:
			fallthrough
		case StandardStartFlag:
			fallthrough
		case StopFlag:
			fallthrough
		case StuffFlag:
			if err := buffer.WriteByte(StuffFlag); err != nil {
				return nil, err
			}
			if err := buffer.WriteByte(0x03 & b); err != nil {
				return nil, err
			}
		default:
			if err := buffer.WriteByte(b); err != nil {
				return nil, err
			}
		}
	}

	switch checksum := Checksum(f.contents.Bytes()); checksum {
	case ExtendedStartFlag:
		fallthrough
	case StandardStartFlag:
		fallthrough
	case StopFlag:
		fallthrough
	case StuffFlag:
		if err := buffer.WriteByte(StuffFlag); err != nil {
			return nil, err
		}
		if err := buffer.WriteByte(0x03 & checksum); err != nil {
			return nil, err
		}
	default:
		if err := buffer.WriteByte(checksum); err != nil {
			return nil, err
		}
	}

	if err := buffer.WriteByte(StopFlag); err != nil {
		return nil, err
	}

	return buffer.Bytes(), nil
}

func GetContentsFromFrame(frame []byte) ([]byte, error) {
	// First byte is USB stuff.
	if frame[1] != StandardStartFlag {
		return nil, errors.New("Bad start byte")
	}

	var output bytes.Buffer

	// Unstuff
	unstuff := false
	for _, b := range frame[2:len(frame)] {
		if b == StopFlag {
			break
		}

		if unstuff {
			output.WriteByte(b | byte(0xff&(0xff<<2)))
			unstuff = false
			continue
		} else if b == StuffFlag {
			unstuff = true
			continue
		}

		output.WriteByte(b)
	}

	response := output.Bytes()
	var checksum byte
	for _, b := range response[0 : len(response)-1] {
		checksum ^= b
	}

	if checksum != response[len(response)-1] {
		log.Println("Checksum failed in response")
	}

	return response[0 : len(response)-1], nil
}