diff --git a/chips/v3s.c b/chips/v3s.c index cf08443..0ea83dd 100644 --- a/chips/v3s.c +++ b/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; }