[v3s]fix v3s sid interface, add fel_read32_array payload

chips/v3s.c

@@ -12,14 +12,39 @@ static int chip_reset(struct xfel_ctx_t * ctx)
 	return 1;
 }
 
+static void fel_read32_array(struct xfel_ctx_t * ctx, uint32_t addr, uint32_t * val, size_t count)
+{
+	uint32_t payload[] = {
+		cpu_to_le32(0xe59f0020),		/* ldr r0, [pc, #32] ; ldr r0,[read_addr]  */
+		cpu_to_le32(0xe28f1024),		/* add r1, pc, #36   ; adr r1, read_data   */
+		cpu_to_le32(0xe59f201c),		/* ldr r2, [pc, #28] ; ldr r2,[read_count] */
+		cpu_to_le32(0xe3520000 + 208),	/* cmp r2, #208 */
+		cpu_to_le32(0xc3a02000 + 208),	/* movgt r2, #208 */
+		/* read_loop: */
+		cpu_to_le32(0xe2522001),		/* subs r2, r2, #1   ; r2 -= 1             */
+		cpu_to_le32(0x412fff1e),		/* bxmi lr           ; return if (r2 < 0)  */
+		cpu_to_le32(0xe4903004),		/* ldr r3, [r0], #4  ; load and post-inc   */
+		cpu_to_le32(0xe4813004),		/* str r3, [r1], #4  ; store and post-inc  */
+		cpu_to_le32(0xeafffffa),		/* b read_loop                             */
+		cpu_to_le32(addr),				/* read_addr */
+		cpu_to_le32(count)				/* read_count */
+		/* read_data values go here */
+	};
+	uint32_t buf[count];
+	size_t i;
+
+	fel_write(ctx, ctx->version.scratchpad, (void *)payload, sizeof(payload));
+	fel_exec(ctx, ctx->version.scratchpad);
+	fel_read(ctx, ctx->version.scratchpad + sizeof(payload), (void *)buf, sizeof(buf));
+	for(i = 0; i < count; i++)
+		val[i] = le32_to_cpu(buf[i]);
+}
+
 static int chip_sid(struct xfel_ctx_t * ctx, char * sid)
 {
 	uint32_t id[4];
 
-	id[0] = R32(0x01c23800 + 0x0);
-	id[1] = R32(0x01c23800 + 0x4);
-	id[2] = R32(0x01c23800 + 0x8);
-	id[3] = R32(0x01c23800 + 0xc);
+	fel_read32_array(ctx, 0x01c23800, id, 4);
 	sprintf(sid, "%08x%08x%08x%08x", id[0], id[1], id[2], id[3]);
 	return 1;
 }