1 /*
2 * i2c_msg.c
3 *
4 * i2c message utils
5 *
6 * Copyright (c) 2023 Huawei Device Co., Ltd.
7 *
8 * HDF is dual licensed: you can use it either under the terms of
9 * the GPL, or the BSD license, at your option.
10 * See the LICENSE file in the root of this repository for complete details.
11 */
12
13 #include "i2c_msg.h"
14 #include "i2c_service.h"
15 #include "osal_mem.h"
16
AssignReplayBuffer(uint32_t lenReply,uint8_t ** bufReply,struct I2cMsg * msgs,int16_t count)17 int32_t AssignReplayBuffer(uint32_t lenReply, uint8_t **bufReply, struct I2cMsg *msgs, int16_t count)
18 {
19 int16_t i;
20 uint8_t *buf = NULL;
21
22 *bufReply = OsalMemCalloc(lenReply);
23 if (*bufReply == NULL) {
24 HDF_LOGE("AssignReplayBuffer: memCalloc for bufReply fail!");
25 return HDF_ERR_MALLOC_FAIL;
26 }
27 for (i = 0, buf = *bufReply; i < count && buf < (*bufReply + lenReply); i++) {
28 if ((msgs[i].flags & I2C_FLAG_READ) != 0) {
29 msgs[i].buf = buf;
30 buf += msgs[i].len;
31 }
32 }
33
34 return HDF_SUCCESS;
35 }
36
RebuildMsgs(struct HdfSBuf * data,struct I2cMsg ** outMsgs,int16_t count)37 static int32_t RebuildMsgs(struct HdfSBuf *data, struct I2cMsg **outMsgs, int16_t count)
38 {
39 int16_t i;
40 uint32_t len;
41 uint32_t lenReply = 0;
42 uint8_t **bufReply = NULL;
43 struct I2cUserMsg *userMsgs = NULL;
44 struct I2cMsg *msgs = NULL;
45 size_t msgSize = sizeof(struct I2cMsg) * count;
46 uint8_t *buf = NULL;
47
48 msgs = OsalMemCalloc(msgSize + sizeof(void *));
49 if (msgs == NULL) {
50 HDF_LOGE("RebuildMsgs: memCalloc for msgs fail!");
51 return HDF_ERR_MALLOC_FAIL;
52 }
53 bufReply = (uint8_t **)((uint8_t *)msgs + msgSize);
54
55 for (i = 0; i < count; i++) {
56 if (!HdfSbufReadBuffer(data, (const void **)&userMsgs, &len) || (userMsgs == NULL) ||
57 (len != sizeof(struct I2cUserMsg))) {
58 HDF_LOGE("RebuildMsgs: read userMsgs fail!");
59 OsalMemFree(msgs);
60 return HDF_ERR_IO;
61 }
62 msgs[i].addr = userMsgs->addr;
63 msgs[i].len = userMsgs->len;
64 msgs[i].buf = NULL;
65 msgs[i].flags = userMsgs->flags;
66 if ((msgs[i].flags & I2C_FLAG_READ) != 0) {
67 lenReply += msgs[i].len;
68 } else if ((!HdfSbufReadBuffer(data, (const void **)&buf, &len)) || (buf == NULL)) {
69 HDF_LOGE("RebuildMsgs: read msg[%d] buf fail!", i);
70 } else {
71 msgs[i].len = len;
72 msgs[i].buf = buf;
73 }
74 }
75
76 if (lenReply > 0 && AssignReplayBuffer(lenReply, bufReply, msgs, count) != HDF_SUCCESS) {
77 HDF_LOGE("RebuildMsgs: assign replay buffer fail!");
78 OsalMemFree(msgs);
79 return HDF_FAILURE;
80 }
81
82 *outMsgs = msgs;
83 return HDF_SUCCESS;
84 }
85
I2cMsgsRebuildFromSbuf(struct HdfSBuf * data,struct I2cMsg ** msgs,int16_t * count)86 int32_t I2cMsgsRebuildFromSbuf(struct HdfSBuf *data, struct I2cMsg **msgs, int16_t *count)
87 {
88 int16_t msgCount = 0;
89 struct I2cMsg *builtMsgs = NULL;
90 int32_t ret;
91
92 if (!HdfSbufReadInt16(data, &msgCount)) {
93 HDF_LOGE("I2cMsgsRebuildFromSbuf: read count fail!");
94 return HDF_ERR_IO;
95 }
96
97 if (msgCount <= 0) {
98 HDF_LOGE("I2cMsgsRebuildFromSbuf: count %d out of range!", msgCount);
99 return HDF_ERR_OUT_OF_RANGE;
100 }
101
102 ret = RebuildMsgs(data, &builtMsgs, msgCount);
103
104 *count = msgCount;
105 *msgs = builtMsgs;
106 return ret;
107 }
108
I2cMsgsWriteToSbuf(struct I2cMsg * msgs,int16_t count,struct HdfSBuf * reply)109 int32_t I2cMsgsWriteToSbuf(struct I2cMsg *msgs, int16_t count, struct HdfSBuf *reply)
110 {
111 int16_t i;
112 for (i = 0; i < count; i++) {
113 if ((msgs[i].flags & I2C_FLAG_READ) == 0) {
114 continue;
115 }
116 if (!HdfSbufWriteBuffer(reply, msgs[i].buf, msgs[i].len)) {
117 HDF_LOGE("I2cTransferWriteBackMsgs: write msg[%hd] reply fail!", i);
118 return HDF_ERR_IO;
119 }
120 }
121
122 return HDF_SUCCESS;
123 }
124
I2cMsgsFree(struct I2cMsg * msgs,int16_t count)125 void I2cMsgsFree(struct I2cMsg *msgs, int16_t count)
126 {
127 uint8_t **bufReply = NULL;
128
129 if (msgs == NULL) {
130 HDF_LOGE("I2cMsgsFree: msgs is null!");
131 return;
132 }
133
134 bufReply = (uint8_t **)((uint8_t *)msgs + sizeof(struct I2cMsg) * count);
135 OsalMemFree(*bufReply);
136 OsalMemFree(msgs);
137 }
138