From fdc0878438417a71b8944f05a00d40e4260d01ef Mon Sep 17 00:00:00 2001 From: Matthias Fulz Date: Mon, 7 Feb 2022 02:20:56 +0100 Subject: [PATCH] Basic features done --- go.mod | 5 ++ go.sum | 2 + qmk-cmd.go | 221 +++++++++++++++++++++++++++++++++++++++++++++++++++++ qmk-dev.go | 162 +++++++++++++++++++++++++++++++++++++++ qmk-err.go | 58 ++++++++++++++ 5 files changed, 448 insertions(+) create mode 100644 go.mod create mode 100644 go.sum create mode 100644 qmk-cmd.go create mode 100644 qmk-dev.go create mode 100644 qmk-err.go diff --git a/go.mod b/go.mod new file mode 100644 index 0000000..5b5ba84 --- /dev/null +++ b/go.mod @@ -0,0 +1,5 @@ +module gitea.olznet.de/OlzNet/qmkenc + +go 1.17 + +require github.com/sstallion/go-hid v0.0.0-20211019232252-c64377bfa49e diff --git a/go.sum b/go.sum new file mode 100644 index 0000000..4f7cab2 --- /dev/null +++ b/go.sum @@ -0,0 +1,2 @@ +github.com/sstallion/go-hid v0.0.0-20211019232252-c64377bfa49e h1:I/6OqnP4mVfub3eeUqqlcIlXbBZtLhtLWuF8s2kip10= +github.com/sstallion/go-hid v0.0.0-20211019232252-c64377bfa49e/go.mod h1:JwBz6izP5UGcbYDU5VGeLkqpRIpSBDPtCB5/XnVXz9Q= diff --git a/qmk-cmd.go b/qmk-cmd.go new file mode 100644 index 0000000..9bea114 --- /dev/null +++ b/qmk-cmd.go @@ -0,0 +1,221 @@ +package qmkenc + +import ( + "encoding/binary" + "fmt" +) + +var magicHeader = []byte{0x03, 0xFF} + +type QEncCmd byte + +const ( + QENC_CMD_RESET QEncCmd = 0x00 + QENC_CMD_ENCRYPT QEncCmd = 0x01 + QENC_CMD_DECRYPT QEncCmd = 0x02 + QENC_CMD_MORE_DATA QEncCmd = 0x03 + QENC_CMD_UNLOCK QEncCmd = 0x04 + QENC_CMD_LOCK QEncCmd = 0x05 + QENC_CMD_SET_CFG QEncCmd = 0x06 + QENC_CMD_GET_MODE QEncCmd = 0x07 + QENC_CMD_GET_BUFFER QEncCmd = 0x08 + QENC_CMD_GET_KEYS QEncCmd = 0x09 + QENC_CMD_GET_BUFFSIZE QEncCmd = 0x0A + QENC_CMD_SET_KEYS QEncCmd = 0x0B +) + +type QEncMode byte + +const ( + QENC_MODE_CLOSED QEncMode = 0x00 + QENC_MODE_OPEN QEncMode = 0x01 + QENC_MODE_LOAD QEncMode = 0x02 + QENC_MODE_INIT QEncMode = 0x03 +) + +type QEncSubMode byte + +const ( + QENC_SUB_MODE_NONE QEncSubMode = 0x00 + QENC_SUB_MODE_SEED QEncSubMode = 0x01 + QENC_SUB_MODE_PASSWORD QEncSubMode = 0x02 + QENC_SUB_MODE_VERIFY_PASSWORD QEncSubMode = 0x03 +) + +type QEncCfg byte + +const ( + QENC_CFG_PARANOIA QEncCfg = 0x00 + QENC_CFG_SECURE QEncCfg = 0x01 + QENC_CFG_MAX_ERROR QEncCfg = 0x02 + QENC_CFG_TIMEOUT QEncCfg = 0x03 +) + +func QCmdReset(dev *QEncDevice) (err error) { + buf, err := initBuffer(dev, QENC_CMD_RESET) + if err != nil { + return err + } + + _, err = dev.SendBuffer(buf) + return err +} + +func QCmdEncrypt(dev *QEncDevice, data []byte) (err error) { + return sendData(dev, data, QENC_CMD_ENCRYPT) +} + +func QCmdDecrypt(dev *QEncDevice, data []byte) (err error) { + return sendData(dev, data, QENC_CMD_DECRYPT) +} + +func QCmdGetMode(dev *QEncDevice) (retm QEncMode, retsm QEncSubMode, err error) { + buf, err := initBuffer(dev, QENC_CMD_GET_MODE) + if err != nil { + return retm, retsm, err + } + + rbuf, err := dev.SendBuffer(buf) + if err != nil { + return retm, retsm, err + } + + dataLen := binary.LittleEndian.Uint16(rbuf) + if int(dataLen) != 2 { + return retm, retsm, fmt.Errorf("Invalid data") + } + + retm = QEncMode(rbuf[3]) + retsm = QEncSubMode(rbuf[4]) + return retm, retsm, nil +} + +func QCmdSetCfgParanoia(dev *QEncDevice) (err error) { + return setCfg(dev, QENC_CFG_PARANOIA, 1) +} + +func QCmdSetCfgSecure(dev *QEncDevice, val bool) (err error) { + if val { + return setCfg(dev, QENC_CFG_SECURE, 1) + } + return setCfg(dev, QENC_CFG_SECURE, 0) +} + +func QCmdSetCfgMaxError(dev *QEncDevice, val uint8) (err error) { + if val < 0 || val > 15 { + return fmt.Errorf("Invalid value") + } + return setCfg(dev, QENC_CFG_MAX_ERROR, val) +} + +func QCmdSetCfgTimeout(dev *QEncDevice, val uint8) (err error) { + if val < 0 || val > 50 { + return fmt.Errorf("Invalid value") + } + return setCfg(dev, QENC_CFG_TIMEOUT, val) +} + +func QCmdGetBuffer(dev *QEncDevice) (ret []byte, err error) { + return receiveData(dev, QENC_CMD_GET_BUFFER) +} + +func QCmdGetKeys(dev *QEncDevice) (ret []byte, err error) { + return receiveData(dev, QENC_CMD_GET_KEYS) +} + +func QCmdSetKeys(dev *QEncDevice, data []byte) (err error) { + return sendData(dev, data, QENC_CMD_SET_KEYS) +} + +func setCfg(dev *QEncDevice, cfg QEncCfg, val uint8) (err error) { + buf, err := initBuffer(dev, QENC_CMD_SET_CFG) + if err != nil { + return err + } + + binary.LittleEndian.PutUint16(buf[3:], uint16(2)) + + buf[5] = byte(cfg) + buf[6] = val + + if _, err = dev.SendBuffer(buf); err != nil { + return err + } + return nil +} + +func sendData(dev *QEncDevice, data []byte, cmd QEncCmd) (err error) { + buf, err := initBuffer(dev, cmd) + if err != nil { + return err + } + + dataLen := len(data) + binary.LittleEndian.PutUint16(buf[3:], uint16(dataLen)) + + if dataLen <= (int(dev.GetBufSize()) - 5) { + copy(buf[5:], data) + if _, err = dev.SendBuffer(buf); err != nil { + return err + } + return nil + } + + dataPos := 0 + copy(buf[5:], data[:dev.GetBufSize()-5]) + if _, err := dev.SendBuffer(buf); err != nil { + return err + } + dataPos += int(dev.GetBufSize()) - 5 + + buf[2] = byte(QENC_CMD_MORE_DATA) + for { + copy(buf[3:], data[dataPos:]) + if _, err := dev.SendBuffer(buf); err != nil { + return nil + } + if (dataLen - dataPos) <= (int(dev.GetBufSize()) - 3) { + return nil + } + dataPos += int(dev.GetBufSize()) - 3 + } +} + +func receiveData(dev *QEncDevice, cmd QEncCmd) (ret []byte, err error) { + buf, err := initBuffer(dev, cmd) + if err != nil { + return ret, err + } + + for { + rbuf, err := dev.SendBuffer(buf) + if err != nil { + return ret, err + } + + if rbuf[0] == 1 { + dataLen := binary.LittleEndian.Uint16(rbuf[1:3]) + if int(dataLen) > int(dev.GetBufSize())-3 { + return ret, fmt.Errorf("Invalid data") + } + + b := make([]byte, dataLen) + copy(b, rbuf[3:]) + ret = append(ret, b...) + return ret, nil + } + + ret = append(ret, rbuf[1:]...) + } +} + +func initBuffer(dev *QEncDevice, cmd QEncCmd) (ret []byte, err error) { + ret = make([]byte, dev.GetBufSize()) + + if copy(ret, magicHeader) != 2 { + return ret, fmt.Errorf("Invalid magic Header") + } + ret[2] = byte(cmd) + + return ret, nil +} diff --git a/qmk-dev.go b/qmk-dev.go new file mode 100644 index 0000000..5f8b6ac --- /dev/null +++ b/qmk-dev.go @@ -0,0 +1,162 @@ +package qmkenc + +import ( + "encoding/binary" + "fmt" + "time" + + "github.com/sstallion/go-hid" +) + +const encDevUsage uint16 = 0x61 +const encDevUsagePage uint16 = 0xFF60 + +type QEncDevice struct { + path string + serial string + product string + manufacturer string + dev *hid.Device + bufSize uint8 + timeout int + retryTimeout int + retryWait int +} + +func (d *QEncDevice) GetPath() string { return d.path } +func (d *QEncDevice) GetSerial() string { return d.serial } +func (d *QEncDevice) GetProduct() string { return d.product } +func (d *QEncDevice) GetManufacturer() string { return d.manufacturer } +func (d *QEncDevice) GetBufSize() uint8 { return d.bufSize } + +func (d *QEncDevice) Open() (err error) { + if d.dev, err = hid.OpenPath(d.path); err != nil { + return fmt.Errorf("Error opening device: '%w'", err) + } + + if err = d.setBufSize(); err != nil { + d.Close() + return err + } + return nil +} + +func (d *QEncDevice) Close() (err error) { + return d.dev.Close() +} + +func (d *QEncDevice) SendBuffer(data []byte) (ret []byte, err error) { + startTime := time.Now() + for { + ret = make([]byte, d.bufSize) + + if len(data) != int(d.bufSize) { + return ret, fmt.Errorf("Invalid data size") + } + + if _, err = d.dev.Write(data); err != nil { + return ret, fmt.Errorf("Failed to write: '%w'", err) + } + + if _, err = d.dev.ReadWithTimeout(ret, time.Duration(time.Second*time.Duration(d.timeout))); err != nil { + return ret, fmt.Errorf("Failed to read: '%w'", err) + } + + ret, err = QEncCheckResponse(ret) + if err != nil { + qerr := ToQEncError(err) + if qerr.errCode != QENC_ERR_RETRY { + return ret, err + } + time.Sleep(time.Second * time.Duration(d.retryWait)) + } else { + return ret, err + } + if time.Now().Sub(startTime).Seconds() >= float64(d.retryTimeout) { + qerr := new(QEncError) + qerr.errCode = QENC_ERR_TIMEOUT + return ret, qerr + } + } +} + +func (d *QEncDevice) setBufSize() (err error) { + buf_sizes := []int{8, 16, 32, 64, 128} + + for _, bs := range buf_sizes { + buf_max := bs + buf := make([]byte, buf_max) + sbuf := make([]byte, buf_max) + + if copy(sbuf, magicHeader) != 2 { + return fmt.Errorf("Invalid magic Header") + } + sbuf[2] = byte(QENC_CMD_GET_BUFFSIZE) + + fmt.Printf("Trying: %d\n", buf_max) + if _, err = d.dev.Write(sbuf); err != nil { + return fmt.Errorf("Failed to write to device: '%w'", err) + } + + _, err = d.dev.ReadWithTimeout(buf, time.Duration(time.Second*1)) + if err != nil { + fmt.Println("next round") + continue + } + if buf, err = QEncCheckResponse(buf); err != nil { + return err + } + + data_len := binary.LittleEndian.Uint16(buf) + if int(data_len) != 1 { + return fmt.Errorf("Invalid data") + } + + d.bufSize = buf[3] + return nil + } + return fmt.Errorf("Unsupported device: '%w'", err) +} + +func QEncScanDevices(ids ...uint16) (ret []*QEncDevice, err error) { + vid := uint16(hid.VendorIDAny) + pid := uint16(hid.ProductIDAny) + + if len(ids) > 2 { + return ret, fmt.Errorf("To many ids given") + } + + if len(ids) == 1 { + vid = ids[0] + } + if len(ids) == 2 { + vid = ids[0] + pid = ids[1] + } + + hid.Enumerate(vid, pid, + func(info *hid.DeviceInfo) error { + if info.Usage == encDevUsage && info.UsagePage == encDevUsagePage { + d := QEncDevice{ + path: info.Path, + serial: info.SerialNbr, + product: info.ProductStr, + manufacturer: info.MfrStr, + timeout: 5, + retryTimeout: 10, + retryWait: 1, + } + ret = append(ret, &d) + } + return nil + }) + + if len(ret) == 0 { + return ret, fmt.Errorf("No device found") + } + return ret, nil +} + +func QEncExit() (err error) { + return hid.Exit() +} diff --git a/qmk-err.go b/qmk-err.go new file mode 100644 index 0000000..617d6c5 --- /dev/null +++ b/qmk-err.go @@ -0,0 +1,58 @@ +package qmkenc + +type QEncErr byte + +const ( + QENC_ERR_OK QEncErr = 0x00 + QENC_ERR_EMPTY_BUF QEncErr = 0x01 + QENC_ERR_NO_CTX QEncErr = 0x02 + QENC_ERR_NOT_ALLOWED QEncErr = 0x03 + QENC_ERR_INVALID QEncErr = 0x04 + QENC_ERR_RETRY QEncErr = 0x05 + // device retry timeout + QENC_ERR_TIMEOUT QEncErr = 0xFF +) + +type QEncError struct { + errCode QEncErr +} + +func (e *QEncError) Error() string { + switch e.errCode { + case QENC_ERR_EMPTY_BUF: + return "Buffer is empty" + case QENC_ERR_NO_CTX: + return "No context" + case QENC_ERR_NOT_ALLOWED: + return "Action not allowed" + case QENC_ERR_INVALID: + return "Invalid data" + case QENC_ERR_RETRY: + return "Retry request" + case QENC_ERR_TIMEOUT: + return "Timeout reached" + default: + return "Unknown error" + } +} + +func QEncCheckResponse(data []byte) (ret []byte, err error) { + if data[0] == byte(QENC_ERR_OK) { + return data[1:], nil + } + + rerr := new(QEncError) + rerr.errCode = QEncErr(data[0]) + + return ret, rerr +} + +func ToQEncError(err error) *QEncError { + if _, ok := err.(*QEncError); ok { + return err.(*QEncError) + } else { + qerr := new(QEncError) + qerr.errCode = QENC_ERR_INVALID + return qerr + } +}