fel: sid: add sid-writel command

fel.c

@@ -469,6 +469,19 @@ void aw_fel_dump_sid(feldev_handle *dev)
 	}
 }
 
+void aw_fel_writel_sid(feldev_handle *dev, uint32_t offset, uint32_t value)
+{
+	soc_info_t *soc_info = dev->soc_info;
+
+	if (!soc_info->sid_base || !soc_info->sid_sections) {
+		printf("SID memory maps for your SoC (%s) are unknown.\n",
+			dev->soc_name);
+		return;
+	}
+
+	fel_write_sid(dev, &value, offset, 4);
+}
+
 void aw_enable_l2_cache(feldev_handle *dev, soc_info_t *soc_info)
 {
 	uint32_t arm_code[] = {
@@ -1285,6 +1298,7 @@ void usage(const char *cmd) {
 		"	sid-registers			Retrieve and output 128-bit SID key,\n"
 		"					using the MMIO register read method\n"
 		"	sid-dump			Dump the content of all the SID eFuses\n"
+		"	sid-writel offset value		Write 32-bit value to SID eFuses\n"
 		"	clear address length		Clear memory\n"
 		"	fill address length value	Fill memory\n"
 		, cmd);
@@ -1423,6 +1437,9 @@ int main(int argc, char **argv)
 			aw_fel_print_sid(handle, true); /* enforce register access */
 		} else if (strcmp(argv[1], "sid-dump") == 0) {
 			aw_fel_dump_sid(handle);
+		} else if (strcmp(argv[1], "sid-writel") == 0 && argc > 3) {
+			aw_fel_writel_sid(handle, strtoul(argv[2], NULL, 0), strtoul(argv[3], NULL, 0));
+			skip = 3;
 		} else if (strcmp(argv[1], "write") == 0 && argc > 3) {
 			skip += 2 * file_upload(handle, 1, argc - 2, argv + 2,
 					pflag_active ? progress_bar : NULL);

fel_lib.c

@@ -627,6 +627,56 @@ int fel_read_sid(feldev_handle *dev, uint32_t *result,
 	return 0;
 }
 
+int fel_write_sid(feldev_handle *dev, uint32_t *data,
+		 unsigned int offset, unsigned int length)
+{
+	const soc_info_t *soc = dev->soc_info;
+
+	if (!soc->sid_base)			/* SID unavailable */
+		return -2;
+	if ((offset & 3) || (length & 3))	/* needs to be 32-bit aligned */
+		return -3;
+
+	uint32_t arm_code[] = {
+		/* <sid_write>: */
+		htole32(0xe59f0044), /*    0:  ldr   r0, [pc, #68] @ 4c <sid_base> */
+		htole32(0xe59f1044), /*    4:  ldr   r1, [pc, #68] @ 50 <offset> */
+		htole32(0xe28f3048), /*    8:  add   r3, pc, #72 @ 0x48      */
+		/* <sid_write_loop>: */
+		htole32(0xe4932004), /*    c:  ldr   r2, [r3], #4            */
+		htole32(0xe5802050), /*   10:  str   r2, [r0, #80] @ 0x50    */
+		htole32(0xe1a02801), /*   14:  lsl   r2, r1, #16             */
+		htole32(0xe3822b2b), /*   18:  orr   r2, r2, #44032 @ 0xac00 */
+		htole32(0xe3822001), /*   1c:  orr   r2, r2, #1              */
+		htole32(0xe5802040), /*   20:  str   r2, [r0, #64] @ 0x40    */
+		/* <sid_write_wait>: */
+		htole32(0xe5902040), /*   24:  ldr   r2, [r0, #64] @ 0x40    */
+		htole32(0xe3120001), /*   28:  tst   r2, #1                  */
+		htole32(0x1afffffc), /*   2c:  bne   24 <sid_write_wait>     */
+		htole32(0xe2811004), /*   30:  add   r1, r1, #4              */
+		htole32(0xe59f2018), /*   34:  ldr   r2, [pc, #24] @ 54 <end> */
+		htole32(0xe1510002), /*   38:  cmp   r1, r2                  */
+		htole32(0x3afffff2), /*   3c:  bcc   c <sid_write_loop>      */
+		htole32(0xe3a02000), /*   40:  mov   r2, #0                  */
+		htole32(0xe5802040), /*   44:  str   r2, [r0, #64] @ 0x40    */
+		htole32(0xe12fff1e), /*   48:  bx    lr                      */
+		/* <sid_base>: */
+		htole32(dev->soc_info->sid_base),
+		/* <offset>: */
+		htole32(offset),
+		/* <end>: */
+		htole32(offset + length),
+		/* SID data to write should be here */
+	};
+
+	/* write code, write data and execute code */
+	aw_fel_write(dev, arm_code, dev->soc_info->scratch_addr, sizeof(arm_code));
+	aw_fel_write(dev, data, dev->soc_info->scratch_addr + sizeof(arm_code), length);
+	aw_fel_execute(dev, dev->soc_info->scratch_addr);
+
+	return 0;
+}
+
 /* general functions, "FEL device" management */
 
 static int feldev_get_endpoint(feldev_handle *dev)

fel_lib.h

@@ -81,6 +81,9 @@ int fel_read_sid(feldev_handle *dev, uint32_t *result,
 		 unsigned int offset, unsigned int length,
 		 bool force_workaround);
 
+int fel_write_sid(feldev_handle *dev, uint32_t *data,
+		 unsigned int offset, unsigned int length);
+
 bool aw_fel_remotefunc_prepare(feldev_handle *dev,
 			       size_t                stack_size,
 			       void                 *arm_code,

thunks/Makefile

@@ -8,6 +8,7 @@ THUNKS += memcpy.h
 THUNKS += readl_writel.h
 THUNKS += rmr-thunk.h
 THUNKS += sid_read_root.h
+THUNKS += sid_write.h
 
 all: $(SPL_THUNK) $(THUNKS)
 # clean up object files afterwards

thunks/sid_write.S

@@ -0,0 +1,71 @@
+ /*
+ * Copyright (C) 2024 Marek Kraus <[email protected]>
+ * Copyright (C) 2016 Bernhard Nortmann <[email protected]>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+/*
+ * ARM thunk code to write the data to SID using register-based access.
+ */
+
+SID_BASE			.req	r0
+sid_offset			.req	r1
+sid_data_offset		.req	r3
+
+.set	SID_PRCTL,	0x40	// SID program/read control register
+.set	SID_PRKEY,	0x50	// SID program key value register
+
+.set	SID_OP_LOCK,    0xAC	 // magic value to prevent accidental programming
+
+.set	SID_PRCTL_READ_BIT,		(1 << 1)	// bit 1 of SID_PRCTL, Software Read Start
+.set	SID_PRCTL_WRITE_BIT,	(1 << 0)	// bit 0 of SID_PRCTL, Software Program Start
+
+.set	SID_PRCTL_OP_LOCK_POS,	8	// bits 8 - 15
+.set	SID_PRCTL_OFFSET_POS,	16	// bits 16 - 23			
+
+sid_write:
+	ldr	SID_BASE, sid_base
+	ldr	sid_offset, offset
+	adr sid_data_offset, sid_data
+sid_write_loop:
+	ldr r2, [sid_data_offset], #4	// load SID data to write into r2, and increment pointer +4 bytes
+	str r2, [SID_BASE, #SID_PRKEY]	// store data from r2 to SID_PRKEY register
+	mov r2, sid_offset, lsl #SID_PRCTL_OFFSET_POS	// shift offset where to write data value to correct pos
+	orr r2, #SID_OP_LOCK << 8	// shift lock magic value to correct pos
+	orr r2, #SID_PRCTL_WRITE_BIT	// set write bit
+	str r2, [SID_BASE, #SID_PRCTL]	// write the PRCTL into the register from r2
+sid_write_wait:
+	ldr r2, [SID_BASE, #SID_PRCTL]	// read SID_PRCTL register to r2
+	tst r2, #SID_PRCTL_WRITE_BIT	// check if write bit is still 1
+	bne sid_write_wait	// if it is still 1, the write is in-progress, so loop until it is 0
+
+	add sid_offset, #4	// incremet sid write offset
+	ldr r2, end		
+	cmp sid_offset, r2
+	blo sid_write_loop
+	mov	r2, #0
+	str	r2, [SID_BASE, #SID_PRCTL]
+	bx	lr
+
+sid_base:	.word 0x0
+offset:		.word 0x0
+end:		.word 0x0
+sid_data:

thunks/sid_write.h

@@ -0,0 +1,28 @@
+		/* <sid_write>: */
+		htole32(0xe59f0044), /*    0:  ldr   r0, [pc, #68] @ 4c <sid_base> */
+		htole32(0xe59f1044), /*    4:  ldr   r1, [pc, #68] @ 50 <offset> */
+		htole32(0xe28f3048), /*    8:  add   r3, pc, #72 @ 0x48      */
+		/* <sid_write_loop>: */
+		htole32(0xe4932004), /*    c:  ldr   r2, [r3], #4            */
+		htole32(0xe5802050), /*   10:  str   r2, [r0, #80] @ 0x50    */
+		htole32(0xe1a02801), /*   14:  lsl   r2, r1, #16             */
+		htole32(0xe3822b2b), /*   18:  orr   r2, r2, #44032 @ 0xac00 */
+		htole32(0xe3822001), /*   1c:  orr   r2, r2, #1              */
+		htole32(0xe5802040), /*   20:  str   r2, [r0, #64] @ 0x40    */
+		/* <sid_write_wait>: */
+		htole32(0xe5902040), /*   24:  ldr   r2, [r0, #64] @ 0x40    */
+		htole32(0xe3120001), /*   28:  tst   r2, #1                  */
+		htole32(0x1afffffc), /*   2c:  bne   24 <sid_write_wait>     */
+		htole32(0xe2811004), /*   30:  add   r1, r1, #4              */
+		htole32(0xe59f2018), /*   34:  ldr   r2, [pc, #24] @ 54 <end> */
+		htole32(0xe1510002), /*   38:  cmp   r1, r2                  */
+		htole32(0x3afffff2), /*   3c:  bcc   c <sid_write_loop>      */
+		htole32(0xe3a02000), /*   40:  mov   r2, #0                  */
+		htole32(0xe5802040), /*   44:  str   r2, [r0, #64] @ 0x40    */
+		htole32(0xe12fff1e), /*   48:  bx    lr                      */
+		/* <sid_base>: */
+		htole32(0x00000000), /*   4c:  .word 0x00000000              */
+		/* <offset>: */
+		htole32(0x00000000), /*   50:  .word 0x00000000              */
+		/* <end>: */
+		htole32(0x00000000), /*   54:  .word 0x00000000              */