package main

import (
	"log"
)

const (
	GOOSE_TYPE_ID uint32 = 35000 //[2]byte{0x88, 0xb8}
	SV_TYPE_ID    uint32 = 35002 //[2]byte{0x88, 0xba}
)

// 以太网报文头
type EtherHeader struct {
	DstHwaddr []byte
	LocHwaddr []byte
	VlanTag   []byte
	TypeId    uint32
	AppId     uint32
}

/* VLAN_ID and VLAN_PRI set in CID(hex mode) ex: "123" = 0x123
CID文件定义了VLAN的参数, 16进制格式; 构建以太网报文头的802.1Q VLAN标记
<Address>
    <P type="MAC-Address">01-0C-CD-01-00-01</P>
    <P type="APPID">4000</P>
    <P type="VLAN-ID">123</P>
    <P type="VLAN-PRIORITY">4</P>
</Address>
VLAN_ID = 0x123  VLAN-PRI = 0x4*/
func BuildVlanTag(VLAN_ID uint16, VLAN_PRI uint16) []byte {
	VLAN_TAG := [4]byte{0x81, 0x00}
	vlan_tag := (VLAN_PRI << 5) | VLAN_ID // PRI: 3bit=4; CFI: 1bit=0; VLANID: 12bit=0x0123
	VLAN_TAG[2] = byte(vlan_tag >> 8)
	VLAN_TAG[3] = byte(vlan_tag & 0xff)
	return VLAN_TAG[0:]
}

// 将以太网包分解为 header(目标mac地址，源mac地址，类型ID（GOOSE为'\x88\xb8'，SV为'\x88\xba'）)，APDU数据包； 如果不是GOOSE或SV，则APDU为b''
func UnpackEtherPacket(pkt []byte) (header EtherHeader, apdu []byte) {
	header = EtherHeader{}
	header.DstHwaddr = pkt[0:6]
	header.LocHwaddr = pkt[6:12]
	var DATA_PKT []byte
	if pkt[12] == 0x81 && pkt[13] == 0x00 { //# QTAG Prefix (IEEE 802.1Q)
		header.VlanTag = pkt[12:16]
		header.TypeId = DecodeUint(pkt[16:18]) // 报文类型：GOOSE SV 等
		DATA_PKT = pkt[18:]
	} else {
		//header.VlanTag
		header.TypeId = DecodeUint(pkt[12:14])
		DATA_PKT = pkt[14:]
	}

	if header.TypeId >= 0x600 { // 大于0x600表示类型，小于为长度
		if header.TypeId == GOOSE_TYPE_ID || header.TypeId == SV_TYPE_ID { // GOOSE or SV
			header.AppId = DecodeUint(DATA_PKT[0:2])
			//DATA_LEN = struct.unpack('>H', DATA_PKT[2:4])[0]
			//APDU_LEN = DATA_LEN - 8
			apdu = DATA_PKT[8:]
		} else { // not goose or sv packet
			log.Println("UnpackEtherPacket: not goose or sv packet.")
		}
	} else { // unknown packet
		log.Println("UnpackEtherPacket: unknown packet.")
	}
	return header, apdu
}

// APP_ID and VLAN_ID and VLAN_PRI set in CID(hex mode) "123" = 0x123
/*
   <Address>
       <P type="MAC-Address">01-0C-CD-01-00-01</P>
       <P type="APPID">3000</P>
       <P type="VLAN-ID">123</P>
       <P type="VLAN-PRIORITY">4</P>
   </Address>
   举例：
   header = {
       'DstHwaddr': b'\x01\x0c\xcd\x01\x00\x01',  # from CID ，前4字节固定，后2字节用户定义: goose: b'\x01\x0c\xcd\x01' + (b'\x00\x00' ~ b'\xff\xff');  sv: b'\x01\x0c\xcd\x04' + (b'\x00\x00' ~ b'\xff\xff')
       'LocHwaddr':sock.local_hwaddr,             # from local iface hardware
       'TypeId':GOOSE_TYPE_ID,               # goose:b'\x88\xb8';  sv: b'\x88\xba'
       'AppId':b'\x30\x00',                        # from CID
       'VlanTag':build_vlan_tag(0x123, 0x4)       # 可为空, 例：b'\x81\x23', 共16bit：  PRI: 3bit=0x4; CFI: 1bit=0; VLANID: 12bit=0x0123
   }
*/
func PackEtherPacket(header EtherHeader, apdu []byte) []byte {
	if header.TypeId != GOOSE_TYPE_ID && header.TypeId != SV_TYPE_ID {
		log.Println("PackEtherPacket: not goose or sv packet.")
		return nil
	}

	packet := make([]byte, 26+len(apdu))
	idx := 0
	copy(packet[idx:], header.DstHwaddr)
	idx += 6
	copy(packet[idx:], header.LocHwaddr)
	idx += 6
	if len(header.VlanTag) != 0 {
		copy(packet[idx:], header.VlanTag)
		idx += 4
	}
	EncodeUint(header.TypeId, packet[idx:idx+2])
	idx += 2
	EncodeUint(header.AppId, packet[idx:idx+2])
	idx += 2
	EncodeUint(uint32(len(apdu)+8), packet[idx:idx+2])
	idx += 2
	EncodeUint(0, packet[idx:idx+4])
	idx += 4
	copy(packet[idx:], apdu)
	idx += len(apdu)

	return packet[:idx]
}