2 files changed, 41 insertions, 0 deletions
diff --git a/src/soc/intel/elkhartlake/include/soc/tsn_gbe.h b/src/soc/intel/elkhartlake/include/soc/tsn_gbe.h
index d006ef252b..e9f6c8d1fe 100644
--- a/src/soc/intel/elkhartlake/include/soc/tsn_gbe.h
+++ b/src/soc/intel/elkhartlake/include/soc/tsn_gbe.h
@@ -3,4 +3,13 @@
 #ifndef _SOC_ELKHARTLAKE_TSN_GBE_H_
 #define _SOC_ELKHARTLAKE_TSN_GBE_H_
 
+#define MAC_ADDR_LEN		6
+
+#define TSN_MAC_ADD0_HIGH	0x300	/* MAC Address0 High register */
+#define TSN_MAC_ADD0_LOW	0x304	/* MAC Address0 Low register */
+
+/* We need one function we can call to get a MAC address to use. */
+/* This function can be coded somewhere else but must exist. */
+enum cb_err mainboard_get_mac_address(struct device *dev, uint8_t mac[MAC_ADDR_LEN]);
+
 #endif /* _SOC_ELKHARTLAKE_TSN_GBE_H_ */
diff --git a/src/soc/intel/elkhartlake/tsn_gbe.c b/src/soc/intel/elkhartlake/tsn_gbe.c
index c96e88b972..4a340b2f6d 100644
--- a/src/soc/intel/elkhartlake/tsn_gbe.c
+++ b/src/soc/intel/elkhartlake/tsn_gbe.c
@@ -5,10 +5,42 @@
 #include <device/pci_ids.h>
 #include <soc/tsn_gbe.h>
 
+static void program_mac_address(struct device *dev, void *base)
+{
+	enum cb_err status;
+	uint8_t mac[MAC_ADDR_LEN];
+
+	/* Check first whether there is a valid MAC address available */
+	status = mainboard_get_mac_address(dev, mac);
+	if (status != CB_SUCCESS) {
+		printk(BIOS_INFO, "TSN GbE: No valid MAC address found\n");
+		return;
+	}
+
+	printk(BIOS_DEBUG, "TSN GbE: Programming MAC Address...\n");
+
+	/* Write the upper 16 bits of the first 6-byte MAC address */
+	clrsetbits32((void *)(base + TSN_MAC_ADD0_HIGH), 0xffff, ((mac[5] << 8) | mac[4]));
+	/* Write the lower 32 bits of the first 6-byte MAC address */
+	clrsetbits32((void *)(base + TSN_MAC_ADD0_LOW), 0xffffffff,
+			(mac[3] << 24) | (mac[2] << 16) | (mac[1] << 8) | mac[0]);
+}
+
+static void gbe_tsn_init(struct device *dev)
+{
+	/* Get the base address of the I/O registers in memory space */
+	struct resource *gbe_tsn_res = find_resource(dev, PCI_BASE_ADDRESS_0);
+	uintptr_t io_mem_base = gbe_tsn_res->base;
+
+	/* Program MAC address */
+	program_mac_address(dev, (void *)io_mem_base);
+}
+
 static struct device_operations gbe_tsn_ops  = {
 	.read_resources   = pci_dev_read_resources,
 	.set_resources    = pci_dev_set_resources,
 	.enable_resources = pci_dev_enable_resources,
+	.init             = gbe_tsn_init,
 };
 
 static const unsigned short gbe_tsn_device_ids[] = { 0x4b32, 0x4ba0, 0x4bb0, 0 };