+int stm32prog_otp_write(struct stm32prog_data *data, u32 offset, u8 *buffer,
+ long *size)
+{
+ pr_debug("%s: %x %lx\n", __func__, offset, *size);
+
+ if (!data->otp_part) {
+ data->otp_part = memalign(CONFIG_SYS_CACHELINE_SIZE, OTP_SIZE);
+ if (!data->otp_part)
+ return -ENOMEM;
+ }
+
+ if (!offset)
+ memset(data->otp_part, 0, OTP_SIZE);
+
+ if (offset + *size > OTP_SIZE)
+ *size = OTP_SIZE - offset;
+
+ memcpy((void *)((u32)data->otp_part + offset), buffer, *size);
+
+ return 0;
+}
+
+int stm32prog_otp_read(struct stm32prog_data *data, u32 offset, u8 *buffer,
+ long *size)
+{
+#ifndef CONFIG_ARM_SMCCC
+ stm32prog_err("OTP update not supported");
+
+ return -1;
+#else
+ int result = 0;
+
+ pr_debug("%s: %x %lx\n", __func__, offset, *size);
+ /* alway read for first packet */
+ if (!offset) {
+ if (!data->otp_part)
+ data->otp_part =
+ memalign(CONFIG_SYS_CACHELINE_SIZE, OTP_SIZE);
+
+ if (!data->otp_part) {
+ result = -ENOMEM;
+ goto end_otp_read;
+ }
+
+ /* init struct with 0 */
+ memset(data->otp_part, 0, OTP_SIZE);
+
+ /* call the service */
+ result = stm32_smc_exec(STM32_SMC_BSEC, STM32_SMC_READ_ALL,
+ (u32)data->otp_part, 0);
+ if (result)
+ goto end_otp_read;
+ }
+
+ if (!data->otp_part) {
+ result = -ENOMEM;
+ goto end_otp_read;
+ }
+
+ if (offset + *size > OTP_SIZE)
+ *size = OTP_SIZE - offset;
+ memcpy(buffer, (void *)((u32)data->otp_part + offset), *size);
+
+end_otp_read:
+ pr_debug("%s: result %i\n", __func__, result);
+
+ return result;
+#endif
+}
+
+int stm32prog_otp_start(struct stm32prog_data *data)
+{
+#ifndef CONFIG_ARM_SMCCC
+ stm32prog_err("OTP update not supported");
+
+ return -1;
+#else
+ int result = 0;
+ struct arm_smccc_res res;
+
+ if (!data->otp_part) {
+ stm32prog_err("start OTP without data");
+ return -1;
+ }
+
+ arm_smccc_smc(STM32_SMC_BSEC, STM32_SMC_WRITE_ALL,
+ (u32)data->otp_part, 0, 0, 0, 0, 0, &res);
+
+ if (!res.a0) {
+ switch (res.a1) {
+ case 0:
+ result = 0;
+ break;
+ case 1:
+ stm32prog_err("Provisioning");
+ result = 0;
+ break;
+ default:
+ pr_err("%s: OTP incorrect value (err = %ld)\n",
+ __func__, res.a1);
+ result = -EINVAL;
+ break;
+ }
+ } else {
+ pr_err("%s: Failed to exec svc=%x op=%x in secure mode (err = %ld)\n",
+ __func__, STM32_SMC_BSEC, STM32_SMC_WRITE_ALL, res.a0);
+ result = -EINVAL;
+ }
+
+ free(data->otp_part);
+ data->otp_part = NULL;
+ pr_debug("%s: result %i\n", __func__, result);
+
+ return result;
+#endif
+}
+