From 7b5a3970caafae8bcb6eace03ed2052fd5c6b96a Mon Sep 17 00:00:00 2001 From: deadprogram Date: Sun, 8 Jan 2023 18:52:57 +0100 Subject: [PATCH] lorawan: add initial LoRaWAN stack support Signed-off-by: deadprogram --- lora/lorawan/adaptor.go | 24 ++++ lora/lorawan/bytes.go | 37 ++++++ lora/lorawan/cmac.go | 253 ++++++++++++++++++++++++++++++++++++++++ lora/lorawan/mic.go | 38 ++++++ lora/lorawan/otaa.go | 102 ++++++++++++++++ lora/lorawan/session.go | 90 ++++++++++++++ lora/radio.go | 13 +++ 7 files changed, 557 insertions(+) create mode 100644 lora/lorawan/adaptor.go create mode 100644 lora/lorawan/bytes.go create mode 100644 lora/lorawan/cmac.go create mode 100644 lora/lorawan/mic.go create mode 100644 lora/lorawan/otaa.go create mode 100644 lora/lorawan/session.go create mode 100644 lora/radio.go diff --git a/lora/lorawan/adaptor.go b/lora/lorawan/adaptor.go new file mode 100644 index 0000000..abebc2e --- /dev/null +++ b/lora/lorawan/adaptor.go @@ -0,0 +1,24 @@ +package lorawan + +import "tinygo.org/x/drivers/lora" + +var ActiveRadio lora.Radio + +func UseRadio(r lora.Radio) { + if ActiveRadio != nil { + panic("lorawan.ActiveRadio is already set") + } + ActiveRadio = r +} + +func Join() error { + return nil +} + +func SendUplink() error { + return nil +} + +func ListenDownlink() error { + return nil +} diff --git a/lora/lorawan/bytes.go b/lora/lorawan/bytes.go new file mode 100644 index 0000000..25f4813 --- /dev/null +++ b/lora/lorawan/bytes.go @@ -0,0 +1,37 @@ +package lorawan + +// revertByteArray inverts de order of a given byte slice +func revertByteArray(s []byte) []byte { + for i, j := 0, len(s)-1; i < j; i, j = i+1, j-1 { + s[i], s[j] = s[j], s[i] + } + return s +} + +// byteToHex return string hex representation of byte +func byteToHex(b byte) string { + bb := (b >> 4) & 0x0F + ret := "" + if bb < 10 { + ret += string(rune('0' + bb)) + } else { + ret += string(rune('A' + (bb - 10))) + } + + bb = (b) & 0xF + if bb < 10 { + ret += string(rune('0' + bb)) + } else { + ret += string(rune('A' + (bb - 10))) + } + return ret +} + +// BytesToHexString converts byte slice to hex string representation +func bytesToHexString(data []byte) string { + s := "" + for i := 0; i < len(data); i++ { + s += byteToHex(data[i]) + } + return s +} diff --git a/lora/lorawan/cmac.go b/lora/lorawan/cmac.go new file mode 100644 index 0000000..9299fff --- /dev/null +++ b/lora/lorawan/cmac.go @@ -0,0 +1,253 @@ +package lorawan + +import ( + "bytes" + "crypto/aes" + "crypto/cipher" + "hash" + "unsafe" +) + +type cmacHash struct { + ciph cipher.Block + k1 []byte + k2 []byte + data []byte + x []byte +} + +const ( + Size = aes.BlockSize + blockSize = Size +) + +var ( + subkeyZero []byte + subkeyRb []byte +) + +func init() { + subkeyZero = bytes.Repeat([]byte{0x00}, blockSize) + subkeyRb = append(bytes.Repeat([]byte{0x00}, blockSize-1), 0x87) +} + +// New returns an AES-CMAC hash using the supplied key. The key must be 16, 24, +// or 32 bytes long. +func NewCmac(key []byte) (hash.Hash, error) { + // Create a cipher. + ciph, err := aes.NewCipher(key) + if err != nil { + return nil, err + } + // Set up the hash object. + h := &cmacHash{ciph: ciph} + h.k1, h.k2 = generateSubkeys(ciph) + h.Reset() + return h, nil +} + +func Xor(dst []byte, a []byte, b []byte) error { + if len(dst) != len(a) || len(a) != len(b) { + panic("crypto/Xor: bad length") + } + for i, _ := range a { + dst[i] = a[i] ^ b[i] + } + return nil +} + +func (h *cmacHash) Reset() { + h.data = h.data[:0] + h.x = make([]byte, blockSize) +} + +func (h *cmacHash) BlockSize() int { + return h.ciph.BlockSize() +} + +func (h *cmacHash) Size() int { + return h.ciph.BlockSize() +} + +func (h *cmacHash) Sum(b []byte) []byte { + dataLen := len(h.data) + + // We should have at most one block left. + if dataLen > blockSize { + panic("cmacHash err1") + } + + // Calculate M_last. + mLast := make([]byte, blockSize) + if dataLen == blockSize { + Xor(mLast, h.data, h.k1) + } else { + // TODO(jacobsa): Accept a destination buffer in common.PadBlock and + // simplify this code. + Xor(mLast, PadBlock(h.data), h.k2) + } + + y := make([]byte, blockSize) + Xor(y, mLast, h.x) + + result := make([]byte, blockSize) + h.ciph.Encrypt(result, y) + + b = append(b, result...) + return b +} + +func (h *cmacHash) Write(p []byte) (n int, err error) { + n = len(p) + + // First step: consume enough data to expand h.data to a full block, if + // possible. + { + toConsume := blockSize - len(h.data) + if toConsume > len(p) { + toConsume = len(p) + } + + h.data = append(h.data, p[:toConsume]...) + p = p[toConsume:] + } + + // If there's no data left in p, it means h.data might not be a full block. + // Even if it is, we're not sure it's the final block, which we must treat + // specially. So we must stop here. + if len(p) == 0 { + return + } + + // h.data is a full block and is not the last; process it. + h.writeBlocks(h.data) + h.data = h.data[:0] + + // Consume any further full blocks in p that we're sure aren't the last. Note + // that we're sure that len(p) is greater than zero here. + blocksToProcess := (len(p) - 1) / blockSize + bytesToProcess := blocksToProcess * blockSize + + h.writeBlocks(p[:bytesToProcess]) + p = p[bytesToProcess:] + + // Store the rest for later. + h.data = append(h.data, p...) + + return +} + +func (h *cmacHash) writeBlocks(p []byte) { + y := make([]byte, blockSize) + + for off := 0; off < len(p); off += blockSize { + block := p[off : off+blockSize] + + xorBlock( + unsafe.Pointer(&y[0]), + unsafe.Pointer(&h.x[0]), + unsafe.Pointer(&block[0])) + + h.ciph.Encrypt(h.x, y) + } + + return +} + +func xorBlock( + dstPtr unsafe.Pointer, + aPtr unsafe.Pointer, + bPtr unsafe.Pointer) { + // Check assumptions. (These are compile-time constants, so this should + // compile out.) + const wordSize = unsafe.Sizeof(uintptr(0)) + if blockSize != 4*wordSize { + panic("xorBlock err1") + } + + // Convert. + a := (*[4]uintptr)(aPtr) + b := (*[4]uintptr)(bPtr) + dst := (*[4]uintptr)(dstPtr) + + // Compute. + dst[0] = a[0] ^ b[0] + dst[1] = a[1] ^ b[1] + dst[2] = a[2] ^ b[2] + dst[3] = a[3] ^ b[3] +} + +func PadBlock(block []byte) []byte { + blockLen := len(block) + if blockLen >= aes.BlockSize { + panic("PadBlock input must be less than 16 bytes.") + } + + result := make([]byte, aes.BlockSize) + copy(result, block) + result[blockLen] = 0x80 + + return result +} + +// Given the supplied cipher, whose block size must be 16 bytes, return two +// subkeys that can be used in MAC generation. See section 5.3 of NIST SP +// 800-38B. Note that the other NIST-approved block size of 8 bytes is not +// supported by this function. +func generateSubkeys(ciph cipher.Block) (k1 []byte, k2 []byte) { + if ciph.BlockSize() != blockSize { + panic("generateSubkeys requires a cipher with a block size of 16 bytes.") + } + + // Step 1 + l := make([]byte, blockSize) + ciph.Encrypt(l, subkeyZero) + + // Step 2: Derive the first subkey. + if Msb(l) == 0 { + // TODO(jacobsa): Accept a destination buffer in ShiftLeft and then hoist + // the allocation in the else branch below. + k1 = ShiftLeft(l) + } else { + k1 = make([]byte, blockSize) + Xor(k1, ShiftLeft(l), subkeyRb) + } + + // Step 3: Derive the second subkey. + if Msb(k1) == 0 { + k2 = ShiftLeft(k1) + } else { + k2 = make([]byte, blockSize) + Xor(k2, ShiftLeft(k1), subkeyRb) + } + + return +} + +func ShiftLeft(b []byte) []byte { + l := len(b) + if l == 0 { + panic("shiftLeft requires a non-empty buffer.") + } + + output := make([]byte, l) + + overflow := byte(0) + for i := int(l - 1); i >= 0; i-- { + output[i] = b[i] << 1 + output[i] |= overflow + overflow = (b[i] & 0x80) >> 7 + } + + return output +} + +// Msb returns the most significant bit of the supplied data (which must be +// non-empty). This is the MSB(L) function of RFC 4493. +func Msb(buf []byte) uint8 { + if len(buf) == 0 { + panic("msb requires non-empty buffer.") + } + + return buf[0] >> 7 +} diff --git a/lora/lorawan/mic.go b/lora/lorawan/mic.go new file mode 100644 index 0000000..c2bcb28 --- /dev/null +++ b/lora/lorawan/mic.go @@ -0,0 +1,38 @@ +package lorawan + +import ( + "encoding/binary" +) + +// genPayloadMIC computes MIC given the payload and the key +func genPayloadMIC(payload []uint8, key [16]uint8) [4]uint8 { + var mic [4]uint8 + hash, _ := NewCmac(key[:]) + hash.Write(payload) + hb := hash.Sum([]byte{}) + copy(mic[:], hb[0:4]) + return mic +} + +func calcMessageMIC(payload []uint8, key [16]uint8, dir uint8, addr []byte, fCnt uint32, lenMessage uint8) [4]uint8 { + var b0 []byte + b0 = append(b0, 0x49, 0x00, 0x00, 0x00, 0x00) + b0 = append(b0, dir) + b0 = append(b0, addr[:]...) + var b [4]byte + binary.LittleEndian.PutUint32(b[:], fCnt) + b0 = append(b0, b[:]...) + b0 = append(b0, 0x00) + b0 = append(b0, lenMessage) + + var full []byte + full = append(full, b0...) + full = append(full, payload...) + + var mic [4]uint8 + hash, _ := NewCmac(key[:]) + hash.Write(full) + hb := hash.Sum([]byte{}) + copy(mic[:], hb[0:4]) + return mic +} diff --git a/lora/lorawan/otaa.go b/lora/lorawan/otaa.go new file mode 100644 index 0000000..395714a --- /dev/null +++ b/lora/lorawan/otaa.go @@ -0,0 +1,102 @@ +package lorawan + +import ( + "bytes" + "crypto/aes" + "errors" +) + +// Otaa is used to store Over The Air Activation data of a LoRaWAN session +type Otaa struct { + DevEUI [8]uint8 + AppEUI [8]uint8 + AppKey [16]uint8 + DevNonce [2]uint8 + AppNonce [3]uint8 + NetID [3]uint8 +} + +// Set configures the Otaa AppEUI, DevEUI, AppKey for the device +func (o *Otaa) Set(appEUI [8]uint8, devEUI [8]uint8, appKey [16]uint8) { + o.AppEUI = appEUI + o.DevEUI = devEUI + o.AppKey = appKey +} + +// GenerateJoinRequest Generates a LoraWAN Join request +func (o *Otaa) GenerateJoinRequest(buf []uint8) error { + // TODO: Add checks + buf = append(buf, 0x00) + buf = append(buf, revertByteArray(o.AppEUI[:])...) + buf = append(buf, revertByteArray(o.DevEUI[:])...) + buf = append(buf, revertByteArray(o.DevNonce[:])...) + mic := genPayloadMIC(buf, o.AppKey) + buf = append(buf, mic[:]...) + return nil +} + +// DecodeJoinAccept Decodes a Lora Join Accept packet +func (o *Otaa) DecodeJoinAccept(phyPload []uint8, s *Session) error { + if len(phyPload) < 12 { + return errors.New("Bad packet") + } + data := phyPload[1:] // Remove trailing 0x20 + + // Prepare AES Cipher + block, err := aes.NewCipher(o.AppKey[:]) + if err != nil { + return errors.New("Lora Cipher error 1") + } + buf := make([]byte, len(data)) + for k := 0; k < len(data)/aes.BlockSize; k++ { + block.Encrypt(buf[k*aes.BlockSize:], data[k*aes.BlockSize:]) + } + + copy(o.AppNonce[:], buf[0:3]) + copy(o.NetID[:], buf[3:6]) + copy(s.DevAddr[:], buf[6:10]) + s.DLSettings = buf[10] + s.RXDelay = buf[11] + + if len(buf) > 16 { + copy(s.CFList[:], buf[12:28]) + } + rxMic := buf[len(buf)-4:] + + dataMic := []byte{} + dataMic = append(dataMic, phyPload[0]) + dataMic = append(dataMic, o.AppNonce[:]...) + dataMic = append(dataMic, o.NetID[:]...) + dataMic = append(dataMic, s.DevAddr[:]...) + dataMic = append(dataMic, s.DLSettings) + dataMic = append(dataMic, s.RXDelay) + dataMic = append(dataMic, s.CFList[:]...) + computedMic := genPayloadMIC(dataMic[:], o.AppKey) + if !bytes.Equal(computedMic[:], rxMic[:]) { + return errors.New("invalid Mic") + } + // Generate NwkSKey + // NwkSKey = aes128_encrypt(AppKey, 0x01|AppNonce|NetID|DevNonce|pad16) + sKey := []byte{} + sKey = append(sKey, 0x01) + sKey = append(sKey, o.AppNonce[:]...) + sKey = append(sKey, o.NetID[:]...) + sKey = append(sKey, o.DevNonce[:]...) + for i := 0; i < 7; i++ { + sKey = append(sKey, 0x00) // PAD to 16 + } + block.Encrypt(buf, sKey) + copy(s.NwkSKey[:], buf[0:16]) + + // Generate AppSKey + // AppSKey = aes128_encrypt(AppKey, 0x02|AppNonce|NetID|DevNonce|pad16) + sKey[0] = 0x02 + block.Encrypt(buf, sKey) + copy(s.AppSKey[:], buf[0:16]) + + // Reset counters + s.FCntDown = 0 + s.FCntUp = 0 + + return nil +} diff --git a/lora/lorawan/session.go b/lora/lorawan/session.go new file mode 100644 index 0000000..91ee64e --- /dev/null +++ b/lora/lorawan/session.go @@ -0,0 +1,90 @@ +package lorawan + +import ( + "crypto/aes" + "encoding/binary" + "errors" + "math" +) + +// Session is used to store session data of a LoRaWAN session +type Session struct { + NwkSKey [16]uint8 + AppSKey [16]uint8 + DevAddr [4]uint8 + FCntDown uint32 + FCntUp uint32 + CFList [16]uint8 + RXDelay uint8 + DLSettings uint8 +} + +// GenMessage Forge an uplink message +func (s *Session) GenMessage(dir uint8, payload []uint8) ([]uint8, error) { + var buf []uint8 + buf = append(buf, 0b01000000) // FHDR Unconfirmed up + buf = append(buf, s.DevAddr[:]...) + + // FCtl : No ADR, No RFU, No ACK, No FPending, No FOpt + buf = append(buf, 0x00) + + // FCnt Up + buf = append(buf, uint8(s.FCntUp&0xFF), uint8((s.FCntUp>>8)&0xFF)) + + // FPort=1 + buf = append(buf, 0x01) + + fCnt := uint32(0) + if dir == 0 { + fCnt = s.FCntUp + s.FCntUp++ + } else { + fCnt = s.FCntDown + } + data, err := s.genFRMPayload(dir, fCnt, payload, false) + if err != nil { + return nil, err + } + buf = append(buf, data[:]...) + + mic := calcMessageMIC(buf, s.NwkSKey, dir, s.DevAddr[:], fCnt, uint8(len(buf))) + buf = append(buf, mic[:]...) + + return buf, nil +} + +func (s *Session) genFRMPayload(dir uint8, fCnt uint32, payload []byte, isFOpts bool) ([]byte, error) { + k := len(payload) / aes.BlockSize + if len(payload)%aes.BlockSize != 0 { + k++ + } + if k > math.MaxUint8 { + return nil, errors.New("Payload too big !") + } + encrypted := make([]byte, 0, k*16) + cipher, err := aes.NewCipher(s.AppSKey[:]) + if err != nil { + panic(err) + } + + var a [aes.BlockSize]byte + a[0] = 0x01 + a[5] = dir + copy(a[6:10], s.DevAddr[:]) + binary.LittleEndian.PutUint32(a[10:14], fCnt) + var ss [aes.BlockSize]byte + var b [aes.BlockSize]byte + for i := uint8(0); i < uint8(k); i++ { + copy(b[:], payload[i*aes.BlockSize:]) + if !isFOpts { + a[15] = i + 1 + } + cipher.Encrypt(ss[:], a[:]) + for j := 0; j < aes.BlockSize; j++ { + b[j] = b[j] ^ ss[j] + } + encrypted = append(encrypted, b[:]...) + } + return encrypted[:len(payload)], nil +} + diff --git a/lora/radio.go b/lora/radio.go new file mode 100644 index 0000000..edf64e6 --- /dev/null +++ b/lora/radio.go @@ -0,0 +1,13 @@ +package lora + +type Radio interface { + Reset() + Tx(pkt []uint8, timeoutMs uint32) error + Rx(timeoutMs uint32) ([]uint8, error) + SetFrequency(freq uint32) + SetIqMode(mode uint8) + SetCodingRate(cr uint8) + SetBandwidth(bw uint8) + SetCrc(enable bool) + SetSpreadingFactor(sf uint8) +}