1 /*
2  * Copyright (c) 2023 Huawei Device Co., Ltd.
3  *
4  * HDF is dual licensed: you can use it either under the terms of
5  * the GPL, or the BSD license, at your option.
6  * See the LICENSE file in the root of this repository for complete details.
7  */
8 
9 #include "clock_core.h"
10 #include "hdf_device_desc.h"
11 #include "hdf_log.h"
12 #include "osal_mem.h"
13 #include "osal_spinlock.h"
14 #include "osal_time.h"
15 #include "platform_core.h"
16 #include "platform_trace.h"
17 
18 #define HDF_LOG_TAG clock_core_c
19 
20 struct ClockManager {
21     struct IDeviceIoService service;
22     struct HdfDeviceObject *device;
23     struct ClockDevice *devices[CLOCK_DEVICES_MAX];
24     OsalSpinlock spin;
25 };
26 
27 static struct ClockManager *g_clockManager = NULL;
28 
ClockDeviceLockDefault(struct ClockDevice * device)29 static int32_t ClockDeviceLockDefault(struct ClockDevice *device)
30 {
31     if (device == NULL) {
32         HDF_LOGE("ClockDeviceLockDefault: device is null!");
33         return HDF_ERR_INVALID_OBJECT;
34     }
35     return OsalSpinLock(&device->spin);
36 }
37 
ClockDeviceUnlockDefault(struct ClockDevice * device)38 static void ClockDeviceUnlockDefault(struct ClockDevice *device)
39 {
40     if (device == NULL) {
41         HDF_LOGE("ClockDeviceUnlockDefault: device is null!");
42         return;
43     }
44     (void)OsalSpinUnlock(&device->spin);
45 }
46 
47 static const struct ClockLockMethod g_clockLockOpsDefault = {
48     .lock = ClockDeviceLockDefault,
49     .unlock = ClockDeviceUnlockDefault,
50 };
51 
ClockDeviceLock(struct ClockDevice * device)52 static inline int32_t ClockDeviceLock(struct ClockDevice *device)
53 {
54     if (device->lockOps == NULL || device->lockOps->lock == NULL) {
55         HDF_LOGE("ClockDeviceLock: clock device lock is not support!");
56         return HDF_ERR_NOT_SUPPORT;
57     }
58     return device->lockOps->lock(device);
59 }
60 
ClockDeviceUnlock(struct ClockDevice * device)61 static inline void ClockDeviceUnlock(struct ClockDevice *device)
62 {
63     if (device->lockOps != NULL && device->lockOps->unlock != NULL) {
64         device->lockOps->unlock(device);
65     }
66 }
67 
ClockManagerAddDevice(struct ClockDevice * device)68 static int32_t ClockManagerAddDevice(struct ClockDevice *device)
69 {
70     int32_t ret;
71     struct ClockManager *manager = g_clockManager;
72 
73     if (device->deviceIndex >= CLOCK_DEVICES_MAX) {
74         HDF_LOGE("ClockManagerAddDevice: deviceIndex:%u exceed!", device->deviceIndex);
75         return HDF_ERR_INVALID_OBJECT;
76     }
77 
78     if (manager == NULL) {
79         HDF_LOGE("ClockManagerAddDevice: get clock manager fail!");
80         return HDF_ERR_NOT_SUPPORT;
81     }
82 
83     if (OsalSpinLockIrq(&manager->spin) != HDF_SUCCESS) {
84         HDF_LOGE("ClockManagerAddDevice: lock clock manager fail!");
85         return HDF_ERR_DEVICE_BUSY;
86     }
87 
88     if (manager->devices[device->deviceIndex]) {
89         HDF_LOGE("ClockManagerAddDevice: clock device num:%u already exits!", device->deviceIndex);
90         ret = HDF_FAILURE;
91     } else {
92         manager->devices[device->deviceIndex] = device;
93         HDF_LOGE("ClockManagerAddDevice: clock device num:%u  add success!", device->deviceIndex);
94         ret = HDF_SUCCESS;
95     }
96 
97     (void)OsalSpinUnlockIrq(&manager->spin);
98     return ret;
99 }
100 
ClockManagerRemoveDevice(struct ClockDevice * device)101 static void ClockManagerRemoveDevice(struct ClockDevice *device)
102 {
103     struct ClockManager *manager = g_clockManager;
104 
105     if (device->deviceIndex < 0 || device->deviceIndex >= CLOCK_DEVICES_MAX) {
106         HDF_LOGE("ClockManagerRemoveDevice: invalid devNum:%u!", device->deviceIndex);
107         return;
108     }
109 
110     if (manager == NULL) {
111         HDF_LOGE("ClockManagerRemoveDevice: get clock manager fail!");
112         return;
113     }
114 
115     if (OsalSpinLockIrq(&manager->spin) != HDF_SUCCESS) {
116         HDF_LOGE("ClockManagerRemoveDevice: lock clock manager fail!");
117         return;
118     }
119 
120     if (manager->devices[device->deviceIndex] != device) {
121         HDF_LOGE("ClockManagerRemoveDevice: clock device(%u) not in manager!", device->deviceIndex);
122     } else {
123         manager->devices[device->deviceIndex] = NULL;
124     }
125 
126     (void)OsalSpinUnlockIrq(&manager->spin);
127 }
128 
ClockDeviceRemove(struct ClockDevice * device)129 void ClockDeviceRemove(struct ClockDevice *device)
130 {
131     if (device == NULL) {
132         HDF_LOGE("ClockDeviceRemove: device is null!");
133         return;
134     }
135     ClockManagerRemoveDevice(device);
136     (void)OsalSpinDestroy(&device->spin);
137 }
138 
ClockManagerFindDevice(uint32_t number)139 static struct ClockDevice *ClockManagerFindDevice(uint32_t number)
140 {
141     struct ClockDevice *device = NULL;
142     struct ClockManager *manager = g_clockManager;
143 
144     if (number >= CLOCK_DEVICES_MAX) {
145         HDF_LOGE("ClockManagerFindDevice: invalid devNum:%u!", number);
146         return NULL;
147     }
148 
149     if (manager == NULL) {
150         HDF_LOGE("ClockManagerFindDevice: get clock manager fail!");
151         return NULL;
152     }
153 
154     if (OsalSpinLockIrq(&manager->spin) != HDF_SUCCESS) {
155         HDF_LOGE("ClockManagerFindDevice: lock clock manager fail!");
156         return NULL;
157     }
158 
159     device = manager->devices[number];
160     (void)OsalSpinUnlockIrq(&manager->spin);
161 
162     return device;
163 }
164 
ClockDeviceAdd(struct ClockDevice * device)165 int32_t ClockDeviceAdd(struct ClockDevice *device)
166 {
167     int32_t ret;
168     if (device == NULL) {
169         HDF_LOGE("ClockDeviceAdd: device is null!");
170         return HDF_ERR_INVALID_OBJECT;
171     }
172 
173     if (device->ops == NULL) {
174         HDF_LOGE("ClockDeviceAdd: no ops supplied!");
175         return HDF_ERR_INVALID_OBJECT;
176     }
177 
178     if (device->lockOps == NULL) {
179         HDF_LOGI("ClockDeviceAdd: use default lockOps!");
180         device->lockOps = &g_clockLockOpsDefault;
181     }
182 
183     if (OsalSpinInit(&device->spin) != HDF_SUCCESS) {
184         HDF_LOGE("ClockDeviceAdd: init lock fail!");
185         return HDF_FAILURE;
186     }
187 
188     ret = ClockManagerAddDevice(device);
189     if (ret != HDF_SUCCESS) {
190         HDF_LOGE("ClockDeviceAdd: clock manager add device fail!");
191         (void)OsalSpinDestroy(&device->spin);
192     }
193     return ret;
194 }
195 
ClockManagerGetAIdleDeviceId()196 int32_t ClockManagerGetAIdleDeviceId()
197 {
198     int32_t ret = -1;
199     int32_t id;
200     struct ClockManager *manager = g_clockManager;
201 
202     if (manager == NULL) {
203         HDF_LOGE("ClockManagerAddDevice: get clock manager fail!");
204         return HDF_ERR_NOT_SUPPORT;
205     }
206 
207     if (OsalSpinLockIrq(&manager->spin) != HDF_SUCCESS) {
208         HDF_LOGE("ClockManagerAddDevice: lock clock manager fail!");
209         return HDF_ERR_DEVICE_BUSY;
210     }
211 
212     for (id = 0; id < CLOCK_DEVICES_MAX; id++) {
213         if (manager->devices[id] == NULL) {
214             ret = id;
215             break;
216         }
217     }
218 
219     (void)OsalSpinUnlockIrq(&manager->spin);
220     return ret;
221 }
222 
ClockDeviceGet(uint32_t number)223 struct ClockDevice *ClockDeviceGet(uint32_t number)
224 {
225     return ClockManagerFindDevice(number);
226 }
227 
ClockDeviceStart(struct ClockDevice * device)228 int32_t ClockDeviceStart(struct ClockDevice *device)
229 {
230     int32_t ret;
231 
232     if (device == NULL) {
233         HDF_LOGE("ClockDeviceStart: device is null!");
234         return HDF_ERR_INVALID_OBJECT;
235     }
236 
237     if (device->ops == NULL || device->ops->start == NULL) {
238         HDF_LOGE("ClockDeviceStart: ops or start is null!");
239         return HDF_ERR_NOT_SUPPORT;
240     }
241 
242     if (ClockDeviceLock(device) != HDF_SUCCESS) {
243         HDF_LOGE("ClockDeviceStart: lock clock device fail!");
244         return HDF_ERR_DEVICE_BUSY;
245     }
246 
247     ret = device->ops->start(device);
248 
249     ClockDeviceUnlock(device);
250     return ret;
251 }
252 
ClockDeviceStop(struct ClockDevice * device)253 int32_t ClockDeviceStop(struct ClockDevice *device)
254 {
255     int32_t ret;
256 
257     if (device == NULL) {
258         HDF_LOGE("ClockDeviceStart: device is null!");
259         return HDF_ERR_INVALID_OBJECT;
260     }
261 
262     if (device->ops == NULL || device->ops->stop == NULL) {
263         HDF_LOGE("ClockDeviceStart: ops or start is null!");
264         return HDF_ERR_NOT_SUPPORT;
265     }
266 
267     if (ClockDeviceLock(device) != HDF_SUCCESS) {
268         HDF_LOGE("ClockDeviceStart: lock clock device fail!");
269         return HDF_ERR_DEVICE_BUSY;
270     }
271 
272     ret = device->ops->stop(device);
273 
274     ClockDeviceUnlock(device);
275     return ret;
276 }
277 
ClockDeviceOpen(uint32_t number)278 struct ClockDevice *ClockDeviceOpen(uint32_t number)
279 {
280     int32_t ret;
281     struct ClockDevice *device = NULL;
282 
283     device = ClockDeviceGet(number);
284     if (device == NULL) {
285         HDF_LOGE("ClockDeviceOpen: get clock device fail!");
286         return NULL;
287     }
288 
289     ret = ClockDeviceStart(device);
290     if (ret != HDF_SUCCESS) {
291         HDF_LOGE("ClockDeviceOpen: start clock device fail!");
292         return NULL;
293     }
294 
295     return device;
296 }
297 
ClockDeviceClose(struct ClockDevice * device)298 int32_t ClockDeviceClose(struct ClockDevice *device)
299 {
300     int32_t ret;
301 
302     if (device == NULL) {
303         HDF_LOGE("ClockDeviceClose: get clock device fail!");
304         return NULL;
305     }
306     ret = ClockDeviceStop(device);
307     if (ret != HDF_SUCCESS) {
308         HDF_LOGE("ClockDeviceClose: start clock device fail!");
309         return NULL;
310     }
311 
312     return HDF_SUCCESS;
313 }
314 
ClockDeviceSetRate(struct ClockDevice * device,uint32_t rate)315 int32_t ClockDeviceSetRate(struct ClockDevice *device, uint32_t rate)
316 {
317     int32_t ret;
318 
319     if (device == NULL) {
320         HDF_LOGE("ClockDeviceSetRate: device is null!");
321         return HDF_ERR_INVALID_OBJECT;
322     }
323 
324     if (device->ops == NULL || device->ops->setRate == NULL) {
325         HDF_LOGE("ClockDeviceSetRate: ops or start is null!");
326         return HDF_ERR_NOT_SUPPORT;
327     }
328 
329     if (ClockDeviceLock(device) != HDF_SUCCESS) {
330         HDF_LOGE("ClockDeviceSetRate: lock clock device fail!");
331         return HDF_ERR_DEVICE_BUSY;
332     }
333 
334     ret = device->ops->setRate(device, rate);
335 
336     ClockDeviceUnlock(device);
337     return ret;
338 }
339 
ClockDeviceGetRate(struct ClockDevice * device,uint32_t * rate)340 int32_t ClockDeviceGetRate(struct ClockDevice *device, uint32_t *rate)
341 {
342     int32_t ret;
343     if (device == NULL) {
344         HDF_LOGE("ClockDeviceGetRate: device is null!");
345         return HDF_ERR_INVALID_OBJECT;
346     }
347 
348     if (device->ops == NULL || device->ops->getRate == NULL) {
349         HDF_LOGE("ClockDeviceGetRate: ops or start is null!");
350         return HDF_ERR_NOT_SUPPORT;
351     }
352 
353     if (ClockDeviceLock(device) != HDF_SUCCESS) {
354         HDF_LOGE("ClockDeviceGetRate: lock clock device fail!");
355         return HDF_ERR_DEVICE_BUSY;
356     }
357 
358     ret = device->ops->getRate(device, rate);
359     ClockDeviceUnlock(device);
360     return ret;
361 }
362 
ClockDeviceGetParent(struct ClockDevice * device)363 struct ClockDevice *ClockDeviceGetParent(struct ClockDevice *device)
364 {
365     struct ClockDevice *parent = NULL;
366 
367     if (device == NULL) {
368         HDF_LOGE("ClockDeviceGetParent: device is null!");
369         return NULL;
370     }
371 
372     if (device->ops == NULL || device->ops->getParent == NULL) {
373         HDF_LOGE("ClockDeviceGetParent: ops or getParent is null!");
374         return NULL;
375     }
376 
377     if (ClockDeviceLock(device) != HDF_SUCCESS) {
378         HDF_LOGE("ClockDeviceGetParent: lock clock device fail!");
379         return NULL;
380     }
381 
382     parent = device->ops->getParent(device);
383 
384     ClockDeviceUnlock(device);
385     return parent;
386 }
387 
ClockDeviceSetParent(struct ClockDevice * device,struct ClockDevice * parent)388 int32_t ClockDeviceSetParent(struct ClockDevice *device, struct ClockDevice *parent)
389 {
390     int32_t ret;
391 
392     if (device == NULL || parent == NULL) {
393         HDF_LOGE("ClockDeviceSetParent: device or is parent null!");
394         return HDF_ERR_INVALID_OBJECT;
395     }
396 
397     if (device->ops == NULL || device->ops->setParent == NULL) {
398         HDF_LOGE("ClockDeviceSetParent: ops or setParent is null!");
399         return HDF_ERR_NOT_SUPPORT;
400     }
401 
402     if (ClockDeviceLock(device) != HDF_SUCCESS) {
403         HDF_LOGE("ClockDeviceSetParent: lock clock device fail!");
404         return HDF_ERR_DEVICE_BUSY;
405     }
406 
407     if (ClockDeviceLock(parent) != HDF_SUCCESS) {
408         ClockDeviceUnlock(device);
409         HDF_LOGE("ClockDeviceGetParent: lock clock device fail!");
410         return HDF_ERR_DEVICE_BUSY;
411     }
412     ret = device->ops->setParent(device, parent);
413 
414     ClockDeviceUnlock(device);
415     ClockDeviceUnlock(parent);
416     return ret;
417 }
418 
ClockDeviceDisable(struct ClockDevice * device)419 int32_t ClockDeviceDisable(struct ClockDevice *device)
420 {
421     int32_t ret;
422 
423     if (device == NULL) {
424         HDF_LOGE("ClockDeviceDisable: device is null!");
425         return HDF_ERR_INVALID_OBJECT;
426     }
427 
428     if (device->ops == NULL || device->ops->disable == NULL) {
429         HDF_LOGE("ClockDeviceDisable: ops or start is null!");
430         return HDF_ERR_NOT_SUPPORT;
431     }
432 
433     if (ClockDeviceLock(device) != HDF_SUCCESS) {
434         HDF_LOGE("ClockDeviceDisable: lock clock device fail!");
435         return HDF_ERR_DEVICE_BUSY;
436     }
437 
438     ret = device->ops->disable(device);
439 
440     ClockDeviceUnlock(device);
441     return ret;
442 }
443 
ClockDeviceEnable(struct ClockDevice * device)444 int32_t ClockDeviceEnable(struct ClockDevice *device)
445 {
446     int32_t ret;
447 
448     if (device == NULL) {
449         HDF_LOGE("ClockDeviceEnable: device is null!");
450         return HDF_ERR_INVALID_OBJECT;
451     }
452 
453     if (device->ops == NULL || device->ops->enable == NULL) {
454         HDF_LOGE("ClockDeviceEnable: ops or start is null!");
455         return HDF_ERR_NOT_SUPPORT;
456     }
457 
458     if (ClockDeviceLock(device) != HDF_SUCCESS) {
459         HDF_LOGE("ClockDeviceEnable: lock clock device fail!");
460         return HDF_ERR_DEVICE_BUSY;
461     }
462 
463     ret = device->ops->enable(device);
464 
465     ClockDeviceUnlock(device);
466     return ret;
467 }
468 
ClockManagerOpen(struct HdfSBuf * data,struct HdfSBuf * reply)469 static int32_t ClockManagerOpen(struct HdfSBuf *data, struct HdfSBuf *reply)
470 {
471     uint32_t number = 0;
472     int32_t ret = HDF_SUCCESS;
473 
474     if (data == NULL || reply == NULL) {
475         HDF_LOGE("ClockManagerOpen: invalid data or reply!");
476         return HDF_ERR_INVALID_PARAM;
477     }
478 
479     if (!HdfSbufReadUint32(data, &number)) {
480         HDF_LOGE("ClockManagerOpen: fail to read number!\n");
481         return HDF_ERR_INVALID_PARAM;
482     }
483 
484     if (number >= CLOCK_DEVICES_MAX) {
485         HDF_LOGE("ClockManagerOpen: invalid number!");
486         return HDF_ERR_INVALID_PARAM;
487     }
488 
489     if (ClockDeviceOpen(number) == NULL) {
490         HDF_LOGE("ClockManagerOpen: get device %u fail!", number);
491         return HDF_ERR_NOT_SUPPORT;
492     }
493 
494     if (!HdfSbufWriteUint32(reply, number + CLOCK_HANDLE_SHIFT)) {
495         HDF_LOGE("ClockManagerOpen:  fail to write number!\n");
496         return HDF_ERR_INVALID_PARAM;
497     }
498 
499     return ret;
500 }
501 
ClockManagerClose(struct HdfSBuf * data,struct HdfSBuf * reply)502 static int32_t ClockManagerClose(struct HdfSBuf *data, struct HdfSBuf *reply)
503 {
504     uint32_t number = 0;
505     int32_t ret;
506     struct ClockDevice *device = NULL;
507 
508     if (data == NULL || reply == NULL) {
509         HDF_LOGE("ClockManagerClose: invalid data or reply!");
510         return HDF_ERR_INVALID_PARAM;
511     }
512 
513     if (!HdfSbufReadUint32(data, &number)) {
514         HDF_LOGE("ClockManagerClose: fail to read handle!\n");
515         return HDF_ERR_INVALID_PARAM;
516     }
517 
518     number -= CLOCK_HANDLE_SHIFT;
519     if (number >= CLOCK_DEVICES_MAX) {
520         HDF_LOGE("ClockManagerClose: invalid number!");
521         return HDF_ERR_INVALID_PARAM;
522     }
523 
524     device = ClockDeviceGet(number);
525     if (device == NULL) {
526         HDF_LOGE("ClockManagerEnable: get clock device fail!");
527         return HDF_ERR_INVALID_PARAM;
528     }
529 
530     ret = ClockDeviceClose(device);
531     return ret;
532 }
533 
ClockManagerEnable(struct HdfSBuf * data)534 static int32_t ClockManagerEnable(struct HdfSBuf *data)
535 {
536     uint32_t number = 0;
537     int32_t ret;
538     struct ClockDevice *device = NULL;
539 
540     if (data == NULL) {
541         HDF_LOGE("ClockManagerEnable: invalid data!");
542         return HDF_ERR_INVALID_PARAM;
543     }
544     if (!HdfSbufReadUint32(data, &number)) {
545         HDF_LOGE("ClockManagerEnable:  fail to read number!\n");
546         return HDF_ERR_INVALID_PARAM;
547     }
548 
549     number -= CLOCK_HANDLE_SHIFT;
550     if (number >= CLOCK_DEVICES_MAX) {
551         HDF_LOGE("ClockManagerEnable: invalid number!");
552         return HDF_ERR_INVALID_PARAM;
553     }
554 
555     device = ClockDeviceGet(number);
556     if (device == NULL) {
557         HDF_LOGE("ClockManagerEnable: get clock device fail!");
558         return HDF_ERR_INVALID_PARAM;
559     }
560 
561     ret = ClockDeviceEnable(device);
562     return ret;
563 }
564 
ClockManagerDisable(struct HdfSBuf * data)565 static int32_t ClockManagerDisable(struct HdfSBuf *data)
566 {
567     uint32_t number = 0;
568     int32_t ret;
569     struct ClockDevice *device = NULL;
570 
571     if (data == NULL) {
572         HDF_LOGE("ClockManagerDisable: invalid data!");
573         return HDF_ERR_INVALID_PARAM;
574     }
575     if (!HdfSbufReadUint32(data, &number)) {
576         HDF_LOGE("ClockManagerDisable:  fail to read number!\n");
577         return HDF_ERR_INVALID_PARAM;
578     }
579 
580     number -= CLOCK_HANDLE_SHIFT;
581     if (number >= CLOCK_DEVICES_MAX) {
582         HDF_LOGE("ClockManagerDisable: invalid number!");
583         return HDF_ERR_INVALID_PARAM;
584     }
585 
586     device = ClockDeviceGet(number);
587     if (device == NULL) {
588         HDF_LOGE("ClockManagerDisable: get clock device fail!");
589         return HDF_ERR_INVALID_PARAM;
590     }
591     ret = ClockDeviceDisable(device);
592     return ret;
593 }
594 
ClockManagerSetRate(struct HdfSBuf * data,struct HdfSBuf * reply)595 static int32_t ClockManagerSetRate(struct HdfSBuf *data, struct HdfSBuf *reply)
596 {
597     uint32_t number = 0;
598     uint32_t rate = 0;
599     int32_t ret;
600     struct ClockDevice *device = NULL;
601     HDF_LOGE("func = %s line = %d", __FUNCTION__, __LINE__);
602     if (data == NULL || reply == NULL) {
603         HDF_LOGE("ClockManagerSetRate: invalid data or reply!");
604         return HDF_ERR_INVALID_PARAM;
605     }
606 
607     if (!HdfSbufReadUint32(data, &number)) {
608         HDF_LOGE("ClockManagerSetRate:  fail to read handle!\n");
609         return HDF_ERR_INVALID_PARAM;
610     }
611 
612     number -= CLOCK_HANDLE_SHIFT;
613     if (number >= CLOCK_DEVICES_MAX) {
614         HDF_LOGE("ClockManagerSetRate: invalid number!");
615         return HDF_ERR_INVALID_PARAM;
616     }
617 
618     if (!HdfSbufReadUint32(data, &rate)) {
619         HDF_LOGE("ClockManagerSetRate:  fail to read rate!\n");
620         return HDF_ERR_INVALID_PARAM;
621     }
622 
623     device = ClockDeviceGet(number);
624     if (device == NULL) {
625         HDF_LOGE("ClockManagerSetRate: get clock device fail!");
626         return HDF_ERR_INVALID_PARAM;
627     }
628 
629     ret = ClockDeviceSetRate(device, rate);
630     return ret;
631 }
632 
ClockManagerGetRate(struct HdfSBuf * data,struct HdfSBuf * reply)633 static int32_t ClockManagerGetRate(struct HdfSBuf *data, struct HdfSBuf *reply)
634 {
635     uint32_t number = 0;
636     uint32_t rate = 0;
637     struct ClockDevice *device = NULL;
638 
639     if (data == NULL || reply == NULL) {
640         HDF_LOGE("ClockManagerSetRate: invalid data or reply!");
641         return HDF_ERR_INVALID_PARAM;
642     }
643 
644     if (!HdfSbufReadUint32(data, &number)) {
645         HDF_LOGE("ClockManagerGetRate:  fail to read handle!\n");
646         return HDF_ERR_INVALID_PARAM;
647     }
648 
649     number -= CLOCK_HANDLE_SHIFT;
650     if (number >= CLOCK_DEVICES_MAX) {
651         HDF_LOGE("ClockManagerGetRate: invalid number!");
652         return HDF_ERR_INVALID_PARAM;
653     }
654 
655     device = ClockDeviceGet(number);
656     if (device == NULL) {
657         HDF_LOGE("ClockManagerSetRate: get clock device fail!");
658         return HDF_ERR_INVALID_PARAM;
659     }
660     ClockDeviceGetRate(device, &rate);
661 
662     if (!HdfSbufWriteUint32(reply, rate)) {
663         HDF_LOGE("ClockManagerGetRate:  fail to write rate!\n");
664         return HDF_ERR_INVALID_PARAM;
665     }
666     HDF_LOGI("ClockManagerGetRate: get rate = %u !", rate);
667     return HDF_SUCCESS;
668 }
669 
ClockManagerGetParent(struct HdfSBuf * data,struct HdfSBuf * reply)670 static int32_t ClockManagerGetParent(struct HdfSBuf *data, struct HdfSBuf *reply)
671 {
672     uint32_t number = 0;
673     struct ClockDevice *device = NULL;
674     struct ClockDevice *parent = NULL;
675 
676     if (reply == NULL) {
677         HDF_LOGE("ClockManagerGetParent: invalid reply!\n");
678         return HDF_ERR_INVALID_PARAM;
679     }
680 
681     if (!HdfSbufReadUint32(data, &number)) {
682         HDF_LOGE("ClockManagerGetParent:  fail to read number!\n");
683         return HDF_ERR_INVALID_PARAM;
684     }
685 
686     number -= CLOCK_HANDLE_SHIFT;
687     if (number >= CLOCK_DEVICES_MAX) {
688         HDF_LOGE("ClockManagerGetParent: invalid number!");
689         return HDF_ERR_INVALID_PARAM;
690     }
691 
692     device = ClockDeviceGet(number);
693     if (device == NULL) {
694         HDF_LOGE("ClockManagerGetParent: get clock device fail!");
695         return HDF_ERR_INVALID_PARAM;
696     }
697 
698     parent = ClockDeviceGetParent(device);
699     if (parent == NULL) {
700         HDF_LOGE("ClockManagerGetParent: get clock device fail!");
701         return HDF_ERR_INVALID_PARAM;
702     }
703 
704     if (!HdfSbufWriteUint32(reply, parent->deviceIndex + CLOCK_HANDLE_SHIFT)) {
705         HDF_LOGE("ClockManagerGetParent:  fail to write parent!\n");
706         return HDF_ERR_INVALID_PARAM;
707     }
708 
709     return HDF_SUCCESS;
710 }
711 
ClockManagerSetParent(struct HdfSBuf * data,struct HdfSBuf * reply)712 static int32_t ClockManagerSetParent(struct HdfSBuf *data, struct HdfSBuf *reply)
713 {
714     uint32_t number = 0;
715     uint32_t number_p = 0;
716     struct ClockDevice *device = NULL;
717     struct ClockDevice *parent = NULL;
718     int32_t ret;
719 
720     if (!HdfSbufReadUint32(data, &number)) {
721         HDF_LOGE("ClockManagerSetParent: fail to read handle!\n");
722         return HDF_ERR_INVALID_PARAM;
723     }
724 
725     number -= CLOCK_HANDLE_SHIFT;
726     if (number >= CLOCK_DEVICES_MAX) {
727         HDF_LOGE("ClockManagerSetParent: invalid number!");
728         return HDF_ERR_INVALID_PARAM;
729     }
730 
731     if (!HdfSbufReadUint32(data, &number_p)) {
732         HDF_LOGE("ClockManagerSetParent: fail to read parent!");
733         return HDF_ERR_INVALID_PARAM;
734     }
735 
736     number_p -= CLOCK_HANDLE_SHIFT;
737     if (number_p >= CLOCK_DEVICES_MAX) {
738         HDF_LOGE("ClockManagerSetParent: invalid number_p!");
739         return HDF_ERR_INVALID_PARAM;
740     }
741 
742     device = ClockDeviceGet(number);
743     if (device == NULL) {
744         HDF_LOGE("ClockManagerSetParent: get clock device fail!");
745         return HDF_ERR_INVALID_PARAM;
746     }
747 
748     parent = ClockDeviceGet(number_p);
749     if (parent == NULL) {
750         HDF_LOGE("ClockManagerSetParent: get clock device fail!");
751         return HDF_ERR_INVALID_PARAM;
752     }
753 
754     ret = ClockDeviceSetParent(device, parent);
755     if (ret != HDF_SUCCESS) {
756         HDF_LOGE("ClockManagerSetParent: number_p =%u , number =%u ", number_p, number);
757     }
758 
759     return ret;
760 }
761 
ClockManagerDispatch(struct HdfDeviceIoClient * client,int cmd,struct HdfSBuf * data,struct HdfSBuf * reply)762 static int32_t ClockManagerDispatch(struct HdfDeviceIoClient *client, int cmd, struct HdfSBuf *data,
763                                     struct HdfSBuf *reply)
764 {
765     if (data == NULL) {
766         HDF_LOGE("ClockManagerDispatch: invalid data cmd =%d!", cmd);
767         return HDF_ERR_INVALID_PARAM;
768     }
769 
770     (void)client;
771     switch (cmd) {
772         case CLOCK_IO_OPEN:
773             return ClockManagerOpen(data, reply);
774             break;
775         case CLOCK_IO_CLOSE:
776             return ClockManagerClose(data, reply);
777             break;
778         case CLOCK_IO_ENABLE:
779             return ClockManagerEnable(data);
780             break;
781         case CLOCK_IO_DISABLE:
782             return ClockManagerDisable(data);
783             break;
784         case CLOCK_IO_SET_RATE:
785             return ClockManagerSetRate(data, reply);
786             break;
787         case CLOCK_IO_GET_RATE:
788             return ClockManagerGetRate(data, reply);
789             break;
790         case CLOCK_IO_SET_PARENT:
791             return ClockManagerSetParent(data, reply);
792             break;
793         case CLOCK_IO_GET_PARENT:
794             return ClockManagerGetParent(data, reply);
795             break;
796         default:
797             HDF_LOGE("ClockManagerDispatch: cmd %d is not support!", cmd);
798             return HDF_ERR_NOT_SUPPORT;
799     }
800     return HDF_SUCCESS;
801 }
802 
ClockManagerBind(struct HdfDeviceObject * device)803 static int32_t ClockManagerBind(struct HdfDeviceObject *device)
804 {
805     (void)device;
806     return HDF_SUCCESS;
807 }
808 
ClockManagerInit(struct HdfDeviceObject * device)809 static int32_t ClockManagerInit(struct HdfDeviceObject *device)
810 {
811     int32_t ret;
812     struct ClockManager *manager = NULL;
813 
814     if (device == NULL) {
815         HDF_LOGE("ClockManagerInit: device is null!");
816         return HDF_ERR_INVALID_OBJECT;
817     }
818 
819     manager = (struct ClockManager *)OsalMemCalloc(sizeof(*manager));
820     if (manager == NULL) {
821         HDF_LOGE("ClockManagerInit: memcalloc for manager fail!");
822         return HDF_ERR_MALLOC_FAIL;
823     }
824 
825     ret = OsalSpinInit(&manager->spin);
826     if (ret != HDF_SUCCESS) {
827         HDF_LOGE("ClockManagerInit: spinlock init fail!");
828         OsalMemFree(manager);
829         return HDF_FAILURE;
830     }
831 
832     manager->device = device;
833     g_clockManager = manager;
834     device->service = &manager->service;
835     device->service->Dispatch = ClockManagerDispatch;
836     return HDF_SUCCESS;
837 }
838 
ClockManagerRelease(struct HdfDeviceObject * device)839 static void ClockManagerRelease(struct HdfDeviceObject *device)
840 {
841     struct ClockManager *manager = NULL;
842 
843     if (device == NULL) {
844         HDF_LOGE("ClockManagerRelease: device is null!");
845         return;
846     }
847 
848     manager = (struct ClockManager *)device->service;
849     if (manager == NULL) {
850         HDF_LOGE("ClockManagerRelease: no service bind!");
851         return;
852     }
853     g_clockManager = NULL;
854     OsalMemFree(manager);
855 }
856 
857 struct HdfDriverEntry g_clockManagerEntry = {
858     .moduleVersion = 1,
859     .Bind = ClockManagerBind,
860     .Init = ClockManagerInit,
861     .Release = ClockManagerRelease,
862     .moduleName = "HDF_PLATFORM_CLOCK_MANAGER",
863 };
864 HDF_INIT(g_clockManagerEntry);
865