diff options
| -rw-r--r-- | drivers/i3c/master/dw-i3c-master.c | 25 |
1 files changed, 23 insertions, 2 deletions
diff --git a/drivers/i3c/master/dw-i3c-master.c b/drivers/i3c/master/dw-i3c-master.c index d7df81c566672d..7eb09ad1017125 100644 --- a/drivers/i3c/master/dw-i3c-master.c +++ b/drivers/i3c/master/dw-i3c-master.c @@ -205,6 +205,10 @@ #define EXTENDED_CAPABILITY 0xe8 #define SLAVE_CONFIG 0xec +#define DYN_ADDR_LO_MASK GENMASK(4, 0) +#define DYN_ADDR_HI_MASK GENMASK(6, 5) +#define IBI_SIR_BIT_MOD 32 /* 32-bit vector */ + #define DW_I3C_DEV_NACK_RETRY_CNT_MAX 0x3 #define DEV_ADDR_TABLE_DEV_NACK_RETRY_MASK GENMASK(30, 29) #define DEV_ADDR_TABLE_DYNAMIC_MASK GENMASK(23, 16) @@ -217,6 +221,7 @@ #define DEV_ADDR_TABLE_DYNAMIC_ADDR(x) FIELD_PREP(DEV_ADDR_TABLE_DYNAMIC_MASK, x) #define DEV_ADDR_TABLE_STATIC_ADDR(x) FIELD_PREP(DEV_ADDR_TABLE_STATIC_MASK, x) #define DEV_ADDR_TABLE_LOC(start, idx) ((start) + ((idx) << 2)) +#define DEV_ADDR_TABLE_GET_DYNAMIC_ADDR(x) FIELD_GET(DEV_ADDR_TABLE_DYNAMIC_MASK, x) #define I3C_BUS_SDR1_SCL_RATE 8000000 #define I3C_BUS_SDR2_SCL_RATE 6000000 @@ -264,6 +269,14 @@ struct dw_i3c_drvdata { u32 flags; }; +static inline u32 get_ibi_sir_bit_index(u8 addr) +{ + u32 lo = FIELD_GET(DYN_ADDR_LO_MASK, addr); + u32 hi = FIELD_GET(DYN_ADDR_HI_MASK, addr); + + return (lo + hi) % IBI_SIR_BIT_MOD; +} + static bool dw_i3c_master_supports_ccc_cmd(struct i3c_master_controller *m, const struct i3c_ccc_cmd *cmd) { @@ -1237,11 +1250,19 @@ static void dw_i3c_master_set_sir_enabled(struct dw_i3c_master *master, { u32 dat_entry, reg; bool global; + u8 dynamic_addr; dat_entry = DEV_ADDR_TABLE_LOC(master->datstartaddr, idx); guard(spinlock_irqsave)(&master->devs_lock); reg = readl(master->regs + dat_entry); + dynamic_addr = DEV_ADDR_TABLE_GET_DYNAMIC_ADDR(reg); + + if (!dynamic_addr) + dev_warn(master->dev, + "<%s> unassigned slave device, dynamic addr:%x\n", + __func__, dynamic_addr); + if (enable) { reg &= ~DEV_ADDR_TABLE_SIR_REJECT; if (dev->info.bcr & I3C_BCR_IBI_PAYLOAD) @@ -1254,11 +1275,11 @@ static void dw_i3c_master_set_sir_enabled(struct dw_i3c_master *master, if (enable) { global = (master->sir_rej_mask == IBI_REQ_REJECT_ALL); - master->sir_rej_mask &= ~BIT(idx); + master->sir_rej_mask &= ~BIT(get_ibi_sir_bit_index(dynamic_addr)); } else { bool hj_rejected = !!(readl(master->regs + DEVICE_CTRL) & DEV_CTRL_HOT_JOIN_NACK); - master->sir_rej_mask |= BIT(idx); + master->sir_rej_mask |= BIT(get_ibi_sir_bit_index(dynamic_addr)); global = (master->sir_rej_mask == IBI_REQ_REJECT_ALL) && hj_rejected; } writel(master->sir_rej_mask, master->regs + IBI_SIR_REQ_REJECT); |