mirror of https://github.com/periph/devices
You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
115 lines
2.3 KiB
Go
115 lines
2.3 KiB
Go
package firmata
|
|
|
|
import (
|
|
"fmt"
|
|
)
|
|
|
|
const SevenBitMask byte = 0b01111111
|
|
|
|
func TwoByteToByte(a, b byte) byte {
|
|
return (a & SevenBitMask) | ((b & SevenBitMask) << 7)
|
|
}
|
|
|
|
func TwoByteToWord(a, b byte) uint16 {
|
|
return uint16(a&SevenBitMask) | (uint16(b&SevenBitMask) << 7)
|
|
}
|
|
|
|
func TwoByteString(bytes []byte) string {
|
|
if len(bytes)%2 == 1 {
|
|
bytes = append(bytes, 0)
|
|
}
|
|
|
|
var s string
|
|
for i := 0; i < len(bytes); i += 2 {
|
|
s += string(TwoByteToByte(bytes[i], bytes[i+1]))
|
|
}
|
|
return s
|
|
}
|
|
|
|
func TwoByteRepresentationToByteSlice(bytes []byte) []byte {
|
|
if len(bytes)%2 == 1 {
|
|
bytes = append(bytes, 0)
|
|
}
|
|
|
|
d := make([]byte, len(bytes)/2)
|
|
i := 0
|
|
for di := range d {
|
|
d[di] = TwoByteToByte(bytes[i], bytes[i+1])
|
|
i += 2
|
|
}
|
|
return d
|
|
}
|
|
|
|
func ByteToTwoByte(b byte) (lsb, msb byte) {
|
|
return b & SevenBitMask, (b >> 7) & SevenBitMask
|
|
}
|
|
|
|
func WordToTwoByte(b uint16) (lsb, msb byte) {
|
|
return byte(b) & SevenBitMask, byte(b>>7) & SevenBitMask
|
|
}
|
|
|
|
func ByteSliceToTwoByteRepresentation(bytes []byte) []byte {
|
|
d := make([]byte, len(bytes)*2)
|
|
i := 0
|
|
for _, b := range bytes {
|
|
d[i], d[i+1] = ByteToTwoByte(b)
|
|
i += 2
|
|
}
|
|
return d
|
|
}
|
|
|
|
func SprintHexArray(data []byte) string {
|
|
s := ""
|
|
if len(data) == 0 {
|
|
return s
|
|
}
|
|
for _, b := range data {
|
|
s += fmt.Sprintf("0x%02X ", b)
|
|
}
|
|
return s[:len(s)-1]
|
|
}
|
|
|
|
// Encoder7Bit logic determined from here:
|
|
// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.cpp:34
|
|
func Encoder7Bit(inData []byte) []byte {
|
|
var outData []byte
|
|
var previous byte
|
|
var shift = 0
|
|
for _, data := range inData {
|
|
if shift == 0 {
|
|
outData = append(outData, data&0x7f)
|
|
shift++
|
|
previous = data >> 7
|
|
} else {
|
|
outData = append(outData, ((data<<shift)&0x7f)|previous)
|
|
if shift == 6 {
|
|
outData = append(outData, data>>1)
|
|
shift = 0
|
|
} else {
|
|
shift++
|
|
previous = data >> (8 - shift)
|
|
}
|
|
}
|
|
}
|
|
if shift > 0 {
|
|
outData = append(outData, previous)
|
|
}
|
|
return outData
|
|
}
|
|
|
|
// Decoder7Bit logic determined from here:
|
|
// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.h:17
|
|
// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.cpp:54
|
|
func Decoder7Bit(inData []byte) []byte {
|
|
var outBytes = ((len(inData)) * 7) >> 3
|
|
|
|
var outData = make([]byte, outBytes)
|
|
for i := 0; i < outBytes; i++ {
|
|
var j = i << 3
|
|
var pos = j / 7
|
|
var shift = byte(j % 7)
|
|
outData[i] = (inData[pos] >> shift) | ((inData[pos+1] << (7 - shift)) & 0xFF)
|
|
}
|
|
return outData
|
|
}
|